Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpenDroneID: added library

Browse Source
This commit is contained in:
Andrew Tridgell 2022-08-28 18:11:22 +10:00
parent 499dca299b
commit 0cc5c411ba
4 changed files with 1188 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

687
libraries/AP_OpenDroneID/AP_OpenDroneID.cpp Normal file
View File

@ -0,0 +1,687 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
*
* original code by:
* BlueMark Innovations BV, Roel Schiphorst
* Contributors: Tom Pittenger, Josh Henderson, Andrew Tridgell
* Parts of this code are based on/copied from the Open Drone ID project https://github.com/opendroneid/opendroneid-core-c
*
* The code has been tested with the BlueMark DroneBeacon MAVLink transponder running this command in the ArduPlane folder:
* sim_vehicle.py --console --map -A --serial1=uart:/dev/ttyUSB1:9600
* (and a DroneBeacon MAVLink transponder connected to ttyUSB1)
*
* See https://github.com/ArduPilot/ArduRemoteID for an open implementation of a transmitter module on serial
* and DroneCAN
*/
#include "AP_OpenDroneID.h"
#if AP_OPENDRONEID_ENABLED
#define IGNORE_RESULT(e) do { if (e) {} } while(0)
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL &hal;
const AP_Param::GroupInfo AP_OpenDroneID::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable ODID subsystem
// @Description: Enable ODID subsystem
// @Values: 0:Disabled,1:Enabled
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_OpenDroneID, _enable, 0, AP_PARAM_FLAG_ENABLE),
// @Param: MAVPORT
// @DisplayName: MAVLink serial port
// @Description: Serial port number to send OpenDroneID MAVLink messages to. Can be -1 if using DroneCAN.
// @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
AP_GROUPINFO("MAVPORT", 2, AP_OpenDroneID, _mav_port, -1),
// @Param: CANDRIVER
// @DisplayName: DroneCAN driver number
// @Description: DroneCAN driver index, 0 to disable DroneCAN
// @Values: 0:Disabled,1:Driver1,2:Driver2
AP_GROUPINFO("CANDRIVER", 3, AP_OpenDroneID, _can_driver, 0),
// @Param: OPTIONS
// @DisplayName: OpenDroneID options
// @Description: Options for OpenDroneID subsystem. Bit 0 means to enforce arming checks
// @Bitmask: 0:EnforceArming
AP_GROUPINFO("OPTIONS", 4, AP_OpenDroneID, _options, 0),
// @Param: BARO_ACC
// @DisplayName: Barometer vertical accuraacy
// @Description: Barometer Vertical Accuracy when installed in the vehicle. Note this is dependent upon installation conditions and thus disabled by default
// @Units: m
// @User: Advanced
AP_GROUPINFO("BARO_ACC", 5, AP_OpenDroneID, _baro_accuracy, -1.0),
AP_GROUPEND
};
// constructor
AP_OpenDroneID::AP_OpenDroneID()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_singleton != nullptr) {
AP_HAL::panic("OpenDroneID must be singleton");
}
#endif
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
void AP_OpenDroneID::init()
{
if (_enable == 0) {
return;
}
_chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
}
// Perform the pre-arm checks and prevent arming if they are not satisifed
// Except in the case of an in-flight reboot
bool AP_OpenDroneID::pre_arm_check(char* failmsg, uint8_t failmsg_len)
{
WITH_SEMAPHORE(_sem);
if (!option_enabled(Options::EnforceArming)) {
return true;
}
if (pkt_basic_id.id_type == MAV_ODID_ID_TYPE_NONE) {
strncpy(failmsg, "UA_TYPE required in BasicID", failmsg_len);
return false;
}
if (pkt_system.operator_latitude == 0 && pkt_system.operator_longitude == 0) {
strncpy(failmsg, "operator location must be set", failmsg_len);
return false;
}
const uint32_t max_age_ms = 3000;
const uint32_t now_ms = AP_HAL::millis();
if (last_arm_status_ms == 0 || now_ms - last_arm_status_ms > max_age_ms) {
strncpy(failmsg, "ARM_STATUS not available", failmsg_len);
return false;
}
if (last_system_ms == 0 ||
(now_ms - last_system_ms > max_age_ms &&
(now_ms - last_system_update_ms > max_age_ms))) {
strncpy(failmsg, "SYSTEM not available", failmsg_len);
return false;
}
if (arm_status.status != MAV_ODID_GOOD_TO_ARM) {
strncpy(failmsg, arm_status.error, failmsg_len);
return false;
}
return true;
}
void AP_OpenDroneID::update()
{
if (_enable == 0) {
return;
}
const bool armed = hal.util->get_soft_armed();
if (armed && !_was_armed) {
// use arm location as takeoff location
IGNORE_RESULT(AP::ahrs().get_position(_takeoff_location));
}
_was_armed = armed;
send_dynamic_out();
send_static_out();
}
// local payload space check which treats invalid channel as having space
// needed to populate the message structures for the DroneCAN backend
#define ODID_HAVE_PAYLOAD_SPACE(id) (_chan == MAV_CHAN_INVALID || HAVE_PAYLOAD_SPACE(_chan, id))
void AP_OpenDroneID::send_dynamic_out()
{
const uint32_t now = AP_HAL::millis();
if (now - _last_send_location_ms >= _mavlink_dynamic_period_ms &&
ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_LOCATION)) {
_last_send_location_ms = now;
send_location_message();
}
// operator location needs to be sent at the same rate as location for FAA compliance
if (now - _last_send_system_update_ms >= _mavlink_dynamic_period_ms &&
ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_SYSTEM_UPDATE)) {
_last_send_system_update_ms = now;
send_system_update_message();
}
}
void AP_OpenDroneID::send_static_out()
{
const uint32_t now_ms = AP_HAL::millis();
// we need to notify user if we lost the transmitter
if (now_ms - last_arm_status_ms > 5000) {
if (now_ms - last_lost_tx_ms > 5000) {
last_lost_tx_ms = now_ms;
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter");
}
} else if (last_lost_tx_ms != 0) {
// we're OK again
last_lost_tx_ms = 0;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODID: transmitter OK");
}
// we need to notify user if we lost system msg with operator location
if (now_ms - last_system_ms > 5000 && now_ms - last_lost_operator_msg_ms > 5000) {
last_lost_operator_msg_ms = now_ms;
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost operator location");
}
const uint32_t msg_spacing_ms = _mavlink_static_period_ms / 4;
if (now_ms - last_msg_send_ms >= msg_spacing_ms) {
// allow update of channel during setup, this makes it easy to debug with a GCS
_chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
bool sent_ok = false;
switch (next_msg_to_send) {
case NEXT_MSG_BASIC_ID:
if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_BASIC_ID)) {
send_basic_id_message();
sent_ok = true;
}
break;
case NEXT_MSG_SYSTEM:
if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_SYSTEM)) {
send_system_message();
sent_ok = true;
}
break;
case NEXT_MSG_SELF_ID:
if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_SELF_ID)) {
send_self_id_message();
sent_ok = true;
}
break;
case NEXT_MSG_OPERATOR_ID:
if (ODID_HAVE_PAYLOAD_SPACE(OPEN_DRONE_ID_OPERATOR_ID)) {
send_operator_id_message();
sent_ok = true;
}
break;
case NEXT_MSG_ENUM_END:
break;
}
if (sent_ok) {
last_msg_send_ms = now_ms;
next_msg_to_send = next_msg((uint8_t(next_msg_to_send) + 1) % uint8_t(NEXT_MSG_ENUM_END));
}
}
}
// The send_location_message
// all open_drone_id send functions use data stored in the open drone id class.
// This location send function is an exception. It uses live location data from the ArduPilot system.
void AP_OpenDroneID::send_location_message()
{
auto &ahrs = AP::ahrs();
const auto &barometer = AP::baro();
const auto &gps = AP::gps();
const AP_GPS::GPS_Status gps_status = gps.status();
const bool got_bad_gps_fix = (gps_status < AP_GPS::GPS_Status::GPS_OK_FIX_3D);
Location current_location;
if (!ahrs.get_position(current_location)) {
return;
}
const uint8_t uav_status = hal.util->get_soft_armed()? MAV_ODID_STATUS_AIRBORNE : MAV_ODID_STATUS_GROUND;
float direction = ODID_INV_DIR;
if (!got_bad_gps_fix) {
direction = wrap_360(degrees(ahrs.groundspeed_vector().angle())); // heading (degrees)
}
const float speed_horizontal = create_speed_horizontal(ahrs.groundspeed());
Vector3f velNED;
UNUSED_RESULT(ahrs.get_velocity_NED(velNED));
const float climb_rate = create_speed_vertical(-velNED.z); //make sure climb_rate is within Remote ID limit
int32_t latitude = 0;
int32_t longitude = 0;
if (current_location.check_latlng()) { //set location if they are valid
latitude = current_location.lat;
longitude = current_location.lng;
}
// altitude referenced against 1013.2mb
const float base_press_mbar = 1013.2;
const float altitude_barometric = create_altitude(barometer.get_altitude_difference(base_press_mbar*100, barometer.get_pressure()));
float altitude_geodetic = -1000;
int32_t alt_amsl_cm;
if (current_location.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
altitude_geodetic = alt_amsl_cm * 0.01;
}
#if 0
float undulation;
if (gps.get_undulation(undulation)) {
altitude_geodetic -= undulation;
}
#endif
// Compute the current height above the takeoff location
float height_above_takeoff = 0; // height above takeoff (meters)
if (hal.util->get_soft_armed()) {
int32_t curr_alt_asml_cm;
int32_t takeoff_alt_asml_cm;
if (current_location.get_alt_cm(Location::AltFrame::ABSOLUTE, curr_alt_asml_cm) &&
_takeoff_location.get_alt_cm(Location::AltFrame::ABSOLUTE, takeoff_alt_asml_cm)) {
height_above_takeoff = (curr_alt_asml_cm - takeoff_alt_asml_cm) * 0.01;
}
}
// Accuracy
// If we have GPS 3D lock we presume that the accuracies of the system will track the GPS's reported accuracy
MAV_ODID_HOR_ACC horizontal_accuracy_mav = MAV_ODID_HOR_ACC_UNKNOWN;
MAV_ODID_VER_ACC vertical_accuracy_mav = MAV_ODID_VER_ACC_UNKNOWN;
MAV_ODID_SPEED_ACC speed_accuracy_mav = MAV_ODID_SPEED_ACC_UNKNOWN;
MAV_ODID_TIME_ACC timestamp_accuracy_mav = MAV_ODID_TIME_ACC_UNKNOWN;
float horizontal_accuracy;
if (gps.horizontal_accuracy(horizontal_accuracy)) {
horizontal_accuracy_mav = create_enum_horizontal_accuracy(horizontal_accuracy);
}
float vertical_accuracy;
if (gps.vertical_accuracy(vertical_accuracy)) {
vertical_accuracy_mav = create_enum_vertical_accuracy(vertical_accuracy);
}
float speed_accuracy;
if (gps.speed_accuracy(speed_accuracy)) {
speed_accuracy_mav = create_enum_speed_accuracy(speed_accuracy);
}
// if we have ever had GPS lock then we will have better than 1s
// accuracy, as we use system timer to propogate time
timestamp_accuracy_mav = create_enum_timestamp_accuracy(1.0);
// Barometer altitude accuraacy will be highly dependent on the airframe and installation of the barometer in use
// thus ArduPilot cannot reasonably fill this in.
// Instead allow a manufacturer to use a parameter to fill this in
uint8_t barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; //ahrs class does not provide accuracy readings
if (!is_equal(_baro_accuracy.get(), -1.0f)) {
barometer_accuracy = create_enum_vertical_accuracy(_baro_accuracy);
}
// Timestamp here is the number of seconds after into the current hour referenced to UTC time (up to one hour)
// FIX we need to only set this if w have a GPS lock is 2D good enough for that?
float timestamp = ODID_INV_TIMESTAMP;
if (!got_bad_gps_fix) {
uint32_t time_week_ms = gps.time_week_ms();
timestamp = float(time_week_ms % (3600 * 1000)) * 0.001;
timestamp = create_location_timestamp(timestamp); //make sure timestamp is within Remote ID limit
}
{
WITH_SEMAPHORE(_sem);
// take semaphore so CAN gets a consistent packet
pkt_location = mavlink_open_drone_id_location_t{
latitude : latitude,
longitude : longitude,
altitude_barometric : altitude_barometric,
altitude_geodetic : altitude_geodetic,
height : height_above_takeoff,
timestamp : timestamp,
direction : uint16_t(direction * 100.0), // Heading (centi-degrees)
speed_horizontal : uint16_t(speed_horizontal * 100.0), // Ground speed (cm/s)
speed_vertical : int16_t(climb_rate * 100.0), // Climb rate (cm/s)
target_system : 0,
target_component : 0,
id_or_mac : {},
status : uint8_t(uav_status),
height_reference : MAV_ODID_HEIGHT_REF_OVER_TAKEOFF, // height reference enum: Above takeoff location or above ground
horizontal_accuracy : uint8_t(horizontal_accuracy_mav),
vertical_accuracy : uint8_t(vertical_accuracy_mav),
barometer_accuracy : barometer_accuracy,
speed_accuracy : uint8_t(speed_accuracy_mav),
timestamp_accuracy : uint8_t(timestamp_accuracy_mav)
};
need_send_location = dronecan_send_all;
}
if (_chan != MAV_CHAN_INVALID) {
mavlink_msg_open_drone_id_location_send_struct(_chan, &pkt_location);
}
}
void AP_OpenDroneID::send_basic_id_message()
{
// note that packet is filled in by the GCS
need_send_basic_id |= dronecan_send_all;
if (_chan != MAV_CHAN_INVALID) {
mavlink_msg_open_drone_id_basic_id_send_struct(_chan, &pkt_basic_id);
}
}
void AP_OpenDroneID::send_system_message()
{
// note that packet is filled in by the GCS
need_send_system |= dronecan_send_all;
if (_chan != MAV_CHAN_INVALID) {
mavlink_msg_open_drone_id_system_send_struct(_chan, &pkt_system);
}
}
void AP_OpenDroneID::send_self_id_message()
{
need_send_self_id |= dronecan_send_all;
if (_chan != MAV_CHAN_INVALID) {
mavlink_msg_open_drone_id_self_id_send_struct(_chan, &pkt_self_id);
}
}
void AP_OpenDroneID::send_system_update_message()
{
need_send_system |= dronecan_send_all;
// note that packet is filled in by the GCS
if (_chan != MAV_CHAN_INVALID) {
const auto pkt_system_update = mavlink_open_drone_id_system_update_t {
operator_latitude : pkt_system.operator_latitude,
operator_longitude : pkt_system.operator_longitude,
operator_altitude_geo : pkt_system.operator_altitude_geo,
timestamp : pkt_system.timestamp,
target_system : pkt_system.target_system,
target_component : pkt_system.target_component,
};
mavlink_msg_open_drone_id_system_update_send_struct(_chan, &pkt_system_update);
}
}
void AP_OpenDroneID::send_operator_id_message()
{
need_send_operator_id |= dronecan_send_all;
// note that packet is filled in by the GCS
if (_chan != MAV_CHAN_INVALID) {
mavlink_msg_open_drone_id_operator_id_send_struct(_chan, &pkt_operator_id);
}
}
/*
* This converts a horizontal accuracy float value to the corresponding enum
*
* @param Accuracy The horizontal accuracy in meters
* @return Enum value representing the accuracy
*/
MAV_ODID_HOR_ACC AP_OpenDroneID::create_enum_horizontal_accuracy(float accuracy) const
{
// Out of bounds return UKNOWN flag
if (accuracy < 0.0 || accuracy >= 18520.0) {
return MAV_ODID_HOR_ACC_UNKNOWN;
}
static const struct {
float accuracy; // Accuracy bound in meters
MAV_ODID_HOR_ACC mavoutput; // mavlink enum output
} horiz_accuracy_table[] = {
{ 1.0, MAV_ODID_HOR_ACC_1_METER},
{ 3.0, MAV_ODID_HOR_ACC_3_METER},
{10.0, MAV_ODID_HOR_ACC_10_METER},
{30.0, MAV_ODID_HOR_ACC_30_METER},
{92.6, MAV_ODID_HOR_ACC_0_05NM},
{185.2, MAV_ODID_HOR_ACC_0_1NM},
{555.6, MAV_ODID_HOR_ACC_0_3NM},
{926.0, MAV_ODID_HOR_ACC_0_5NM},
{1852.0, MAV_ODID_HOR_ACC_1NM},
{3704.0, MAV_ODID_HOR_ACC_2NM},
{7408.0, MAV_ODID_HOR_ACC_4NM},
{18520.0, MAV_ODID_HOR_ACC_10NM},
};
for (auto elem : horiz_accuracy_table) {
if (accuracy < elem.accuracy) {
return elem.mavoutput;
}
}
// Should not reach this
return MAV_ODID_HOR_ACC_UNKNOWN;
}
/**
* This converts a vertical accuracy float value to the corresponding enum
*
* @param Accuracy The vertical accuracy in meters
* @return Enum value representing the accuracy
*/
MAV_ODID_VER_ACC AP_OpenDroneID::create_enum_vertical_accuracy(float accuracy) const
{
// Out of bounds return UKNOWN flag
if (accuracy < 0.0 || accuracy >= 150.0) {
return MAV_ODID_VER_ACC_UNKNOWN;
}
static const struct {
float accuracy; // Accuracy bound in meters
MAV_ODID_VER_ACC mavoutput; // mavlink enum output
} vertical_accuracy_table[] = {
{ 1.0, MAV_ODID_VER_ACC_1_METER},
{ 3.0, MAV_ODID_VER_ACC_3_METER},
{10.0, MAV_ODID_VER_ACC_10_METER},
{25.0, MAV_ODID_VER_ACC_25_METER},
{45.0, MAV_ODID_VER_ACC_45_METER},
{150.0, MAV_ODID_VER_ACC_150_METER},
};
for (auto elem : vertical_accuracy_table) {
if (accuracy < elem.accuracy) {
return elem.mavoutput;
}
}
// Should not reach this
return MAV_ODID_VER_ACC_UNKNOWN;
}
/**
* This converts a speed accuracy float value to the corresponding enum
*
* @param Accuracy The speed accuracy in m/s
* @return Enum value representing the accuracy
*/
MAV_ODID_SPEED_ACC AP_OpenDroneID::create_enum_speed_accuracy(float accuracy) const
{
// Out of bounds return UKNOWN flag
if (accuracy < 0.0 || accuracy >= 10.0) {
return MAV_ODID_SPEED_ACC_UNKNOWN;
}
if (accuracy < 0.3) {
return MAV_ODID_SPEED_ACC_0_3_METERS_PER_SECOND;
} else if (accuracy < 1.0) {
return MAV_ODID_SPEED_ACC_1_METERS_PER_SECOND;
} else if (accuracy < 3.0) {
return MAV_ODID_SPEED_ACC_3_METERS_PER_SECOND;
} else if (accuracy < 10.0) {
return MAV_ODID_SPEED_ACC_10_METERS_PER_SECOND;
}
// Should not reach this
return MAV_ODID_SPEED_ACC_UNKNOWN;
}
/**
* This converts a timestamp accuracy float value to the corresponding enum
*
* @param Accuracy The timestamp accuracy in seconds
* @return Enum value representing the accuracy
*/
MAV_ODID_TIME_ACC AP_OpenDroneID::create_enum_timestamp_accuracy(float accuracy) const
{
// Out of bounds return UKNOWN flag
if (accuracy < 0.0 || accuracy >= 1.5) {
return MAV_ODID_TIME_ACC_UNKNOWN;
}
static const MAV_ODID_TIME_ACC mavoutput [15] = {
MAV_ODID_TIME_ACC_0_1_SECOND,
MAV_ODID_TIME_ACC_0_2_SECOND,
MAV_ODID_TIME_ACC_0_3_SECOND,
MAV_ODID_TIME_ACC_0_4_SECOND,
MAV_ODID_TIME_ACC_0_5_SECOND,
MAV_ODID_TIME_ACC_0_6_SECOND,
MAV_ODID_TIME_ACC_0_7_SECOND,
MAV_ODID_TIME_ACC_0_8_SECOND,
MAV_ODID_TIME_ACC_0_9_SECOND,
MAV_ODID_TIME_ACC_1_0_SECOND,
MAV_ODID_TIME_ACC_1_1_SECOND,
MAV_ODID_TIME_ACC_1_2_SECOND,
MAV_ODID_TIME_ACC_1_3_SECOND,
MAV_ODID_TIME_ACC_1_4_SECOND,
MAV_ODID_TIME_ACC_1_5_SECOND,
};
for (int8_t i = 1; i <= 15; i++) {
if (accuracy <= 0.1 * i) {
return mavoutput[i-1];
}
}
// Should not reach this
return MAV_ODID_TIME_ACC_UNKNOWN;
}
// make sure value is within limits of remote ID standard
uint16_t AP_OpenDroneID::create_speed_horizontal(uint16_t speed) const
{
if (speed > ODID_MAX_SPEED_H) { // constraint function can't be used, because out of range value is invalid
speed = ODID_INV_SPEED_H;
}
return speed;
}
// make sure value is within limits of remote ID standard
int16_t AP_OpenDroneID::create_speed_vertical(int16_t speed) const
{
if (speed > ODID_MAX_SPEED_V) { // constraint function can't be used, because out of range value is invalid
speed = ODID_INV_SPEED_V;
} else if (speed < ODID_MIN_SPEED_V) {
speed = ODID_INV_SPEED_V;
}
return speed;
}
// make sure value is within limits of remote ID standard
float AP_OpenDroneID::create_altitude(float altitude) const
{
if (altitude > ODID_MAX_ALT) { // constraint function can't be used, because out of range value is invalid
altitude = ODID_INV_ALT;
} else if (altitude < ODID_MIN_ALT) {
altitude = ODID_INV_ALT;
}
return altitude;
}
// make sure value is within limits of remote ID standard
float AP_OpenDroneID::create_location_timestamp(float timestamp) const
{
if (timestamp > ODID_MAX_TIMESTAMP) { // constraint function can't be used, because out of range value is invalid
timestamp = ODID_INV_TIMESTAMP;
} else if (timestamp < 0) {
timestamp = ODID_INV_TIMESTAMP;
}
return timestamp;
}
// handle a message from the GCS
void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
{
WITH_SEMAPHORE(_sem);
switch (msg.msgid) {
// only accept ARM_STATUS from the transmitter
case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: {
if (chan == _chan) {
mavlink_msg_open_drone_id_arm_status_decode(&msg, &arm_status);
last_arm_status_ms = AP_HAL::millis();
}
break;
}
// accept other messages from the GCS
case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID:
mavlink_msg_open_drone_id_operator_id_decode(&msg, &pkt_operator_id);
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID:
mavlink_msg_open_drone_id_self_id_decode(&msg, &pkt_self_id);
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID:
mavlink_msg_open_drone_id_basic_id_decode(&msg, &pkt_basic_id);
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
mavlink_msg_open_drone_id_system_decode(&msg, &pkt_system);
last_system_ms = AP_HAL::millis();
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: {
mavlink_open_drone_id_system_update_t pkt_system_update;
mavlink_msg_open_drone_id_system_update_decode(&msg, &pkt_system_update);
pkt_system.operator_latitude = pkt_system_update.operator_latitude;
pkt_system.operator_longitude = pkt_system_update.operator_longitude;
pkt_system.operator_altitude_geo = pkt_system_update.operator_altitude_geo;
pkt_system.timestamp = pkt_system_update.timestamp;
last_system_update_ms = AP_HAL::millis();
if (last_system_ms != 0) {
// we can only mark system as updated if we have the other
// information already
last_system_ms = last_system_update_ms;
}
break;
}
}
}
// singleton instance
AP_OpenDroneID *AP_OpenDroneID::_singleton;
namespace AP
{
AP_OpenDroneID &opendroneid()
{
return *AP_OpenDroneID::get_singleton();
}
}
#endif //AP_OPENDRONEID_ENABLED

