Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: move send_proximity to GCS common code (NFC)

Browse Source 
Also clean two comments
This commit is contained in:
khancyr 2017-07-14 18:00:01 +01:00 committed by Francisco Ferreira
parent 49755063ab
commit ec2ea1c903
2 changed files with 53 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -14,6 +14,7 @@
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Mount/AP_Mount.h>
#include <AP_Avoidance/AP_Avoidance.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
@ -155,6 +156,7 @@ public:
bool send_distance_sensor(const RangeFinder &rangefinder) const;
void send_distance_sensor_downward(const RangeFinder &rangefinder) const;
void send_rangefinder_downward(const RangeFinder &rangefinder) const;
void send_proximity(const AP_Proximity &proximity, uint16_t count_max) const;
void send_ahrs2(AP_AHRS &ahrs);
bool send_gps_raw(AP_GPS &gps);
void send_system_time(AP_GPS &gps);

52
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -228,7 +228,7 @@ void GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder, const uin
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 (in cm?)
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
@ -269,6 +269,56 @@ void GCS_MAVLINK::send_rangefinder_downward(const RangeFinder &rangefinder) cons
rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
}
void GCS_MAVLINK::send_proximity(const AP_Proximity &proximity, uint16_t count_max) const
{
// return immediately if no proximity sensor is present
if (proximity.get_status() == AP_Proximity::Proximity_NotConnected) {
return;
}
// return immediately if no tx buffer room to send messages
if (count_max == 0) {
return;
}
bool send_upwards = true;
// send horizontal distances
AP_Proximity::Proximity_Distance_Array dist_array;
const uint8_t horiz_count = MIN(count_max, PROXIMITY_MAX_DIRECTION); // send at most PROXIMITY_MAX_DIRECTION horizontal distances
if (proximity.get_horizontal_distances(dist_array)) {
for (uint8_t i = 0; i < horiz_count; i++) {
mavlink_msg_distance_sensor_send(
chan,
AP_HAL::millis(), // time since system boot
(uint16_t)(proximity.distance_min() * 100.0f), // minimum distance the sensor can measure in centimeters
(uint16_t)(proximity.distance_max() * 100.0f), // maximum distance the sensor can measure in centimeters
(uint16_t)(dist_array.distance[i] * 100.0f), // current distance reading
MAV_DISTANCE_SENSOR_LASER, // type from MAV_DISTANCE_SENSOR enum
PROXIMITY_SENSOR_ID_START + i, // onboard ID of the sensor
dist_array.orientation[i], // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
0); // Measurement covariance in centimeters, 0 for unknown / invalid readings
}
// check if we still have room to send upwards distance
send_upwards = (count_max > 8);
}
// send upward distance
float dist_up;
if (send_upwards && proximity.get_upward_distance(dist_up)) {
mavlink_msg_distance_sensor_send(
chan,
AP_HAL::millis(), // time since system boot
(uint16_t)(proximity.distance_min() * 100.0f), // minimum distance the sensor can measure in centimeters
(uint16_t)(proximity.distance_max() * 100.0f), // maximum distance the sensor can measure in centimeters
(uint16_t)(dist_up * 100.0f), // current distance reading
MAV_DISTANCE_SENSOR_LASER, // type from MAV_DISTANCE_SENSOR enum
PROXIMITY_SENSOR_ID_START + PROXIMITY_MAX_DIRECTION + 1, // onboard ID of the sensor
MAV_SENSOR_ROTATION_PITCH_90, // direction upwards
0); // Measurement covariance in centimeters, 0 for unknown / invalid readings
}
}
// report AHRS2 state
void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
{