zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_UAVCAN: position and attitude broadcast

master
DOMINATOR\Eugene 7 years ago committed by Tom Pittenger
parent
2f7196d71f
commit
c49d4aef50
2 changed files with 327 additions and 1 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 297
      libraries/AP_UAVCAN/AP_UAVCAN.cpp
  2. 31
      libraries/AP_UAVCAN/AP_UAVCAN.h

297
libraries/AP_UAVCAN/AP_UAVCAN.cpp
Unescape View File

@ -17,10 +17,12 @@ @@ -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[] = { @@ -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() : @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -20,6 +20,8 @@ @@ -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 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -272,6 +301,8 @@ public:
{
_parent_can_mgr = parent_can_mgr;
}
void UAVCAN_AHRS_update(const AP_AHRS_NavEKF &ahrs);
};
#endif /* AP_UAVCAN_H_ */

Write Preview
Loading…
Cancel
Save