Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: support for multiple instances on different interfaces

Browse Source
This commit is contained in:
Eugene Shamaev 2017-05-06 12:13:31 +03:00 committed by Francisco Ferreira
parent aa1f6a7587
commit 5341e51f45
3 changed files with 30 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_GPS/AP_GPS.cpp
View File

@ -38,6 +38,7 @@
#include "GPS_Backend.h"
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include "AP_GPS_UAVCAN.h"
#endif
@ -417,23 +418,29 @@ void AP_GPS::detect_instance(uint8_t instance)
#if HAL_WITH_UAVCAN
// user has to explicitly set the UAVCAN type, do not use AUTO
// do not try to detect the UAVCAN type, assume it's there
case GPS_TYPE_UAVCAN:
dstate->auto_detected_baud = false; // specified, not detected
if (AP_BoardConfig::get_can_enable() >= 1) {
new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
// register new listener at first empty node
if (hal.can_mgr != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
if (ap_uavcan != nullptr) {
ap_uavcan->register_gps_listener(new_gps, 0);
if (uavcan != nullptr) {
uint8_t gps_node = uavcan->find_gps_without_listener();
if (AP_BoardConfig::get_can_debug() >= 2) {
hal.console->printf("AP_GPS_UAVCAN registered\n\r");
if (gps_node != UINT8_MAX) {
new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i);
if (uavcan->register_gps_listener_to_node(new_gps, gps_node)) {
if (AP_BoardConfig_CAN::get_can_debug() >= 2) {
printf("AP_GPS_UAVCAN registered\n\r");
}
goto found_gps;
} else {
delete new_gps;
}
}
}
goto found_gps;
}
}
}

12
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -22,10 +22,11 @@
#if HAL_WITH_UAVCAN
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
extern const AP_HAL::HAL& hal;
#define debug_gps_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0)
#define debug_gps_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { 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)
@ -37,8 +38,8 @@ AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UA
// For each instance we need to deregister from AP_UAVCAN class
AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
{
if (hal.can_mgr != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
if (hal.can_mgr[_manager] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[_manager]->get_UAVCAN();
if (ap_uavcan != nullptr) {
ap_uavcan->remove_gps_listener(this);
@ -47,6 +48,11 @@ AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
}
}
void AP_GPS_UAVCAN::set_uavcan_manager(uint8_t mgr)
{
_manager = mgr;
}
// Consume new data and mark it received
bool AP_GPS_UAVCAN::read(void)
{

2
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -30,6 +30,7 @@ public:
~AP_GPS_UAVCAN() override;
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;
@ -38,6 +39,7 @@ public:
private:
bool _new_data;
uint8_t _manager;
AP_GPS::GPS_State _interm_state;
AP_HAL::Semaphore *_sem_gnss;