Browse Source

AP_RangeFinder: MAV only accepts DISTANCE_SENSOR with orient 25

25 is  MAV_SENSOR_ROTATION_PITCH_270 meaning downward facing
master
Randy Mackay 8 years ago
parent
commit
8215b92371
  1. 7
      libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp

7
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp

@ -51,8 +51,11 @@ void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg) @@ -51,8 +51,11 @@ void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg)
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(msg, &packet);
last_update_ms = AP_HAL::millis();
distance_cm = packet.current_distance;
// only accept distances for downward facing sensors
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
last_update_ms = AP_HAL::millis();
distance_cm = packet.current_distance;
}
}
/*

Loading…
Cancel
Save