ardupilot/libraries/AP_UAVCAN/AP_UAVCAN.h

/*
 * 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/>.
 *
 * Author: Eugene Shamaev, Siddharth Bharat Purohit
 */
#pragma once

#include <AP_HAL/AP_HAL.h>

#if HAL_ENABLE_LIBUAVCAN_DRIVERS

#include <uavcan/uavcan.hpp>
#include "AP_UAVCAN_IfaceMgr.h"
#include "AP_UAVCAN_Clock.h"
#include <AP_CANManager/AP_CANManager.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <SRV_Channel/SRV_Channel_config.h>


#ifndef UAVCAN_SRV_NUMBER
#define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS
#endif

#ifndef AP_DRONECAN_SEND_GPS
#define AP_DRONECAN_SEND_GPS (BOARD_FLASH_SIZE > 1024)
#endif

#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

#define AP_UAVCAN_MAX_LED_DEVICES 4

#ifndef AP_DRONECAN_HOBBYWING_ESC_ENABLED
#define AP_DRONECAN_HOBBYWING_ESC_ENABLED (BOARD_FLASH_SIZE>1024)
#endif

#ifndef AP_DRONECAN_HIMARK_SERVO_ENABLED
#define AP_DRONECAN_HIMARK_SERVO_ENABLED (BOARD_FLASH_SIZE>1024)
#endif

// fwd-declare callback classes
class ButtonCb;
class TrafficReportCb;
class ActuatorStatusCb;
class ActuatorStatusVolzCb;
class ESCStatusCb;
class DebugCb;
class ParamGetSetCb;
class ParamExecuteOpcodeCb;
class AP_PoolAllocator;
class AP_UAVCAN_DNA_Server;

#if AP_DRONECAN_HIMARK_SERVO_ENABLED
class HimarkServoInfoCb;
#endif

#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
class HobbywingESCIDCb;
class HobbywingStatus1Cb;
class HobbywingStatus2Cb;
#endif


#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
#if defined(__GNUC__) && (__GNUC__ >= 11)
#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID (void*)
#else
#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID
#endif

/*
    Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
    the Callback will invoke registry to register the node as separate backend.
*/
#define UC_REGISTRY_BINDER(ClassName_, DataType_) \
    class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
        typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
        public: \
            ClassName_() : RegistryBinder() {} \
            DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
            ClassName_(AP_UAVCAN* uc,  CN_Registry ffunc) : \
                RegistryBinder(uc, (Registry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
            DISABLE_W_CAST_FUNCTION_TYPE_POP \
    }

#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) : \
                ClientCallRegistryBinder(uc, (ClientCallRegistry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
            DISABLE_W_CAST_FUNCTION_TYPE_POP \
    }

class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
    friend class AP_UAVCAN_DNA_Server;
public:
    AP_UAVCAN();
    ~AP_UAVCAN();

    static const struct AP_Param::GroupInfo var_info[];

    // 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);
    bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;

    void init(uint8_t driver_index, bool enable_filters) override;
    bool add_interface(AP_HAL::CANIface* can_iface) override;

    uavcan::Node<0>* get_node() { return _node; }
    uint8_t get_driver_index() const { return _driver_index; }

    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);

    ///// SRV output /////
    void SRV_push_servos(void);

    ///// LED /////
    bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);

    // buzzer
    void set_buzzer_tone(float frequency, float duration_s);

    // send RTCMStream packets
    void send_RTCMStream(const uint8_t *data, uint32_t len);

    // 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);

    template <typename DataType_>
    class RegistryBinder {
    protected:
        typedef void (*Registry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const RegistryBinder& _cb);
        AP_UAVCAN* _uc;
        Registry _ffunc;

    public:
        RegistryBinder() :
            _uc(),
            _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;
    };

    // 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;
    };

    // options bitmask
    enum class Options : uint16_t {
        DNA_CLEAR_DATABASE        = (1U<<0),
        DNA_IGNORE_DUPLICATE_NODE = (1U<<1),
        CANFD_ENABLED             = (1U<<2),
        DNA_IGNORE_UNHEALTHY_NODE = (1U<<3),
        USE_ACTUATOR_PWM          = (1U<<4),
        SEND_GNSS                 = (1U<<5),
        USE_HIMARK_SERVO          = (1U<<6),
        USE_HOBBYWING_ESC         = (1U<<7),
    };

    // 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);

    // This will be needed to implement if UAVCAN is used with multithreading
    // Such cases will be firmware update, etc.
    class RaiiSynchronizer {};

