forked from Archive/PX4-Autopilot
UAVCAN Air Data Support
This commit is contained in:
avionicsanonymous
GitHub
parent
3177b8d763
commit
c8df77b3d2
|
|
@ -127,6 +127,7 @@ px4_add_module(
|
|||
sensors/differential_pressure.cpp
|
||||
sensors/baro.cpp
|
||||
sensors/battery.cpp
|
||||
sensors/airspeed.cpp
|
||||
sensors/flow.cpp
|
||||
sensors/gnss.cpp
|
||||
sensors/mag.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,117 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author RJ Gritter <rjgritter657@gmail.com>
|
||||
*/
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "airspeed.hpp"
|
||||
#include <math.h>
|
||||
#include <lib/ecl/geo/geo.h> // For CONSTANTS_*
|
||||
|
||||
const char *const UavcanAirspeedBridge::NAME = "airspeed";
|
||||
|
||||
UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) :
|
||||
UavcanCDevSensorBridgeBase("uavcan_airspeed", "/dev/uavcan/airspeed", AIRSPEED_BASE_DEVICE_PATH, ORB_ID(airspeed)),
|
||||
_sub_ias_data(node),
|
||||
_sub_tas_data(node),
|
||||
_sub_oat_data(node)
|
||||
{ }
|
||||
|
||||
int UavcanAirspeedBridge::init()
|
||||
{
|
||||
int res = device::CDev::init();
|
||||
|
||||
if (res < 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_ias_data.start(IASCbBinder(this, &UavcanAirspeedBridge::ias_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
DEVICE_LOG("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_tas_data.start(TASCbBinder(this, &UavcanAirspeedBridge::tas_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
DEVICE_LOG("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_oat_data.start(OATCbBinder(this, &UavcanAirspeedBridge::oat_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
DEVICE_LOG("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanAirspeedBridge::oat_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
|
||||
{
|
||||
_last_outside_air_temp_k = msg.static_temperature;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanAirspeedBridge::tas_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &msg)
|
||||
{
|
||||
_last_tas_m_s = msg.true_airspeed;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanAirspeedBridge::ias_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &msg)
|
||||
{
|
||||
airspeed_s report{};
|
||||
|
||||
/*
|
||||
* FIXME HACK
|
||||
* This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
|
||||
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
|
||||
* to use an independent time source (based on hardware TIM5) instead of HRT.
|
||||
* The proper solution is to be developed.
|
||||
*/
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.indicated_airspeed_m_s = msg.indicated_airspeed;
|
||||
report.true_airspeed_m_s = _last_tas_m_s;
|
||||
report.air_temperature_celsius = _last_outside_air_temp_k + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
|
||||
|
||||
publish(msg.getSrcNodeID().get(), &report);
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author RJ Gritter <rjgritter657@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sensor_bridge.hpp"
|
||||
#include <drivers/drv_airspeed.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
|
||||
#include <uavcan/equipment/air_data/IndicatedAirspeed.hpp>
|
||||
#include <uavcan/equipment/air_data/TrueAirspeed.hpp>
|
||||
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
|
||||
|
||||
class UavcanAirspeedBridge : public UavcanCDevSensorBridgeBase
|
||||
{
|
||||
public:
|
||||
static const char *const NAME;
|
||||
|
||||
UavcanAirspeedBridge(uavcan::INode &node);
|
||||
|
||||
const char *get_name() const override { return NAME; }
|
||||
|
||||
int init() override;
|
||||
|
||||
private:
|
||||
|
||||
void ias_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &msg);
|
||||
void tas_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &msg);
|
||||
void oat_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanAirspeedBridge *,
|
||||
void (UavcanAirspeedBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &) >
|
||||
IASCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanAirspeedBridge *,
|
||||
void (UavcanAirspeedBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &) >
|
||||
TASCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanAirspeedBridge *,
|
||||
void (UavcanAirspeedBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
|
||||
OATCbBinder;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::IndicatedAirspeed, IASCbBinder> _sub_ias_data;
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed, TASCbBinder> _sub_tas_data;
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, OATCbBinder> _sub_oat_data;
|
||||
|
||||
float _last_tas_m_s{0.0f};
|
||||
float _last_outside_air_temp_k{0.0f};
|
||||
|
||||
};
|
||||
|
|
@ -41,6 +41,7 @@
|
|||
#include "differential_pressure.hpp"
|
||||
#include "baro.hpp"
|
||||
#include "battery.hpp"
|
||||
#include "airspeed.hpp"
|
||||
#include "gnss.hpp"
|
||||
#include "flow.hpp"
|
||||
#include "mag.hpp"
|
||||
|
|
@ -55,6 +56,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
|||
list.add(new UavcanGnssBridge(node));
|
||||
list.add(new UavcanFlowBridge(node));
|
||||
list.add(new UavcanBatteryBridge(node));
|
||||
list.add(new UavcanAirspeedBridge(node));
|
||||
list.add(new UavcanDifferentialPressureBridge(node));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue