mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-25 01:03:59 -04:00
AP_UAVCAN: position and attitude broadcast
This commit is contained in:
DOMINATOR\Eugene
Tom Pittenger
parent
2f7196d71f
commit
c49d4aef50
@ -17,10 +17,12 @@
|
|||||||
|
|
||||||
// Zubax GPS and other GPS, baro, magnetic sensors
|
// Zubax GPS and other GPS, baro, magnetic sensors
|
||||||
#include <uavcan/equipment/gnss/Fix.hpp>
|
#include <uavcan/equipment/gnss/Fix.hpp>
|
||||||
|
#include <uavcan/equipment/gnss/Fix2.hpp>
|
||||||
#include <uavcan/equipment/gnss/Auxiliary.hpp>
|
#include <uavcan/equipment/gnss/Auxiliary.hpp>
|
||||||
|
|
||||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
|
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
|
||||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
|
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
|
||||||
|
#include <uavcan/equipment/ahrs/Solution.hpp>
|
||||||
#include <uavcan/equipment/air_data/StaticPressure.hpp>
|
#include <uavcan/equipment/air_data/StaticPressure.hpp>
|
||||||
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
|
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
|
||||||
|
|
||||||
@ -71,9 +73,228 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
|
|||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 255),
|
AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 255),
|
||||||
|
|
||||||
|
// @Param: NODE
|
||||||
|
// @DisplayName: UAVCAN GNSS Fix broadcast rate
|
||||||
|
// @Description: UAVCAN GNSS Fix broadcast rate in times per second. 0 - disable broadcast.
|
||||||
|
// @Range: 0 20
|
||||||
|
// @User: Advanced
|
||||||
|
AP_GROUPINFO("BR_FIX_R", 4, AP_UAVCAN, _broadcast_fix_rate, 10),
|
||||||
|
|
||||||
|
// @Param: NODE
|
||||||
|
// @DisplayName: UAVCAN GNSS Fix2 broadcast rate
|
||||||
|
// @Description: UAVCAN GNSS Fix2 broadcast rate in times per second. 0 - disable broadcast.
|
||||||
|
// @Range: 0 20
|
||||||
|
// @User: Advanced
|
||||||
|
AP_GROUPINFO("BR_FIX2_R", 5, AP_UAVCAN, _broadcast_fix2_rate, 10),
|
||||||
|
|
||||||
|
// @Param: NODE
|
||||||
|
// @DisplayName: UAVCAN attitude broadcast rate
|
||||||
|
// @Description: UAVCAN attitude broadcast rate in times per second. 0 - disable broadcast.
|
||||||
|
// @Range: 0 20
|
||||||
|
// @User: Advanced
|
||||||
|
AP_GROUPINFO("BR_ATT_R", 6, AP_UAVCAN, _broadcast_att_rate, 10),
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// publisher interfaces
|
||||||
|
static uavcan::Publisher<uavcan::equipment::gnss::Fix> *fix_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||||
|
static uavcan::Publisher<uavcan::equipment::gnss::Fix2> *fix2_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||||
|
static uavcan::Publisher<uavcan::equipment::ahrs::Solution> *attitude_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
|
||||||
|
|
||||||
|
static uavcan::equipment::gnss::Fix _fix_state;
|
||||||
|
static uavcan::equipment::gnss::Fix2 _fix2_state;
|
||||||
|
static uavcan::equipment::ahrs::Solution _att_state;
|
||||||
|
|
||||||
|
void AP_UAVCAN::UAVCAN_AHRS_update(const AP_AHRS_NavEKF &ahrs)
|
||||||
|
{
|
||||||
|
bool sem_ret = _fix_out_sem->take(1);
|
||||||
|
|
||||||
|
if (sem_ret) {
|
||||||
|
if (fix_out_array[_uavcan_i] != nullptr || fix2_out_array[_uavcan_i] != nullptr) {
|
||||||
|
const AP_GPS &cgps = AP::gps();
|
||||||
|
Location loc;
|
||||||
|
Vector3f vel_NED;
|
||||||
|
|
||||||
|
if (ahrs.healthy() && ahrs.get_location(loc)
|
||||||
|
&& ahrs.get_velocity_NED(vel_NED)) {
|
||||||
|
float velVar, posVar, hgtVar;
|
||||||
|
Vector3f magVar;
|
||||||
|
float tasVar;
|
||||||
|
Vector2f offset;
|
||||||
|
ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||||
|
|
||||||
|
// Manual resize to diagonal form
|
||||||
|
_fix_state.position_covariance.resize(3);
|
||||||
|
_fix_state.position_covariance[0] = posVar;
|
||||||
|
_fix_state.position_covariance[1] = posVar;
|
||||||
|
_fix_state.position_covariance[2] = hgtVar;
|
||||||
|
|
||||||
|
// Manual resize to diagonal form
|
||||||
|
_fix_state.velocity_covariance.resize(3);
|
||||||
|
_fix_state.velocity_covariance[0] = velVar;
|
||||||
|
_fix_state.velocity_covariance[1] = velVar;
|
||||||
|
_fix_state.velocity_covariance[2] = velVar;
|
||||||
|
|
||||||
|
// Manual resize to diagonal form
|
||||||
|
_fix2_state.covariance.resize(6);
|
||||||
|
_fix2_state.covariance[0] = posVar;
|
||||||
|
_fix2_state.covariance[1] = posVar;
|
||||||
|
_fix2_state.covariance[2] = hgtVar;
|
||||||
|
_fix2_state.covariance[3] = velVar;
|
||||||
|
_fix2_state.covariance[4] = velVar;
|
||||||
|
_fix2_state.covariance[5] = velVar;
|
||||||
|
} else {
|
||||||
|
loc = cgps.location();
|
||||||
|
vel_NED = cgps.velocity();
|
||||||
|
|
||||||
|
float vel_acc, hacc, vert_acc;
|
||||||
|
cgps.horizontal_accuracy(hacc);
|
||||||
|
cgps.vertical_accuracy(vert_acc);
|
||||||
|
cgps.speed_accuracy(vel_acc);
|
||||||
|
|
||||||
|
// Manual resize to diagonal form
|
||||||
|
_fix_state.position_covariance.resize(3);
|
||||||
|
_fix_state.position_covariance[0] = hacc * hacc;
|
||||||
|
_fix_state.position_covariance[1] = _fix_state.position_covariance[0];
|
||||||
|
_fix_state.position_covariance[2] = vert_acc * vert_acc;
|
||||||
|
|
||||||
|
// Manual resize to diagonal form
|
||||||
|
_fix_state.velocity_covariance.resize(3);
|
||||||
|
_fix_state.velocity_covariance[0] = vel_acc * vel_acc;
|
||||||
|
_fix_state.velocity_covariance[1] = _fix_state.velocity_covariance[0];
|
||||||
|
_fix_state.velocity_covariance[2] = _fix_state.velocity_covariance[0];
|
||||||
|
|
||||||
|
// Manual resize to diagonal form
|
||||||
|
_fix2_state.covariance.resize(6);
|
||||||
|
_fix2_state.covariance[0] = _fix_state.position_covariance[0];
|
||||||
|
_fix2_state.covariance[1] = _fix_state.position_covariance[1];
|
||||||
|
_fix2_state.covariance[2] = _fix_state.position_covariance[2];
|
||||||
|
_fix2_state.covariance[3] = _fix_state.velocity_covariance[0];
|
||||||
|
_fix2_state.covariance[4] = _fix_state.velocity_covariance[1];
|
||||||
|
_fix2_state.covariance[5] = _fix_state.velocity_covariance[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
_fix_state.height_msl_mm = loc.alt * 10;
|
||||||
|
_fix_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10;
|
||||||
|
_fix_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10;
|
||||||
|
|
||||||
|
_fix2_state.height_msl_mm = loc.alt * 10;
|
||||||
|
_fix2_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10;
|
||||||
|
_fix2_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10;
|
||||||
|
|
||||||
|
// Not saved in AP
|
||||||
|
//_fix_state.height_ellipsoid_mm
|
||||||
|
|
||||||
|
_fix_state.ned_velocity[0] = vel_NED.x;
|
||||||
|
_fix_state.ned_velocity[1] = vel_NED.y;
|
||||||
|
_fix_state.ned_velocity[2] = vel_NED.z;
|
||||||
|
|
||||||
|
_fix2_state.ned_velocity[0] = vel_NED.x;
|
||||||
|
_fix2_state.ned_velocity[1] = vel_NED.y;
|
||||||
|
_fix2_state.ned_velocity[2] = vel_NED.z;
|
||||||
|
|
||||||
|
_fix_state.sats_used = cgps.num_sats();
|
||||||
|
_fix_state.pdop = cgps.get_hdop();
|
||||||
|
|
||||||
|
_fix2_state.sats_used = _fix_state.sats_used;
|
||||||
|
_fix2_state.pdop = _fix_state.pdop;
|
||||||
|
|
||||||
|
switch(cgps.status()) {
|
||||||
|
case AP_GPS::GPS_Status::NO_GPS:
|
||||||
|
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX;
|
||||||
|
break;
|
||||||
|
case AP_GPS::GPS_Status::NO_FIX:
|
||||||
|
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX;
|
||||||
|
break;
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||||
|
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_2D_FIX;
|
||||||
|
break;
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||||
|
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_3D_FIX;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch(cgps.status()) {
|
||||||
|
case AP_GPS::GPS_Status::NO_GPS:
|
||||||
|
case AP_GPS::GPS_Status::NO_FIX:
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||||
|
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
|
||||||
|
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
|
||||||
|
break;
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||||
|
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS;
|
||||||
|
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
|
||||||
|
break;
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||||
|
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
|
||||||
|
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT;
|
||||||
|
break;
|
||||||
|
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||||
|
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
|
||||||
|
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
_fix_state.gnss_time_standard = uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC;
|
||||||
|
_fix2_state.gnss_time_standard = _fix_state.gnss_time_standard;
|
||||||
|
uavcan::Timestamp ts;
|
||||||
|
ts.usec = 1000ULL * ((cgps.time_week() * AP_MSEC_PER_WEEK + cgps.time_week_ms()) + UNIX_OFFSET_MSEC);
|
||||||
|
_fix_state.gnss_timestamp = ts;
|
||||||
|
_fix2_state.gnss_timestamp = ts;
|
||||||
|
|
||||||
|
_fix_state.num_leap_seconds = 0;
|
||||||
|
_fix2_state.num_leap_seconds = 0;
|
||||||
|
ts.usec = 0;
|
||||||
|
_fix_state.timestamp = ts;
|
||||||
|
_fix2_state.timestamp = ts;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (attitude_out_array[_uavcan_i] != nullptr) {
|
||||||
|
uavcan::Timestamp ts;
|
||||||
|
ts.usec = AP_HAL::micros64();
|
||||||
|
_att_state.timestamp = ts;
|
||||||
|
|
||||||
|
Quaternion qt;
|
||||||
|
Matrix3f rot = ahrs.get_rotation_body_to_ned();
|
||||||
|
qt.from_rotation_matrix(rot);
|
||||||
|
_att_state.orientation_xyzw[0] = qt.q1;
|
||||||
|
_att_state.orientation_xyzw[1] = qt.q2;
|
||||||
|
_att_state.orientation_xyzw[2] = qt.q3;
|
||||||
|
_att_state.orientation_xyzw[3] = qt.q4;
|
||||||
|
|
||||||
|
// TODO: extract from EKF
|
||||||
|
//_att_state.orientation_covariance
|
||||||
|
|
||||||
|
Vector3f av = ahrs.get_gyro();
|
||||||
|
_att_state.angular_velocity[0] = av.x;
|
||||||
|
_att_state.angular_velocity[1] = av.y;
|
||||||
|
_att_state.angular_velocity[2] = av.z;
|
||||||
|
|
||||||
|
// TODO: extract from EKF
|
||||||
|
//_att_state.angular_velocity_covariance
|
||||||
|
|
||||||
|
Vector3f la = ahrs.get_accel_ef();
|
||||||
|
_att_state.linear_acceleration[0] = la.x;
|
||||||
|
_att_state.linear_acceleration[1] = la.y;
|
||||||
|
_att_state.linear_acceleration[2] = la.z;
|
||||||
|
|
||||||
|
// TODO: extract from EKF
|
||||||
|
//_att_state.linear_acceleration_covariance
|
||||||
|
}
|
||||||
|
|
||||||
|
_fix_out_sem->give();
|
||||||
|
} else {
|
||||||
|
if (sem_ret) {
|
||||||
|
_fix_out_sem->give();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
|
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
|
||||||
{
|
{
|
||||||
if (hal.can_mgr[mgr] != nullptr) {
|
if (hal.can_mgr[mgr] != nullptr) {
|
||||||
@ -388,6 +609,8 @@ AP_UAVCAN::AP_UAVCAN() :
|
|||||||
}
|
}
|
||||||
|
|
||||||
_rc_out_sem = hal.util->new_semaphore();
|
_rc_out_sem = hal.util->new_semaphore();
|
||||||
|
_fix_out_sem = hal.util->new_semaphore();
|
||||||
|
_att_out_sem = hal.util->new_semaphore();
|
||||||
_led_out_sem = hal.util->new_semaphore();
|
_led_out_sem = hal.util->new_semaphore();
|
||||||
|
|
||||||
debug_uavcan(2, "AP_UAVCAN constructed\n\r");
|
debug_uavcan(2, "AP_UAVCAN constructed\n\r");
|
||||||
@ -511,9 +734,26 @@ bool AP_UAVCAN::try_init(void)
|
|||||||
rgb_led[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*node);
|
rgb_led[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*node);
|
||||||
rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
|
rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
|
||||||
rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
|
rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
|
||||||
|
|
||||||
_led_conf.devices_count = 0;
|
_led_conf.devices_count = 0;
|
||||||
|
|
||||||
|
if(_broadcast_bm & AP_UAVCAN_BROADCAST_POSITION_FIX) {
|
||||||
|
fix_out_array[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::gnss::Fix>(*node);
|
||||||
|
fix_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
|
||||||
|
fix_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(_broadcast_bm & AP_UAVCAN_BROADCAST_POSITION_FIX2) {
|
||||||
|
fix2_out_array[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::gnss::Fix2>(*node);
|
||||||
|
fix2_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
|
||||||
|
fix2_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(_broadcast_bm & AP_UAVCAN_BROADCAST_ATTITUDE) {
|
||||||
|
attitude_out_array[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::ahrs::Solution>(*node);
|
||||||
|
attitude_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
|
||||||
|
attitude_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Informing other nodes that we're ready to work.
|
* Informing other nodes that we're ready to work.
|
||||||
* Default mode is INITIALIZING.
|
* Default mode is INITIALIZING.
|
||||||
@ -537,6 +777,34 @@ bool AP_UAVCAN::try_init(void)
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool AP_UAVCAN::att_out_sem_take()
|
||||||
|
{
|
||||||
|
bool sem_ret = _att_out_sem->take(10);
|
||||||
|
if (!sem_ret) {
|
||||||
|
debug_uavcan(1, "AP_UAVCAN attitude semaphore fail\n\r");
|
||||||
|
}
|
||||||
|
return sem_ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_UAVCAN::att_out_sem_give()
|
||||||
|
{
|
||||||
|
_att_out_sem->give();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_UAVCAN::fix_out_sem_take()
|
||||||
|
{
|
||||||
|
bool sem_ret = _fix_out_sem->take(10);
|
||||||
|
if (!sem_ret) {
|
||||||
|
debug_uavcan(1, "AP_UAVCAN GNSS fix semaphore fail\n\r");
|
||||||
|
}
|
||||||
|
return sem_ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_UAVCAN::fix_out_sem_give()
|
||||||
|
{
|
||||||
|
_fix_out_sem->give();
|
||||||
|
}
|
||||||
|
|
||||||
bool AP_UAVCAN::rc_out_sem_take()
|
bool AP_UAVCAN::rc_out_sem_take()
|
||||||
{
|
{
|
||||||
bool sem_ret = _rc_out_sem->take(10);
|
bool sem_ret = _rc_out_sem->take(10);
|
||||||
@ -687,6 +955,33 @@ void AP_UAVCAN::do_cyclic(void)
|
|||||||
led_out_sem_give();
|
led_out_sem_give();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint64_t now = AP_HAL::millis64();
|
||||||
|
|
||||||
|
if (fix_out_sem_take()) {
|
||||||
|
if (fix_out_array[_uavcan_i] != nullptr && _broadcast_fix_rate > 0
|
||||||
|
&& now >= fix_out_next_send_time_ms) {
|
||||||
|
fix_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate);
|
||||||
|
fix_out_array[_uavcan_i]->broadcast(_fix_state);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (fix2_out_array[_uavcan_i] != nullptr && _broadcast_fix2_rate > 0
|
||||||
|
&& now >= fix2_out_next_send_time_ms) {
|
||||||
|
fix2_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate);
|
||||||
|
fix2_out_array[_uavcan_i]->broadcast(_fix2_state);
|
||||||
|
}
|
||||||
|
|
||||||
|
fix_out_sem_give();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (att_out_sem_take()) {
|
||||||
|
if (attitude_out_array[_uavcan_i] != nullptr && _broadcast_att_rate > 0
|
||||||
|
&& now >= att_out_next_send_time_ms) {
|
||||||
|
att_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate);
|
||||||
|
attitude_out_array[_uavcan_i]->broadcast(_att_state);
|
||||||
|
}
|
||||||
|
|
||||||
|
att_out_sem_give();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool AP_UAVCAN::led_out_sem_take()
|
bool AP_UAVCAN::led_out_sem_take()
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
||||||
#include <uavcan/equipment/indication/RGB565.hpp>
|
#include <uavcan/equipment/indication/RGB565.hpp>
|
||||||
|
|
||||||
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
|
|
||||||
#ifndef UAVCAN_NODE_POOL_SIZE
|
#ifndef UAVCAN_NODE_POOL_SIZE
|
||||||
#define UAVCAN_NODE_POOL_SIZE 8192
|
#define UAVCAN_NODE_POOL_SIZE 8192
|
||||||
#endif
|
#endif
|
||||||
@ -47,6 +49,10 @@
|
|||||||
#define AP_UAVCAN_MAX_LED_DEVICES 4
|
#define AP_UAVCAN_MAX_LED_DEVICES 4
|
||||||
#define AP_UAVCAN_LED_DELAY_MILLISECONDS 50
|
#define AP_UAVCAN_LED_DELAY_MILLISECONDS 50
|
||||||
|
|
||||||
|
#define AP_UAVCAN_BROADCAST_POSITION_FIX 1
|
||||||
|
#define AP_UAVCAN_BROADCAST_POSITION_FIX2 2
|
||||||
|
#define AP_UAVCAN_BROADCAST_ATTITUDE 4
|
||||||
|
|
||||||
class AP_UAVCAN {
|
class AP_UAVCAN {
|
||||||
public:
|
public:
|
||||||
AP_UAVCAN();
|
AP_UAVCAN();
|
||||||
@ -128,6 +134,18 @@ public:
|
|||||||
void rc_out_send_servos();
|
void rc_out_send_servos();
|
||||||
void rc_out_send_esc();
|
void rc_out_send_esc();
|
||||||
|
|
||||||
|
// synchronization for GNSS fix output
|
||||||
|
bool fix_out_sem_take();
|
||||||
|
void fix_out_sem_give();
|
||||||
|
|
||||||
|
// synchronization for attitude output
|
||||||
|
bool att_out_sem_take();
|
||||||
|
void att_out_sem_give();
|
||||||
|
|
||||||
|
bool need_AHRS_update(void) {
|
||||||
|
return (_broadcast_bm != 0);
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// ------------------------- GPS
|
// ------------------------- GPS
|
||||||
// 255 - means free node
|
// 255 - means free node
|
||||||
@ -165,6 +183,10 @@ private:
|
|||||||
uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS];
|
uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS];
|
||||||
AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS];
|
AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS];
|
||||||
|
|
||||||
|
uint64_t fix_out_next_send_time_ms = AP_HAL::millis64();
|
||||||
|
uint64_t fix2_out_next_send_time_ms = AP_HAL::millis64();
|
||||||
|
uint64_t att_out_next_send_time_ms = AP_HAL::millis64();
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
uint16_t pulse;
|
uint16_t pulse;
|
||||||
uint16_t safety_pulse;
|
uint16_t safety_pulse;
|
||||||
@ -190,6 +212,8 @@ private:
|
|||||||
} _led_conf;
|
} _led_conf;
|
||||||
|
|
||||||
AP_HAL::Semaphore *_rc_out_sem;
|
AP_HAL::Semaphore *_rc_out_sem;
|
||||||
|
AP_HAL::Semaphore *_fix_out_sem;
|
||||||
|
AP_HAL::Semaphore *_att_out_sem;
|
||||||
AP_HAL::Semaphore *_led_out_sem;
|
AP_HAL::Semaphore *_led_out_sem;
|
||||||
|
|
||||||
class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
|
class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
|
||||||
@ -256,6 +280,11 @@ private:
|
|||||||
|
|
||||||
AP_HAL::CANManager* _parent_can_mgr;
|
AP_HAL::CANManager* _parent_can_mgr;
|
||||||
|
|
||||||
|
AP_Int16 _broadcast_bm;
|
||||||
|
AP_Int16 _broadcast_fix_rate;
|
||||||
|
AP_Int16 _broadcast_fix2_rate;
|
||||||
|
AP_Int16 _broadcast_att_rate;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
void do_cyclic(void);
|
void do_cyclic(void);
|
||||||
bool try_init(void);
|
bool try_init(void);
|
||||||
@ -272,6 +301,8 @@ public:
|
|||||||
{
|
{
|
||||||
_parent_can_mgr = parent_can_mgr;
|
_parent_can_mgr = parent_can_mgr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void UAVCAN_AHRS_update(const AP_AHRS_NavEKF &ahrs);
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* AP_UAVCAN_H_ */
|
#endif /* AP_UAVCAN_H_ */
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user