zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
40291 Commits
23 Branches
0 Tags
282 MiB
 
 
 
 
 
 
Tree: de8080407b
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'de8080407b'
${ noResults }
ardupilot/libraries/AP_UAVCAN/AP_UAVCAN.h

389 lines
11 KiB
Raw Blame 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/>.
*
* Author: Eugene Shamaev, Siddharth Bharat Purohit
*/
#ifndef AP_UAVCAN_H_
#define AP_UAVCAN_H_
#include <uavcan/uavcan.hpp>
#include "AP_UAVCAN_Server.h"
#include <AP_HAL/CAN.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
#ifndef UAVCAN_NODE_POOL_SIZE
#define UAVCAN_NODE_POOL_SIZE 8192
#endif
#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
#endif
#ifndef UAVCAN_SRV_NUMBER
#define UAVCAN_SRV_NUMBER 18
#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
// fwd-declare callback classes
class ButtonCb;
class TrafficReportCb;
class ZrCamStateCb;
class ZrCamStatusCb;
class ZrCamVersionCb;
class ZrCamFeedBackCb;
class ZrMessageCb;
class ZrRequestCb;
class ZrCamAmountCb;
typedef struct
{
uint8_t id; // bit len 8
uint8_t gps_state; // bit len 8
uint8_t fly_status; // bit len 8
uint8_t wp_number;
bool armd; // bit len 1
} zr_uav_state_data_t;
typedef struct
{
uint32_t week; // bit len 32
uint32_t weekms; // bit len 32
uint32_t latitude_deg_1e7; // bit len 32
uint32_t longitude_deg_1e7; // bit len 32
uint32_t height_abs_cm; // bit len 24
uint16_t index; // bit len 16
uint8_t cam_number;
} ppk_pos_data_t;
/*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
the Callback will invoke registery 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() {} \
ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \
RegistryBinder(uc, (Registry)ffunc) {} \
}
class AP_UAVCAN : public AP_HAL::CANProtocol {
public:
AP_UAVCAN();
~AP_UAVCAN();
typedef struct
{
uint8_t software_major; // bit len 8
uint8_t software_minor; // bit len 8
uint8_t software_patch; // bit len 8
uint8_t hardware_major; // bit len 8
uint8_t hardware_minor; // bit len 8
uint8_t hardware_patch; // bit len 8
} zr_uav_version_data_t;
typedef enum
{
ZR_UAVCAN_REQUEST_TIMESTAMP=1,
ZR_UAVCAN_REQUSET_VERSION,
}zr_uav_request_e;
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);
void init(uint8_t driver_index, bool enable_filters) override;
uavcan::Node<0>* get_node() { return _node; }
uint8_t get_driver_index() { return _driver_index; }
///// 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);
bool set_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue);
void set_zr_cam_cmd(uint8_t gimbal_id, uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4);
void set_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t height_cm,int8_t height_type);
void set_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw);
void set_zr_timestamp(uint8_t id,uint64_t timestamp);
void set_zr_uav_state(zr_uav_state_data_t &data);
void set_zr_uav_version(zr_uav_version_data_t &data);
void set_zr_ppk_pos(ppk_pos_data_t &data);
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;
};
private:
class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
public:
SystemClock() = default;
void adjustUtc(uavcan::UtcDuration adjustment) override {
utc_adjustment_usec = adjustment.toUSec();
}
uavcan::MonotonicTime getMonotonic() const override {
return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64());
}
uavcan::UtcTime getUtc() const override {
return uavcan::UtcTime::fromUSec(AP_HAL::micros64() + utc_adjustment_usec);
}
static SystemClock& instance() {
static SystemClock inst;
return inst;
}
private:
int64_t utc_adjustment_usec;
};
// This will be needed to implement if UAVCAN is used with multithreading
// Such cases will be firmware update, etc.
class RaiiSynchronizer {};
void loop(void);
///// SRV output /////
void SRV_send_actuator();
void SRV_send_esc();
///// LED /////
void led_out_send();
// buzzer
void buzzer_send();
// SafetyState
void safety_state_send();
// send GNSS injection
void rtcm_stream_send();
void cam_geopoi_send();
void cam_aular_send();
void zr_cam_cmd_send();
void zr_rgb_led_send();
void zr_state_send();
void zr_uav_timestamp_send();
void zr_uav_state_send();
void zr_uav_version_send();
void zr_uav_pos_send();
void zr_uav_euler_send();
void zr_uav_ppk_send();
uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
// UAVCAN parameters
AP_Int8 _uavcan_node;
AP_Int32 _servo_bm;
AP_Int32 _esc_bm;
AP_Int16 _servo_rate_hz;
uavcan::Node<0> *_node;
uint8_t _driver_index;
char _thread_name[9];
bool _initialized;
///// SRV output /////
struct {
uint16_t pulse;
bool esc_pending;
bool servo_pending;
} _SRV_conf[UAVCAN_SRV_NUMBER];
uint8_t _SRV_armed;
uint32_t _SRV_last_send_us;
uint32_t _pos_last_send_ms;
uint32_t _ppk_last_send_ms;
HAL_Semaphore SRV_sem;
HAL_Semaphore uav_pos_sem;
///// 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;
// GNSS RTCM injection
struct {
HAL_Semaphore sem;
uint32_t last_send_ms;
ByteBuffer *buf;
} _rtcm_stream;
struct
{
HAL_Semaphore sem;
uint8_t gimbal_id;
uint8_t mode;
uint8_t v1;
uint8_t v2;
uint8_t v3;
uint8_t v4;
bool need_send;
} _zr_cam_cmd;
struct
{
HAL_Semaphore sem;
int32_t longitude_deg_1e7;
int32_t latitude_deg_1e7;
int32_t height_cm;
uint8_t id;
uint8_t mode;
int8_t height_type;
bool need_send;
} _cam_geopoi;
struct
{
HAL_Semaphore sem;
uint8_t id;
uint8_t mode;
float pitch;
float roll;
float yaw;
bool need_send;
} _cam_angular;
struct
{
HAL_Semaphore sem;
uint8_t mode;
uint8_t red;
uint8_t green;
uint8_t blue;
bool need_send;
} _zr_rgb_led;
struct
{
HAL_Semaphore sem;
uint64_t timestamp;
bool need_send;
} _zr_uav_timestamp;
struct
{
HAL_Semaphore sem;
zr_uav_state_data_t data;
bool need_send;
} _zr_uav_state;
struct
{
HAL_Semaphore sem;
zr_uav_version_data_t data;
bool need_send;
} _zr_uav_version;
typedef struct
{
// FieldTypes
uint8_t id; // bit len 8
uint8_t command; // bit len 8
uint8_t param1; // bit len 8
uint8_t param2; // bit len 8
uint8_t param3; // bit len 8
uint8_t param4; // bit len 8
} zr_request_t;
struct
{
HAL_Semaphore sem;
ppk_pos_data_t data;
bool need_send;
} _zr_ppk_pos;
// safety status send state
uint32_t _last_safety_state_ms;
uint32_t _last_uav_state_ms;
uint32_t _last_euler_ms;
// safety 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_zr_camera_old_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamStatusCb &cb);
static void handle_zr_camera_version(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamVersionCb &cb);
static void handle_zr_camera_feedback(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb);
static void handle_zr_message(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrMessageCb &cb);
static void handle_zr_request(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrRequestCb &cb);
static void handle_zr_camera_state(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamStateCb &cb);
static void handle_zr_camera_sum(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamAmountCb &cb);
};
#endif /* AP_UAVCAN_H_ */