Skip to content

Commit

Permalink
AP_UAVCAN: add GPS-out
Browse files Browse the repository at this point in the history
  • Loading branch information
@magicrub @tridge
magicrub authored and tridge committed Mar 1, 2023
1 parent fe37282 commit e346eb8
Show file tree
Hide file tree
Showing 2 changed files with 210 additions and 2 deletions.
194 changes: 192 additions & 2 deletions libraries/AP_UAVCAN/AP_UAVCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,12 @@
#include <ardupilot/indication/Button.hpp>
#include <ardupilot/indication/NotifyState.hpp>
#include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
#if AP_DRONECAN_SEND_GPS
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>
#include <ardupilot/gnss/Heading.hpp>
#include <ardupilot/gnss/Status.hpp>
#endif
#include <uavcan/equipment/gnss/RTCMStream.hpp>
#include <uavcan/protocol/debug/LogMessage.hpp>

Expand All @@ -50,6 +56,7 @@
#include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
#include <AP_EFI/AP_EFI_DroneCAN.h>
#include <AP_GPS/AP_GPS_UAVCAN.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_BattMonitor/AP_BattMonitor_UAVCAN.h>
#include <AP_Compass/AP_Compass_UAVCAN.h>
#include <AP_Airspeed/AP_Airspeed_UAVCAN.h>
Expand Down Expand Up @@ -132,7 +139,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
// @Param: OPTION
// @DisplayName: UAVCAN options
// @Description: Option flags
// @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM
// @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS
// @User: Advanced
AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0),

