ardupilot/libraries/AP_OpenDroneID/AP_OpenDroneID.h

209 lines
7.1 KiB
C
Raw Normal View History

/*
* 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