214
libraries/AP_OpenDroneID/AP_OpenDroneID.h Normal file
View File

@ -0,0 +1,214 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by:
* BlueMark Innovations BV, Roel Schiphorst
* Contributors: Tom Pittenger, Josh Henderson
* Parts of this code are based on/copied from the Open Drone ID project https://github.com/opendroneid/opendroneid-core-c
*
* The code has been tested with the BlueMark DroneBeacon MAVLink transponder running this command in the ArduPlane folder:
* sim_vehicle.py --wipe-eeprom --console --map -A --serial1=uart:/dev/ttyUSB1:9600
* (and a DroneBeacon MAVLink transponder connected to ttyUSB1)
*
* The Remote ID implementation expects a transponder that caches the received MAVLink messages from ArduPilot
* and transmits them at the required intervals. So static messages are only sent once to the transponder.
*/
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include "AP_OpenDroneID_config.h"
#ifndef AP_OPENDRONEID_ENABLED
// default to off. Enabled in hwdef.dat
#define AP_OPENDRONEID_ENABLED 0
#endif
#define AP_OPENDRONEID_DRONECAN_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS)
#define HAL_MAX_CAN_PROTOCOL_DRIVERS 2
#if AP_OPENDRONEID_ENABLED
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Common/Location.h>
#define ODID_ID_SIZE 20
#define ODID_STR_SIZE 23
#define ODID_MIN_DIR 0 // Minimum direction
#define ODID_MAX_DIR 360 // Maximum direction
#define ODID_INV_DIR 361 // Invalid direction
#define ODID_MIN_SPEED_H 0 // Minimum speed horizontal
#define ODID_MAX_SPEED_H 254.25f // Maximum speed horizontal
#define ODID_INV_SPEED_H 255 // Invalid speed horizontal
#define ODID_MIN_SPEED_V (-62) // Minimum speed vertical
#define ODID_MAX_SPEED_V 62 // Maximum speed vertical
#define ODID_INV_SPEED_V 63 // Invalid speed vertical
#define ODID_MIN_ALT (-1000) // Minimum altitude
#define ODID_MAX_ALT 31767.5f// Maximum altitude
#define ODID_INV_ALT ODID_MIN_ALT // Invalid altitude
#define ODID_MAX_TIMESTAMP (60 * 60)
#define ODID_INV_TIMESTAMP 0xFFFF // Invalid, No Value or Unknown Timestamp
#define ODID_MAX_AREA_RADIUS 2550
#define ODID_AREA_COUNT_MIN 1
#define ODID_AREA_COUNT_MAX 65000
class AP_UAVCAN;
// used to forbid copy of objects
#define CLASS_NO_COPY(c) c(const c &other) = delete; c &operator=(const c&) = delete
class AP_OpenDroneID
{
public:
AP_OpenDroneID();
/* Do not allow copies */
CLASS_NO_COPY(AP_OpenDroneID);
// parameter block
static const struct AP_Param::GroupInfo var_info[];
void init();
bool pre_arm_check(char* failmsg, uint8_t failmsg_len);
void update();
// send pending dronecan messages
void dronecan_send(AP_UAVCAN *);
// handle a message from the GCS
void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);
bool enabled(void) const {
return _enable != 0;
}
void set_arm_status(mavlink_open_drone_id_arm_status_t &status);
// get singleton instance
static AP_OpenDroneID *get_singleton()
{
return _singleton;
}
private:
static AP_OpenDroneID *_singleton;
// parameters
AP_Int8 _enable;
AP_Float _baro_accuracy; // Vertical accuracy of the barometer when installed
AP_Int16 _options;
AP_Int8 _mav_port;
AP_Int8 _can_driver;
enum Options : int16_t {
EnforceArming = (1U << 0U),
};
// check if an option is set
bool option_enabled(const Options option) const
{
return (uint8_t(_options.get()) & uint8_t(option)) != 0;
}
mavlink_channel_t _chan; // MAVLink channel that communicates with the Remote ID Transceiver
const mavlink_channel_t MAV_CHAN_INVALID = mavlink_channel_t(255U);
uint32_t _last_send_location_ms;
uint32_t _last_send_system_update_ms;
uint32_t _last_send_static_messages_ms;
const uint32_t _mavlink_dynamic_period_ms = 1000; //how often are mavlink dynamic messages sent in ms. E.g. 1000 = 1 Hz
const uint32_t _mavlink_static_period_ms = 5000; //how often are mavlink static messages sent in ms
bool _have_height_above_takeoff;
Location _takeoff_location;
bool _was_armed;
// packets ready to be sent, updated with semaphore held
HAL_Semaphore _sem;
mavlink_open_drone_id_location_t pkt_location;
mavlink_open_drone_id_basic_id_t pkt_basic_id;
mavlink_open_drone_id_system_t pkt_system;
mavlink_open_drone_id_self_id_t pkt_self_id;
mavlink_open_drone_id_operator_id_t pkt_operator_id;
// last time we got a SYSTEM message
uint32_t last_system_ms;
// last time we got a SYSTEM_UPDATE message
uint32_t last_system_update_ms;
// arm status from the transmitter
mavlink_open_drone_id_arm_status_t arm_status;
uint32_t last_arm_status_ms;
// last time we sent a lost transmitter message
uint32_t last_lost_tx_ms;
// last time we sent a lost operator location notice
uint32_t last_lost_operator_msg_ms;
// transmit functions to manually send a static MAVLink message
void send_dynamic_out();
void send_static_out();
void send_basic_id_message();
void send_system_message();
void send_system_update_message();
void send_self_id_message();
void send_operator_id_message();
void send_location_message();
enum next_msg : uint8_t {
NEXT_MSG_BASIC_ID = 0,
NEXT_MSG_SYSTEM,
NEXT_MSG_SELF_ID,
NEXT_MSG_OPERATOR_ID,
NEXT_MSG_ENUM_END
} next_msg_to_send;
uint32_t last_msg_send_ms;
// helper functions
MAV_ODID_HOR_ACC create_enum_horizontal_accuracy(float Accuracy) const;
MAV_ODID_VER_ACC create_enum_vertical_accuracy(float Accuracy) const;
MAV_ODID_SPEED_ACC create_enum_speed_accuracy(float Accuracy) const;
MAV_ODID_TIME_ACC create_enum_timestamp_accuracy(float Accuracy) const;
uint16_t create_direction(uint16_t direction) const;
uint16_t create_speed_horizontal(uint16_t speed) const;
int16_t create_speed_vertical(int16_t speed) const;
float create_altitude(float altitude) const;
float create_location_timestamp(float timestamp) const;
// mask of what UAVCAN drivers need to send each packet
const uint8_t dronecan_send_all = (1U<<HAL_MAX_CAN_PROTOCOL_DRIVERS)-1;
uint8_t need_send_location;
uint8_t need_send_basic_id;
uint8_t need_send_system;
uint8_t need_send_self_id;
uint8_t need_send_operator_id;
uint8_t dronecan_done_init;
uint8_t dronecan_init_failed;
void dronecan_init(AP_UAVCAN *uavcan);
void dronecan_send_location(AP_UAVCAN *uavcan);
void dronecan_send_basic_id(AP_UAVCAN *uavcan);
void dronecan_send_system(AP_UAVCAN *uavcan);
void dronecan_send_self_id(AP_UAVCAN *uavcan);
void dronecan_send_operator_id(AP_UAVCAN *uavcan);
};
namespace AP
{
AP_OpenDroneID &opendroneid();
};
#endif // AP_OPENDRONEID_ENABLED