Expand Down Expand Up @@ -172,6 +179,12 @@ static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::safety::ArmingStatus>* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS];
#if AP_DRONECAN_SEND_GPS
static uavcan::Publisher<uavcan::equipment::gnss::Fix2>* gnss_fix2[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>* gnss_auxiliary[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<ardupilot::gnss::Heading>* gnss_heading[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<ardupilot::gnss::Status>* gnss_status[HAL_MAX_CAN_PROTOCOL_DRIVERS];
#endif
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS];
static uavcan::Publisher<ardupilot::indication::NotifyState>* notify_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];

Expand Down Expand Up @@ -256,7 +269,7 @@ bool AP_UAVCAN::add_interface(AP_HAL::CANIface* can_iface) {
}

#pragma GCC diagnostic push
#pragma GCC diagnostic error "-Wframe-larger-than=1700"
#pragma GCC diagnostic error "-Wframe-larger-than=2200"
void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
{
_driver_index = driver_index;
Expand Down Expand Up @@ -400,6 +413,24 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
arming_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
arming_status[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);

#if AP_DRONECAN_SEND_GPS
gnss_fix2[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::Fix2>(*_node);
gnss_fix2[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
gnss_fix2[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

gnss_auxiliary[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>(*_node);
gnss_auxiliary[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
gnss_auxiliary[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

gnss_heading[driver_index] = new uavcan::Publisher<ardupilot::gnss::Heading>(*_node);
gnss_heading[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
gnss_heading[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

gnss_status[driver_index] = new uavcan::Publisher<ardupilot::gnss::Status>(*_node);
gnss_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
gnss_status[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
#endif

rtcm_stream[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>(*_node);
rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
Expand Down Expand Up @@ -521,6 +552,15 @@ void AP_UAVCAN::loop(void)
#if AP_OPENDRONEID_ENABLED
AP::opendroneid().dronecan_send(this);
#endif

#if AP_DRONECAN_SEND_GPS
if (option_is_set(AP_UAVCAN::Options::SEND_GNSS) && !AP_GPS_UAVCAN::instance_exists(this)) {
// send if enabled and this interface/driver is not used by the AP_GPS driver
gnss_send_fix();
gnss_send_yaw();
}
#endif

logging();
}
}
Expand Down Expand Up @@ -830,6 +870,156 @@ void AP_UAVCAN::notify_state_send()
_last_notify_state_ms = AP_HAL::native_millis();
}

#if AP_DRONECAN_SEND_GPS
void AP_UAVCAN::gnss_send_fix()
{
const AP_GPS &gps = AP::gps();

const uint32_t gps_lib_time_ms = gps.last_message_time_ms();
if (_gnss.last_gps_lib_fix_ms == gps_lib_time_ms) {
return;
}
_gnss.last_gps_lib_fix_ms = gps_lib_time_ms;

/*
send Fix2 packet
*/

uavcan::equipment::gnss::Fix2 pkt {};
const Location &loc = gps.location();
const Vector3f &vel = gps.velocity();

pkt.timestamp.usec = AP_HAL::native_micros64();
pkt.gnss_timestamp.usec = gps.time_epoch_usec();
if (pkt.gnss_timestamp.usec == 0) {
pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_NONE;
} else {
pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC;
}
pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
pkt.height_ellipsoid_mm = loc.alt * 10;
pkt.height_msl_mm = loc.alt * 10;
for (uint8_t i=0; i<3; i++) {
pkt.ned_velocity[i] = vel[i];
}
pkt.sats_used = gps.num_sats();
switch (gps.status()) {
case AP_GPS::GPS_Status::NO_GPS:
case AP_GPS::GPS_Status::NO_FIX:
pkt.status = uavcan::equipment::gnss::Fix2::STATUS_NO_FIX;
pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
pkt.status = uavcan::equipment::gnss::Fix2::STATUS_2D_FIX;
pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
pkt.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS;
pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_SBAS;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT;
break;
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED;
break;
}

pkt.covariance.resize(6);
float hacc;
if (gps.horizontal_accuracy(hacc)) {
pkt.covariance[0] = pkt.covariance[1] = sq(hacc);
}
float vacc;
if (gps.vertical_accuracy(vacc)) {
pkt.covariance[2] = sq(vacc);
}
float sacc;
if (gps.speed_accuracy(sacc)) {
const float vc3 = sq(sacc);
pkt.covariance[3] = pkt.covariance[4] = pkt.covariance[5] = vc3;
}

gnss_fix2[_driver_index]->broadcast(pkt);



const uint32_t now_ms = AP_HAL::native_millis();
if (now_ms - _gnss.last_send_status_ms >= 1000) {
_gnss.last_send_status_ms = now_ms;

/*
send aux packet
*/
uavcan::equipment::gnss::Auxiliary pkt_auxiliary {};
pkt_auxiliary.hdop = gps.get_hdop() * 0.01;
pkt_auxiliary.vdop = gps.get_vdop() * 0.01;

gnss_auxiliary[_driver_index]->broadcast(pkt_auxiliary);


/*
send Status packet
*/
ardupilot::gnss::Status pkt_status {};
pkt_status.healthy = gps.is_healthy();
if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {
pkt_status.status |= ardupilot::gnss::Status::STATUS_LOGGING;
}
uint8_t idx; // unused
if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) {
pkt_status.status |= ardupilot::gnss::Status::STATUS_ARMABLE;
}

uint32_t error_codes;
if (gps.get_error_codes(error_codes)) {
pkt_status.error_codes = error_codes;
}

gnss_status[_driver_index]->broadcast(pkt_status);
}
}

void AP_UAVCAN::gnss_send_yaw()
{
const AP_GPS &gps = AP::gps();

if (!gps.have_gps_yaw()) {
return;
}

float yaw_deg, yaw_acc_deg;
uint32_t yaw_time_ms;
if (!gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != _gnss.last_lib_yaw_time_ms) {
return;
}

_gnss.last_lib_yaw_time_ms = yaw_time_ms;

ardupilot::gnss::Heading pkt_heading {};
pkt_heading.heading_valid = true;
pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg);
pkt_heading.heading_rad = radians(yaw_deg);
pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg);

gnss_heading[_driver_index]->broadcast(pkt_heading);
}
#endif // AP_DRONECAN_SEND_GPS


void AP_UAVCAN::rtcm_stream_send()
{
WITH_SEMAPHORE(_rtcm_stream.sem);
Expand Down
18 changes: 18 additions & 0 deletions libraries/AP_UAVCAN/AP_UAVCAN.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@
#define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS
#endif

#ifndef AP_DRONECAN_SEND_GPS
#define AP_DRONECAN_SEND_GPS (BOARD_FLASH_SIZE > 1024)
#endif

#define AP_UAVCAN_SW_VERS_MAJOR 1
#define AP_UAVCAN_SW_VERS_MINOR 0

Expand Down Expand Up @@ -205,6 +209,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
CANFD_ENABLED = (1U<<2),
DNA_IGNORE_UNHEALTHY_NODE = (1U<<3),
USE_ACTUATOR_PWM = (1U<<4),
SEND_GNSS = (1U<<5),
};

// check if a option is set
Expand Down Expand Up @@ -326,6 +331,19 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
uint8_t pending_mask; // mask of interfaces to send to
} _buzzer;

#if AP_DRONECAN_SEND_GPS
// send GNSS Fix and yaw, same thing AP_GPS_UAVCAN would receive
void gnss_send_fix();
void gnss_send_yaw();

// GNSS Fix and Status
struct {
uint32_t last_gps_lib_fix_ms;
uint32_t last_send_status_ms;
uint32_t last_lib_yaw_time_ms;
} _gnss;
#endif

// GNSS RTCM injection
struct {
HAL_Semaphore sem;
Expand Down

0 comments on commit e346eb8

Please sign in to comment.