Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_Proximity_config.h"
      17             : 
      18             : #if AP_PROXIMITY_MAV_ENABLED
      19             : 
      20             : #include "AP_Proximity_MAV.h"
      21             : #include <AP_HAL/AP_HAL.h>
      22             : #include <ctype.h>
      23             : #include <stdio.h>
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : #define PROXIMITY_MAV_TIMEOUT_MS    500 // distance messages must arrive within this many milliseconds
      28             : #define PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS  50  // obstacles will be transferred from temp boundary to actual boundary if mavlink message does not arrive within this many milliseconds
      29             : 
      30             : // update the state of the sensor
      31       45093 : void AP_Proximity_MAV::update(void)
      32             : {
      33             :     // check for timeout and set health status
      34       71909 :     if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_MAV_TIMEOUT_MS)) &&
      35       26816 :         (_last_upward_update_ms == 0 || (AP_HAL::millis() - _last_upward_update_ms > PROXIMITY_MAV_TIMEOUT_MS))) {
      36       26816 :         set_status(AP_Proximity::Status::NoData);
      37             :     } else {
      38       18277 :         set_status(AP_Proximity::Status::Good);
      39             :     }
      40       45093 : }
      41             : 
      42             : // get distance upwards in meters. returns true on success
      43       14105 : bool AP_Proximity_MAV::get_upward_distance(float &distance) const
      44             : {
      45       14105 :     if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_MAV_TIMEOUT_MS)) {
      46           0 :         distance = _distance_upward;
      47           0 :         return true;
      48             :     }
      49       14105 :     return false;
      50             : }
      51             : 
      52             : // handle mavlink messages
      53         460 : void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
      54             : {   
      55         460 :     switch (msg.msgid) {
      56          81 :         case (MAVLINK_MSG_ID_DISTANCE_SENSOR):
      57          81 :             handle_distance_sensor_msg(msg);
      58          81 :             break;
      59             : 
      60           3 :         case (MAVLINK_MSG_ID_OBSTACLE_DISTANCE):
      61           3 :             handle_obstacle_distance_msg(msg);
      62           3 :             break;
      63             : 
      64         376 :         case (MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D):
      65         376 :             handle_obstacle_distance_3d_msg(msg);
      66         376 :             break;    
      67             :     }
      68         460 : }
      69             : 
      70             : // handle mavlink DISTANCE_SENSOR messages
      71          81 : void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
      72             : {
      73             :     mavlink_distance_sensor_t packet;
      74          81 :     mavlink_msg_distance_sensor_decode(&msg, &packet);
      75             : 
      76             :     // store distance to appropriate sector based on orientation field
      77          81 :     if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
      78          81 :         const uint32_t previous_sys_time = _last_update_ms;
      79          81 :         _last_update_ms = AP_HAL::millis();
      80             : 
      81             :         // time_diff will check if the new message arrived significantly later than the last message
      82          81 :         const uint32_t time_diff = _last_update_ms - previous_sys_time;
      83             : 
      84          81 :         const uint32_t previous_msg_timestamp = _last_msg_update_timestamp_ms;
      85          81 :         _last_msg_update_timestamp_ms = packet.time_boot_ms;
      86             : 
      87             :         // we will add on to the last fence if the time stamp is the same
      88             :         // provided we got the new obstacle in less than PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS
      89          81 :         if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) {
      90             :             // push data from temp boundary to the main 3-D proximity boundary
      91          67 :             temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
      92             :             // clear temp boundary for new data
      93          67 :             temp_boundary.reset();
      94             :         }
      95             :         // store in meters
      96          81 :         const float distance = packet.current_distance * 0.01f;
      97          81 :         const uint8_t sector = packet.orientation;
      98             :         // get the face for this sector
      99          81 :         const float yaw_angle_deg = sector * 45;
     100          81 :         const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
     101          81 :         _distance_min = packet.min_distance * 0.01f;
     102          81 :         _distance_max = packet.max_distance * 0.01f;
     103          81 :         const bool in_range = distance <= _distance_max && distance >= _distance_min;
     104          81 :         if (in_range && !ignore_reading(yaw_angle_deg, distance, false)) {
     105          81 :             temp_boundary.add_distance(face, yaw_angle_deg, distance);
     106             :             // update OA database
     107          81 :             database_push(yaw_angle_deg, distance);
     108             :         }
     109             :     }
     110             : 
     111             :     // store upward distance
     112          81 :     if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
     113           0 :         _distance_upward = packet.current_distance * 0.01f;
     114           0 :         _last_upward_update_ms = AP_HAL::millis();
     115             :     }
     116          81 :     return;
     117             : }
     118             : 
     119             : // handle mavlink OBSTACLE_DISTANCE messages
     120           3 : void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg)
     121             : {
     122             :     mavlink_obstacle_distance_t packet;
     123           3 :     mavlink_msg_obstacle_distance_decode(&msg, &packet);
     124             : 
     125             :     // check increment (message's sector width)
     126             :     float increment;
     127           3 :     if (!is_zero(packet.increment_f)) {
     128             :         // use increment float
     129           3 :         increment = packet.increment_f;
     130           0 :     } else if (packet.increment != 0) {
     131             :         // use increment uint8_t
     132           0 :         increment = packet.increment;
     133             :     } else {
     134             :         // invalid increment
     135           0 :         return;
     136             :     }
     137             : 
     138           3 :     const uint8_t total_distances = MIN(((360.0f / fabsf(increment)) + 0.5f), MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
     139             : 
     140             :     // set distance min and max
     141           3 :     _distance_min = packet.min_distance * 0.01f;
     142           3 :     _distance_max = packet.max_distance * 0.01f;
     143           3 :     _last_update_ms = AP_HAL::millis();
     144             : 
     145             :     // get user configured yaw correction from front end
     146           3 :     const float param_yaw_offset = constrain_float(params.yaw_correction, -360.0f, +360.0f);
     147           3 :     const float yaw_correction = wrap_360(param_yaw_offset + packet.angle_offset);
     148           3 :     if (params.orientation != 0) {
     149           0 :         increment *= -1;
     150             :     }
     151             : 
     152           3 :     Vector3f current_pos;
     153           3 :     Matrix3f body_to_ned;
     154           3 :     const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
     155             : 
     156             :     // variables to calculate closest angle and distance for each face
     157           3 :     AP_Proximity_Boundary_3D::Face face;
     158           3 :     float face_distance = FLT_MAX;
     159           3 :     float face_yaw_deg = 0.0f;
     160           3 :     bool face_distance_valid = false;
     161             : 
     162             :     // reset this  boundary to fill with new data
     163           3 :     frontend.boundary.reset();
     164             : 
     165             :     // iterate over message's sectors
     166         147 :     for (uint8_t j = 0; j < total_distances; j++) {
     167         144 :         const uint16_t distance_cm = packet.distances[j];
     168         144 :         const float packet_distance_m = distance_cm * 0.01f;
     169         144 :         const float mid_angle = wrap_360((float)j * increment + yaw_correction);
     170             : 
     171         288 :         const bool range_check = distance_cm == 0 || distance_cm == 65535 || distance_cm < packet.min_distance ||
     172         144 :                                  distance_cm > packet.max_distance;
     173         144 :         if (range_check || ignore_reading(mid_angle, packet_distance_m, false)) {
     174             :             // sanity check failed, ignore this distance value
     175          66 :             continue;
     176             :         }
     177             : 
     178             :         // get face for this latest reading
     179          78 :         AP_Proximity_Boundary_3D::Face latest_face = frontend.boundary.get_face(mid_angle);
     180          78 :         if (latest_face != face) {
     181             :             // store previous face
     182           5 :             if (face_distance_valid) {
     183           2 :                 frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance, state.instance);
     184             :             } else {
     185           3 :                 frontend.boundary.reset_face(face, state.instance);
     186             :             }
     187             :             // init for latest face
     188           5 :             face = latest_face;
     189           5 :             face_distance_valid = false;
     190             :         }
     191             : 
     192             :         // update minimum distance found so far
     193          78 :         if (!face_distance_valid || (packet_distance_m < face_distance)) {
     194          40 :             face_yaw_deg = mid_angle;
     195          40 :             face_distance = packet_distance_m;
     196          40 :             face_distance_valid = true;
     197             :         }
     198             : 
     199             :         // update Object Avoidance database with Earth-frame point
     200          78 :         if (database_ready) {
     201           0 :             database_push(mid_angle, packet_distance_m, _last_update_ms, current_pos, body_to_ned);
     202             :         }
     203             :     }
     204             : 
     205             :     // process the last face
     206           3 :     if (face_distance_valid) {
     207           3 :         frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance, state.instance);
     208             :     } else {
     209           0 :         frontend.boundary.reset_face(face, state.instance);
     210             :     }
     211           3 :     return;
     212             : }
     213             : 
     214             : // handle mavlink OBSTACLE_DISTANCE_3D messages
     215         376 : void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &msg)
     216             : {
     217             :     mavlink_obstacle_distance_3d_t packet;
     218         376 :     mavlink_msg_obstacle_distance_3d_decode(&msg, &packet);
     219             : 
     220         376 :     const uint32_t previous_sys_time = _last_update_ms;
     221         376 :     _last_update_ms = AP_HAL::millis();
     222             : 
     223             :     // time_diff will check if the new message arrived significantly later than the last message
     224         376 :     const uint32_t time_diff = _last_update_ms - previous_sys_time;
     225             : 
     226         376 :     const uint32_t previous_msg_timestamp = _last_msg_update_timestamp_ms;
     227         376 :     _last_msg_update_timestamp_ms = packet.time_boot_ms;
     228             : 
     229         376 :     if (packet.frame != MAV_FRAME_BODY_FRD) {
     230             :         // we do not support this frame of reference yet
     231           0 :         return;
     232             :     }
     233             : 
     234         376 :     if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) {
     235             :         // push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived
     236         376 :         temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
     237             :         // clear temp boundary for new data
     238         376 :         temp_boundary.reset();
     239             :     }
     240             : 
     241         376 :     _distance_min = packet.min_distance;
     242         376 :     _distance_max = packet.max_distance;
     243             : 
     244         376 :     Vector3f current_pos;
     245         376 :     Matrix3f body_to_ned;
     246         376 :     const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
     247             : 
     248         376 :     const Vector3f obstacle_FRD(packet.x, packet.y, packet.z);
     249         376 :     const float obstacle_distance = obstacle_FRD.length();
     250         376 :     if (obstacle_distance < _distance_min || obstacle_distance > _distance_max || is_zero(obstacle_distance)) {
     251             :         // message isn't healthy
     252           0 :         return;
     253             :     }
     254             : 
     255             :     // convert to FRU
     256         376 :     const Vector3f obstacle(obstacle_FRD.x, obstacle_FRD.y, obstacle_FRD.z * -1.0f);
     257             : 
     258             :     // extract yaw and pitch from Obstacle Vector
     259         376 :     const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
     260         376 :     const float pitch = wrap_180(degrees(M_PI_2 - atan2f(obstacle.xy().length(), obstacle.z))); 
     261             : 
     262         376 :     if (ignore_reading(pitch, yaw, obstacle_distance, false)) {
     263             :         // obstacle is probably near ground or out of range
     264           0 :         return;
     265             :     }
     266             : 
     267             :     // allot to correct layer and sector based on calculated pitch and yaw
     268         376 :     const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(pitch, yaw);
     269         376 :     temp_boundary.add_distance(face, pitch, yaw, obstacle.length());
     270             : 
     271         376 :     if (database_ready) {
     272           0 :         database_push(yaw, pitch, obstacle.length(),_last_update_ms, current_pos, body_to_ned);
     273             :     }
     274         376 :     return;
     275             : }
     276             : 
     277             : #endif // AP_PROXIMITY_MAV_ENABLED