279
libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp Normal file
View File

@ -0,0 +1,279 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
DroneCAN support for OpenDroneID
*/
#include "AP_OpenDroneID.h"
#if AP_OPENDRONEID_ENABLED
#if AP_OPENDRONEID_DRONECAN_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <dronecan/remoteid/Location.hpp>
#include <dronecan/remoteid/BasicID.hpp>
#include <dronecan/remoteid/SelfID.hpp>
#include <dronecan/remoteid/OperatorID.hpp>
#include <dronecan/remoteid/System.hpp>
#include <dronecan/remoteid/ArmStatus.hpp>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
static uavcan::Publisher<dronecan::remoteid::Location>* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::BasicID>* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::SelfID>* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::System>* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<dronecan::remoteid::OperatorID>* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
/*
backport helpers
*/
// send text when we do have a GCS
#ifndef GCS_SEND_TEXT
#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
#endif
/*
strncpy without the warning for not leaving room for nul termination
*/
static void strncpy_noterm(char *dest, const char *src, size_t n)
{
size_t len = strnlen(src, n);
if (len < n) {
// include nul term if it fits
len++;
}
memcpy(dest, src, len);
}
// handle ArmStatus
UC_REGISTRY_BINDER(ArmStatusCb, dronecan::remoteid::ArmStatus);
static uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusCb> *arm_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static void handle_arm_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ArmStatusCb &cb);
void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan)
{
const uint8_t driver_index = uavcan->get_driver_index();
const uint8_t driver_mask = 1U<<driver_index;
if (dronecan_done_init & driver_mask) {
// already initialised
return;
}
dc_location[driver_index] = new uavcan::Publisher<dronecan::remoteid::Location>(*uavcan->get_node());
if (dc_location[driver_index] == nullptr) {
goto alloc_failed;
}
dc_location[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
dc_location[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
dc_basic_id[driver_index] = new uavcan::Publisher<dronecan::remoteid::BasicID>(*uavcan->get_node());
if (dc_basic_id[driver_index] == nullptr) {
goto alloc_failed;
}
dc_basic_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
dc_basic_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
dc_self_id[driver_index] = new uavcan::Publisher<dronecan::remoteid::SelfID>(*uavcan->get_node());
if (dc_self_id[driver_index] == nullptr) {
goto alloc_failed;
}
dc_self_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
dc_self_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
dc_system[driver_index] = new uavcan::Publisher<dronecan::remoteid::System>(*uavcan->get_node());
if (dc_system[driver_index] == nullptr) {
goto alloc_failed;
}
dc_system[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
dc_system[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
dc_operator_id[driver_index] = new uavcan::Publisher<dronecan::remoteid::OperatorID>(*uavcan->get_node());
if (dc_operator_id[driver_index] == nullptr) {
goto alloc_failed;
}
dc_operator_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
dc_operator_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
arm_status_listener[driver_index] = new uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusCb>(*uavcan->get_node());
if (arm_status_listener[driver_index] == nullptr) {
goto alloc_failed;
}
arm_status_listener[driver_index]->start(ArmStatusCb(uavcan, &handle_arm_status));
dronecan_done_init |= driver_mask;
return;
alloc_failed:
dronecan_init_failed |= driver_mask;
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "OpenDroneID DroneCAN alloc failed");
}
/*
send pending DroneCAN OpenDroneID packets
*/
void AP_OpenDroneID::dronecan_send(AP_UAVCAN *uavcan)
{
const uint8_t driver_index = uavcan->get_driver_index();
const uint8_t driver_mask = 1U<<driver_index;
if (driver_index+1 != _can_driver) {
// not enabled for this CAN driver
return;
}
dronecan_init(uavcan);
if (dronecan_init_failed & driver_mask) {
return;
}
if (need_send_basic_id & driver_mask) {
WITH_SEMAPHORE(_sem);
dronecan_send_basic_id(uavcan);
need_send_basic_id &= ~driver_mask;
}
if (need_send_system & driver_mask) {
WITH_SEMAPHORE(_sem);
dronecan_send_system(uavcan);
need_send_system &= ~driver_mask;
}
if (need_send_self_id & driver_mask) {
WITH_SEMAPHORE(_sem);
dronecan_send_self_id(uavcan);
need_send_self_id &= ~driver_mask;
}
if (need_send_operator_id & driver_mask) {
WITH_SEMAPHORE(_sem);
dronecan_send_operator_id(uavcan);
need_send_operator_id &= ~driver_mask;
}
if (need_send_location & driver_mask) {
WITH_SEMAPHORE(_sem);
dronecan_send_location(uavcan);
need_send_location &= ~driver_mask;
}
}
#define ODID_COPY(name) msg.name = pkt.name
#define ODID_COPY_STR(name) do { for (uint8_t i = 0; i<sizeof(pkt.name) && pkt.name[i]; i++) msg.name.push_back(pkt.name[i]); } while(0)
void AP_OpenDroneID::dronecan_send_location(AP_UAVCAN *uavcan)
{
dronecan::remoteid::Location msg {};
const auto &pkt = pkt_location;
ODID_COPY_STR(id_or_mac);
ODID_COPY(status);
ODID_COPY(direction);
ODID_COPY(speed_horizontal);
ODID_COPY(speed_vertical);
ODID_COPY(latitude);
ODID_COPY(longitude);
ODID_COPY(altitude_barometric);
ODID_COPY(altitude_geodetic);
ODID_COPY(height_reference);
ODID_COPY(height);
ODID_COPY(horizontal_accuracy);
ODID_COPY(vertical_accuracy);
ODID_COPY(barometer_accuracy);
ODID_COPY(speed_accuracy);
ODID_COPY(timestamp);
ODID_COPY(timestamp_accuracy);
dc_location[uavcan->get_driver_index()]->broadcast(msg);
}
void AP_OpenDroneID::dronecan_send_basic_id(AP_UAVCAN *uavcan)
{
dronecan::remoteid::BasicID msg {};
const auto &pkt = pkt_basic_id;
ODID_COPY_STR(id_or_mac);
ODID_COPY(id_type);
ODID_COPY(ua_type);
ODID_COPY_STR(uas_id);
dc_basic_id[uavcan->get_driver_index()]->broadcast(msg);
}
void AP_OpenDroneID::dronecan_send_system(AP_UAVCAN *uavcan)
{
dronecan::remoteid::System msg {};
const auto &pkt = pkt_system;
ODID_COPY_STR(id_or_mac);
ODID_COPY(operator_location_type);
ODID_COPY(classification_type);
ODID_COPY(operator_latitude);
ODID_COPY(operator_longitude);
ODID_COPY(area_count);
ODID_COPY(area_radius);
ODID_COPY(area_ceiling);
ODID_COPY(area_floor);
ODID_COPY(category_eu);
ODID_COPY(class_eu);
ODID_COPY(operator_altitude_geo);
ODID_COPY(timestamp);
dc_system[uavcan->get_driver_index()]->broadcast(msg);
}
void AP_OpenDroneID::dronecan_send_self_id(AP_UAVCAN *uavcan)
{
dronecan::remoteid::SelfID msg {};
const auto &pkt = pkt_self_id;
ODID_COPY_STR(id_or_mac);
ODID_COPY(description_type);
ODID_COPY_STR(description);
dc_self_id[uavcan->get_driver_index()]->broadcast(msg);
}
void AP_OpenDroneID::dronecan_send_operator_id(AP_UAVCAN *uavcan)
{
dronecan::remoteid::OperatorID msg {};
const auto &pkt = pkt_operator_id;
ODID_COPY_STR(id_or_mac);
ODID_COPY(operator_id_type);
ODID_COPY_STR(operator_id);
dc_operator_id[uavcan->get_driver_index()]->broadcast(msg);
}
/*
handle ArmStatus message from DroneCAN
*/
static void handle_arm_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ArmStatusCb &cb)
{
const auto &msg = *cb.msg;
mavlink_open_drone_id_arm_status_t status {};
status.status = msg.status;
strncpy_noterm(status.error, msg.error.c_str(), sizeof(status.error));
AP::opendroneid().set_arm_status(status);
// Push DroneCAN Arm Message to GCS
gcs().send_to_active_channels(MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS,(const char*)&status);
}
// copy arm status for DroneCAN
void AP_OpenDroneID::set_arm_status(mavlink_open_drone_id_arm_status_t &status)
{
WITH_SEMAPHORE(_sem);
arm_status = status;
last_arm_status_ms = AP_HAL::millis();
}
#endif // AP_OPENDRONEID_DRONECAN_ENABLED
#endif // AP_OPENDRONEID_ENABLED

8
libraries/AP_OpenDroneID/AP_OpenDroneID_config.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_OPENDRONEID_ENABLED
// default to off. Enabled in hwdef.dat
#define AP_OPENDRONEID_ENABLED 0
#endif