Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: move send_proximity to GCS common code (NFC)

This commit is contained in:
khancyr 2017-07-14 18:00:01 +01:00 committed by Francisco Ferreira
parent 1c31211874
commit 49755063ab
2 changed files with 3 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -709,7 +709,6 @@ private:
void send_hwstatus(mavlink_channel_t chan); void send_hwstatus(mavlink_channel_t chan);
void send_vfr_hud(mavlink_channel_t chan); void send_vfr_hud(mavlink_channel_t chan);
void send_current_waypoint(mavlink_channel_t chan); void send_current_waypoint(mavlink_channel_t chan);
void send_proximity(mavlink_channel_t chan, uint16_t count_max);
void send_rpm(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan);
void rpm_update(); void rpm_update();
void button_update(); void button_update();

56
ArduCopter/GCS_Mavlink.cpp
View File

@ -207,58 +207,6 @@ void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan)
mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
} }
void NOINLINE Copter::send_proximity(mavlink_channel_t chan, uint16_t count_max)
{
#if PROXIMITY_ENABLED == ENABLED
// return immediately if no proximity sensor is present
if (g2.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 (g2.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)(g2.proximity.distance_min() * 100), // minimum distance the sensor can measure in centimeters
(uint16_t)(g2.proximity.distance_max() * 100), // maximum distance the sensor can measure in centimeters
(uint16_t)(dist_array.distance[i] * 100), // current distance reading (in cm?)
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 && g2.proximity.get_upward_distance(dist_up)) {
mavlink_msg_distance_sensor_send(
chan,
AP_HAL::millis(), // time since system boot
(uint16_t)(g2.proximity.distance_min() * 100), // minimum distance the sensor can measure in centimeters
(uint16_t)(g2.proximity.distance_max() * 100), // maximum distance the sensor can measure in centimeters
(uint16_t)(dist_up * 100), // 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
}
#endif
}
/* /*
send RPM packet send RPM packet
*/ */
@ -467,8 +415,10 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);
send_distance_sensor_downward(copter.rangefinder); send_distance_sensor_downward(copter.rangefinder);
#endif #endif
#if PROXIMITY_ENABLED == ENABLED
CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);
copter.send_proximity(chan, comm_get_txspace(chan) / (packet_overhead()+9)); send_proximity(copter.g2.proximity, comm_get_txspace(chan) / (packet_overhead()+9));
#endif
break; break;
case MSG_RPM: case MSG_RPM: