Browse Source

GCS_MAVLink: add send_distance_sensor_downward function

mission-4.1.18
Pierre Kancir 8 years ago committed by Francisco Ferreira
parent
commit
0848d96354
  1. 1
      libraries/GCS_MAVLink/GCS.h
  2. 11
      libraries/GCS_MAVLink/GCS_Common.cpp

1
libraries/GCS_MAVLink/GCS.h

@ -144,6 +144,7 @@ public:
bool send_battery_status(const AP_BattMonitor &battery) const; bool send_battery_status(const AP_BattMonitor &battery) const;
void send_distance_sensor(const RangeFinder &rangefinder, const uint8_t instance) const; void send_distance_sensor(const RangeFinder &rangefinder, const uint8_t instance) const;
bool send_distance_sensor(const RangeFinder &rangefinder) const; bool send_distance_sensor(const RangeFinder &rangefinder) const;
void send_distance_sensor_downward(const RangeFinder &rangefinder) const;
void send_ahrs2(AP_AHRS &ahrs); void send_ahrs2(AP_AHRS &ahrs);
bool send_gps_raw(AP_GPS &gps); bool send_gps_raw(AP_GPS &gps);
void send_system_time(AP_GPS &gps); void send_system_time(AP_GPS &gps);

11
libraries/GCS_MAVLink/GCS_Common.cpp

@ -245,6 +245,17 @@ bool GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder) const
return true; return true;
} }
void GCS_MAVLINK::send_distance_sensor_downward(const RangeFinder &rangefinder) const
{
// exit immediately if rangefinder is disabled or not downward looking
if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
return;
}
uint8_t instance;
rangefinder.find_instance(ROTATION_PITCH_270, instance);
send_distance_sensor(rangefinder, instance);
}
// report AHRS2 state // report AHRS2 state
void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs) void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
{ {

Loading…
Cancel
Save