mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Extend SENS_GPS_PRIME usage for UAVCAN GNSS devices (#26126)
* UAVCAN: extent SENS_GPS_PRIME usage to UAVCAN GNSS devices * use convenience function Co-authored-by: Matthias Grob <maetugr@gmail.com> * Update src/drivers/uavcan/sensors/gnss.cpp Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> * Apply suggestion from @MaEtUgR Co-authored-by: Matthias Grob <maetugr@gmail.com> * Fix type casting in GPS prime range check * reverted parameter default * UAVCAN: fix and improve device_id logic (#26135) * UAVCAN: extent SENS_GPS_PRIME usage to UAVCAN GNSS devices * use convenience function Co-authored-by: Matthias Grob <maetugr@gmail.com> * Update src/drivers/uavcan/sensors/gnss.cpp Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> * Apply suggestion from @MaEtUgR Co-authored-by: Matthias Grob <maetugr@gmail.com> * Fix type casting in GPS prime range check * UAVCAN: fix and improve device_id logic * Added bus information to more UAVCAN drivers * Fix device_id registration in UavcanBarometerBridge --------- Co-authored-by: Matthias Grob <maetugr@gmail.com> Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --------- Co-authored-by: Matthias Grob <maetugr@gmail.com> Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
This commit is contained in:
Claudio Chies
GitHub
parent
450cf79fc8
commit
86e1356e0a
@ -184,6 +184,7 @@
|
||||
#define DRV_IMU_DEVTYPE_UAVCAN 0x87
|
||||
#define DRV_MAG_DEVTYPE_UAVCAN 0x88
|
||||
#define DRV_DIST_DEVTYPE_UAVCAN 0x89
|
||||
#define DRV_HYGRO_DEVTYPE_UAVCAN 0x8A
|
||||
|
||||
#define DRV_ADC_DEVTYPE_ADS1115 0x90
|
||||
|
||||
|
||||
@ -43,7 +43,9 @@ const char *const UavcanAccelBridge::NAME = "accel";
|
||||
UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
|
||||
UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel), node_info_publisher),
|
||||
_sub_imu_data(node)
|
||||
{ }
|
||||
{
|
||||
set_device_type(DRV_ACC_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanAccelBridge::init()
|
||||
{
|
||||
@ -59,7 +61,7 @@ int UavcanAccelBridge::init()
|
||||
|
||||
void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::RawIMU> &msg)
|
||||
{
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
@ -87,13 +89,10 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
|
||||
|
||||
int UavcanAccelBridge::init_driver(uavcan_bridge::Channel *channel)
|
||||
{
|
||||
// update device id as we now know our device node_id
|
||||
DeviceId device_id{_device_id};
|
||||
// Build device ID using node_id and interface index
|
||||
uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
|
||||
|
||||
device_id.devid_s.devtype = DRV_ACC_DEVTYPE_UAVCAN;
|
||||
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
|
||||
|
||||
channel->h_driver = new PX4Accelerometer(device_id.devid);
|
||||
channel->h_driver = new PX4Accelerometer(device_id);
|
||||
|
||||
if (channel->h_driver == nullptr) {
|
||||
return PX4_ERROR;
|
||||
|
||||
@ -49,7 +49,9 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublis
|
||||
UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher),
|
||||
_sub_air_pressure_data(node),
|
||||
_sub_air_temperature_data(node)
|
||||
{ }
|
||||
{
|
||||
set_device_type(DRV_BARO_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanBarometerBridge::init()
|
||||
{
|
||||
@ -91,7 +93,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
|
||||
{
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
|
||||
if (channel == nullptr) {
|
||||
// Something went wrong - no channel to publish on; return
|
||||
@ -105,23 +107,18 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceId device_id{};
|
||||
device_id.devid_s.bus = 0;
|
||||
device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
|
||||
|
||||
device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN;
|
||||
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
|
||||
uint32_t device_id = make_uavcan_device_id(msg);
|
||||
|
||||
// Register barometer capability with NodeInfoPublisher after first successful message
|
||||
if (_node_info_publisher != nullptr) {
|
||||
_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id.devid,
|
||||
_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id,
|
||||
NodeInfoPublisher::DeviceCapability::BAROMETER);
|
||||
}
|
||||
|
||||
// publish
|
||||
sensor_baro_s sensor_baro{};
|
||||
sensor_baro.timestamp_sample = timestamp_sample;
|
||||
sensor_baro.device_id = device_id.devid;
|
||||
sensor_baro.device_id = device_id;
|
||||
sensor_baro.pressure = msg.static_pressure;
|
||||
|
||||
if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) {
|
||||
|
||||
@ -49,6 +49,7 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode
|
||||
UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure), node_info_publisher),
|
||||
_sub_air(node)
|
||||
{
|
||||
set_device_type(DRV_DIFF_PRESS_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanDifferentialPressureBridge::init()
|
||||
@ -68,8 +69,6 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
|
||||
{
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN;
|
||||
_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
|
||||
float diff_press_pa = msg.differential_pressure;
|
||||
int32_t differential_press_rev = 0;
|
||||
param_get(param_find("SENS_DPRES_REV"), &differential_press_rev);
|
||||
@ -83,7 +82,7 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
|
||||
|
||||
differential_pressure_s report{};
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id.devid;
|
||||
report.device_id = make_uavcan_device_id(msg);
|
||||
report.differential_pressure_pa = diff_press_pa;
|
||||
report.temperature = temperature_c;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
@ -41,6 +41,7 @@ UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_
|
||||
UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow), node_info_publisher),
|
||||
_sub_flow(node)
|
||||
{
|
||||
set_device_type(DRV_FLOW_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int
|
||||
@ -61,13 +62,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex:
|
||||
sensor_optical_flow_s flow{};
|
||||
flow.timestamp_sample = hrt_absolute_time(); // TODO
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
|
||||
device_id.devid_s.bus = 0;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_UAVCAN;
|
||||
device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
|
||||
|
||||
flow.device_id = device_id.devid;
|
||||
flow.device_id = make_uavcan_device_id(msg);
|
||||
|
||||
flow.pixel_flow[0] = msg.flow_integral[0];
|
||||
flow.pixel_flow[1] = msg.flow_integral[1];
|
||||
|
||||
@ -404,7 +404,8 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
||||
const uint8_t spoofing_state)
|
||||
{
|
||||
sensor_gps_s sensor_gps{};
|
||||
sensor_gps.device_id = get_device_id();
|
||||
|
||||
sensor_gps.device_id = make_uavcan_device_id(msg);
|
||||
|
||||
// Register GPS capability with NodeInfoPublisher after first successful message
|
||||
if (_node_info_publisher != nullptr) {
|
||||
|
||||
@ -42,6 +42,7 @@ UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfo
|
||||
UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative), node_info_publisher),
|
||||
_sub_rel_pos_heading(node)
|
||||
{
|
||||
set_device_type(DRV_GPS_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int
|
||||
@ -70,7 +71,7 @@ void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const
|
||||
sensor_gnss_relative.position_length = msg.relative_distance_m;
|
||||
sensor_gnss_relative.position[2] = msg.relative_down_pos_m;
|
||||
|
||||
sensor_gnss_relative.device_id = get_device_id();
|
||||
sensor_gnss_relative.device_id = make_uavcan_device_id(msg);
|
||||
|
||||
// Register GPS capability with NodeInfoPublisher after first successful message
|
||||
if (_node_info_publisher != nullptr) {
|
||||
|
||||
@ -43,7 +43,9 @@ const char *const UavcanGyroBridge::NAME = "gyro";
|
||||
UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
|
||||
UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro), node_info_publisher),
|
||||
_sub_imu_data(node)
|
||||
{ }
|
||||
{
|
||||
set_device_type(DRV_GYR_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanGyroBridge::init()
|
||||
{
|
||||
@ -59,7 +61,7 @@ int UavcanGyroBridge::init()
|
||||
|
||||
void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::RawIMU> &msg)
|
||||
{
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
|
||||
if (channel == nullptr) {
|
||||
// Something went wrong - no channel to publish on; return
|
||||
@ -87,13 +89,10 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
|
||||
|
||||
int UavcanGyroBridge::init_driver(uavcan_bridge::Channel *channel)
|
||||
{
|
||||
// update device id as we now know our device node_id
|
||||
DeviceId device_id{_device_id};
|
||||
// Build device ID using node_id and interface index
|
||||
uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
|
||||
|
||||
device_id.devid_s.devtype = DRV_GYR_DEVTYPE_UAVCAN;
|
||||
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
|
||||
|
||||
channel->h_driver = new PX4Gyroscope(device_id.devid);
|
||||
channel->h_driver = new PX4Gyroscope(device_id);
|
||||
|
||||
if (channel->h_driver == nullptr) {
|
||||
return PX4_ERROR;
|
||||
|
||||
@ -42,6 +42,7 @@ UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPubl
|
||||
UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer), node_info_publisher),
|
||||
_sub_hygro(node)
|
||||
{
|
||||
set_device_type(DRV_HYGRO_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanHygrometerBridge::init()
|
||||
@ -64,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure<dr
|
||||
|
||||
sensor_hygrometer_s report{};
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = 0; // TODO
|
||||
report.device_id = make_uavcan_device_id(msg);
|
||||
report.temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius;
|
||||
report.humidity = msg.humidity;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
@ -49,6 +49,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node, NodeInfo
|
||||
_sub_mag(node),
|
||||
_sub_mag2(node)
|
||||
{
|
||||
set_device_type(DRV_MAG_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanMagnetometerBridge::init()
|
||||
@ -73,7 +74,7 @@ int UavcanMagnetometerBridge::init()
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg)
|
||||
{
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
|
||||
if (channel == nullptr) {
|
||||
// Something went wrong - no channel to publish on; return
|
||||
@ -104,7 +105,7 @@ void
|
||||
UavcanMagnetometerBridge::mag2_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg)
|
||||
{
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
|
||||
if (channel == nullptr || channel->instance < 0) {
|
||||
// Something went wrong - no channel to publish on; return
|
||||
@ -134,13 +135,10 @@ UavcanMagnetometerBridge::mag2_sub_cb(const
|
||||
|
||||
int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
|
||||
{
|
||||
// update device id as we now know our device node_id
|
||||
DeviceId device_id{_device_id};
|
||||
// Build device ID using node_id and interface index
|
||||
uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
|
||||
|
||||
device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN;
|
||||
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
|
||||
|
||||
channel->h_driver = new PX4Magnetometer(device_id.devid, ROTATION_NONE);
|
||||
channel->h_driver = new PX4Magnetometer(device_id, ROTATION_NONE);
|
||||
|
||||
if (channel->h_driver == nullptr) {
|
||||
return PX4_ERROR;
|
||||
|
||||
@ -45,7 +45,9 @@ const char *const UavcanRangefinderBridge::NAME = "rangefinder";
|
||||
UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) :
|
||||
UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor), node_info_publisher),
|
||||
_sub_range_data(node)
|
||||
{ }
|
||||
{
|
||||
set_device_type(DRV_DIST_DEVTYPE_UAVCAN);
|
||||
}
|
||||
|
||||
int UavcanRangefinderBridge::init()
|
||||
{
|
||||
@ -66,7 +68,7 @@ int UavcanRangefinderBridge::init()
|
||||
void UavcanRangefinderBridge::range_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::range_sensor::Measurement> &msg)
|
||||
{
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
|
||||
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
|
||||
if (channel == nullptr || channel->instance < 0) {
|
||||
// Something went wrong - no channel to publish on; return
|
||||
@ -125,13 +127,10 @@ void UavcanRangefinderBridge::range_sub_cb(const
|
||||
|
||||
int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel)
|
||||
{
|
||||
// update device id as we now know our device node_id
|
||||
DeviceId device_id{_device_id};
|
||||
// Build device ID using node_id and interface index
|
||||
uint32_t device_id = make_uavcan_device_id(static_cast<uint8_t>(channel->node_id), channel->iface_index);
|
||||
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_UAVCAN;
|
||||
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
|
||||
|
||||
channel->h_driver = new PX4Rangefinder(device_id.devid, distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||
channel->h_driver = new PX4Rangefinder(device_id, distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||
|
||||
if (channel->h_driver == nullptr) {
|
||||
return PX4_ERROR;
|
||||
|
||||
@ -316,7 +316,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report)
|
||||
(void)orb_publish(_orb_topic, channel->orb_advert, report);
|
||||
}
|
||||
|
||||
uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id)
|
||||
uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id, uint8_t iface_index)
|
||||
{
|
||||
uavcan_bridge::Channel *channel = nullptr;
|
||||
|
||||
@ -354,6 +354,7 @@ uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id
|
||||
|
||||
// initialize the driver, which registers the class device name and uORB publisher
|
||||
channel->node_id = node_id;
|
||||
channel->iface_index = iface_index;
|
||||
int ret = init_driver(channel);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
|
||||
@ -92,6 +92,7 @@ struct Channel {
|
||||
orb_advert_t orb_advert{nullptr};
|
||||
int instance{-1};
|
||||
void *h_driver{nullptr};
|
||||
uint8_t iface_index{0};
|
||||
};
|
||||
} // namespace uavcan_bridge
|
||||
|
||||
@ -138,7 +139,34 @@ protected:
|
||||
*/
|
||||
virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; };
|
||||
|
||||
uavcan_bridge::Channel *get_channel_for_node(int node_id);
|
||||
uavcan_bridge::Channel *get_channel_for_node(int node_id, uint8_t iface_index);
|
||||
|
||||
/**
|
||||
* Builds a unique device ID from a UAVCAN message
|
||||
* @param msg UAVCAN message (must have getSrcNodeID() and getIfaceIndex() methods)
|
||||
* @return Complete device ID with node address and interface encoded
|
||||
*/
|
||||
template<typename T>
|
||||
uint32_t make_uavcan_device_id(const uavcan::ReceivedDataStructure<T> &msg) const
|
||||
{
|
||||
return make_uavcan_device_id(msg.getSrcNodeID().get(), msg.getIfaceIndex());
|
||||
}
|
||||
|
||||
/**
|
||||
* Builds a unique device ID from node ID and interface index
|
||||
* @param node_id UAVCAN node ID
|
||||
* @param iface_index CAN interface index (0 = CAN1, 1 = CAN2, etc.)
|
||||
* @return Complete device ID with node address and interface encoded
|
||||
*/
|
||||
uint32_t make_uavcan_device_id(uint8_t node_id, uint8_t iface_index) const
|
||||
{
|
||||
device::Device::DeviceId device_id{};
|
||||
device_id.devid_s.devtype = get_device_type();
|
||||
device_id.devid_s.address = node_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType_UAVCAN;
|
||||
device_id.devid_s.bus = iface_index;
|
||||
return device_id.devid;
|
||||
}
|
||||
|
||||
public:
|
||||
virtual ~UavcanSensorBridgeBase();
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
|
||||
namespace sensors
|
||||
{
|
||||
@ -95,7 +96,12 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
|
||||
_gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC);
|
||||
_gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC);
|
||||
_gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get());
|
||||
_gps_blending.setPrimaryInstance(_param_sens_gps_prime.get());
|
||||
|
||||
const int gps_prime = _param_sens_gps_prime.get();
|
||||
|
||||
if (math::isInRange(gps_prime, -1, 1)) {
|
||||
_gps_blending.setPrimaryInstance(gps_prime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -113,6 +119,7 @@ void VehicleGPSPosition::Run()
|
||||
// Check all GPS instance
|
||||
bool any_gps_updated = false;
|
||||
bool gps_updated = false;
|
||||
const int32_t gps_prime = _param_sens_gps_prime.get();
|
||||
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
gps_updated = _sensor_gps_sub[i].updated();
|
||||
@ -125,6 +132,16 @@ void VehicleGPSPosition::Run()
|
||||
_sensor_gps_sub[i].copy(&gps_data);
|
||||
_gps_blending.setGpsData(gps_data, i);
|
||||
|
||||
if (math::isInRange(static_cast<int>(gps_prime), 2, 127)) {
|
||||
device::Device::DeviceId device_id{};
|
||||
device_id.devid = gps_data.device_id;
|
||||
|
||||
if (device_id.devid_s.bus_type == device::Device::DeviceBusType_UAVCAN
|
||||
&& device_id.devid_s.address == static_cast<uint8_t>(gps_prime)) {
|
||||
_gps_blending.setPrimaryInstance(i);
|
||||
}
|
||||
}
|
||||
|
||||
if (!_sensor_gps_sub[i].registered()) {
|
||||
_sensor_gps_sub[i].registerCallback();
|
||||
}
|
||||
|
||||
@ -70,13 +70,16 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
|
||||
* send data to the EKF even if a secondary instance is already available.
|
||||
* The secondary instance is then only used if the primary one times out.
|
||||
*
|
||||
* To have an equal priority of all the instances, set this parameter to -1 and
|
||||
* the best receiver will be used.
|
||||
* Accepted values:
|
||||
* -1 : Auto (equal priority for all instances)
|
||||
* 0 : Main serial GPS instance
|
||||
* 1 : Secondary serial GPS instance
|
||||
* 2-127 : UAVCAN module node ID
|
||||
*
|
||||
* This parameter has no effect if blending is active.
|
||||
*
|
||||
* @group Sensors
|
||||
* @min -1
|
||||
* @max 1
|
||||
* @max 127
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user