aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: add rangefinder msg

Browse Source
This commit is contained in:
Pierre Kancir 2017-05-30 12:33:37 +02:00 committed by Francisco Ferreira
parent 0848d96354
commit e9685ba13e
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

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

13
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -256,6 +256,19 @@ void GCS_MAVLINK::send_distance_sensor_downward(const RangeFinder &rangefinder)
send_distance_sensor(rangefinder, instance);
}
void GCS_MAVLINK::send_rangefinder(const RangeFinder &rangefinder) const
{
// exit immediately if rangefinder is disabled or not downward looking
if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
// no sonar to report
return;
}
mavlink_msg_rangefinder_send(
chan,
rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f,
rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
}
// report AHRS2 state
void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
{