Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: position and attitude broadcast

This commit is contained in:
DOMINATOR\Eugene 2017-11-15 16:36:14 +02:00 committed by Tom Pittenger
parent 2f7196d71f
commit c49d4aef50
2 changed files with 327 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

297
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -17,10 +17,12 @@
// Zubax GPS and other GPS, baro, magnetic sensors
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.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/StaticTemperature.hpp>
@ -71,9 +73,228 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
// @User: Advanced
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
};
// 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)
{
if (hal.can_mgr[mgr] != nullptr) {
@ -388,6 +609,8 @@ AP_UAVCAN::AP_UAVCAN() :
}
_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();
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]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
_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.
* Default mode is INITIALIZING.
@ -537,6 +777,34 @@ bool AP_UAVCAN::try_init(void)
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 sem_ret = _rc_out_sem->take(10);
@ -687,6 +955,33 @@ void AP_UAVCAN::do_cyclic(void)
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()

31
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -20,6 +20,8 @@
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
#include <uavcan/equipment/indication/RGB565.hpp>
#include <AP_AHRS/AP_AHRS.h>
#ifndef UAVCAN_NODE_POOL_SIZE
#define UAVCAN_NODE_POOL_SIZE 8192
#endif
@ -47,6 +49,10 @@
#define AP_UAVCAN_MAX_LED_DEVICES 4
#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 {
public:
AP_UAVCAN();
@ -128,6 +134,18 @@ public:
void rc_out_send_servos();
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:
// ------------------------- GPS
// 255 - means free node
@ -165,6 +183,10 @@ private:
uint16_t _bi_BM_listener_to_id[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 {
uint16_t pulse;
uint16_t safety_pulse;
@ -190,6 +212,8 @@ private:
} _led_conf;
AP_HAL::Semaphore *_rc_out_sem;
AP_HAL::Semaphore *_fix_out_sem;
AP_HAL::Semaphore *_att_out_sem;
AP_HAL::Semaphore *_led_out_sem;
class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
@ -256,6 +280,11 @@ private:
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:
void do_cyclic(void);
bool try_init(void);
@ -272,6 +301,8 @@ public:
{
_parent_can_mgr = parent_can_mgr;
}
void UAVCAN_AHRS_update(const AP_AHRS_NavEKF &ahrs);
};
#endif /* AP_UAVCAN_H_ */