mirror of https://github.com/ArduPilot/ardupilot
258 lines
7.5 KiB
C++
258 lines
7.5 KiB
C++
/*
|
|
*
|
|
* Author: Eugene Shamaev
|
|
*/
|
|
#ifndef AP_UAVCAN_H_
|
|
#define AP_UAVCAN_H_
|
|
|
|
#include <uavcan/uavcan.hpp>
|
|
|
|
#include <AP_HAL/CAN.h>
|
|
#include <AP_HAL/Semaphores.h>
|
|
#include <AP_GPS/AP_GPS.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include <AP_GPS/GPS_Backend.h>
|
|
#include <AP_Baro/AP_Baro_Backend.h>
|
|
#include <AP_Compass/AP_Compass.h>
|
|
#include <AP_BattMonitor/AP_BattMonitor_Backend.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 256
|
|
#endif
|
|
|
|
#ifndef UAVCAN_SRV_NUMBER
|
|
#define UAVCAN_SRV_NUMBER 18
|
|
#endif
|
|
|
|
#define AP_UAVCAN_MAX_LISTENERS 4
|
|
#define AP_UAVCAN_MAX_GPS_NODES 4
|
|
#define AP_UAVCAN_MAX_MAG_NODES 4
|
|
#define AP_UAVCAN_MAX_BARO_NODES 4
|
|
#define AP_UAVCAN_MAX_BI_NUMBER 4
|
|
|
|
#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
|
|
|
|
class AP_UAVCAN : public AP_HAL::CANProtocol {
|
|
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);
|
|
|
|
void init(uint8_t driver_index) override;
|
|
|
|
|
|
///// SRV output /////
|
|
void SRV_push_servos(void);
|
|
|
|
///// LED /////
|
|
bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
|
|
|
|
///// GPS /////
|
|
|
|
uint8_t find_gps_without_listener(void);
|
|
|
|
// this function will register the listening class on a first free channel or on the specified channel
|
|
// if preferred_channel = 0 then free channel will be searched for
|
|
// if preferred_channel > 0 then listener will be added to specific channel
|
|
// return value is the number of assigned channel or 0 if fault
|
|
// channel numbering starts from 1
|
|
uint8_t register_gps_listener(AP_GPS_Backend* new_listener, uint8_t preferred_channel);
|
|
|
|
uint8_t register_gps_listener_to_node(AP_GPS_Backend* new_listener, uint8_t node);
|
|
|
|
// Removes specified listener from all nodes
|
|
void remove_gps_listener(AP_GPS_Backend* rem_listener);
|
|
|
|
// Returns pointer to GPS state connected with specified node.
|
|
// If node is not found and there are free space, locate a new one
|
|
AP_GPS::GPS_State *find_gps_node(uint8_t node);
|
|
|
|
// Updates all listeners of specified node
|
|
void update_gps_state(uint8_t node);
|
|
|
|
///// BARO /////
|
|
|
|
struct Baro_Info {
|
|
float pressure;
|
|
float pressure_variance;
|
|
float temperature;
|
|
float temperature_variance;
|
|
};
|
|
|
|
uint8_t find_smallest_free_baro_node();
|
|
uint8_t register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel);
|
|
uint8_t register_baro_listener_to_node(AP_Baro_Backend* new_listener, uint8_t node);
|
|
void remove_baro_listener(AP_Baro_Backend* rem_listener);
|
|
Baro_Info *find_baro_node(uint8_t node);
|
|
void update_baro_state(uint8_t node);
|
|
|
|
///// COMPASS /////
|
|
|
|
struct Mag_Info {
|
|
Vector3f mag_vector;
|
|
};
|
|
|
|
uint8_t find_smallest_free_mag_node();
|
|
uint8_t register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel);
|
|
uint8_t register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node);
|
|
void remove_mag_listener(AP_Compass_Backend* rem_listener);
|
|
Mag_Info *find_mag_node(uint8_t node, uint8_t sensor_id);
|
|
void update_mag_state(uint8_t node, uint8_t sensor_id);
|
|
|
|
///// BATTERY /////
|
|
|
|
struct BatteryInfo_Info {
|
|
float temperature;
|
|
float voltage;
|
|
float current;
|
|
float remaining_capacity_wh;
|
|
float full_charge_capacity_wh;
|
|
uint8_t status_flags;
|
|
};
|
|
|
|
uint8_t find_smallest_free_bi_id();
|
|
uint8_t register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id);
|
|
void remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener);
|
|
BatteryInfo_Info *find_bi_id(uint8_t id);
|
|
void update_bi_state(uint8_t id);
|
|
|
|
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();
|
|
|
|
|
|
// UAVCAN parameters
|
|
AP_Int8 _uavcan_node;
|
|
AP_Int32 _servo_bm;
|
|
AP_Int32 _esc_bm;
|
|
AP_Int16 _servo_rate_hz;
|
|
|
|
|
|
uavcan::Node<0> *_node;
|
|
uavcan::HeapBasedPoolAllocator<UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
|
|
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;
|
|
AP_HAL::Semaphore *SRV_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;
|
|
|
|
AP_HAL::Semaphore *_led_out_sem;
|
|
|
|
///// GPS /////
|
|
// 255 - means free node
|
|
uint8_t _gps_nodes[AP_UAVCAN_MAX_GPS_NODES];
|
|
|
|
// Counter of how many listeners are connected to this source
|
|
uint8_t _gps_node_taken[AP_UAVCAN_MAX_GPS_NODES];
|
|
|
|
// GPS data of the sources
|
|
AP_GPS::GPS_State _gps_node_state[AP_UAVCAN_MAX_GPS_NODES];
|
|
|
|
// 255 - means no connection
|
|
uint8_t _gps_listener_to_node[AP_UAVCAN_MAX_LISTENERS];
|
|
|
|
// Listeners with callbacks to be updated
|
|
AP_GPS_Backend* _gps_listeners[AP_UAVCAN_MAX_LISTENERS];
|
|
|
|
///// BARO /////
|
|
uint8_t _baro_nodes[AP_UAVCAN_MAX_BARO_NODES];
|
|
uint8_t _baro_node_taken[AP_UAVCAN_MAX_BARO_NODES];
|
|
Baro_Info _baro_node_state[AP_UAVCAN_MAX_BARO_NODES];
|
|
uint8_t _baro_listener_to_node[AP_UAVCAN_MAX_LISTENERS];
|
|
AP_Baro_Backend* _baro_listeners[AP_UAVCAN_MAX_LISTENERS];
|
|
|
|
///// COMPASS /////
|
|
uint8_t _mag_nodes[AP_UAVCAN_MAX_MAG_NODES];
|
|
uint8_t _mag_node_taken[AP_UAVCAN_MAX_MAG_NODES];
|
|
Mag_Info _mag_node_state[AP_UAVCAN_MAX_MAG_NODES];
|
|
uint8_t _mag_node_max_sensorid_count[AP_UAVCAN_MAX_MAG_NODES];
|
|
uint8_t _mag_listener_to_node[AP_UAVCAN_MAX_LISTENERS];
|
|
AP_Compass_Backend* _mag_listeners[AP_UAVCAN_MAX_LISTENERS];
|
|
uint8_t _mag_listener_sensor_ids[AP_UAVCAN_MAX_LISTENERS];
|
|
|
|
///// BATTERY /////
|
|
uint16_t _bi_id[AP_UAVCAN_MAX_BI_NUMBER];
|
|
uint16_t _bi_id_taken[AP_UAVCAN_MAX_BI_NUMBER];
|
|
BatteryInfo_Info _bi_id_state[AP_UAVCAN_MAX_BI_NUMBER];
|
|
uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS];
|
|
AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS];
|
|
};
|
|
|
|
#endif /* AP_UAVCAN_H_ */
|