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

AP_GPS: support Fix2 message for UAVCAN 

this allows for RTK fix levels
master
Andrew Tridgell 5 years ago
parent
2b8a2a82f9
commit
17ec9534cb
2 changed files with 133 additions and 0 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. 129
      libraries/AP_GPS/AP_GPS_UAVCAN.cpp
  2. 4
      libraries/AP_GPS/AP_GPS_UAVCAN.h

129
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
Unescape View File

@ -25,6 +25,7 @@ @@ -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; @@ -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) @@ -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 @@ -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) @@ -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 @@ -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
Unescape View File

@ -26,6 +26,7 @@ @@ -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,10 +42,12 @@ public: @@ -41,10 +42,12 @@ 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);
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();
@ -58,6 +61,7 @@ private: @@ -58,6 +61,7 @@ private:
uint8_t _detected_module;
bool seen_message;
bool seen_fix2;
// Module Detection Registry
static struct DetectedModules {

Write Preview
Loading…
Cancel
Save