private:
    void loop(void);

    ///// SRV output /////
    void SRV_send_actuator();
    void SRV_send_esc();
#if AP_DRONECAN_HIMARK_SERVO_ENABLED
    void SRV_send_himark();
#endif

    ///// LED /////
    void led_out_send();

    // buzzer
    void buzzer_send();

    // SafetyState
    void safety_state_send();

    // send notify vehicle state
    void notify_state_send();

    // send GNSS injection
    void rtcm_stream_send();

    // send parameter get/set request
    void send_parameter_request();
    
    // send parameter save request
    void send_parameter_save_request();

    // periodic logging
    void logging();
    
    // 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;

    // UAVCAN parameters
    AP_Int8 _uavcan_node;
    AP_Int32 _servo_bm;
    AP_Int32 _esc_bm;
    AP_Int8 _esc_offset;
    AP_Int16 _servo_rate_hz;
    AP_Int16 _options;
    AP_Int16 _notify_state_hz;
    AP_Int16 _pool_size;

    AP_PoolAllocator *_allocator;
    AP_UAVCAN_DNA_Server *_dna_server;

    uavcan::Node<0> *_node;

    uint8_t _driver_index;

    uavcan::CanIfaceMgr* _iface_mgr;
    char _thread_name[13];
    bool _initialized;
    ///// SRV output /////
    struct {
        uint16_t pulse;
        bool esc_pending;
        bool servo_pending;
    } _SRV_conf[UAVCAN_SRV_NUMBER];

    uint32_t _esc_send_count;
    uint32_t _srv_send_count;
    uint32_t _fail_send_count;

    uint8_t _SRV_armed;
    uint32_t _SRV_last_send_us;
    HAL_Semaphore SRV_sem;

    // last log time
    uint32_t last_log_ms;

    ///// LED /////
    struct led_device {
        uint8_t led_index;
        uint8_t red;
        uint8_t green;
        uint8_t blue;
    };

    struct {
        led_device devices[AP_UAVCAN_MAX_LED_DEVICES];
        uint8_t devices_count;
        uint64_t last_update;
    } _led_conf;

    HAL_Semaphore _led_out_sem;

    // buzzer
    struct {
        HAL_Semaphore sem;
        float frequency;
        float duration;
        uint8_t pending_mask; // mask of interfaces to send to
    } _buzzer;

#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

    // GNSS RTCM injection
    struct {
        HAL_Semaphore sem;
        uint32_t last_send_ms;
        ByteBuffer *buf;
    } _rtcm_stream;
    
     // ESC

    static HAL_Semaphore _telem_sem;

    // safety status send state
    uint32_t _last_safety_state_ms;

    // notify vehicle state
    uint32_t _last_notify_state_ms;

#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
    /*
      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 {
        bool enabled;
        uint32_t last_GetId_send_ms;
        uint8_t thr_chan[HOBBYWING_MAX_ESC];
    } hobbywing;
    void hobbywing_ESC_update();

    void SRV_send_esc_hobbywing();
    bool hobbywing_find_esc_index(uint8_t node_id, uint8_t &esc_index) const;
    static void handle_hobbywing_GetEscID(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HobbywingESCIDCb &cb);
    static void handle_hobbywing_StatusMsg1(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HobbywingStatus1Cb &cb);
    static void handle_hobbywing_StatusMsg2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HobbywingStatus2Cb &cb);
#endif // AP_DRONECAN_HOBBYWING_ESC_ENABLED

#if AP_DRONECAN_HIMARK_SERVO_ENABLED
    bool himark_enabled;
    static void handle_himark_servoinfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HimarkServoInfoCb &cb);
#endif
    
    // incoming button handling
    static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
    static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
    static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
    static void handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb);
    static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
    static bool is_esc_data_index_valid(const uint8_t index);
    static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
    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);
};

#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS