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

GCS_MAVLink: send_collision_all

Browse Source
This commit is contained in:
Peter Barker 2016-05-11 16:19:15 +10:00 committed by Randy Mackay
parent 7bc9a1cf83
commit aae84b3edc
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -15,6 +15,7 @@
#include "MAVLink_routing.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Mount/AP_Mount.h>
#include <AP_Avoidance/AP_Avoidance.h>
#include <AP_HAL/utility/RingBuffer.h>
// check if a message will fit in the payload space available
@ -170,6 +171,7 @@ public:
static void send_home_all(const Location &home);
void send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status);
void send_servo_output_raw(bool hil);
static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels

20
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1700,3 +1700,23 @@ void GCS_MAVLINK::send_servo_output_raw(bool hil)
values[8], values[9], values[10], values[11],
values[12], values[13], values[14], values[15]);
}
void GCS_MAVLINK::send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_COLLISION) {
mavlink_msg_collision_send(
chan,
MAV_COLLISION_SRC_ADSB,
threat.src_id,
behaviour,
threat.threat_level,
threat.time_to_closest_approach,
threat.closest_approach_z,
threat.closest_approach_xy
);
}
}
}
}