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

AP_GPS: support yaw for UAVCAN GPS

This commit is contained in:
Michael Oborne 2020-10-30 12:39:01 +08:00 committed by Andrew Tridgell
parent 5ef3e1bec3
commit d2720da4a2
2 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -27,6 +27,7 @@
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
#include <ardupilot/gnss/Heading.hpp>
extern const AP_HAL::HAL& hal;
@ -35,6 +36,7 @@ extern const AP_HAL::HAL& hal;
UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix);
UC_REGISTRY_BINDER(Fix2Cb, uavcan::equipment::gnss::Fix2);
UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary);
UC_REGISTRY_BINDER(HeadingCb, ardupilot::gnss::Heading);
AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
@ -82,6 +84,14 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
return;
}
uavcan::Subscriber<ardupilot::gnss::Heading, HeadingCb> *gnss_heading;
gnss_heading = new uavcan::Subscriber<ardupilot::gnss::Heading, HeadingCb>(*node);
const int gnss_heading_start_res = gnss_heading->start(HeadingCb(ap_uavcan, &handle_heading_msg_trampoline));
if (gnss_heading_start_res < 0) {
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
return;
}
}
AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
@ -390,6 +400,21 @@ void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb)
}
}
void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb)
{
WITH_SEMAPHORE(sem);
if (interim_state.gps_yaw_configured == false) {
interim_state.gps_yaw_configured = cb.msg->heading_valid;
}
interim_state.have_gps_yaw = cb.msg->heading_valid;
interim_state.gps_yaw = degrees(cb.msg->heading_rad);
interim_state.have_gps_yaw_accuracy = cb.msg->heading_accuracy_valid;
interim_state.gps_yaw_accuracy = degrees(cb.msg->heading_accuracy_rad);
}
void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb)
{
WITH_SEMAPHORE(_sem_registry);
@ -420,6 +445,16 @@ void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node
}
}
void AP_GPS_UAVCAN::handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb)
{
WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
if (driver != nullptr) {
driver->handle_heading_msg(cb);
}
}
// Consume new data and mark it received
bool AP_GPS_UAVCAN::read(void)
{

3
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -28,6 +28,7 @@
class FixCb;
class Fix2Cb;
class AuxCb;
class HeadingCb;
class AP_GPS_UAVCAN : public AP_GPS_Backend {
public:
@ -44,6 +45,7 @@ public:
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);
static void handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb);
void inject_data(const uint8_t *data, uint16_t len) override;
@ -51,6 +53,7 @@ private:
void handle_fix_msg(const FixCb &cb);
void handle_fix2_msg(const Fix2Cb &cb);
void handle_aux_msg(const AuxCb &cb);
void handle_heading_msg(const HeadingCb &cb);
static bool take_registry();
static void give_registry();