diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp new file mode 100644 index 0000000000..1dd5e5e178 --- /dev/null +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp @@ -0,0 +1,611 @@ +/* + * 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 . + */ +/* + * + * 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 + +#include +#include +#include +#include +#include +#include +#include + +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) { + 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 uint32_t now = AP_HAL::millis(); + + const bool armed = hal.util->get_soft_armed(); + if (armed && !_was_armed) { + // use arm location as takeoff location + AP::ahrs().get_location(_takeoff_location); + } + _was_armed = armed; + + if (now - _last_send_dynamic_messages_ms >= _mavlink_dynamic_period_ms) { + _last_send_dynamic_messages_ms = now; + send_dynamic_out(); + } + + send_static_out(); +} + +void AP_OpenDroneID::send_dynamic_out() +{ + send_location_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) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter"); + } + + const uint32_t msg_spacing_ms = _mavlink_dynamic_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)); + + last_msg_send_ms = now_ms; + switch (next_msg_to_send) { + case NEXT_MSG_BASIC_ID: + send_basic_id_message(); + break; + case NEXT_MSG_SYSTEM: + send_system_message(); + break; + case NEXT_MSG_SELF_ID: + send_self_id_message(); + break; + case NEXT_MSG_OPERATOR_ID: + send_operator_id_message(); + break; + case NEXT_MSG_ENUM_END: + break; + } + 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_location(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; + float undulation; + if (current_location.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) { + altitude_geodetic = alt_amsl_cm * 0.01; + } + if (gps.get_undulation(undulation)) { + altitude_geodetic -= undulation; + } + + // 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 = { + 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, + 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; + // note that packet is filled in by the GCS + if (_chan != MAV_CHAN_INVALID) { + mavlink_msg_open_drone_id_self_id_send_struct(_chan, &pkt_self_id); + } +} + +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; + } +} + +// singleton instance +AP_OpenDroneID *AP_OpenDroneID::_singleton; + +namespace AP +{ + +AP_OpenDroneID &opendroneid() +{ + return *AP_OpenDroneID::get_singleton(); +} + +} +#endif //AP_OPENDRONEID_ENABLED diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.h b/libraries/AP_OpenDroneID/AP_OpenDroneID.h new file mode 100644 index 0000000000..48c520a8f3 --- /dev/null +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.h @@ -0,0 +1,196 @@ +/* + * 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 . + * + * 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 + +#ifndef AP_OPENDRONEID_ENABLED +// default to off. Enabled in hwdef.dat +#define AP_OPENDRONEID_ENABLED 0 +#endif + +#if AP_OPENDRONEID_ENABLED + +#include +#include +#include +#include + +#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; + +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_dynamic_messages_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; + + // arm status from the transmitter + mavlink_open_drone_id_arm_status_t arm_status; + uint32_t last_arm_status_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_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<. + */ +/* + DroneCAN support for OpenDroneID + */ + +#include "AP_OpenDroneID.h" + +#if AP_OPENDRONEID_ENABLED + +#include +#include +#include +#include +#include +#include +#include + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS +static uavcan::Publisher* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; + +// handle ArmStatus +UC_REGISTRY_BINDER(ArmStatusCb, dronecan::remoteid::ArmStatus); +static uavcan::Subscriber *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<(*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(*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(*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(*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(*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(*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<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); +} + +// 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 +#endif // AP_OPENDRONEID_ENABLED