2017-04-02 11:55:40 -03:00
|
|
|
/*
|
2018-07-18 06:26:48 -03:00
|
|
|
* 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.
|
2017-04-02 11:55:40 -03:00
|
|
|
*
|
2018-07-18 06:26:48 -03:00
|
|
|
* 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/>.
|
|
|
|
*
|
|
|
|
* Author: Eugene Shamaev, Siddharth Bharat Purohit
|
2017-04-02 11:55:40 -03:00
|
|
|
*/
|
2020-07-30 14:53:31 -03:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
2023-04-08 01:09:10 -03:00
|
|
|
#if HAL_ENABLE_DRONECAN_DRIVERS
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
#include "AP_Canard_iface.h"
|
2022-04-04 09:24:15 -03:00
|
|
|
#include <AP_CANManager/AP_CANManager.h>
|
2017-04-02 11:55:40 -03:00
|
|
|
#include <AP_HAL/Semaphores.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2021-02-27 12:44:01 -04:00
|
|
|
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
|
2023-01-24 17:39:44 -04:00
|
|
|
#include <SRV_Channel/SRV_Channel_config.h>
|
2023-01-04 21:09:23 -04:00
|
|
|
#include <canard/publisher.h>
|
|
|
|
#include <canard/subscriber.h>
|
|
|
|
#include <canard/service_client.h>
|
|
|
|
#include <canard/service_server.h>
|
|
|
|
#include <stdio.h>
|
2023-04-06 21:18:01 -03:00
|
|
|
#include "AP_DroneCAN_DNA_Server.h"
|
2023-01-04 21:09:23 -04:00
|
|
|
#include <canard.h>
|
|
|
|
#include <dronecan_msgs.h>
|
2023-11-18 16:36:20 -04:00
|
|
|
#include <AP_SerialManager/AP_SerialManager_config.h>
|
2024-01-01 16:55:03 -04:00
|
|
|
#include <AP_Relay/AP_Relay_config.h>
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2023-04-08 01:27:51 -03:00
|
|
|
#ifndef DRONECAN_SRV_NUMBER
|
|
|
|
#define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS
|
2017-04-02 11:55:40 -03:00
|
|
|
#endif
|
|
|
|
|
2023-02-28 18:50:23 -04:00
|
|
|
#ifndef AP_DRONECAN_SEND_GPS
|
|
|
|
#define AP_DRONECAN_SEND_GPS (BOARD_FLASH_SIZE > 1024)
|
|
|
|
#endif
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
#define AP_DRONECAN_SW_VERS_MAJOR 1
|
|
|
|
#define AP_DRONECAN_SW_VERS_MINOR 0
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
#define AP_DRONECAN_HW_VERS_MAJOR 1
|
|
|
|
#define AP_DRONECAN_HW_VERS_MINOR 0
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-01-17 03:18:00 -04:00
|
|
|
|
2023-05-15 19:51:17 -03:00
|
|
|
#ifndef AP_DRONECAN_HOBBYWING_ESC_SUPPORT
|
|
|
|
#define AP_DRONECAN_HOBBYWING_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
|
|
|
|
#endif
|
|
|
|
|
2023-09-02 03:37:27 -03:00
|
|
|
#ifndef AP_DRONECAN_HIMARK_SERVO_SUPPORT
|
|
|
|
#define AP_DRONECAN_HIMARK_SERVO_SUPPORT (BOARD_FLASH_SIZE>1024)
|
|
|
|
#endif
|
|
|
|
|
2023-11-18 16:36:20 -04:00
|
|
|
#ifndef AP_DRONECAN_SERIAL_ENABLED
|
|
|
|
#define AP_DRONECAN_SERIAL_ENABLED AP_SERIALMANAGER_REGISTER_ENABLED && (BOARD_FLASH_SIZE>1024)
|
|
|
|
#endif
|
|
|
|
|
2024-08-22 17:01:54 -03:00
|
|
|
#ifndef AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
|
|
|
|
#define AP_DRONECAN_VOLZ_FEEDBACK_ENABLED 0
|
|
|
|
#endif
|
|
|
|
|
2023-11-18 16:36:20 -04:00
|
|
|
#if AP_DRONECAN_SERIAL_ENABLED
|
|
|
|
#include "AP_DroneCAN_serial.h"
|
|
|
|
#endif
|
|
|
|
|
2019-09-02 23:25:39 -03:00
|
|
|
// fwd-declare callback classes
|
2023-04-06 21:18:01 -03:00
|
|
|
class AP_DroneCAN_DNA_Server;
|
2023-11-22 23:16:08 -04:00
|
|
|
class CANSensor;
|
2019-09-02 23:25:39 -03:00
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
|
|
|
|
friend class AP_DroneCAN_DNA_Server;
|
2017-04-02 11:55:40 -03:00
|
|
|
public:
|
2023-04-06 21:18:01 -03:00
|
|
|
AP_DroneCAN(const int driver_index);
|
|
|
|
~AP_DroneCAN();
|
2017-04-02 11:55:40 -03:00
|
|
|
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
// Return uavcan from @driver_index or nullptr if it's not ready or doesn't exist
|
2023-04-08 01:27:51 -03:00
|
|
|
static AP_DroneCAN *get_dronecan(uint8_t driver_index);
|
2022-04-04 07:33:33 -03:00
|
|
|
bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
|
2018-07-20 10:46:29 -03:00
|
|
|
|
2018-11-16 05:12:07 -04:00
|
|
|
void init(uint8_t driver_index, bool enable_filters) override;
|
2020-06-24 09:07:28 -03:00
|
|
|
bool add_interface(AP_HAL::CANIface* can_iface) override;
|
2021-05-03 09:58:40 -03:00
|
|
|
|
2023-11-22 23:16:08 -04:00
|
|
|
// add an 11 bit auxillary driver
|
|
|
|
bool add_11bit_driver(CANSensor *sensor) override;
|
|
|
|
|
|
|
|
// handler for outgoing frames for auxillary drivers
|
|
|
|
bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override;
|
|
|
|
|
2021-02-01 12:26:35 -04:00
|
|
|
uint8_t get_driver_index() const { return _driver_index; }
|
2018-07-19 16:37:12 -03:00
|
|
|
|
2023-09-15 08:29:29 -03:00
|
|
|
// define string with length structure
|
|
|
|
struct string { uint8_t len; uint8_t data[128]; };
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
|
|
|
|
FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_DroneCAN*, const uint8_t, const char*, float &);
|
2023-09-15 08:29:29 -03:00
|
|
|
FUNCTOR_TYPEDEF(ParamGetSetStringCb, bool, AP_DroneCAN*, const uint8_t, const char*, string &);
|
2023-04-06 21:18:01 -03:00
|
|
|
FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_DroneCAN*, const uint8_t, bool);
|
2018-07-20 10:46:29 -03:00
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
void send_node_status();
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
///// SRV output /////
|
|
|
|
void SRV_push_servos(void);
|
|
|
|
|
|
|
|
///// LED /////
|
|
|
|
bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
|
|
|
|
|
2019-08-30 23:45:02 -03:00
|
|
|
// buzzer
|
|
|
|
void set_buzzer_tone(float frequency, float duration_s);
|
2018-07-19 16:37:12 -03:00
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
// Send Reboot command
|
|
|
|
// Note: Do not call this from outside UAVCAN thread context,
|
2023-01-04 21:09:23 -04:00
|
|
|
// you can call this from dronecan callbacks and handlers.
|
2021-07-16 13:27:11 -03:00
|
|
|
// THIS IS NOT A THREAD SAFE API!
|
|
|
|
void send_reboot_request(uint8_t node_id);
|
|
|
|
|
2023-08-08 02:37:25 -03:00
|
|
|
// get or set param value
|
|
|
|
// returns true on success, false on failure
|
|
|
|
// failures occur when waiting on node to respond to previous get or set request
|
2021-07-16 13:27:11 -03:00
|
|
|
bool set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb);
|
|
|
|
bool set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb);
|
2023-09-15 08:29:29 -03:00
|
|
|
bool set_parameter_on_node(uint8_t node_id, const char *name, const string &value, ParamGetSetStringCb *cb);
|
2021-07-16 13:27:11 -03:00
|
|
|
bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb);
|
|
|
|
bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb);
|
2023-09-15 08:29:29 -03:00
|
|
|
bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetStringCb *cb);
|
2021-07-16 13:27:11 -03:00
|
|
|
|
|
|
|
// Save parameters
|
|
|
|
bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb);
|
|
|
|
|
2021-09-10 23:47:02 -03:00
|
|
|
// options bitmask
|
|
|
|
enum class Options : uint16_t {
|
|
|
|
DNA_CLEAR_DATABASE = (1U<<0),
|
|
|
|
DNA_IGNORE_DUPLICATE_NODE = (1U<<1),
|
2021-05-03 09:58:40 -03:00
|
|
|
CANFD_ENABLED = (1U<<2),
|
2022-07-25 00:30:50 -03:00
|
|
|
DNA_IGNORE_UNHEALTHY_NODE = (1U<<3),
|
2022-09-12 00:31:04 -03:00
|
|
|
USE_ACTUATOR_PWM = (1U<<4),
|
2023-02-28 18:50:23 -04:00
|
|
|
SEND_GNSS = (1U<<5),
|
2022-12-01 23:16:47 -04:00
|
|
|
USE_HIMARK_SERVO = (1U<<6),
|
2023-05-15 19:51:17 -03:00
|
|
|
USE_HOBBYWING_ESC = (1U<<7),
|
2023-07-07 04:20:37 -03:00
|
|
|
ENABLE_STATS = (1U<<8),
|
2021-09-10 23:47:02 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
// check if a option is set
|
|
|
|
bool option_is_set(Options option) const {
|
|
|
|
return (uint16_t(_options.get()) & uint16_t(option)) != 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if a option is set and if it is then reset it to
|
|
|
|
// 0. return true if it was set
|
|
|
|
bool check_and_reset_option(Options option);
|
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
CanardInterface& get_canard_iface() { return canard_iface; }
|
2018-07-20 10:46:29 -03:00
|
|
|
|
2023-05-02 03:02:46 -03:00
|
|
|
Canard::Publisher<uavcan_equipment_indication_LightsCommand> rgb_led{canard_iface};
|
|
|
|
Canard::Publisher<uavcan_equipment_indication_BeepCommand> buzzer{canard_iface};
|
|
|
|
Canard::Publisher<uavcan_equipment_gnss_RTCMStream> rtcm_stream{canard_iface};
|
|
|
|
|
2023-06-20 02:06:11 -03:00
|
|
|
// xacti specific publishers
|
|
|
|
Canard::Publisher<com_xacti_CopterAttStatus> xacti_copter_att_status{canard_iface};
|
|
|
|
Canard::Publisher<com_xacti_GimbalControlData> xacti_gimbal_control_data{canard_iface};
|
|
|
|
Canard::Publisher<com_xacti_GnssStatus> xacti_gnss_status{canard_iface};
|
|
|
|
|
2024-01-01 16:55:03 -04:00
|
|
|
#if AP_RELAY_DRONECAN_ENABLED
|
|
|
|
// Hardpoint for relay
|
|
|
|
// Needs to be public so relay can edge trigger as well as streaming
|
|
|
|
Canard::Publisher<uavcan_equipment_hardpoint_Command> relay_hardpoint{canard_iface};
|
|
|
|
#endif
|
|
|
|
|
2022-03-01 12:47:00 -04:00
|
|
|
private:
|
2018-07-20 10:46:29 -03:00
|
|
|
void loop(void);
|
|
|
|
|
|
|
|
///// SRV output /////
|
|
|
|
void SRV_send_actuator();
|
2018-03-09 11:36:21 -04:00
|
|
|
void SRV_send_esc();
|
2023-09-02 03:37:27 -03:00
|
|
|
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
|
2022-12-01 23:16:47 -04:00
|
|
|
void SRV_send_himark();
|
2023-09-02 03:37:27 -03:00
|
|
|
#endif
|
2018-02-08 19:42:58 -04:00
|
|
|
|
2023-07-19 16:08:39 -03:00
|
|
|
//scale servo output appropriately before sending
|
|
|
|
int16_t scale_esc_output(uint8_t idx);
|
|
|
|
|
2019-08-31 01:30:49 -03:00
|
|
|
// SafetyState
|
|
|
|
void safety_state_send();
|
2019-10-21 08:10:33 -03:00
|
|
|
|
2021-09-15 03:48:19 -03:00
|
|
|
// send notify vehicle state
|
|
|
|
void notify_state_send();
|
|
|
|
|
2023-08-08 02:38:45 -03:00
|
|
|
// check for parameter get/set response timeout
|
|
|
|
void check_parameter_callback_timeout();
|
|
|
|
|
2023-08-08 02:37:25 -03:00
|
|
|
// send queued parameter get/set request. called from loop
|
2021-07-16 13:27:11 -03:00
|
|
|
void send_parameter_request();
|
|
|
|
|
2023-08-08 02:37:25 -03:00
|
|
|
// send queued parameter save request. called from loop
|
2021-07-16 13:27:11 -03:00
|
|
|
void send_parameter_save_request();
|
|
|
|
|
2022-12-06 22:36:20 -04:00
|
|
|
// periodic logging
|
|
|
|
void logging();
|
|
|
|
|
2023-08-08 02:37:25 -03:00
|
|
|
// get parameter on a node
|
|
|
|
ParamGetSetIntCb *param_int_cb; // latest get param request callback function (for integers)
|
|
|
|
ParamGetSetFloatCb *param_float_cb; // latest get param request callback function (for floats)
|
2023-09-15 08:29:29 -03:00
|
|
|
ParamGetSetStringCb *param_string_cb; // latest get param request callback function (for strings)
|
2023-08-08 02:37:25 -03:00
|
|
|
bool param_request_sent = true; // true after a param request has been sent, false when queued to be sent
|
2023-08-08 02:38:45 -03:00
|
|
|
uint32_t param_request_sent_ms; // system time that get param request was sent
|
2023-08-08 02:37:25 -03:00
|
|
|
HAL_Semaphore _param_sem; // semaphore protecting this block of variables
|
|
|
|
uint8_t param_request_node_id; // node id of most recent get param request
|
2021-07-16 13:27:11 -03:00
|
|
|
|
|
|
|
// save parameters on a node
|
2023-08-08 02:37:25 -03:00
|
|
|
ParamSaveCb *save_param_cb; // latest save param request callback function
|
|
|
|
bool param_save_request_sent = true; // true after a save param request has been sent, false when queued to be sent
|
|
|
|
HAL_Semaphore _param_save_sem; // semaphore protecting this block of variables
|
|
|
|
uint8_t param_save_request_node_id; // node id of most recent save param request
|
2021-07-16 13:27:11 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
// UAVCAN parameters
|
2023-01-04 21:09:23 -04:00
|
|
|
AP_Int8 _dronecan_node;
|
2018-07-20 10:46:29 -03:00
|
|
|
AP_Int32 _servo_bm;
|
|
|
|
AP_Int32 _esc_bm;
|
AP_UAVCAN: added CAN_Dx_UC_ESC_OF parameter
this allows for an offset in ESC numbering for much more efficient CAN
bandwidth usage.
For example, on a coaxial OctoQuad quadplane the ESCs are typically
setup as outputs 5 to 12. An ideal setup is to split these over 2 CAN
buses, with one CAN bus for the top layer and the one bus for the
bottom layer (allowing for VTOL flight with one bus failed).
Without this offset parameter you would be sending RawCommand messages
like this:
bus1: [ 0, 0, 0, 0, ESC1, ESC2, ESC3, ESC4 ]
bus2: [ 0, 0, 0, 0, 0, 0, 0, 0, ESC1, ESC2, ESC3, ESC4 ]
this is very wasteful of bus bandwidth, with bus1 using 3x the
bandwidth it should and bus2 using 4x the bandwidth it should (the
above will take 3 can frames for bus1, and 4 can frames for bus 2)
With this patch you can set:
CAN_D1_UC_ESC_OF = 4
CAN_D2_UC_ESC_OF = 8
and you will get this on the bus:
bus1: [ ESC1, ESC2, ESC3, ESC4 ]
bus2: [ ESC1, ESC2, ESC3, ESC4 ]
that takes just 1 can frame per send on each bus
2022-05-18 03:35:28 -03:00
|
|
|
AP_Int8 _esc_offset;
|
2018-07-20 10:46:29 -03:00
|
|
|
AP_Int16 _servo_rate_hz;
|
2021-09-10 23:47:02 -03:00
|
|
|
AP_Int16 _options;
|
2021-09-15 03:48:19 -03:00
|
|
|
AP_Int16 _notify_state_hz;
|
2022-06-02 21:55:47 -03:00
|
|
|
AP_Int16 _pool_size;
|
2023-07-19 16:08:39 -03:00
|
|
|
AP_Int32 _esc_rv;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
uint32_t *mem_pool;
|
2022-04-04 07:33:33 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
uint8_t _driver_index;
|
2020-06-24 09:07:28 -03:00
|
|
|
|
2024-09-20 02:39:22 -03:00
|
|
|
CanardInterface canard_iface;
|
|
|
|
|
|
|
|
AP_DroneCAN_DNA_Server _dna_server;
|
|
|
|
|
2019-12-30 17:41:18 -04:00
|
|
|
char _thread_name[13];
|
2018-07-20 10:46:29 -03:00
|
|
|
bool _initialized;
|
|
|
|
///// SRV output /////
|
2017-04-02 11:55:40 -03:00
|
|
|
struct {
|
|
|
|
uint16_t pulse;
|
2018-05-24 07:23:00 -03:00
|
|
|
bool esc_pending;
|
|
|
|
bool servo_pending;
|
2023-04-08 01:27:51 -03:00
|
|
|
} _SRV_conf[DRONECAN_SRV_NUMBER];
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2022-12-06 22:36:20 -04:00
|
|
|
uint32_t _esc_send_count;
|
|
|
|
uint32_t _srv_send_count;
|
|
|
|
uint32_t _fail_send_count;
|
|
|
|
|
2023-06-27 20:29:00 -03:00
|
|
|
uint32_t _SRV_armed_mask; // mask of servo outputs that are active
|
|
|
|
uint32_t _ESC_armed_mask; // mask of ESC outputs that are active
|
2018-05-24 07:23:00 -03:00
|
|
|
uint32_t _SRV_last_send_us;
|
2018-10-11 20:35:04 -03:00
|
|
|
HAL_Semaphore SRV_sem;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2022-12-06 22:36:20 -04:00
|
|
|
// last log time
|
|
|
|
uint32_t last_log_ms;
|
|
|
|
|
2023-02-28 18:50:23 -04:00
|
|
|
#if AP_DRONECAN_SEND_GPS
|
2023-04-08 01:09:10 -03:00
|
|
|
// send GNSS Fix and yaw, same thing AP_GPS_DroneCAN would receive
|
2023-02-28 18:50:23 -04:00
|
|
|
void gnss_send_fix();
|
|
|
|
void gnss_send_yaw();
|
|
|
|
|
|
|
|
// GNSS Fix and Status
|
|
|
|
struct {
|
|
|
|
uint32_t last_gps_lib_fix_ms;
|
|
|
|
uint32_t last_send_status_ms;
|
|
|
|
uint32_t last_lib_yaw_time_ms;
|
|
|
|
} _gnss;
|
|
|
|
#endif
|
2020-08-05 12:04:55 -03:00
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
// node status send
|
|
|
|
uint32_t _node_status_last_send_ms;
|
|
|
|
|
2019-09-02 23:25:39 -03:00
|
|
|
// safety status send state
|
2019-08-31 01:30:49 -03:00
|
|
|
uint32_t _last_safety_state_ms;
|
2019-09-02 23:25:39 -03:00
|
|
|
|
2021-09-15 03:48:19 -03:00
|
|
|
// notify vehicle state
|
|
|
|
uint32_t _last_notify_state_ms;
|
2023-01-04 21:09:23 -04:00
|
|
|
uavcan_protocol_NodeStatus node_status_msg;
|
|
|
|
|
2024-01-01 16:55:03 -04:00
|
|
|
#if AP_RELAY_DRONECAN_ENABLED
|
|
|
|
void relay_hardpoint_send();
|
|
|
|
struct {
|
|
|
|
AP_Int16 rate_hz;
|
|
|
|
uint32_t last_send_ms;
|
|
|
|
uint8_t last_index;
|
|
|
|
} _relay;
|
|
|
|
#endif
|
|
|
|
|
2023-11-18 16:36:20 -04:00
|
|
|
#if AP_DRONECAN_SERIAL_ENABLED
|
|
|
|
AP_DroneCAN_Serial serial;
|
|
|
|
#endif
|
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Publisher<uavcan_protocol_NodeStatus> node_status{canard_iface};
|
2023-07-07 04:20:37 -03:00
|
|
|
Canard::Publisher<dronecan_protocol_CanStats> can_stats{canard_iface};
|
|
|
|
Canard::Publisher<dronecan_protocol_Stats> protocol_stats{canard_iface};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Publisher<uavcan_equipment_actuator_ArrayCommand> act_out_array{canard_iface};
|
|
|
|
Canard::Publisher<uavcan_equipment_esc_RawCommand> esc_raw{canard_iface};
|
|
|
|
Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};
|
|
|
|
Canard::Publisher<uavcan_equipment_safety_ArmingStatus> arming_status{canard_iface};
|
|
|
|
Canard::Publisher<ardupilot_indication_NotifyState> notify_state{canard_iface};
|
2023-09-02 03:37:27 -03:00
|
|
|
|
|
|
|
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
|
2022-12-01 23:16:47 -04:00
|
|
|
Canard::Publisher<com_himark_servo_ServoCmd> himark_out{canard_iface};
|
2023-09-02 03:37:27 -03:00
|
|
|
#endif
|
2023-01-04 21:09:23 -04:00
|
|
|
|
|
|
|
#if AP_DRONECAN_SEND_GPS
|
|
|
|
Canard::Publisher<uavcan_equipment_gnss_Fix2> gnss_fix2{canard_iface};
|
|
|
|
Canard::Publisher<uavcan_equipment_gnss_Auxiliary> gnss_auxiliary{canard_iface};
|
|
|
|
Canard::Publisher<ardupilot_gnss_Heading> gnss_heading{canard_iface};
|
|
|
|
Canard::Publisher<ardupilot_gnss_Status> gnss_status{canard_iface};
|
|
|
|
#endif
|
|
|
|
// incoming messages
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, ardupilot_indication_Button> safety_button_cb{this, &AP_DroneCAN::handle_button};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Subscriber<ardupilot_indication_Button> safety_button_listener{safety_button_cb, _driver_index};
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index};
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index};
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
|
|
|
|
|
2024-07-27 16:44:13 -03:00
|
|
|
#if AP_EXTENDED_ESC_TELEM_ENABLED
|
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_StatusExtended> esc_status_extended_cb{this, &AP_DroneCAN::handle_esc_ext_status};
|
|
|
|
Canard::Subscriber<uavcan_equipment_esc_StatusExtended> esc_status_extended_listener{esc_status_extended_cb, _driver_index};
|
|
|
|
#endif
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
|
|
|
|
|
2024-08-22 17:01:54 -03:00
|
|
|
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
|
|
|
|
Canard::ObjCallback<AP_DroneCAN, com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz};
|
|
|
|
Canard::Subscriber<com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index};
|
|
|
|
#endif
|
|
|
|
|
2023-01-04 21:09:23 -04:00
|
|
|
// param client
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_param_GetSetResponse> param_get_set_res_cb{this, &AP_DroneCAN::handle_param_get_set_response};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Client<uavcan_protocol_param_GetSetResponse> param_get_set_client{canard_iface, param_get_set_res_cb};
|
|
|
|
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_param_ExecuteOpcodeResponse> param_save_res_cb{this, &AP_DroneCAN::handle_param_save_response};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Client<uavcan_protocol_param_ExecuteOpcodeResponse> param_save_client{canard_iface, param_save_res_cb};
|
|
|
|
|
|
|
|
// reboot client
|
|
|
|
void handle_restart_node_response(const CanardRxTransfer& transfer, const uavcan_protocol_RestartNodeResponse& msg) {}
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_RestartNodeResponse> restart_node_res_cb{this, &AP_DroneCAN::handle_restart_node_response};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Client<uavcan_protocol_RestartNodeResponse> restart_node_client{canard_iface, restart_node_res_cb};
|
|
|
|
|
|
|
|
uavcan_protocol_param_ExecuteOpcodeRequest param_save_req;
|
|
|
|
uavcan_protocol_param_GetSetRequest param_getset_req;
|
|
|
|
|
|
|
|
// Node Info Server
|
2023-04-06 21:18:01 -03:00
|
|
|
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_GetNodeInfoRequest> node_info_req_cb{this, &AP_DroneCAN::handle_node_info_request};
|
2023-01-04 21:09:23 -04:00
|
|
|
Canard::Server<uavcan_protocol_GetNodeInfoRequest> node_info_server{canard_iface, node_info_req_cb};
|
|
|
|
uavcan_protocol_GetNodeInfoResponse node_info_rsp;
|
2021-09-15 03:48:19 -03:00
|
|
|
|
2023-05-15 19:51:17 -03:00
|
|
|
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
|
|
|
|
/*
|
|
|
|
Hobbywing ESC support. Note that we need additional meta-data as
|
|
|
|
the status messages do not have an ESC ID in them, so we need a
|
|
|
|
mapping from node ID
|
|
|
|
*/
|
|
|
|
#define HOBBYWING_MAX_ESC 8
|
|
|
|
struct {
|
|
|
|
uint32_t last_GetId_send_ms;
|
|
|
|
uint8_t thr_chan[HOBBYWING_MAX_ESC];
|
|
|
|
} hobbywing;
|
|
|
|
void hobbywing_ESC_update();
|
|
|
|
|
|
|
|
void SRV_send_esc_hobbywing();
|
|
|
|
Canard::Publisher<com_hobbywing_esc_RawCommand> esc_hobbywing_raw{canard_iface};
|
|
|
|
Canard::Publisher<com_hobbywing_esc_GetEscID> esc_hobbywing_GetEscID{canard_iface};
|
|
|
|
Canard::ObjCallback<AP_DroneCAN, com_hobbywing_esc_GetEscID> esc_hobbywing_GetEscID_cb{this, &AP_DroneCAN::handle_hobbywing_GetEscID};
|
|
|
|
Canard::Subscriber<com_hobbywing_esc_GetEscID> esc_hobbywing_GetEscID_listener{esc_hobbywing_GetEscID_cb, _driver_index};
|
|
|
|
Canard::ObjCallback<AP_DroneCAN, com_hobbywing_esc_StatusMsg1> esc_hobbywing_StatusMSG1_cb{this, &AP_DroneCAN::handle_hobbywing_StatusMsg1};
|
|
|
|
Canard::Subscriber<com_hobbywing_esc_StatusMsg1> esc_hobbywing_StatusMSG1_listener{esc_hobbywing_StatusMSG1_cb, _driver_index};
|
|
|
|
Canard::ObjCallback<AP_DroneCAN, com_hobbywing_esc_StatusMsg2> esc_hobbywing_StatusMSG2_cb{this, &AP_DroneCAN::handle_hobbywing_StatusMsg2};
|
|
|
|
Canard::Subscriber<com_hobbywing_esc_StatusMsg2> esc_hobbywing_StatusMSG2_listener{esc_hobbywing_StatusMSG2_cb, _driver_index};
|
|
|
|
bool hobbywing_find_esc_index(uint8_t node_id, uint8_t &esc_index) const;
|
|
|
|
void handle_hobbywing_GetEscID(const CanardRxTransfer& transfer, const com_hobbywing_esc_GetEscID& msg);
|
|
|
|
void handle_hobbywing_StatusMsg1(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg1& msg);
|
|
|
|
void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
|
|
|
|
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
|
2023-09-02 03:37:27 -03:00
|
|
|
|
|
|
|
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
|
|
|
|
void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
|
|
|
|
#endif
|
2023-05-15 19:51:17 -03:00
|
|
|
|
2020-11-29 19:47:32 -04:00
|
|
|
// incoming button handling
|
2023-01-04 21:09:23 -04:00
|
|
|
void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
|
|
|
|
void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
|
|
|
|
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
|
2023-04-12 20:46:32 -03:00
|
|
|
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
|
2023-01-04 21:09:23 -04:00
|
|
|
void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
|
2024-07-27 16:44:13 -03:00
|
|
|
#if AP_EXTENDED_ESC_TELEM_ENABLED
|
|
|
|
void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);
|
|
|
|
#endif
|
2020-08-05 12:04:55 -03:00
|
|
|
static bool is_esc_data_index_valid(const uint8_t index);
|
2023-01-04 21:09:23 -04:00
|
|
|
void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
|
|
|
|
void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);
|
|
|
|
void handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp);
|
|
|
|
void handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req);
|
2017-04-02 11:55:40 -03:00
|
|
|
};
|
|
|
|
|
2023-04-08 01:09:10 -03:00
|
|
|
#endif // #if HAL_ENABLE_DRONECAN_DRIVERS
|