forked from Archive/PX4-Autopilot
MAVLink Streams: added collision stream
This commit is contained in:
parent
8a2399eee1
commit
dd8d178168
|
@ -122,6 +122,7 @@ set(msg_file_names
|
||||||
wind_estimate.msg
|
wind_estimate.msg
|
||||||
vehicle_roi.msg
|
vehicle_roi.msg
|
||||||
mount_status.msg
|
mount_status.msg
|
||||||
|
collision_report.msg
|
||||||
)
|
)
|
||||||
|
|
||||||
# Get absolute paths
|
# Get absolute paths
|
||||||
|
|
|
@ -0,0 +1,7 @@
|
||||||
|
uint8 src
|
||||||
|
uint32 id
|
||||||
|
uint8 action
|
||||||
|
uint8 threat_level
|
||||||
|
float32 time_to_minimum_delta
|
||||||
|
float32 altitude_minimum_delta
|
||||||
|
float32 horizontal_minimum_delta
|
|
@ -1970,6 +1970,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
configure_stream("ALTITUDE", 1.0f);
|
configure_stream("ALTITUDE", 1.0f);
|
||||||
configure_stream("GPS_RAW_INT", 1.0f);
|
configure_stream("GPS_RAW_INT", 1.0f);
|
||||||
configure_stream("ADSB_VEHICLE", 2.0f);
|
configure_stream("ADSB_VEHICLE", 2.0f);
|
||||||
|
configure_stream("COLLISION", 2.0f);
|
||||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||||
configure_stream("OPTICAL_FLOW_RAD", 1.0f);
|
configure_stream("OPTICAL_FLOW_RAD", 1.0f);
|
||||||
configure_stream("VISION_POSITION_NED", 1.0f);
|
configure_stream("VISION_POSITION_NED", 1.0f);
|
||||||
|
@ -1995,6 +1996,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
configure_stream("ALTITUDE", 10.0f);
|
configure_stream("ALTITUDE", 10.0f);
|
||||||
configure_stream("GPS_RAW_INT", 5.0f);
|
configure_stream("GPS_RAW_INT", 5.0f);
|
||||||
configure_stream("ADSB_VEHICLE", 10.0f);
|
configure_stream("ADSB_VEHICLE", 10.0f);
|
||||||
|
configure_stream("COLLISION", 10.0f);
|
||||||
configure_stream("DISTANCE_SENSOR", 10.0f);
|
configure_stream("DISTANCE_SENSOR", 10.0f);
|
||||||
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
||||||
configure_stream("VISION_POSITION_NED", 10.0f);
|
configure_stream("VISION_POSITION_NED", 10.0f);
|
||||||
|
@ -2050,6 +2052,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
configure_stream("ALTITUDE", 10.0f);
|
configure_stream("ALTITUDE", 10.0f);
|
||||||
configure_stream("GPS_RAW_INT", 10.0f);
|
configure_stream("GPS_RAW_INT", 10.0f);
|
||||||
configure_stream("ADSB_VEHICLE", 20.0f);
|
configure_stream("ADSB_VEHICLE", 20.0f);
|
||||||
|
configure_stream("COLLISION", 20.0f);
|
||||||
configure_stream("DISTANCE_SENSOR", 10.0f);
|
configure_stream("DISTANCE_SENSOR", 10.0f);
|
||||||
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
||||||
configure_stream("VISION_POSITION_NED", 10.0f);
|
configure_stream("VISION_POSITION_NED", 10.0f);
|
||||||
|
|
|
@ -90,6 +90,7 @@
|
||||||
#include <uORB/topics/vtol_vehicle_status.h>
|
#include <uORB/topics/vtol_vehicle_status.h>
|
||||||
#include <uORB/topics/wind_estimate.h>
|
#include <uORB/topics/wind_estimate.h>
|
||||||
#include <uORB/topics/mount_status.h>
|
#include <uORB/topics/mount_status.h>
|
||||||
|
#include <uORB/topics/collision_report.h>
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
|
|
||||||
|
|
||||||
|
@ -1292,6 +1293,73 @@ protected:
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
class MavlinkStreamCollision : public MavlinkStream
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
const char *get_name() const
|
||||||
|
{
|
||||||
|
return MavlinkStreamCollision::get_name_static();
|
||||||
|
}
|
||||||
|
|
||||||
|
static const char *get_name_static()
|
||||||
|
{
|
||||||
|
return "COLLISION";
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint16_t get_id_static()
|
||||||
|
{
|
||||||
|
return MAVLINK_MSG_ID_COLLISION;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t get_id()
|
||||||
|
{
|
||||||
|
return get_id_static();
|
||||||
|
}
|
||||||
|
|
||||||
|
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||||
|
{
|
||||||
|
return new MavlinkStreamCollision(mavlink);
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned get_size()
|
||||||
|
{
|
||||||
|
return (_collision_time > 0) ? MAVLINK_MSG_ID_COLLISION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
MavlinkOrbSubscription *_collision_sub;
|
||||||
|
uint64_t _collision_time;
|
||||||
|
|
||||||
|
/* do not allow top copying this class */
|
||||||
|
MavlinkStreamCollision(MavlinkStreamCollision &);
|
||||||
|
MavlinkStreamCollision &operator = (const MavlinkStreamCollision &);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||||
|
_collision_sub(_mavlink->add_orb_subscription(ORB_ID(collision_report))),
|
||||||
|
_collision_time(0)
|
||||||
|
{}
|
||||||
|
|
||||||
|
void send(const hrt_abstime t)
|
||||||
|
{
|
||||||
|
struct collision_report_s report;
|
||||||
|
|
||||||
|
if (_collision_sub->update(&_collision_time, &report)) {
|
||||||
|
mavlink_collision_t msg = {};
|
||||||
|
|
||||||
|
msg.src = report.src;
|
||||||
|
msg.id = report.id;
|
||||||
|
msg.action = report.action;
|
||||||
|
msg.threat_level = report.threat_level;
|
||||||
|
msg.time_to_minimum_delta = report.time_to_minimum_delta;
|
||||||
|
msg.altitude_minimum_delta = report.altitude_minimum_delta;
|
||||||
|
msg.horizontal_minimum_delta = report.horizontal_minimum_delta;
|
||||||
|
|
||||||
|
mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
class MavlinkStreamCameraTrigger : public MavlinkStream
|
class MavlinkStreamCameraTrigger : public MavlinkStream
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -3567,6 +3635,7 @@ const StreamListItem *streams_list[] = {
|
||||||
new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
|
new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
|
||||||
new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
|
new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
|
||||||
new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
|
new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
|
||||||
|
new StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static),
|
||||||
new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
|
new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
|
||||||
new StreamListItem(&MavlinkStreamMountStatus::new_instance, &MavlinkStreamMountStatus::get_name_static, &MavlinkStreamMountStatus::get_id_static),
|
new StreamListItem(&MavlinkStreamMountStatus::new_instance, &MavlinkStreamMountStatus::get_name_static, &MavlinkStreamMountStatus::get_id_static),
|
||||||
nullptr
|
nullptr
|
||||||
|
|
|
@ -128,6 +128,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||||
_time_offset_pub(nullptr),
|
_time_offset_pub(nullptr),
|
||||||
_follow_target_pub(nullptr),
|
_follow_target_pub(nullptr),
|
||||||
_transponder_report_pub(nullptr),
|
_transponder_report_pub(nullptr),
|
||||||
|
_collision_report_pub(nullptr),
|
||||||
_control_state_pub(nullptr),
|
_control_state_pub(nullptr),
|
||||||
_gps_inject_data_pub(nullptr),
|
_gps_inject_data_pub(nullptr),
|
||||||
_command_ack_pub(nullptr),
|
_command_ack_pub(nullptr),
|
||||||
|
@ -258,6 +259,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||||
handle_message_adsb_vehicle(msg);
|
handle_message_adsb_vehicle(msg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case MAVLINK_MSG_ID_COLLISION:
|
||||||
|
handle_message_collision(msg);
|
||||||
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
|
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
|
||||||
handle_message_gps_rtcm_data(msg);
|
handle_message_gps_rtcm_data(msg);
|
||||||
break;
|
break;
|
||||||
|
@ -1927,6 +1932,30 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
|
||||||
|
{
|
||||||
|
mavlink_collision_t collision;
|
||||||
|
collision_report_s t = { };
|
||||||
|
|
||||||
|
mavlink_msg_collision_decode(msg, &collision);
|
||||||
|
|
||||||
|
t.timestamp = hrt_absolute_time();
|
||||||
|
t.src = collision.src;
|
||||||
|
t.id = collision.id;
|
||||||
|
t.action = collision.action;
|
||||||
|
t.threat_level = collision.threat_level;
|
||||||
|
t.time_to_minimum_delta = collision.time_to_minimum_delta;
|
||||||
|
t.altitude_minimum_delta = collision.altitude_minimum_delta;
|
||||||
|
t.horizontal_minimum_delta = collision.horizontal_minimum_delta;
|
||||||
|
|
||||||
|
if (_collision_report_pub == nullptr) {
|
||||||
|
_collision_report_pub = orb_advertise(ORB_ID(collision_report), &t);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
orb_publish(ORB_ID(collision_report), _collision_report_pub, &t);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
|
void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
|
mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
|
||||||
|
|
|
@ -78,6 +78,7 @@
|
||||||
#include <uORB/topics/transponder_report.h>
|
#include <uORB/topics/transponder_report.h>
|
||||||
#include <uORB/topics/gps_inject_data.h>
|
#include <uORB/topics/gps_inject_data.h>
|
||||||
#include <uORB/topics/control_state.h>
|
#include <uORB/topics/control_state.h>
|
||||||
|
#include <uORB/topics/collision_report.h>
|
||||||
|
|
||||||
|
|
||||||
#include "mavlink_ftp.h"
|
#include "mavlink_ftp.h"
|
||||||
|
@ -143,6 +144,7 @@ private:
|
||||||
void handle_message_distance_sensor(mavlink_message_t *msg);
|
void handle_message_distance_sensor(mavlink_message_t *msg);
|
||||||
void handle_message_follow_target(mavlink_message_t *msg);
|
void handle_message_follow_target(mavlink_message_t *msg);
|
||||||
void handle_message_adsb_vehicle(mavlink_message_t *msg);
|
void handle_message_adsb_vehicle(mavlink_message_t *msg);
|
||||||
|
void handle_message_collision(mavlink_message_t *msg);
|
||||||
void handle_message_gps_rtcm_data(mavlink_message_t *msg);
|
void handle_message_gps_rtcm_data(mavlink_message_t *msg);
|
||||||
void handle_message_battery_status(mavlink_message_t *msg);
|
void handle_message_battery_status(mavlink_message_t *msg);
|
||||||
void handle_message_serial_control(mavlink_message_t *msg);
|
void handle_message_serial_control(mavlink_message_t *msg);
|
||||||
|
@ -223,6 +225,7 @@ private:
|
||||||
orb_advert_t _time_offset_pub;
|
orb_advert_t _time_offset_pub;
|
||||||
orb_advert_t _follow_target_pub;
|
orb_advert_t _follow_target_pub;
|
||||||
orb_advert_t _transponder_report_pub;
|
orb_advert_t _transponder_report_pub;
|
||||||
|
orb_advert_t _collision_report_pub;
|
||||||
orb_advert_t _control_state_pub;
|
orb_advert_t _control_state_pub;
|
||||||
static const int _gps_inject_data_queue_size = 6;
|
static const int _gps_inject_data_queue_size = 6;
|
||||||
orb_advert_t _gps_inject_data_pub;
|
orb_advert_t _gps_inject_data_pub;
|
||||||
|
|
Loading…
Reference in New Issue