mirror of https://github.com/ArduPilot/ardupilot
GCS_MAVLink: use rangefinder backend accessors
This commit is contained in:
parent
f1d350bbb1
commit
c79cbd71af
|
@ -153,7 +153,7 @@ public:
|
||||||
void send_power_status(void);
|
void send_power_status(void);
|
||||||
void send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const;
|
void send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const;
|
||||||
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 AP_RangeFinder_Backend *sensor) 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_distance_sensor_downward(const RangeFinder &rangefinder) const;
|
||||||
void send_rangefinder_downward(const RangeFinder &rangefinder) const;
|
void send_rangefinder_downward(const RangeFinder &rangefinder) const;
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_OpticalFlow/AP_OpticalFlow.h>
|
#include <AP_OpticalFlow/AP_OpticalFlow.h>
|
||||||
#include <AP_Vehicle/AP_Vehicle.h>
|
#include <AP_Vehicle/AP_Vehicle.h>
|
||||||
|
#include <AP_RangeFinder/RangeFinder_Backend.h>
|
||||||
|
|
||||||
#include "ap_version.h"
|
#include "ap_version.h"
|
||||||
#include "GCS.h"
|
#include "GCS.h"
|
||||||
|
@ -214,54 +215,60 @@ bool GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery) const
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder, const uint8_t instance) const
|
void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor) const
|
||||||
{
|
{
|
||||||
if (rangefinder.status(instance) != RangeFinder::RangeFinder_NotConnected &&
|
if (sensor == nullptr) {
|
||||||
rangefinder.status(instance) != RangeFinder::RangeFinder_NoData) {
|
// should not happen
|
||||||
mavlink_msg_distance_sensor_send(
|
return;
|
||||||
chan,
|
|
||||||
AP_HAL::millis(), // time since system boot TODO: take time of measurement
|
|
||||||
rangefinder.min_distance_cm(instance), // minimum distance the sensor can measure in centimeters
|
|
||||||
rangefinder.max_distance_cm(instance), // maximum distance the sensor can measure in centimeters
|
|
||||||
rangefinder.distance_cm(instance), // current distance reading
|
|
||||||
rangefinder.get_sensor_type(instance), // type from MAV_DISTANCE_SENSOR enum
|
|
||||||
instance, // onboard ID of the sensor == instance
|
|
||||||
rangefinder.get_orientation(instance), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
|
|
||||||
0); // Measurement covariance in centimeters, 0 for unknown / invalid readings
|
|
||||||
}
|
}
|
||||||
|
if (!sensor->has_data()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
mavlink_msg_distance_sensor_send(
|
||||||
|
chan,
|
||||||
|
AP_HAL::millis(), // time since system boot TODO: take time of measurement
|
||||||
|
sensor->min_distance_cm(), // minimum distance the sensor can measure in centimeters
|
||||||
|
sensor->max_distance_cm(), // maximum distance the sensor can measure in centimeters
|
||||||
|
sensor->distance_cm(), // current distance reading
|
||||||
|
sensor->get_sensor_type(), // type from MAV_DISTANCE_SENSOR enum
|
||||||
|
sensor->instance(), // onboard ID of the sensor == instance
|
||||||
|
sensor->orientation(), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
|
||||||
|
0); // Measurement covariance in centimeters, 0 for unknown / invalid readings
|
||||||
}
|
}
|
||||||
|
|
||||||
bool GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder) const
|
bool GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder) const
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
|
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
|
||||||
CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);
|
CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);
|
||||||
send_distance_sensor(rangefinder, i);
|
AP_RangeFinder_Backend *sensor = rangefinder.get_backend(i);
|
||||||
|
if (sensor == nullptr) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
send_distance_sensor(sensor);
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS_MAVLINK::send_distance_sensor_downward(const RangeFinder &rangefinder) const
|
void GCS_MAVLINK::send_distance_sensor_downward(const RangeFinder &rangefinder) const
|
||||||
{
|
{
|
||||||
// exit immediately if rangefinder is disabled or not downward looking
|
AP_RangeFinder_Backend *s = rangefinder.find_instance(ROTATION_PITCH_270);
|
||||||
if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
|
if (s == nullptr) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
uint8_t instance;
|
send_distance_sensor(s);
|
||||||
rangefinder.find_instance(ROTATION_PITCH_270, instance);
|
|
||||||
send_distance_sensor(rangefinder, instance);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS_MAVLINK::send_rangefinder_downward(const RangeFinder &rangefinder) const
|
void GCS_MAVLINK::send_rangefinder_downward(const RangeFinder &rangefinder) const
|
||||||
{
|
{
|
||||||
// exit immediately if rangefinder is disabled or not downward looking
|
AP_RangeFinder_Backend *s = rangefinder.find_instance(ROTATION_PITCH_270);
|
||||||
if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
|
if (s == nullptr) {
|
||||||
// no sonar to report
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
mavlink_msg_rangefinder_send(
|
mavlink_msg_rangefinder_send(
|
||||||
chan,
|
chan,
|
||||||
rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f,
|
s->distance_cm() * 0.01f,
|
||||||
rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
|
s->voltage_mv() * 0.001f);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool GCS_MAVLINK::send_proximity(const AP_Proximity &proximity) const
|
bool GCS_MAVLINK::send_proximity(const AP_Proximity &proximity) const
|
||||||
|
|
Loading…
Reference in New Issue