AP_GPS: move UAVCAN GPS related code to AP_GPS_UAVCAN backend
This commit is contained in:
Siddharth Purohit
Francisco Ferreira
parent
6e85003b56
commit
ca54123b7d
@ -414,50 +414,24 @@ void AP_GPS::detect_instance(uint8_t instance)
|
||||
state[instance].status = NO_GPS;
|
||||
state[instance].hdop = GPS_UNKNOWN_DOP;
|
||||
state[instance].vdop = GPS_UNKNOWN_DOP;
|
||||
|
||||
|
||||
switch (_type[instance]) {
|
||||
// user has to explicitly set the MAV type, do not use AUTO
|
||||
// do not try to detect the MAV type, assume it's there
|
||||
case GPS_TYPE_MAV: {
|
||||
case GPS_TYPE_MAV:
|
||||
dstate->auto_detected_baud = false; // specified, not detected
|
||||
new_gps = new AP_GPS_MAV(*this, state[instance], nullptr);
|
||||
goto found_gps;
|
||||
break;
|
||||
}
|
||||
|
||||
// user has to explicitly set the UAVCAN type, do not use AUTO
|
||||
case GPS_TYPE_UAVCAN: {
|
||||
case GPS_TYPE_UAVCAN:
|
||||
#if HAL_WITH_UAVCAN
|
||||
dstate->auto_detected_baud = false; // specified, not detected
|
||||
|
||||
uint8_t can_num_drivers = AP::can().get_num_drivers();
|
||||
|
||||
for (uint8_t i = 0; i < can_num_drivers; i++) {
|
||||
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
|
||||
if (ap_uavcan == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t gps_node = ap_uavcan->find_gps_without_listener();
|
||||
if (gps_node == UINT8_MAX) {
|
||||
continue;
|
||||
}
|
||||
|
||||
new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
|
||||
((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i);
|
||||
if (ap_uavcan->register_gps_listener_to_node(new_gps, gps_node)) {
|
||||
if (AP::can().get_debug_level_driver(i) >= 2) {
|
||||
printf("AP_GPS_UAVCAN registered\n\r");
|
||||
}
|
||||
goto found_gps;
|
||||
} else {
|
||||
delete new_gps;
|
||||
}
|
||||
}
|
||||
new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]);
|
||||
goto found_gps;
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
return; // We don't do anything here if UAVCAN is not supported
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@ -16,68 +16,289 @@
|
||||
//
|
||||
// UAVCAN GPS driver
|
||||
//
|
||||
#include "AP_GPS_UAVCAN.h"
|
||||
#include <stdint.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_WITH_UAVCAN
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
#include "AP_GPS_UAVCAN.h"
|
||||
|
||||
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
|
||||
#include <AP_Common/Semaphore.h>
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
#include <uavcan/equipment/gnss/Fix.hpp>
|
||||
#include <uavcan/equipment/gnss/Auxiliary.hpp>
|
||||
|
||||
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)
|
||||
|
||||
AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
||||
AP_GPS_Backend(_gps, _state, _port)
|
||||
{
|
||||
_sem_gnss = hal.util->new_semaphore();
|
||||
}
|
||||
UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix);
|
||||
UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary);
|
||||
|
||||
AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
|
||||
AP_HAL::Semaphore* AP_GPS_UAVCAN::_sem_registry = nullptr;
|
||||
|
||||
// Member Methods
|
||||
AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state) :
|
||||
AP_GPS_Backend(_gps, _state, nullptr)
|
||||
{}
|
||||
|
||||
// For each instance we need to deregister from AP_UAVCAN class
|
||||
AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
|
||||
{
|
||||
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager);
|
||||
if (take_registry()) {
|
||||
_detected_modules[_detected_module].driver = nullptr;
|
||||
give_registry();
|
||||
}
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
|
||||
{
|
||||
if (ap_uavcan == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
ap_uavcan->remove_gps_listener(this);
|
||||
delete _sem_gnss;
|
||||
auto* node = ap_uavcan->get_node();
|
||||
|
||||
debug_gps_uavcan(2, _manager, "AP_GPS_UAVCAN destructed\n\r");
|
||||
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCb> *gnss_fix;
|
||||
gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCb>(*node);
|
||||
const int gnss_fix_start_res = gnss_fix->start(FixCb(ap_uavcan, &handle_fix_msg_trampoline));
|
||||
if (gnss_fix_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));
|
||||
if (gnss_aux_start_res < 0) {
|
||||
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::set_uavcan_manager(uint8_t mgr)
|
||||
bool AP_GPS_UAVCAN::take_registry()
|
||||
{
|
||||
_manager = mgr;
|
||||
if (_sem_registry == nullptr) {
|
||||
_sem_registry = hal.util->new_semaphore();
|
||||
}
|
||||
return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER);
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::give_registry()
|
||||
{
|
||||
_sem_registry->give();
|
||||
}
|
||||
|
||||
AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
|
||||
{
|
||||
if (!take_registry()) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_GPS_UAVCAN* backend = nullptr;
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) {
|
||||
backend = new AP_GPS_UAVCAN(_gps, _state);
|
||||
if (backend == nullptr) {
|
||||
debug_gps_uavcan(2,
|
||||
_detected_modules[i].ap_uavcan->get_driver_index(),
|
||||
"Failed to register UAVCAN GPS Node %d on Bus %d\n",
|
||||
_detected_modules[i].node_id,
|
||||
_detected_modules[i].ap_uavcan->get_driver_index());
|
||||
} else {
|
||||
_detected_modules[i].driver = backend;
|
||||
backend->_detected_module = i;
|
||||
debug_gps_uavcan(2,
|
||||
_detected_modules[i].ap_uavcan->get_driver_index(),
|
||||
"Registered UAVCAN GPS Node %d on Bus %d\n",
|
||||
_detected_modules[i].node_id,
|
||||
_detected_modules[i].ap_uavcan->get_driver_index());
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
give_registry();
|
||||
return backend;
|
||||
}
|
||||
|
||||
AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id)
|
||||
{
|
||||
if (ap_uavcan == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
if (_detected_modules[i].driver != nullptr &&
|
||||
_detected_modules[i].ap_uavcan == ap_uavcan &&
|
||||
_detected_modules[i].node_id == node_id) {
|
||||
return _detected_modules[i].driver;
|
||||
}
|
||||
}
|
||||
|
||||
bool already_detected = false;
|
||||
// Check if there's an empty spot for possible registeration
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) {
|
||||
// Already Detected
|
||||
already_detected = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!already_detected) {
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
if (_detected_modules[i].ap_uavcan == nullptr) {
|
||||
_detected_modules[i].ap_uavcan = ap_uavcan;
|
||||
_detected_modules[i].node_id = node_id;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb)
|
||||
{
|
||||
bool process = false;
|
||||
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) {
|
||||
interim_state.status = AP_GPS::GPS_Status::NO_FIX;
|
||||
} else {
|
||||
if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_TIME_ONLY) {
|
||||
interim_state.status = AP_GPS::GPS_Status::NO_FIX;
|
||||
} else if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_2D_FIX) {
|
||||
interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_2D;
|
||||
process = true;
|
||||
} else if (cb.msg->status == uavcan::equipment::gnss::Fix::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::Fix::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 (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;
|
||||
interim_state.location.options = 0;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
float pos_cov[9];
|
||||
cb.msg->position_covariance.unpackSquareMatrix(pos_cov);
|
||||
if (!uavcan::isNaN(pos_cov[8])) {
|
||||
if (pos_cov[8] > 0) {
|
||||
interim_state.vertical_accuracy = sqrtf(pos_cov[8]);
|
||||
interim_state.have_vertical_accuracy = true;
|
||||
} else {
|
||||
interim_state.have_vertical_accuracy = false;
|
||||
}
|
||||
} else {
|
||||
interim_state.have_vertical_accuracy = false;
|
||||
}
|
||||
|
||||
const float horizontal_pos_variance = MAX(pos_cov[0], pos_cov[4]);
|
||||
if (!uavcan::isNaN(horizontal_pos_variance)) {
|
||||
if (horizontal_pos_variance > 0) {
|
||||
interim_state.horizontal_accuracy = sqrtf(horizontal_pos_variance);
|
||||
interim_state.have_horizontal_accuracy = true;
|
||||
} else {
|
||||
interim_state.have_horizontal_accuracy = false;
|
||||
}
|
||||
} else {
|
||||
interim_state.have_horizontal_accuracy = false;
|
||||
}
|
||||
|
||||
float vel_cov[9];
|
||||
cb.msg->velocity_covariance.unpackSquareMatrix(vel_cov);
|
||||
if (!uavcan::isNaN(vel_cov[0])) {
|
||||
interim_state.speed_accuracy = sqrtf((vel_cov[0] + vel_cov[4] + vel_cov[8]) / 3.0);
|
||||
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;
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
if (!uavcan::isNaN(cb.msg->hdop)) {
|
||||
interim_state.hdop = cb.msg->hdop * 100.0;
|
||||
}
|
||||
|
||||
if (!uavcan::isNaN(cb.msg->vdop)) {
|
||||
interim_state.vdop = cb.msg->vdop * 100.0;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb)
|
||||
{
|
||||
if (take_registry()) {
|
||||
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
|
||||
if (driver == nullptr) {
|
||||
return;
|
||||
}
|
||||
driver->handle_fix_msg(cb);
|
||||
give_registry();
|
||||
}
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb)
|
||||
{
|
||||
if (take_registry()) {
|
||||
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
|
||||
if (driver == nullptr) {
|
||||
return;
|
||||
}
|
||||
driver->handle_aux_msg(cb);
|
||||
give_registry();
|
||||
}
|
||||
}
|
||||
|
||||
// Consume new data and mark it received
|
||||
bool AP_GPS_UAVCAN::read(void)
|
||||
{
|
||||
if (_sem_gnss->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if (_new_data) {
|
||||
_new_data = false;
|
||||
WITH_SEMAPHORE(sem);
|
||||
if (_new_data) {
|
||||
_new_data = false;
|
||||
|
||||
state = _interm_state;
|
||||
_sem_gnss->give();
|
||||
state = interim_state;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
_sem_gnss->give();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::handle_gnss_msg(const AP_GPS::GPS_State &msg)
|
||||
{
|
||||
if (_sem_gnss->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
_interm_state = msg;
|
||||
_new_data = true;
|
||||
_sem_gnss->give();
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAL_WITH_UAVCAN
|
||||
|
||||
@ -20,27 +20,50 @@
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_GPS.h"
|
||||
#include "GPS_Backend.h"
|
||||
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
class FixCb;
|
||||
class AuxCb;
|
||||
|
||||
class AP_GPS_UAVCAN : public AP_GPS_Backend {
|
||||
public:
|
||||
AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
|
||||
~AP_GPS_UAVCAN() override;
|
||||
AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state);
|
||||
~AP_GPS_UAVCAN();
|
||||
|
||||
bool read() override;
|
||||
void set_uavcan_manager(uint8_t mgr);
|
||||
|
||||
// This method is called from UAVCAN thread
|
||||
void handle_gnss_msg(const AP_GPS::GPS_State &msg) override;
|
||||
|
||||
const char *name() const override { return "UAVCAN"; }
|
||||
|
||||
private:
|
||||
bool _new_data;
|
||||
uint8_t _manager;
|
||||
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
|
||||
static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
|
||||
|
||||
AP_GPS::GPS_State _interm_state;
|
||||
AP_HAL::Semaphore *_sem_gnss;
|
||||
static void handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &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_aux_msg(const AuxCb &cb);
|
||||
|
||||
static bool take_registry();
|
||||
static void give_registry();
|
||||
static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
|
||||
|
||||
bool _new_data;
|
||||
AP_GPS::GPS_State interim_state;
|
||||
|
||||
HAL_Semaphore sem;
|
||||
|
||||
uint8_t _detected_module;
|
||||
|
||||
// Module Detection Registry
|
||||
static struct DetectedModules {
|
||||
AP_UAVCAN* ap_uavcan;
|
||||
uint8_t node_id;
|
||||
AP_GPS_UAVCAN* driver;
|
||||
} _detected_modules[GPS_MAX_RECEIVERS];
|
||||
|
||||
static AP_HAL::Semaphore *_sem_registry;
|
||||
};
|
||||
|
||||
@ -50,7 +50,6 @@ public:
|
||||
virtual void broadcast_configuration_failure_reason(void) const { return ; }
|
||||
|
||||
virtual void handle_msg(const mavlink_message_t *msg) { return ; }
|
||||
virtual void handle_gnss_msg(const AP_GPS::GPS_State &msg) { return ; }
|
||||
|
||||
// driver specific lag, returns true if the driver is confident in the provided lag
|
||||
virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user