You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
208 lines
7.1 KiB
208 lines
7.1 KiB
/* |
|
* 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 |
|
|
|
#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; |
|
|
|
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
|
|
|