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>
|
|
|
|
|
|
|
|
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
2017-04-02 11:55:40 -03:00
|
|
|
|
|
|
|
#include <uavcan/uavcan.hpp>
|
2020-06-24 09:07:28 -03:00
|
|
|
#include "AP_UAVCAN_IfaceMgr.h"
|
|
|
|
#include "AP_UAVCAN_Clock.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>
|
2021-07-16 13:27:11 -03:00
|
|
|
#include <uavcan/protocol/param/GetSet.hpp>
|
|
|
|
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
|
2023-01-24 17:39:44 -04:00
|
|
|
#include <SRV_Channel/SRV_Channel_config.h>
|
2020-08-05 12:04:55 -03:00
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2018-03-09 11:36:21 -04:00
|
|
|
#ifndef UAVCAN_SRV_NUMBER
|
2022-05-20 08:33:43 -03:00
|
|
|
#define UAVCAN_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
|
|
|
|
|
2017-04-02 11:55:40 -03:00
|
|
|
#define AP_UAVCAN_SW_VERS_MAJOR 1
|
|
|
|
#define AP_UAVCAN_SW_VERS_MINOR 0
|
|
|
|
|
|
|
|
#define AP_UAVCAN_HW_VERS_MAJOR 1
|
|
|
|
#define AP_UAVCAN_HW_VERS_MINOR 0
|
|
|
|
|
2018-01-17 03:18:00 -04:00
|
|
|
#define AP_UAVCAN_MAX_LED_DEVICES 4
|
|
|
|
|
2019-09-02 23:25:39 -03:00
|
|
|
// fwd-declare callback classes
|
|
|
|
class ButtonCb;
|
2019-10-02 06:44:16 -03:00
|
|
|
class TrafficReportCb;
|
2020-01-03 23:52:59 -04:00
|
|
|
class ActuatorStatusCb;
|
2023-02-13 09:49:17 -04:00
|
|
|
class ActuatorStatusVolzCb;
|
2020-01-04 00:57:01 -04:00
|
|
|
class ESCStatusCb;
|
2020-11-29 19:47:32 -04:00
|
|
|
class DebugCb;
|
2021-07-16 13:27:11 -03:00
|
|
|
class ParamGetSetCb;
|
|
|
|
class ParamExecuteOpcodeCb;
|
2022-06-02 21:55:47 -03:00
|
|
|
class AP_PoolAllocator;
|
2022-04-04 07:33:33 -03:00
|
|
|
class AP_UAVCAN_DNA_Server;
|
2019-09-02 23:25:39 -03:00
|
|
|
|
2020-09-07 05:35:56 -03:00
|
|
|
#if defined(__GNUC__) && (__GNUC__ > 8)
|
|
|
|
#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
|
|
|
|
_Pragma("GCC diagnostic push") \
|
|
|
|
_Pragma("GCC diagnostic ignored \"-Wcast-function-type\"")
|
|
|
|
#define DISABLE_W_CAST_FUNCTION_TYPE_POP \
|
|
|
|
_Pragma("GCC diagnostic pop")
|
|
|
|
#else
|
|
|
|
#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH
|
|
|
|
#define DISABLE_W_CAST_FUNCTION_TYPE_POP
|
|
|
|
#endif
|
2021-10-15 06:55:39 -03:00
|
|
|
#if defined(__GNUC__) && (__GNUC__ >= 11)
|
|
|
|
#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID (void*)
|
|
|
|
#else
|
|
|
|
#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID
|
|
|
|
#endif
|
2020-09-07 05:35:56 -03:00
|
|
|
|
2018-07-19 16:37:12 -03:00
|
|
|
/*
|
|
|
|
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
|
2021-02-27 12:44:01 -04:00
|
|
|
the Callback will invoke registry to register the node as separate backend.
|
2018-07-19 16:37:12 -03:00
|
|
|
*/
|
|
|
|
#define UC_REGISTRY_BINDER(ClassName_, DataType_) \
|
2020-09-07 05:35:56 -03:00
|
|
|
class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
|
2019-03-23 01:30:07 -03:00
|
|
|
typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
|
2020-09-07 05:35:56 -03:00
|
|
|
public: \
|
|
|
|
ClassName_() : RegistryBinder() {} \
|
|
|
|
DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
|
|
|
|
ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \
|
2021-10-15 06:55:39 -03:00
|
|
|
RegistryBinder(uc, (Registry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
|
2020-09-07 05:35:56 -03:00
|
|
|
DISABLE_W_CAST_FUNCTION_TYPE_POP \
|
|
|
|
}
|
2018-07-19 16:37:12 -03:00
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
#define UC_CLIENT_CALL_REGISTRY_BINDER(ClassName_, DataType_) \
|
|
|
|
class ClassName_ : public AP_UAVCAN::ClientCallRegistryBinder<DataType_> { \
|
|
|
|
typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
|
|
|
|
public: \
|
|
|
|
ClassName_() : ClientCallRegistryBinder() {} \
|
|
|
|
DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
|
|
|
|
ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \
|
2021-10-15 06:55:39 -03:00
|
|
|
ClientCallRegistryBinder(uc, (ClientCallRegistry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
|
2021-07-16 13:27:11 -03:00
|
|
|
DISABLE_W_CAST_FUNCTION_TYPE_POP \
|
|
|
|
}
|
|
|
|
|
2021-02-27 12:44:01 -04:00
|
|
|
class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
|
2022-04-04 07:33:33 -03:00
|
|
|
friend class AP_UAVCAN_DNA_Server;
|
2017-04-02 11:55:40 -03:00
|
|
|
public:
|
|
|
|
AP_UAVCAN();
|
|
|
|
~AP_UAVCAN();
|
|
|
|
|
|
|
|
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
|
|
|
|
static AP_UAVCAN *get_uavcan(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
|
|
|
|
2018-07-19 16:37:12 -03:00
|
|
|
uavcan::Node<0>* get_node() { return _node; }
|
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
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &);
|
|
|
|
FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_UAVCAN*, const uint8_t, const char*, float &);
|
|
|
|
FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_UAVCAN*, const uint8_t, bool);
|
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
|
|
|
|
2019-10-21 08:10:33 -03:00
|
|
|
// send RTCMStream packets
|
|
|
|
void send_RTCMStream(const uint8_t *data, uint32_t len);
|
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
// Send Reboot command
|
|
|
|
// Note: Do not call this from outside UAVCAN thread context,
|
|
|
|
// you can call this from uavcan callbacks and handlers.
|
|
|
|
// THIS IS NOT A THREAD SAFE API!
|
|
|
|
void send_reboot_request(uint8_t node_id);
|
|
|
|
|
|
|
|
// set param value
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
|
|
|
|
// Save parameters
|
|
|
|
bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb);
|
|
|
|
|
2018-07-19 16:37:12 -03:00
|
|
|
template <typename DataType_>
|
|
|
|
class RegistryBinder {
|
|
|
|
protected:
|
2019-03-23 01:30:07 -03:00
|
|
|
typedef void (*Registry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const RegistryBinder& _cb);
|
2018-07-19 16:37:12 -03:00
|
|
|
AP_UAVCAN* _uc;
|
|
|
|
Registry _ffunc;
|
|
|
|
|
|
|
|
public:
|
|
|
|
RegistryBinder() :
|
2021-02-27 12:44:01 -04:00
|
|
|
_uc(),
|
2018-07-19 16:37:12 -03:00
|
|
|
_ffunc(),
|
|
|
|
msg() {}
|
|
|
|
|
|
|
|
RegistryBinder(AP_UAVCAN* uc, Registry ffunc) :
|
|
|
|
_uc(uc),
|
|
|
|
_ffunc(ffunc),
|
|
|
|
msg(nullptr) {}
|
|
|
|
|
|
|
|
void operator()(const uavcan::ReceivedDataStructure<DataType_>& _msg) {
|
|
|
|
msg = &_msg;
|
|
|
|
_ffunc(_uc, _msg.getSrcNodeID().get(), *this);
|
|
|
|
}
|
|
|
|
|
|
|
|
const uavcan::ReceivedDataStructure<DataType_> *msg;
|
|
|
|
};
|
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
// ClientCallRegistryBinder
|
|
|
|
template <typename DataType_>
|
|
|
|
class ClientCallRegistryBinder {
|
|
|
|
protected:
|
|
|
|
typedef void (*ClientCallRegistry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const ClientCallRegistryBinder& _cb);
|
|
|
|
AP_UAVCAN* _uc;
|
|
|
|
ClientCallRegistry _ffunc;
|
|
|
|
public:
|
|
|
|
ClientCallRegistryBinder() :
|
|
|
|
_uc(),
|
|
|
|
_ffunc(),
|
|
|
|
rsp() {}
|
|
|
|
|
|
|
|
ClientCallRegistryBinder(AP_UAVCAN* uc, ClientCallRegistry ffunc) :
|
|
|
|
_uc(uc),
|
|
|
|
_ffunc(ffunc),
|
|
|
|
rsp(nullptr) {}
|
|
|
|
|
|
|
|
void operator()(const uavcan::ServiceCallResult<DataType_>& _rsp) {
|
|
|
|
rsp = &_rsp;
|
|
|
|
_ffunc(_uc, _rsp.getCallID().server_node_id.get(), *this);
|
|
|
|
}
|
|
|
|
const uavcan::ServiceCallResult<DataType_> *rsp;
|
|
|
|
};
|
|
|
|
|
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),
|
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);
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
// This will be needed to implement if UAVCAN is used with multithreading
|
|
|
|
// Such cases will be firmware update, etc.
|
|
|
|
class RaiiSynchronizer {};
|
|
|
|
|
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();
|
2018-02-08 19:42:58 -04:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
///// LED /////
|
|
|
|
void led_out_send();
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2019-08-30 23:45:02 -03:00
|
|
|
// buzzer
|
|
|
|
void buzzer_send();
|
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();
|
|
|
|
|
2019-10-21 08:10:33 -03:00
|
|
|
// send GNSS injection
|
|
|
|
void rtcm_stream_send();
|
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
// send parameter get/set request
|
|
|
|
void send_parameter_request();
|
|
|
|
|
|
|
|
// send parameter save request
|
|
|
|
void send_parameter_save_request();
|
|
|
|
|
2022-12-06 22:36:20 -04:00
|
|
|
// periodic logging
|
|
|
|
void logging();
|
|
|
|
|
2021-07-16 13:27:11 -03:00
|
|
|
// set parameter on a node
|
|
|
|
ParamGetSetIntCb *param_int_cb;
|
|
|
|
ParamGetSetFloatCb *param_float_cb;
|
|
|
|
bool param_request_sent = true;
|
|
|
|
HAL_Semaphore _param_sem;
|
|
|
|
uint8_t param_request_node_id;
|
|
|
|
|
|
|
|
// save parameters on a node
|
|
|
|
ParamSaveCb *save_param_cb;
|
|
|
|
bool param_save_request_sent = true;
|
|
|
|
HAL_Semaphore _param_save_sem;
|
|
|
|
uint8_t param_save_request_node_id;
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
// UAVCAN parameters
|
|
|
|
AP_Int8 _uavcan_node;
|
|
|
|
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;
|
2017-04-02 11:55:40 -03:00
|
|
|
|
2022-06-02 21:55:47 -03:00
|
|
|
AP_PoolAllocator *_allocator;
|
2022-04-04 07:33:33 -03:00
|
|
|
AP_UAVCAN_DNA_Server *_dna_server;
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
uavcan::Node<0> *_node;
|
2018-06-18 16:47:57 -03:00
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
uint8_t _driver_index;
|
2020-06-24 09:07:28 -03:00
|
|
|
|
|
|
|
uavcan::CanIfaceMgr* _iface_mgr;
|
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;
|
2018-03-09 11:36:21 -04:00
|
|
|
} _SRV_conf[UAVCAN_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;
|
|
|
|
|
2018-03-09 11:36:21 -04:00
|
|
|
uint8_t _SRV_armed;
|
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;
|
|
|
|
|
2018-07-20 10:46:29 -03:00
|
|
|
///// LED /////
|
|
|
|
struct led_device {
|
2018-01-17 03:18:00 -04:00
|
|
|
uint8_t led_index;
|
2018-07-18 00:08:08 -03:00
|
|
|
uint8_t red;
|
|
|
|
uint8_t green;
|
|
|
|
uint8_t blue;
|
2018-07-20 10:46:29 -03:00
|
|
|
};
|
2018-01-17 03:18:00 -04:00
|
|
|
|
|
|
|
struct {
|
2018-07-20 10:46:29 -03:00
|
|
|
led_device devices[AP_UAVCAN_MAX_LED_DEVICES];
|
2018-01-17 03:18:00 -04:00
|
|
|
uint8_t devices_count;
|
|
|
|
uint64_t last_update;
|
|
|
|
} _led_conf;
|
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
HAL_Semaphore _led_out_sem;
|
2019-08-30 23:45:02 -03:00
|
|
|
|
|
|
|
// buzzer
|
|
|
|
struct {
|
|
|
|
HAL_Semaphore sem;
|
|
|
|
float frequency;
|
|
|
|
float duration;
|
|
|
|
uint8_t pending_mask; // mask of interfaces to send to
|
|
|
|
} _buzzer;
|
2019-08-31 01:30:49 -03:00
|
|
|
|
2023-02-28 18:50:23 -04:00
|
|
|
#if AP_DRONECAN_SEND_GPS
|
|
|
|
// send GNSS Fix and yaw, same thing AP_GPS_UAVCAN would receive
|
|
|
|
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
|
|
|
|
|
2019-10-21 08:10:33 -03:00
|
|
|
// GNSS RTCM injection
|
|
|
|
struct {
|
|
|
|
HAL_Semaphore sem;
|
|
|
|
uint32_t last_send_ms;
|
|
|
|
ByteBuffer *buf;
|
|
|
|
} _rtcm_stream;
|
|
|
|
|
2020-08-05 12:04:55 -03:00
|
|
|
// ESC
|
|
|
|
|
|
|
|
static HAL_Semaphore _telem_sem;
|
|
|
|
|
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;
|
|
|
|
|
2020-11-29 19:47:32 -04:00
|
|
|
// incoming button handling
|
2019-09-02 23:25:39 -03:00
|
|
|
static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
|
2019-10-02 06:44:16 -03:00
|
|
|
static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
|
2020-01-03 23:52:59 -04:00
|
|
|
static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
|
2023-02-13 09:49:17 -04:00
|
|
|
static void handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb);
|
2020-01-04 00:57:01 -04:00
|
|
|
static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
|
2020-08-05 12:04:55 -03:00
|
|
|
static bool is_esc_data_index_valid(const uint8_t index);
|
2020-11-29 19:47:32 -04:00
|
|
|
static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
|
2021-07-16 13:27:11 -03:00
|
|
|
static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);
|
|
|
|
static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb);
|
2017-04-02 11:55:40 -03:00
|
|
|
};
|
|
|
|
|
2020-07-30 14:53:31 -03:00
|
|
|
#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS
|