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

AP_GPS: support Fix2 message for UAVCAN 

this allows for RTK fix levels
This commit is contained in:
Andrew Tridgell 2019-11-06 21:32:25 +11:00
parent 632114d687
commit b7c9391619
2 changed files with 133 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -25,6 +25,7 @@
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
extern const AP_HAL::HAL& hal;
@ -32,6 +33,7 @@ extern const AP_HAL::HAL& hal;
#define debug_gps_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0)
UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix);
UC_REGISTRY_BINDER(Fix2Cb, uavcan::equipment::gnss::Fix2);
UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary);
AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
@ -65,6 +67,14 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
return;
}
uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2Cb> *gnss_fix2;
gnss_fix2 = new uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2Cb>(*node);
const int gnss_fix2_start_res = gnss_fix2->start(Fix2Cb(ap_uavcan, &handle_fix2_msg_trampoline));
if (gnss_fix2_start_res < 0) {
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
return;
}
uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxCb> *gnss_aux;
gnss_aux = new uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxCb>(*node);
const int gnss_aux_start_res = gnss_aux->start(AuxCb(ap_uavcan, &handle_aux_msg_trampoline));
@ -140,6 +150,10 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
{
if (seen_fix2) {
// use Fix2 instead
return;
}
bool process = false;
WITH_SEMAPHORE(sem);
@ -240,6 +254,111 @@ void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
}
}
void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
{
bool process = false;
seen_fix2 = true;
WITH_SEMAPHORE(sem);
if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_NO_FIX) {
interim_state.status = AP_GPS::GPS_Status::NO_FIX;
} else {
if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_TIME_ONLY) {
interim_state.status = AP_GPS::GPS_Status::NO_FIX;
} else if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_2D_FIX) {
interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_2D;
process = true;
} else if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_3D_FIX) {
interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D;
process = true;
}
if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC) {
uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec();
epoch_ms /= 1000;
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK);
interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK);
}
if (interim_state.status == AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
if (cb.msg->mode == uavcan::equipment::gnss::Fix2::MODE_DGPS) {
interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS;
} else if (cb.msg->mode == uavcan::equipment::gnss::Fix2::MODE_RTK) {
if (cb.msg->sub_mode == uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT) {
interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT;
} else if (cb.msg->sub_mode == uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED) {
interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED;
}
}
}
}
if (process) {
Location loc = { };
loc.lat = cb.msg->latitude_deg_1e8 / 10;
loc.lng = cb.msg->longitude_deg_1e8 / 10;
loc.alt = cb.msg->height_msl_mm / 10;
interim_state.location = loc;
if (!uavcan::isNaN(cb.msg->ned_velocity[0])) {
Vector3f vel(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]);
interim_state.velocity = vel;
interim_state.ground_speed = norm(vel.x, vel.y);
interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
interim_state.have_vertical_velocity = true;
} else {
interim_state.have_vertical_velocity = false;
}
if (cb.msg->covariance.size() == 6) {
if (!uavcan::isNaN(cb.msg->covariance[0])) {
interim_state.horizontal_accuracy = sqrtf(cb.msg->covariance[0]);
interim_state.have_horizontal_accuracy = true;
} else {
interim_state.have_horizontal_accuracy = false;
}
if (!uavcan::isNaN(cb.msg->covariance[2])) {
interim_state.vertical_accuracy = sqrtf(cb.msg->covariance[2]);
interim_state.have_vertical_accuracy = true;
} else {
interim_state.have_vertical_accuracy = false;
}
if (!uavcan::isNaN(cb.msg->covariance[3]) &&
!uavcan::isNaN(cb.msg->covariance[4]) &&
!uavcan::isNaN(cb.msg->covariance[5])) {
interim_state.speed_accuracy = sqrtf((cb.msg->covariance[3] + cb.msg->covariance[4] + cb.msg->covariance[5])/3);
interim_state.have_speed_accuracy = true;
} else {
interim_state.have_speed_accuracy = false;
}
}
interim_state.num_sats = cb.msg->sats_used;
} else {
interim_state.have_vertical_velocity = false;
interim_state.have_vertical_accuracy = false;
interim_state.have_horizontal_accuracy = false;
interim_state.have_speed_accuracy = false;
interim_state.num_sats = 0;
}
interim_state.last_gps_time_ms = AP_HAL::millis();
_new_data = true;
if (!seen_message) {
if (interim_state.status == AP_GPS::GPS_Status::NO_GPS) {
// the first time we see a fix message we change from
// NO_GPS to NO_FIX, indicating to user that a UAVCAN GPS
// has been seen
interim_state.status = AP_GPS::GPS_Status::NO_FIX;
}
seen_message = true;
}
}
void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb)
{
WITH_SEMAPHORE(sem);
@ -263,6 +382,16 @@ void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node
}
}
void AP_GPS_UAVCAN::handle_fix2_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Fix2Cb &cb)
{
WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
if (driver != nullptr) {
driver->handle_fix2_msg(cb);
}
}
void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb)
{
WITH_SEMAPHORE(_sem_registry);

4
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -26,6 +26,7 @@
#include <AP_UAVCAN/AP_UAVCAN.h>
class FixCb;
class Fix2Cb;
class AuxCb;
class AP_GPS_UAVCAN : public AP_GPS_Backend {
@ -41,12 +42,14 @@ public:
static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
static void handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb);
static void handle_fix2_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Fix2Cb &cb);
static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
void inject_data(const uint8_t *data, uint16_t len) override;
private:
void handle_fix_msg(const FixCb &cb);
void handle_fix2_msg(const Fix2Cb &cb);
void handle_aux_msg(const AuxCb &cb);
static bool take_registry();
@ -60,6 +63,7 @@ private:
uint8_t _detected_module;
bool seen_message;
bool seen_fix2;
// Module Detection Registry
static struct DetectedModules {