fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 11:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

wip: squashed patch

Browse Source
This commit is contained in:
Thomas Stastny
2022-12-19 13:12:42 +01:00
parent 283aad01fd
commit ebe0a4095c
659 changed files with 23727 additions and 8530 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/adc/board_adc/ADC.cpp
+9 -3
View File
@@ -39,8 +39,9 @@
#include <nuttx/ioexpander/gpio.h>
#endif
ADC::ADC(uint32_t base_address, uint32_t channels) :
ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_publish_adc_report(publish_adc_report),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
_base_address(base_address)
{
@@ -116,7 +117,10 @@ void ADC::Run()
_samples[i].am_data = sample(_samples[i].am_channel);
}
update_adc_report(now);
if (_publish_adc_report) {
update_adc_report(now);
}
update_system_power(now);
}
@@ -352,7 +356,8 @@ int ADC::custom_command(int argc, char *argv[])
int ADC::task_spawn(int argc, char *argv[])
{
ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS);
bool publish_adc_report = !(argc >= 2 && strcmp(argv[1], "-n") == 0);
ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS, publish_adc_report);
if (instance) {
_object.store(instance);
@@ -389,6 +394,7 @@ ADC driver.
PRINT_MODULE_USAGE_NAME("adc", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND("test");
PRINT_MODULE_USAGE_PARAM_FLAG('n', "Do not publish ADC report, only system power", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
src/drivers/adc/board_adc/ADC.hpp
+4 -1
View File
@@ -64,7 +64,7 @@ using namespace time_literals;
class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
{
public:
ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS);
ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS, bool publish_adc_report = true);
~ADC() override;
@@ -102,6 +102,8 @@ private:
static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */
bool _publish_adc_report;
perf_counter_t _sample_perf;
unsigned _channel_count{0};
@@ -110,6 +112,7 @@ private:
uORB::Publication<adc_report_s> _to_adc_report{ORB_ID(adc_report)};
uORB::Publication<system_power_s> _to_system_power{ORB_ID(system_power)};
#ifdef BOARD_GPIO_VDD_5V_COMP_VALID
int _5v_comp_valid_fd {-1};
#endif
src/drivers/barometer/bmp388/bmp388.cpp
+11 -2
View File
@@ -68,11 +68,19 @@ BMP388::init()
return -EIO;
}
if (_interface->get_reg(BMP3_CHIP_ID_ADDR) != BMP3_CHIP_ID) {
PX4_WARN("id of your baro is not: 0x%02x", BMP3_CHIP_ID);
_chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR);
if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) {
PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID);
return -EIO;
}
if (_chip_id == BMP390_CHIP_ID) {
_interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
}
_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);
_cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR);
if (!_cal) {
@@ -99,6 +107,7 @@ void
BMP388::print_status()
{
I2CSPIDriverBase::print_status();
printf("chip id: 0x%02x rev id: 0x%02x\n", _chip_id, _chip_rev_id);
perf_print_counter(_sample_perf);
perf_print_counter(_measure_perf);
perf_print_counter(_comms_errors);
src/drivers/barometer/bmp388/bmp388.h
+8 -1
View File
@@ -51,7 +51,8 @@
// From https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h
#define BMP3_CHIP_ID (0x50)
#define BMP388_CHIP_ID (0x50)
#define BMP390_CHIP_ID (0x60)
/* Over sampling macros */
#define BMP3_NO_OVERSAMPLING (0x00)
@@ -79,6 +80,7 @@
/* Register Address */
#define BMP3_CHIP_ID_ADDR (0x00)
#define BMP3_REV_ID_ADDR (0x01)
#define BMP3_ERR_REG_ADDR (0x02)
#define BMP3_SENS_STATUS_REG_ADDR (0x03)
#define BMP3_DATA_ADDR (0x04)
@@ -304,6 +306,8 @@ public:
virtual uint32_t get_device_id() const = 0;
virtual uint8_t get_device_address() const = 0;
virtual void set_device_type(uint8_t devtype) = 0;
};
class BMP388 : public I2CSPIDriver<BMP388>
@@ -339,6 +343,9 @@ private:
bool _collect_phase{false};
uint8_t _chip_id{0};
uint8_t _chip_rev_id{0};
void start();
int measure();
int collect(); //get results and publish
src/drivers/barometer/bmp388/bmp388_i2c.cpp
+6
View File
@@ -58,6 +58,7 @@ public:
uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
void set_device_type(uint8_t devtype);
private:
struct calibration_s _cal;
};
@@ -108,3 +109,8 @@ calibration_s *BMP388_I2C::get_calibration(uint8_t addr)
return nullptr;
}
}
void BMP388_I2C::set_device_type(uint8_t devtype)
{
device::Device::set_device_type(devtype);
}
src/drivers/barometer/bmp388/bmp388_spi.cpp
+7
View File
@@ -74,6 +74,8 @@ public:
uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
void set_device_type(uint8_t devtype);
private:
spi_calibration_s _cal;
};
@@ -124,3 +126,8 @@ calibration_s *BMP388_SPI::get_calibration(uint8_t addr)
return nullptr;
}
}
void BMP388_SPI::set_device_type(uint8_t devtype)
{
device::Device::set_device_type(devtype);
}
src/drivers/batt_smbus/batt_smbus.cpp
+3 -3
View File
@@ -166,7 +166,7 @@ void BATT_SMBUS::RunImpl()
if (ret == PX4_OK) {
new_report.capacity = _batt_capacity;
new_report.cycle_count = _cycle_count;
new_report.serial_number = _serial_number;
memcpy(new_report.serial_number, _serial_number, sizeof(new_report.serial_number));
new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
new_report.cell_count = _cell_count;
new_report.state_of_health = _state_of_health;
@@ -392,7 +392,7 @@ int BATT_SMBUS::get_startup_info()
ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);
if (!ret) {
_serial_number = serial_num;
itoa(serial_num, _serial_number, 10);
_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
_cycle_count = cycle_count;
_batt_capacity = (uint16_t)((float)full_cap * _c_mult);
@@ -567,7 +567,7 @@ BATT_SMBUS::custom_method(const BusCLIArguments &cli)
case 1: {
PX4_INFO("The manufacturer name: %s", _manufacturer_name);
PX4_INFO("The manufacturer date: %" PRId16, _manufacture_date);
PX4_INFO("The serial number: %d" PRId16, _serial_number);
PX4_INFO("The serial number: %d" PRId16, atoi(_serial_number));
}
break;
case 2:
src/drivers/batt_smbus/batt_smbus.h
+1 -1
View File
@@ -257,7 +257,7 @@ private:
uint16_t _cycle_count{0};
/** @param _serial_number Serial number register. */
uint16_t _serial_number{0};
char _serial_number[sizeof(battery_status_s::serial_number)] {};
/** @param _crit_thr Critical battery threshold param. */
float _crit_thr{0.f};
src/drivers/camera_capture/camera_capture.cpp
+27 -1
View File
@@ -40,11 +40,15 @@
*/
#include "camera_capture.hpp"
#include <px4_platform_common/events.h>
#include <systemlib/mavlink_log.h>
#include <board_config.h>
#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
using namespace time_literals;
namespace camera_capture
{
CameraCapture *g_camera_capture{nullptr};
@@ -104,6 +108,21 @@ CameraCapture::~CameraCapture()
void
CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
// Maximum acceptable rate is 5kHz
if ((edge_time - _trigger.hrt_edge_time) < 200_us) {
++_trigger_rate_exceeded_counter;
if (_trigger_rate_exceeded_counter > 100) {
// Trigger rate too high, stop future interrupts
up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
_trigger_rate_failure.store(true);
}
} else if (_trigger_rate_exceeded_counter > 0) {
--_trigger_rate_exceeded_counter;
}
_trigger.chan_index = chan_index;
_trigger.hrt_edge_time = edge_time;
_trigger.edge_state = edge_state;
@@ -140,6 +159,13 @@ CameraCapture::publish_trigger()
{
bool publish = false;
if (_trigger_rate_failure.load()) {
mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Camera capture disabled\t");
events::send(events::ID("camera_capture_trigger_rate_exceeded"),
events::Log::Error, "Hardware fault: Camera capture disabled");
_trigger_rate_failure.store(false);
}
camera_trigger_s trigger{};
// MODES 1 and 2 are not fully tested
@@ -244,7 +270,7 @@ CameraCapture::Run()
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
src/drivers/camera_capture/camera_capture.hpp
+5 -1
View File
@@ -96,7 +96,7 @@ public:
static struct work_s _work_publisher;
private:
int _capture_channel = 5; ///< by default, use FMU output 6
int _capture_channel = 6; ///< by default, use FMU output 7
// Publishers
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -134,6 +134,10 @@ private:
hrt_abstime _pps_hrt_timestamp{0};
uint64_t _pps_rtc_timestamp{0};
uint8_t _trigger_rate_exceeded_counter{0};
px4::atomic<bool> _trigger_rate_failure{false};
orb_advert_t _mavlink_log_pub{nullptr};
// Signal capture callback
void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
src/drivers/camera_trigger/camera_trigger.cpp
+7 -7
View File
@@ -508,7 +508,7 @@ CameraTrigger::Run()
int poll_interval_usec = 50000;
vehicle_command_s cmd{};
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
unsigned cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
bool need_ack = false;
// this flag is set when the polling loop is slowed down to allow the camera to power on
@@ -530,7 +530,7 @@ CameraTrigger::Run()
if (now - _last_trigger_timestamp < _min_interval * 1000) {
// triggering too fast, abort
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
if (commandParamToInt(cmd.param7) == 1) {
@@ -543,7 +543,7 @@ CameraTrigger::Run()
_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
@@ -570,7 +570,7 @@ CameraTrigger::Run()
_trigger_enabled = false;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
PX4_DEBUG("received DO_SET_CAM_TRIGG_DIST");
@@ -608,7 +608,7 @@ CameraTrigger::Run()
_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
PX4_DEBUG("received DO_SET_CAM_TRIGG_INTERVAL");
@@ -627,7 +627,7 @@ CameraTrigger::Run()
}
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
PX4_INFO("received OBLIQUE_SURVEY");
@@ -676,7 +676,7 @@ CameraTrigger::Run()
_CAMPOS_num_poses = 0;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
goto unknown_cmd;
src/drivers/cyphal/NodeManager.cpp
+5
View File
@@ -87,6 +87,11 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
}
}
if (i + 1 > sizeof(nodeid_registry) / sizeof(nodeid_registry[0])) {
PX4_ERR("access out of bounds nodeid_registry");
return false;
}
nodeid_registry[i].register_setup = false; // Re-instantiate register setup
nodeid_registry[i].register_index = 0;
nodeid_registry[i].retry_count = 0;
src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp
+1 -1
View File
@@ -96,7 +96,7 @@ public:
bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
bat_status.source = 1; // External
bat_status.capacity = bat_info.full_charge_capacity_wh;
bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
sprintf(bat_status.serial_number, "%d", (int)(bat_info.model_instance_id & 0xFFFF));
bat_status.state_of_health = bat_info.state_of_health_pct; // External
bat_status.id = bat_info.battery_id;
src/drivers/cyphal/Subscribers/udral/Battery.hpp
+1 -1
View File
@@ -163,7 +163,7 @@ public:
bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
bat_status.cycle_count = parameters.cycle_count;
bat_status.serial_number = parameters.unique_id & 0xFFFF;
sprintf(bat_status.serial_number, "%d", (int)(parameters.unique_id & 0xFFFF));
bat_status.state_of_health = parameters.state_of_health_pct;
bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
bat_status.id = 0; //TODO instancing
src/drivers/differential_pressure/Kconfig
+1
View File
@@ -7,6 +7,7 @@ menu "Differential pressure"
select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO
select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO
select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X
select DRIVERS_DIFFERENTIAL_PRESSURE_SDP6X
---help---
Enable default set of differential pressure drivers
rsource "*/Kconfig"
src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp
+1
View File
@@ -53,6 +53,7 @@ extern "C" int sdp3x_main(int argc, char *argv[])
BusCLIArguments cli{true, false};
cli.i2c_address = I2C_ADDRESS_1_SDP3X;
cli.default_i2c_frequency = I2C_SPEED;
cli.support_keep_running = true;
const char *verb = cli.parseDefaultArguments(argc, argv);
src/drivers/differential_pressure/sdp6x/CMakeLists.txt
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2017 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.
#
############################################################################
px4_add_module(
MODULE drivers__differential_pressure__sdp6x
MAIN sdp6x
COMPILE_FLAGS
SRCS
SDP6X.cpp
SDP6X_main.cpp
DEPENDS
px4_work_queue
)
src/drivers/differential_pressure/sdp6x/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_SDP6X
bool "sdp6x"
default n
---help---
Enable support for sdp6x
src/drivers/differential_pressure/sdp6x/SDP6X.cpp
+182
View File
@@ -0,0 +1,182 @@
/****************************************************************************
*
* Copyright (c) 2017-2022 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.
*
****************************************************************************/
/**
* @file SDP6X.hpp
*
* Driver for Sensirion SDP6X Differential Pressure Sensor
*
*/
#include "SDP6X.hpp"
SDP6X::SDP6X(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config)
{
}
SDP6X::~SDP6X()
{
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int SDP6X::init()
{
int ret = I2C::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("I2C::init failed (%i)", ret);
return ret;
}
if (ret == PX4_OK) {
ScheduleNow();
}
return ret;
}
void SDP6X::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
}
int
SDP6X::probe()
{
return !init_sdp6x();
}
bool
SDP6X::init_sdp6x()
{
uint8_t reset_cmd = SDP6X_RESET_CMD;
int ret = transfer(&reset_cmd, 1, nullptr, 0);
if (ret == 0) {
_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31;
// wait until sensor is ready
px4_usleep(20000);
ScheduleNow();
}
return ret == 0;
}
int
SDP6X::collect()
{
perf_begin(_sample_perf);
const hrt_abstime timestamp_sample = hrt_absolute_time();
// this takes about 5ms. But even if we split the write and read with a delay in between,
// the device will still block for several ms. So effectively can't run another device on that bus.
uint8_t measure_cmd = SDP6X_MEASURE_CMD;
uint8_t data[3];
int ret = transfer(&measure_cmd, 1, (uint8_t *)&data[0], sizeof(data));
if (ret != PX4_OK) {
perf_count(_comms_errors);
return ret;
}
// Check the CRC
if (!crc(&data[0], 2, data[2])) {
perf_count(_comms_errors);
return EAGAIN;
}
int16_t P = (((int16_t)data[0]) << 8) | data[1];
float diff_press_pa_raw = static_cast<float>(P) / 60.f; // 500Pa -> scale factor=60
differential_pressure_s differential_pressure{};
differential_pressure.timestamp = hrt_absolute_time();
differential_pressure.timestamp_sample = timestamp_sample;
differential_pressure.error_count = perf_event_count(_comms_errors);
differential_pressure.temperature = 0.f;
differential_pressure.differential_pressure_pa = diff_press_pa_raw;
differential_pressure.device_id = get_device_id();
_differential_pressure_pub.publish(differential_pressure);
perf_end(_sample_perf);
return ret;
}
void
SDP6X::RunImpl()
{
int ret = PX4_ERROR;
// measurement phase
ret = collect();
if (PX4_OK != ret) {
DEVICE_DEBUG("measure error");
}
// schedule a fresh cycle call when the measurement is done
ScheduleDelayed(CONVERSION_INTERVAL);
}
bool SDP6X::crc(const uint8_t data[], unsigned size, uint8_t checksum)
{
uint8_t crc_value = 0;
// calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1)
for (unsigned i = 0; i < size; i++) {
crc_value ^= (data[i]);
for (int bit = 8; bit > 0; --bit) {
if (crc_value & 0x80) {
crc_value = (crc_value << 1) ^ 0x31;
} else {
crc_value = (crc_value << 1);
}
}
}
// verify checksum
return (crc_value == checksum);
}
src/drivers/differential_pressure/sdp6x/SDP6X.hpp
+100
View File
@@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 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.
*
****************************************************************************/
/**
* @file SDP6X.hpp
*
* Driver for Sensirion SDP6X Differential Pressure Sensor
*
* Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Datasheets/Sensirion_Differential_Pressure_Sensors_SDP600Series_Datasheet.pdf
*
* Note:
* - Expected to be used with SDP610 - 500Pa
* - The board needs to set:
* CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=50
* CONFIG_STM32F7_I2C_DYNTIMEO_USECPERBYTE=10000
*/
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/differential_pressure.h>
#define I2C_ADDRESS_SDP6X 0x40
#define SDP6X_RESET_CMD 0xfe
#define SDP6X_MEASURE_CMD 0xf1
#define PATH_SDP6X "/dev/sdp6x"
#define SPD6X_MEAS_RATE 20
#define SDP6X_MEAS_DRIVER_FILTER_FREQ 3.0f
#define CONVERSION_INTERVAL (1000000 / SPD6X_MEAS_RATE) /* microseconds */
static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
class SDP6X : public device::I2C, public I2CSPIDriver<SDP6X>
{
public:
SDP6X(const I2CSPIDriverConfig &config);
~SDP6X() override;
static void print_usage();
void RunImpl();
int init() override;
void print_status() override;
private:
int probe() override;
int collect();
bool init_sdp6x();
/**
* Calculate the CRC8 for the sensor payload data
*/
bool crc(const uint8_t data[], unsigned size, uint8_t checksum);
uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")};
};
src/drivers/differential_pressure/sdp6x/SDP6X_main.cpp
+79
View File
@@ -0,0 +1,79 @@
/****************************************************************************
*
* Copyright (c) 2017-2022 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.
*
****************************************************************************/
#include "SDP6X.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void
SDP6X::print_usage()
{
PRINT_MODULE_USAGE_NAME("sdp6x", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
extern "C" int sdp6x_main(int argc, char *argv[])
{
using ThisDriver = SDP6X;
BusCLIArguments cli{true, false};
cli.i2c_address = I2C_ADDRESS_SDP6X;
cli.default_i2c_frequency = I2C_SPEED;
const char *verb = cli.parseDefaultArguments(argc, argv);
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_SDP31);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}
src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
+3 -3
View File
@@ -175,21 +175,21 @@ int LightwareLaser::init()
case 4:
/* SF11/c (120m 20Hz) */
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_min_distance(0.2f);
_px4_rangefinder.set_max_distance(120.0f);
_conversion_interval = 50000;
break;
case 5:
/* SF/LW20/b (50m 48-388Hz) */
_px4_rangefinder.set_min_distance(0.001f);
_px4_rangefinder.set_min_distance(0.2f);
_px4_rangefinder.set_max_distance(50.0f);
_conversion_interval = 20834;
break;
case 6:
/* SF/LW20/c (100m 48-388Hz) */
_px4_rangefinder.set_min_distance(0.001f);
_px4_rangefinder.set_min_distance(0.2f);
_px4_rangefinder.set_max_distance(100.0f);
_conversion_interval = 20834;
_type = Type::LW20c;
src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
+1 -1
View File
@@ -106,7 +106,7 @@ LightwareLaserSerial::init()
case 5:
/* SF11/c (120m 20Hz) */
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_min_distance(0.2f);
_px4_rangefinder.set_max_distance(120.0f);
_interval = 50000;
break;
src/drivers/distance_sensor/tfmini/tfmini_parser.cpp
+1 -1
View File
@@ -58,7 +58,7 @@ const char *parser_state[] = {
"5_GOT_STRENGTH_L",
"6_GOT_STRENGTH_H",
"7_GOT_PRESERVED",
"8_GOT_QUALITY"
"8_GOT_QUALITY",
"9_GOT_CHECKSUM"
};
#endif
src/drivers/drv_pwm_output.h
+5
View File
@@ -86,6 +86,11 @@ struct pwm_output_values {
*/
#define PWM_MOTOR_OFF 900
/**
* Default value for a servo stop
*/
#define PWM_SERVO_STOP 1500
/**
* Default minimum PWM in us
*/
src/drivers/drv_sensor.h
+1 -1
View File
@@ -140,7 +140,7 @@
#define DRV_FLOW_DEVTYPE_PAW3902 0x6d
#define DRV_FLOW_DEVTYPE_PX4FLOW 0x6e
#define DRV_FLOW_DEVTYPE_PAA3905 0x6f
#define DRV_BARO_DEVTYPE_BMP390 0x6E
#define DRV_DIST_DEVTYPE_LL40LS 0x70
#define DRV_DIST_DEVTYPE_MAPPYDOT 0x71
src/drivers/dshot/DShot.cpp
+2 -2
View File
@@ -624,7 +624,7 @@ void DShot::handle_vehicle_commands()
command_ack.command = vehicle_command.command;
command_ack.target_system = vehicle_command.source_system;
command_ack.target_component = vehicle_command.source_component;
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
if (index != -1) {
PX4_DEBUG("setting command: index: %i type: %i", index, type);
@@ -646,7 +646,7 @@ void DShot::handle_vehicle_commands()
PX4_WARN("unknown command: %i", type);
} else {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
_current_command.motor_mask = 1 << index;
_current_command.num_repetitions = 10;
_current_command.save = true;
src/drivers/gps/devices
+1 -1
Submodule src/drivers/gps/devices updated: 016c37cd1f...fa2177d690
src/drivers/gps/gps.cpp
+14 -3
View File
@@ -103,7 +103,7 @@ struct GPS_Sat_Info {
satellite_info_s _data;
};
static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1760);
static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1940);
class GPS : public ModuleBase<GPS>, public device::Device
@@ -317,7 +317,7 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
set_device_bus(atoi(&c));
set_device_bus(c - 48); // sub 48 to convert char to integer
} else if (_interface == GPSHelper::Interface::SPI) {
set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
@@ -761,9 +761,20 @@ GPS::run()
} else if (gps_ubx_mode == 4) {
ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
} else if (gps_ubx_mode == 5) { // rover with static base on Uart2
ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
}
}
handle = param_find("GPS_UBX_BAUD2");
int32_t f9p_uart2_baudrate = 57600;
if (handle != PARAM_INVALID) {
param_get(handle, &f9p_uart2_baudrate);
}
int32_t gnssSystemsParam = static_cast<int32_t>(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS);
if (_instance == Instance::Main) {
@@ -824,7 +835,7 @@ GPS::run()
/* FALLTHROUGH */
case gps_driver_mode_t::UBX:
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
gps_ubx_dynmodel, heading_offset, ubx_mode);
gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode);
set_device_type(DRV_GPS_DEVTYPE_UBX);
break;
#ifndef CONSTRAINED_FLASH
src/drivers/gps/params.c
+18
View File
@@ -87,6 +87,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
* Select the u-blox configuration setup. Most setups will use the default, including RTK and
* dual GPS without heading.
*
* If rover has RTCM corrections from a static base (or other static correction source) coming in on UART2, then select Mode 5.
* The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output
* heading information, whereas the secondary will act as moving base.
* Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the
@@ -101,6 +102,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
* @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover)
* @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
* @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
* @value 5 Rover with Static Base on UART2 (similar to Default, except coming in on UART2)
*
* @reboot_required true
* @group GPS
@@ -108,6 +110,22 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
/**
* u-blox F9P UART2 Baudrate
*
* Select a baudrate for the F9P's UART2 port.
* In GPS_UBX_MODE 1, 2, and 3, the F9P's UART2 port is configured to send/receive RTCM corrections.
* Set this to 57600 if you want to attach a telemetry radio on UART2.
*
* @min 0
* @unit B/s
*
* @reboot_required true
* @group GPS
*/
PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400);
/**
* Heading/Yaw offset for dual antenna GPS
*
src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
+1
View File
@@ -41,6 +41,7 @@
#pragma once
#include <cstdint>
#include <cstddef>
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
+1
View File
@@ -41,6 +41,7 @@
#pragma once
#include <cstdint>
#include <cstddef>
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
+1
View File
@@ -41,6 +41,7 @@
#pragma once
#include <cstdint>
#include <cstddef>
namespace InvenSense_ICM20948
{
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+28 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 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
@@ -171,21 +171,9 @@ void ICM42688P::RunImpl()
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading from FIFO
_state = STATE::FIFO_READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(100_ms);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
FIFOReset();
// if configure succeeded then reset the FIFO
_state = STATE::FIFO_RESET;
ScheduleDelayed(1_ms);
} else {
// CONFIGURE not complete
@@ -202,6 +190,24 @@ void ICM42688P::RunImpl()
break;
case STATE::FIFO_RESET:
_state = STATE::FIFO_READ;
FIFOReset();
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(100_ms);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
@@ -643,6 +649,9 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
}
}
// avoid scale changes for now as it's not handled correctly down the pipeline (fixed in upstream with 8478d1ea37d2bf)
scale_20bit = true;
if (!scale_20bit) {
// if highres enabled accel data is always 8192 LSB/g
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
@@ -722,6 +731,9 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
gyro.samples++;
}
// avoid scale changes for now as it's not handled correctly down the pipeline (fixed in upstream with 8478d1ea37d2bf)
scale_20bit = true;
if (!scale_20bit) {
// if highres enabled gyro data is always 131 LSB/dps
_px4_gyro.set_scale(math::radians(1.f / 131.f));
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+1
View File
@@ -165,6 +165,7 @@ private:
RESET,
WAIT_FOR_RESET,
CONFIGURE,
FIFO_RESET,
FIFO_READ,
} _state{STATE::RESET};
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
+1
View File
@@ -41,6 +41,7 @@
#pragma once
#include <cstdint>
#include <cstddef>
namespace InvenSense_ICM42688P
{
src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
+204 -9
View File
@@ -35,6 +35,10 @@
using namespace time_literals;
#define IST8310_TEMP_RESOLUTION (1/67.2)
#define IST8310_TEMP_25C_OFFSET 27100
#define IST8310_OTP_SENSITIVITY 330
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
{
return (msb << 8u) | lsb;
@@ -81,18 +85,41 @@ void IST8310::print_status()
perf_print_counter(_reset_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
print_cross_axis_info();
}
void
IST8310::print_cross_axis_info()
{
PX4_INFO("cross axis cal: [%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f]",
(double)_crossaxis_inv[0],
(double)_crossaxis_inv[1],
(double)_crossaxis_inv[2],
(double)_crossaxis_inv[3],
(double)_crossaxis_inv[4],
(double)_crossaxis_inv[5],
(double)_crossaxis_inv[6],
(double)_crossaxis_inv[7],
(double)_crossaxis_inv[8]);
}
int IST8310::probe()
{
const uint8_t WAI = RegisterRead(Register::WAI);
_retries = 1;
if (WAI != Device_ID) {
DEVICE_DEBUG("unexpected WAI 0x%02x", WAI);
return PX4_ERROR;
for (int i = 0; i < 10; i++) {
const uint8_t WAI = RegisterRead(Register::WAI);
if (WAI == Device_ID) {
return PX4_OK;
} else {
PX4_DEBUG("unexpected WAI 0x%02x", WAI);
px4_usleep(10);
}
}
return PX4_OK;
return PX4_ERROR;
}
void IST8310::RunImpl()
@@ -173,21 +200,34 @@ void IST8310::RunImpl()
uint8_t DATAZH;
} buffer{};
struct Temp {
uint8_t TEMPL;
uint8_t TEMPH;
} buffer_temp;
bool success = false;
uint8_t cmd = static_cast<uint8_t>(Register::STAT1);
uint8_t cmd_temp = static_cast<uint8_t>(Register::TEMPL);
if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK
&& transfer(&cmd_temp, 1, (uint8_t *)&buffer_temp, sizeof(buffer_temp)) == PX4_OK) {
if (buffer.STAT1 & STAT1_BIT::DRDY) {
int16_t x = combine(buffer.DATAXH, buffer.DATAXL);
int16_t y = combine(buffer.DATAYH, buffer.DATAYL);
int16_t z = combine(buffer.DATAZH, buffer.DATAZL);
int16_t t = combine(buffer_temp.TEMPH, buffer_temp.TEMPL);
// sensor's frame is +x forward, +y right, +z up
z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
_px4_mag.set_temperature((IST8310_TEMP_25C_OFFSET - t) * IST8310_TEMP_RESOLUTION + 25);
float xf = (float)x * _crossaxis_inv[0] + (float)y * _crossaxis_inv[1] + (float)z * _crossaxis_inv[2];
float yf = (float)x * _crossaxis_inv[3] + (float)y * _crossaxis_inv[4] + (float)z * _crossaxis_inv[5];
float zf = (float)x * _crossaxis_inv[6] + (float)y * _crossaxis_inv[7] + (float)z * _crossaxis_inv[8];
_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
_px4_mag.update(now, x, y, z);
// sensor's frame is +x forward, +y right, +z up
_px4_mag.update(now, xf, yf, -zf);
success = true;
@@ -251,9 +291,158 @@ bool IST8310::Configure()
_px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss
/* Pull & process cross-axis compensation */
cross_axis_comp();
return success;
}
void
IST8310::cross_axis_comp()
{
int ret;
uint8_t bOTPDataFlags;
uint8_t tempBuff[2];
bool crossaxis_enable = 0;
float inv[9] = {0};
short OTPcrossaxis[9] = {0};
uint8_t crossxbuf[6];
uint8_t crossybuf[6];
uint8_t crosszbuf[6];
update_crossaxis();
//check otp_date (Sensors before some date appears to have the matrix stored transposed)
bOTPDataFlags = 0;
ret = read(Register::OTP, (uint8_t *)&tempBuff, sizeof(tempBuff));
if (OK != ret) {
perf_count(_bad_transfer_perf);
return;
}
if (tempBuff[0] == 0xff) {
bOTPDataFlags = 0;
} else if (tempBuff[0] <= 0x12) {
if (tempBuff[1] <= 0x07) {
bOTPDataFlags = 1;
}
}
ret = read(Register::XX_CROSS_L, (uint8_t *)&tempBuff, sizeof(tempBuff));
if (OK != ret) {
perf_count(_bad_transfer_perf);
return;
}
if ((tempBuff[0] == 0xFF) && (tempBuff[1] == 0xFF)) {
crossaxis_enable = 0;
} else {
crossaxis_enable = 1;
}
if (crossaxis_enable == 0) {
update_crossaxis();
return;
} else {
ret = read(Register::XX_CROSS_L, (uint8_t *)&crossxbuf, sizeof(crossxbuf));
if (OK != ret) {
perf_count(_bad_transfer_perf);
return;
}
ret = read(Register::YX_CROSS_L, (uint8_t *)&crossybuf, sizeof(crossybuf));
if (OK != ret) {
perf_count(_bad_transfer_perf);
return;
}
ret = read(Register::ZX_CROSS_L, (uint8_t *)&crosszbuf, sizeof(crosszbuf));
if (OK != ret) {
perf_count(_bad_transfer_perf);
return;
}
if (bOTPDataFlags) {
//before
OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0];
OTPcrossaxis[1] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2];
OTPcrossaxis[2] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4];
OTPcrossaxis[3] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0];
OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2];
OTPcrossaxis[5] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4];
OTPcrossaxis[6] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0];
OTPcrossaxis[7] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2];
OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4];
} else {
//after
OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0];
OTPcrossaxis[3] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2];
OTPcrossaxis[6] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4];
OTPcrossaxis[1] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0];
OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2];
OTPcrossaxis[7] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4];
OTPcrossaxis[2] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0];
OTPcrossaxis[5] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2];
OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4];
}
_crossaxis_det = ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[8]) +
((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[6]) +
((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[7]) -
((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[7]) -
((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[6]) -
((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[8]);
if (_crossaxis_det == 0) {
update_crossaxis();
}
inv[0] = (float)OTPcrossaxis[4] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[5] * (float)OTPcrossaxis[7];
inv[1] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[8];
inv[2] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[5] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[4];
inv[3] = (float)OTPcrossaxis[5] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[3] * (float)OTPcrossaxis[8];
inv[4] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[6];
inv[5] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[3] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[5];
inv[6] = (float)OTPcrossaxis[3] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[4] * (float)OTPcrossaxis[6];
inv[7] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[7];
inv[8] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[4] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[3];
for (int i = 0; i < 9; i++) {
_crossaxis_inv[i] = inv[i] * ((float)IST8310_OTP_SENSITIVITY) / ((float)_crossaxis_det);
}
}
return;
}
void
IST8310::update_crossaxis()
{
*_crossaxis_inv = 1;
*(_crossaxis_inv + 1) = 0;
*(_crossaxis_inv + 2) = 0;
*(_crossaxis_inv + 3) = 0;
*(_crossaxis_inv + 4) = 1;
*(_crossaxis_inv + 5) = 0;
*(_crossaxis_inv + 6) = 0;
*(_crossaxis_inv + 7) = 0;
*(_crossaxis_inv + 8) = 1;
_crossaxis_det = 1;
}
bool IST8310::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;
@@ -281,6 +470,12 @@ uint8_t IST8310::RegisterRead(Register reg)
return buffer;
}
int IST8310::read(Register reg, void *data, unsigned count)
{
const uint8_t cmd = static_cast<uint8_t>(reg);
return transfer(&cmd, 1, (uint8_t *)data, count);
}
void IST8310::RegisterWrite(Register reg, uint8_t value)
{
uint8_t buffer[2] { (uint8_t)reg, value };
src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
+26
View File
@@ -83,6 +83,27 @@ private:
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
int read(Register reg, void *data, unsigned count);
/**
* Pull cross axis compensation from device and calculate compensation matrix.
*
*/
void cross_axis_comp();
/**
* Update cross axis matrix to identity
*
*/
void update_crossaxis();
/**
* Print cross axis matrix
*
*/
void print_cross_axis_info();
PX4Magnetometer _px4_mag;
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -110,4 +131,9 @@ private:
{ Register::AVGCNTL, AVGCNTL_BIT::Y_16TIMES_SET | AVGCNTL_BIT::XZ_16TIMES_SET, AVGCNTL_BIT::Y_16TIMES_CLEAR | AVGCNTL_BIT::XZ_16TIMES_CLEAR },
{ Register::PDCNTL, PDCNTL_BIT::PULSE_NORMAL, 0 },
};
float _crossaxis_inv[9];
int32_t _crossaxis_det;
};
src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp
+2
View File
@@ -82,6 +82,8 @@ enum class Register : uint8_t {
AVGCNTL = 0x41, // Average Control Register
PDCNTL = 0x42, // Pulse Duration Control Register
OTP = 0x99,
XX_CROSS_L = 0x9C, // cross axis xx low byte
XX_CROSS_H = 0x9D, // cross axis xx high byte
XY_CROSS_L = 0x9E, // cross axis xy low byte
src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2021 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.
#
############################################################################
px4_add_module(
MODULE drivers__power_monitor_pm_selector_auterion
MAIN pm_selector_auterion
SRCS
PowerMonitorSelectorAuterion.cpp
DEPENDS
px4_work_queue
)
src/drivers/power_monitor/pm_selector_auterion/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION
bool "pm_selector_auterion"
default n
---help---
Enable support for pm_selector_auterion
src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
+223
View File
@@ -0,0 +1,223 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
/**
* Automatic handling power monitors
*/
#include "PowerMonitorSelectorAuterion.h"
#include "../ina226/ina226.h"
#include <builtin/builtin.h>
#include <sys/wait.h>
PowerMonitorSelectorAuterion::PowerMonitorSelectorAuterion() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
}
PowerMonitorSelectorAuterion::~PowerMonitorSelectorAuterion() = default;
bool PowerMonitorSelectorAuterion::init()
{
int32_t sens_en = 0;
param_get(param_find("SENS_EN_INA226"), &sens_en);
if (sens_en == 1) {
sens_en = 0;
param_set(param_find("SENS_EN_INA226"), &sens_en);
const char *stop_argv[] {"ina226", "stop", NULL};
exec_builtin("ina226", (char **)stop_argv, NULL, 0);
}
ScheduleNow();
return true;
}
void PowerMonitorSelectorAuterion::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
}
actuator_armed_s actuator_armed{};
_actuator_armed_sub.copy(&actuator_armed);
if (actuator_armed.armed) {
exit_and_cleanup();
return;
}
for (uint32_t i = 0U; i < SENSORS_NUMBER; ++i) {
if (!_sensors[i].started) {
int ret_val = ina226_probe(i);
if (ret_val == PX4_OK) {
float current_shunt_value = 0.0f;
param_get(param_find("INA226_SHUNT"), &current_shunt_value);
if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) {
param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value));
}
char bus_number[4] = {0};
itoa(_sensors[i].bus_number, bus_number, 10);
const char *start_argv[] {
_sensors[i].name,
"-X", "-b", bus_number, "-a", _sensors[i].i2c_addr,
"-t", _sensors[i].id, "-q", "start", NULL
};
int status = PX4_ERROR;
int pid = exec_builtin(_sensors[i].name, (char **)start_argv, NULL, 0);
if (pid != -1) {
waitpid(pid, &status, WUNTRACED);
}
if (status == PX4_OK) {
_sensors[i].started = true;
}
}
}
}
ScheduleDelayed(RUN_INTERVAL);
}
int PowerMonitorSelectorAuterion::ina226_probe(uint32_t instance)
{
struct i2c_master_s *i2c = px4_i2cbus_initialize(_sensors[instance].bus_number);
int ret = PX4_ERROR;
if (i2c != nullptr) {
struct i2c_msg_s msgv[2];
uint8_t txdata[1] = {0};
uint8_t rxdata[2] = {0};
msgv[0].frequency = I2C_SPEED_STANDARD;
msgv[0].addr = static_cast<uint16_t>(strtol(_sensors[instance].i2c_addr, NULL, 0));
msgv[0].flags = 0;
msgv[0].buffer = txdata;
msgv[0].length = sizeof(txdata);
msgv[1].frequency = I2C_SPEED_STANDARD;
msgv[1].addr = static_cast<uint16_t>(strtol(_sensors[instance].i2c_addr, NULL, 0));
msgv[1].flags = I2C_M_READ;
msgv[1].buffer = rxdata;
msgv[1].length = sizeof(rxdata);
txdata[0] = {INA226_MFG_ID};
ret = I2C_TRANSFER(i2c, msgv, 2);
uint16_t value = static_cast<uint16_t>(rxdata[1] | rxdata[0] << 8);
if (ret != PX4_OK || value != INA226_MFG_ID_TI) {
ret = PX4_ERROR;
} else {
txdata[0] = {INA226_MFG_DIEID};
ret = I2C_TRANSFER(i2c, msgv, 2);
value = static_cast<uint16_t>(rxdata[1] | rxdata[0] << 8);
if (ret != PX4_OK || value != INA226_MFG_DIE) {
ret = PX4_ERROR;
}
}
px4_i2cbus_uninitialize(i2c);
}
return ret;
}
int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[])
{
PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int PowerMonitorSelectorAuterion::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int PowerMonitorSelectorAuterion::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Driver for starting and auto-detecting different power monitors.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("pm_selector_auterion", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int pm_selector_auterion_main(int argc, char *argv[])
{
return PowerMonitorSelectorAuterion::main(argc, argv);
}
src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h
+118
View File
@@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (C) 2021 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.
*
****************************************************************************/
#pragma once
#include <stdarg.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
using namespace time_literals;
class PowerMonitorSelectorAuterion : public ModuleBase<PowerMonitorSelectorAuterion>, public px4::ScheduledWorkItem
{
public:
PowerMonitorSelectorAuterion();
virtual ~PowerMonitorSelectorAuterion();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
private:
void Run() override;
bool init();
int ina226_probe(uint32_t instance);
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; ///< system armed control topic
struct Sensor {
const char *name;
const char *i2c_addr;
const uint8_t bus_number;
float shunt_value;
bool started;
const char *id;
};
static constexpr int RUN_INTERVAL{500_ms};
static constexpr int SENSORS_NUMBER{4};
Sensor _sensors[SENSORS_NUMBER] = {
{
.name = "ina226",
.i2c_addr = "0x41",
.bus_number = 1,
.shunt_value = 0.0008f,
.started = false,
.id = "1"
},
{
.name = "ina226",
.i2c_addr = "0x40",
.bus_number = 1,
.shunt_value = 0.0005f,
.started = false,
.id = "1"
},
{
.name = "ina226",
.i2c_addr = "0x41",
.bus_number = 2,
.shunt_value = 0.0008f,
.started = false,
.id = "2"
},
{
.name = "ina226",
.i2c_addr = "0x40",
.bus_number = 2,
.shunt_value = 0.0005f,
.started = false,
.id = "2"
}
};
};
src/drivers/pps_capture/Kconfig
+1 -1
View File
@@ -2,4 +2,4 @@ menuconfig DRIVERS_PPS_CAPTURE
bool "pps_capture"
default n
---help---
Enable support for pps_capture
Enable support for pps_capture
src/drivers/pps_capture/PPSCapture.cpp
+23 -1
View File
@@ -42,6 +42,8 @@
#include <px4_arch/io_timer.h>
#include <board_config.h>
#include <parameters/param.h>
#include <px4_platform_common/events.h>
#include <systemlib/mavlink_log.h>
PPSCapture::PPSCapture() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -130,6 +132,7 @@ void PPSCapture::Run()
pps_capture_s pps_capture;
pps_capture.timestamp = _hrt_timestamp;
pps_capture.pps_rate_exceeded_counter = _pps_rate_exceeded_counter;
// GPS UTC time when the GPIO interrupt was triggered
// Last UTC time received from the GPS + elapsed time to the PPS interrupt
uint64_t gps_utc_time = _last_gps_utc_timestamp + (_hrt_timestamp - _last_gps_timestamp);
@@ -141,13 +144,32 @@ void PPSCapture::Run()
pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC;
_pps_capture_pub.publish(pps_capture);
if (_pps_rate_failure.load()) {
mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: GPS PPS disabled\t");
events::send(events::ID("pps_capture_pps_rate_exceeded"),
events::Log::Error, "Hardware fault: GPS PPS disabled");
_pps_rate_failure.store(false);
}
}
int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
{
PPSCapture *instance = static_cast<PPSCapture *>(arg);
instance->_hrt_timestamp = hrt_absolute_time();
hrt_abstime interrupt_time = hrt_absolute_time();
if ((interrupt_time - instance->_hrt_timestamp) < 50_ms) {
++(instance->_pps_rate_exceeded_counter);
if (instance->_pps_rate_exceeded_counter >= 10) {
// Trigger rate too high, stop future interrupts
px4_arch_gpiosetevent(instance->_pps_capture_gpio, false, false, false, nullptr, nullptr);
instance->_pps_rate_failure.store(true);
}
}
instance->_hrt_timestamp = interrupt_time;
instance->ScheduleNow(); // schedule work queue to publish PPS captured time
return PX4_OK;
src/drivers/pps_capture/PPSCapture.hpp
+3 -1
View File
@@ -73,10 +73,12 @@ private:
uint32_t _pps_capture_gpio{0};
uORB::Publication<pps_capture_s> _pps_capture_pub{ORB_ID(pps_capture)};
uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
orb_advert_t _mavlink_log_pub{nullptr};
hrt_abstime _hrt_timestamp{0};
hrt_abstime _last_gps_timestamp{0};
uint64_t _last_gps_utc_timestamp{0};
uint8_t _pps_rate_exceeded_counter{0};
px4::atomic<bool> _pps_rate_failure{false};
};
src/drivers/pwm_out/PWMOut.cpp
+8
View File
@@ -613,6 +613,14 @@ void PWMOut::update_params()
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
if (pwm_default_channel_mask & 1 << i) {
_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
} else {
_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
}
}
} else {
src/drivers/px4io/px4io.cpp
+11 -3
View File
@@ -590,10 +590,10 @@ void PX4IO::Run()
/* publish ACK */
if (dsm_ret == OK) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
}
}
}
@@ -803,6 +803,14 @@ void PX4IO::update_params()
int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
param_set(param_find(str), &pwm_fail_new);
}
} else {
if (pwm_default_channel_mask & 1 << i) {
_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
} else {
_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
}
}
}
}
@@ -1155,7 +1163,7 @@ int PX4IO::io_get_status()
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
for (unsigned i = 0; i < raw_inputs; i++) {
for (unsigned i = 0; (i < raw_inputs) && (i < _max_rc_input); i++) {
status.raw_inputs[i] = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i);
}
src/drivers/rc_input/RCInput.cpp
+3 -3
View File
@@ -334,7 +334,7 @@ void RCInput::Run()
// Check for a pairing command
if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
uint8_t cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
#if defined(SPEKTRUM_POWER)
if (!_rc_scan_locked && !_armed) {
@@ -356,11 +356,11 @@ void RCInput::Run()
bind_spektrum(dsm_bind_pulses);
cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
} else {
cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
#endif // SPEKTRUM_POWER
src/drivers/smart_battery/batmon/batmon.cpp
+1 -1
View File
@@ -192,7 +192,7 @@ void Batmon::RunImpl()
if (ret == PX4_OK) {
new_report.capacity = _batt_capacity;
new_report.cycle_count = _cycle_count;
new_report.serial_number = _serial_number;
sprintf(new_report.serial_number, "%d", _serial_number);
new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
new_report.cell_count = _cell_count;
new_report.state_of_health = _state_of_health;
src/drivers/uavcan/rgbled.cpp
+9 -9
View File
@@ -89,27 +89,27 @@ void UavcanRGBController::periodic_update(const uavcan::TimerEvent &)
// FALLTHROUGH
case led_control_s::COLOR_YELLOW:
cmd.color.red = brightness >> 3;
cmd.color.green = brightness >> 2;
cmd.color.red = (brightness / 2) >> 3;
cmd.color.green = (brightness / 2) >> 2;
cmd.color.blue = 0;
break;
case led_control_s::COLOR_PURPLE:
cmd.color.red = brightness >> 3;
cmd.color.red = (brightness / 2) >> 3;
cmd.color.green = 0;
cmd.color.blue = brightness >> 3;
cmd.color.blue = (brightness / 2) >> 3;
break;
case led_control_s::COLOR_CYAN:
cmd.color.red = 0;
cmd.color.green = brightness >> 2;
cmd.color.blue = brightness >> 3;
cmd.color.green = (brightness / 2) >> 2;
cmd.color.blue = (brightness / 2) >> 3;
break;
case led_control_s::COLOR_WHITE:
cmd.color.red = brightness >> 3;
cmd.color.green = brightness >> 2;
cmd.color.blue = brightness >> 3;
cmd.color.red = (brightness / 3) >> 3;
cmd.color.green = (brightness / 3) >> 2;
cmd.color.blue = (brightness / 3) >> 3;
break;
default: // led_control_s::COLOR_OFF
src/drivers/uavcan/sensors/battery.cpp
+11 -141
View File
@@ -41,10 +41,7 @@ const char *const UavcanBatteryBridge::NAME = "battery";
UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
UavcanSensorBridgeBase("uavcan_battery", ORB_ID(battery_status)),
ModuleParams(nullptr),
_sub_battery(node),
_sub_battery_aux(node),
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
_sub_battery(node)
{
}
@@ -57,149 +54,22 @@ int UavcanBatteryBridge::init()
return res;
}
res = _sub_battery_aux.start(BatteryInfoAuxCbBinder(this, &UavcanBatteryBridge::battery_aux_sub_cb));
if (res < 0) {
PX4_ERR("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
void
UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg)
{
uint8_t instance = 0;
_battery.setConnected(true);
_battery.updateVoltage(msg.voltage);
_battery.updateCurrent(msg.current);
_battery.updateBatteryStatus(hrt_absolute_time());
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
if (battery_status[instance].id == msg.getSrcNodeID().get() || battery_status[instance].id == 0) {
break;
}
}
/* Override data that is expected to arrive from UAVCAN msg*/
battery_status_s battery_status = _battery.getBatteryStatus();
battery_status.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius
itoa(msg.model_instance_id, battery_status.serial_number, 10);
battery_status.id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
if (instance >= battery_status_s::MAX_INSTANCES) {
return;
}
battery_status[instance].timestamp = hrt_absolute_time();
battery_status[instance].voltage_v = msg.voltage;
battery_status[instance].voltage_filtered_v = msg.voltage;
battery_status[instance].current_a = msg.current;
battery_status[instance].current_filtered_a = msg.current;
battery_status[instance].current_average_a = msg.current;
if (battery_aux_support[instance] == false) {
sumDischarged(battery_status[instance].timestamp, battery_status[instance].current_a);
battery_status[instance].discharged_mah = _discharged_mah;
}
battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
// battery_status[instance].scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
battery_status[instance].temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius
// battery_status[instance].cell_count = msg.;
battery_status[instance].connected = true;
battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
// battery_status[instance].priority = msg.;
battery_status[instance].capacity = msg.full_charge_capacity_wh;
battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
// battery_status[instance].cycle_count = msg.;
battery_status[instance].time_remaining_s = NAN;
// battery_status[instance].average_time_to_empty = msg.;
battery_status[instance].serial_number = msg.model_instance_id;
battery_status[instance].id = msg.getSrcNodeID().get();
if (battery_aux_support[instance] == false) {
// Mavlink 2 needs individual cell voltages or cell[0] if cell voltages are not available.
battery_status[instance].voltage_cell_v[0] = msg.voltage;
// Set cell count to 1 so the the battery code in mavlink_messages.cpp copies the values correctly (hack?)
battery_status[instance].cell_count = 1;
}
// battery_status[instance].max_cell_voltage_delta = msg.;
// battery_status[instance].is_powering_off = msg.;
determineWarning(battery_status[instance].remaining);
battery_status[instance].warning = _warning;
if (battery_aux_support[instance] == false) {
publish(msg.getSrcNodeID().get(), &battery_status[instance]);
}
}
void
UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux>
&msg)
{
uint8_t instance = 0;
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
if (battery_status[instance].id == msg.getSrcNodeID().get()) {
break;
}
}
if (instance >= battery_status_s::MAX_INSTANCES) {
return;
}
battery_aux_support[instance] = true;
battery_status[instance].discharged_mah = (battery_status[instance].full_charge_capacity_wh -
battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
1000;
battery_status[instance].cell_count = math::min((uint8_t)msg.voltage_cell.size(), (uint8_t)14);
battery_status[instance].cycle_count = msg.cycle_count;
battery_status[instance].over_discharge_count = msg.over_discharge_count;
battery_status[instance].nominal_voltage = msg.nominal_voltage;
battery_status[instance].time_remaining_s = math::isZero(battery_status[instance].current_a) ? 0 :
(battery_status[instance].remaining_capacity_wh /
battery_status[instance].nominal_voltage / battery_status[instance].current_a * 3600);
battery_status[instance].is_powering_off = msg.is_powering_off;
for (uint8_t i = 0; i < battery_status[instance].cell_count; i++) {
battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i];
}
publish(msg.getSrcNodeID().get(), &battery_status[instance]);
}
void
UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
{
// Not a valid measurement
if (current_a < 0.f) {
// Because the measurement was invalid we need to stop integration
// and re-initialize with the next valid measurement
_last_timestamp = 0;
return;
}
// Ignore first update because we don't know dt.
if (_last_timestamp != 0) {
const float dt = (timestamp - _last_timestamp) / 1e6;
// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
_discharged_mah += _discharged_mah_loop;
}
_last_timestamp = timestamp;
}
void
UavcanBatteryBridge::determineWarning(float remaining)
{
// propagate warning state only if the state is higher, otherwise remain in current warning state
if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
}
publish(msg.getSrcNodeID().get(), &battery_status);
}
src/drivers/uavcan/sensors/battery.hpp
+8 -23
View File
@@ -38,11 +38,13 @@
#pragma once
#include "sensor_bridge.hpp"
#include <uORB/topics/battery_status.h>
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <uORB/topics/battery_status.h>
using namespace time_literals;
class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
{
@@ -58,32 +60,15 @@ public:
private:
void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
void sumDischarged(hrt_abstime timestamp, float current_a);
void determineWarning(float remaining);
typedef uavcan::MethodBinder < UavcanBatteryBridge *,
void (UavcanBatteryBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &) >
BatteryInfoCbBinder;
typedef uavcan::MethodBinder < UavcanBatteryBridge *,
void (UavcanBatteryBridge::*)
(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &) >
BatteryInfoAuxCbBinder;
static constexpr int BATTERY_INDEX = 1;
static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz
Battery _battery{BATTERY_INDEX, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BatteryInfoCbBinder> _sub_battery;
uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BatteryInfoAuxCbBinder> _sub_battery_aux;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr
)
float _discharged_mah = 0.f;
float _discharged_mah_loop = 0.f;
uint8_t _warning;
hrt_abstime _last_timestamp;
battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {};
bool battery_aux_support[battery_status_s::MAX_INSTANCES] {false};
};
src/drivers/uavcan/uavcan_main.cpp
+5 -3
View File
@@ -866,7 +866,7 @@ UavcanNode::Run()
vehicle_command_s cmd{};
_vcmd_sub.copy(&cmd);
uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
acknowledge = true;
@@ -935,7 +935,8 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
if (value != _idle_throttle_when_armed) {
_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
_mixing_interface_esc.mixingOutput().setAllMinValues(
value ? _idle_throttle_when_armed_param : 0);
_idle_throttle_when_armed = value;
}
}
@@ -1200,7 +1201,8 @@ UavcanNode::param_count(uavcan::NodeID node_id)
req.index = 0;
int call_res = _param_getset_client.call(node_id, req);
if (call_res < 0) {
// -ErrInvalidParam is returned when no UAVCAN device is connected to the CAN bus
if ((call_res < 0) && (-uavcan::ErrInvalidParam != call_res)) {
PX4_ERR("couldn't start parameter count: %d", call_res);
} else {
src/drivers/uavcan/uavcan_module.hpp
+1
View File
@@ -50,6 +50,7 @@
// firmware paths
#define UAVCAN_MAX_PATH_LENGTH (128 + 40)
#define UAVCAN_SD_ROOT_PATH "/fs/microsd/"
#define UAVCAN_SD_ALT_PATH "/fs/microsd/ext_autostart/uavcan/"
#define UAVCAN_FIRMWARE_PATH UAVCAN_SD_ROOT_PATH"ufw"
#define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw"
#define UAVCAN_ROMFS_FW_PREFIX "_"
src/drivers/uavcan/uavcan_params.c
+3 -2
View File
@@ -78,9 +78,10 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
/**
* UAVCAN ESC will spin at idle throttle when armed, even if the mixer outputs zero setpoints.
* UAVCAN ESC will spin at specified idle throttle when armed, even if the mixer outputs zero setpoints.
*
* @boolean
* @min 0
* @max 2048
* @reboot_required true
* @group UAVCAN
*/
src/drivers/uavcan/uavcan_servers.cpp
+8
View File
@@ -131,6 +131,14 @@ int UavcanServers::init()
return ret;
}
/*
Check for firmware in the ext_autostart directory, move it to appropriate location on
the SD card, as defined by the APDesc.
These firmware files will be overwritten by binaries located in the root directory,
which will be copied in the next step.
*/
migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ALT_PATH);
/*
Check for firmware in the root directory, move it to appropriate location on
the SD card, as defined by the APDesc.
src/drivers/uavcannode/Publishers/BatteryInfo.hpp
+1 -1
View File
@@ -83,7 +83,7 @@ public:
battery_info.state_of_charge_pct_stdev = battery.max_error;
battery_info.model_instance_id = 0; // TODO: what goes here?
battery_info.model_name = "ARK BMS Rev 0.2";
battery_info.battery_id = battery.serial_number;
battery_info.battery_id = atoi(battery.serial_number);
battery_info.hours_to_full_charge = 0; // TODO: Read BQ40Z80_TIME_TO_FULL
battery_info.state_of_health_pct = battery.state_of_health;
src/examples/ext_update_checker/CMakeLists.txt
+39
View File
@@ -0,0 +1,39 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_module(
MODULE examples__ext_update_checker
MAIN update_checker
SRCS
update_checker.cpp
DEPENDS
)
src/examples/ext_update_checker/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig EXAMPLES_EXT_UPDATE_CHECKER
bool "ext_update_checker"
default n
---help---
Enable support for ext_update_checker
src/examples/ext_update_checker/update_checker.cpp
+111
View File
@@ -0,0 +1,111 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file update_checker.cpp
* fake update checker application to test external component updates preflight checks.
*
* @author Claudio Micheli <claudio@auterion.com>
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <sys/stat.h>
extern "C" __EXPORT int update_checker_main(int argc, char *argv[]);
static constexpr char ext_component_path[] = PX4_STORAGEDIR "/ext_component_updated";
static constexpr int FLASH_DURATION = 15;
static void usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: update_checker {fake_success|fake_fail} \n\n");
}
bool file_exists(const char *filename)
{
struct stat buffer;
return stat(filename, &buffer) == 0;
}
void fake_update(const char flash_result[], uint8_t return_code)
{
FILE *fp = fopen(ext_component_path, "w");
if (fp == nullptr) {
PX4_ERR("Error unable to open file");
return;
}
PX4_INFO("Start fake flashing, duration: %d s", FLASH_DURATION);
sleep(FLASH_DURATION);
PX4_INFO("Fake flashing done %s", flash_result);
fprintf(fp, "%s %d\n", flash_result, return_code);
fclose(fp);
}
int update_checker_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
return 1;
}
if (!file_exists(ext_component_path)) {
if (!strcmp(argv[1], "fake_success")) {
fake_update("SUCCESS", 0);
}
if (!strcmp(argv[1], "fake_fail")) {
fake_update("FAIL", (rand() % 10) + 1);
}
} else {
PX4_INFO("Update file already exists - Nothing to do");
}
PX4_INFO("exiting");
return 0;
}
src/include/containers/Bitset.hpp
+1
View File
@@ -34,6 +34,7 @@
#pragma once
#include <stdint.h>
#include <stddef.h>
namespace px4
{
src/include/visibility.h
+10 -8
View File
@@ -60,14 +60,19 @@
#endif
#define system_exit exit
#define system_clock_gettime clock_gettime
#define system_clock_settime clock_settime
#define system_pthread_cond_timedwait pthread_cond_timedwait
#define system_usleep usleep
#define system_sleep sleep
#ifndef PX4_DISABLE_GCC_POISON
/* exit() is used on NuttX to exit a task. However on Posix, it will exit the
* whole application, so we prevent its use there. There are cases where it
* still needs to be used, thus we remap system_exit to exit.
*/
#define system_exit exit
#if !defined(__PX4_NUTTX)
#include <stdlib.h>
#ifdef __cplusplus
@@ -83,8 +88,6 @@
/* For SITL lockstep we fake the clock, sleeping, and timedwaits
* Therefore, we prefix these syscalls with system_. */
#include <time.h>
#define system_clock_gettime clock_gettime
#define system_clock_settime clock_settime
/* We can't poison clock_settime/clock_gettime because they are
* used in DriverFramework. */
@@ -93,20 +96,17 @@
// We can't include this for NuttX otherwise we get conflicts for read/write
// symbols in cannode.
#endif // !defined(__PX4_NUTTX)
#define system_pthread_cond_timedwait pthread_cond_timedwait
/* We can't poison pthread_cond_timedwait because it seems to be used in the
* <string> include. */
/* We don't poison usleep and sleep because it is used in dependencies
* like uavcan and DriverFramework. */
* like uavcan. */
#if !defined(__PX4_NUTTX)
#include <unistd.h>
// We can't include this for NuttX otherwise we get conflicts for read/write
// symbols in cannode.
#endif // !defined(__PX4_NUTTX)
#define system_usleep usleep
#define system_sleep sleep
/* On NuttX we call clearenv() so we cannot use getenv() and others (see
@@ -123,3 +123,5 @@
* need to get changed. */
#pragma GCC poison getenv setenv putenv
#endif // defined(__PX4_NUTTX)
#endif // PX4_DISABLE_GCC_POISON
src/lib/CMakeLists.txt
+2 -1
View File
@@ -31,6 +31,7 @@
#
############################################################################
add_subdirectory(adsb)
add_subdirectory(airspeed)
add_subdirectory(avoidance)
add_subdirectory(battery)
@@ -48,7 +49,6 @@ add_subdirectory(field_sensor_bias_estimator)
add_subdirectory(geo)
add_subdirectory(hysteresis)
add_subdirectory(l1)
add_subdirectory(landing_slope)
add_subdirectory(led)
add_subdirectory(matrix)
add_subdirectory(mathlib)
@@ -66,6 +66,7 @@ add_subdirectory(slew_rate)
add_subdirectory(systemlib)
add_subdirectory(system_identification)
add_subdirectory(tecs)
add_subdirectory(terrain)
add_subdirectory(terrain_estimation)
add_subdirectory(tunes)
add_subdirectory(version)
src/lib/landing_slope/CMakeLists.txt → src/lib/adsb/CMakeLists.txt
+2 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
# Copyright (c) 2022 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
@@ -31,4 +31,4 @@
#
############################################################################
px4_add_library(landing_slope Landingslope.cpp)
px4_add_library(adsb adsb.cpp)
src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp → src/lib/adsb/adsb.cpp
+1 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (C) 2022 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
@@ -30,20 +30,3 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowMe.cpp
*/
#include "FlightTaskAutoFollowMe.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
bool FlightTaskAutoFollowMe::update()
{
_position_setpoint = _target;
matrix::Vector2f vel_sp = _getTargetVelocityXY();
_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
return true;
}
src/lib/adsb/parameters.c
+228
View File
@@ -0,0 +1,228 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
/**
* ADSB-Out squawk code configuration
*
* This parameter defines the squawk code. Value should be between 0000 and 7777.
*
* @reboot_required false
* @min 0
* @max 7777
* @group ADSB
*
*/
PARAM_DEFINE_INT32(ADSB_SQUAWK, 1200);
/**
* ADSB-Out Ident Configuration
*
* Enable Identification of Position feature
*
* @boolean
* @reboot_required false
* @group ADSB
*/
PARAM_DEFINE_INT32(ADSB_IDENT, 0);
/**
* ADSB-In Vehicle List Size
*
* Change number of targets to track
*
* @min 0
* @max 50
* @reboot_required true
* @group ADSB
*/
PARAM_DEFINE_INT32(ADSB_LIST_MAX, 25);
/**
* ADSB-Out ICAO configuration
*
* Defines the ICAO ID of the vehicle
*
* @reboot_required true
* @min -1
* @max 16777215
* @group ADSB
*
*/
PARAM_DEFINE_INT32(ADSB_ICAO_ID, 1194684);
/**
* ADSB-Out Vehicle Size Configuration
*
* Report the length and width of the vehicle in meters. In most cases, use '1' for the smallest vehicle size.
*
* @reboot_required true
* @min 0
* @max 15
* @group ADSB
*
* @value 0 SizeUnknown
* @value 1 Len15_Wid23
* @value 2 Len25_Wid28
* @value 3 Len25_Wid34
* @value 4 Len35_Wid33
* @value 5 Len35_Wid38
* @value 6 Len45_Wid39
* @value 7 Len45_Wid45
* @value 8 Len55_Wid45
* @value 9 Len55_Wid52
* @value 10 Len65_Wid59
* @value 11 Len65_Wid67
* @value 12 Len75_Wid72
* @value 13 Len75_Wid80
* @value 14 Len85_Wid80
* @value 15 Len85_Wid90
*/
PARAM_DEFINE_INT32(ADSB_LEN_WIDTH, 1);
/**
* ADSB-Out Vehicle Emitter Type
*
* Configure the emitter type of the vehicle.
*
* @reboot_required true
* @min 0
* @max 15
* @group ADSB
*
* @value 0 Unknown
* @value 1 Light
* @value 2 Small
* @value 3 Large
* @value 4 HighVortex
* @value 5 Heavy
* @value 6 Performance
* @value 7 Rotorcraft
* @value 8 RESERVED
* @value 9 Glider
* @value 10 LightAir
* @value 11 Parachute
* @value 12 UltraLight
* @value 13 RESERVED
* @value 14 UAV
* @value 15 Space
* @value 16 RESERVED
* @value 17 EmergencySurf
* @value 18 ServiceSurf
* @value 19 PointObstacle
*/
PARAM_DEFINE_INT32(ADSB_EMIT_TYPE, 14);
/**
* ADSB-Out Vehicle Max Speed
*
* Informs ADSB vehicles of this vehicle's max speed capability
*
* @reboot_required true
* @min 0
* @max 6
* @value 0 UnknownMaxSpeed
* @value 1 75Kts
* @value 2 150Kts
* @value 3 300Kts
* @value 4 600Kts
* @value 5 1200Kts
* @value 6 Over1200Kts
* @group ADSB
*/
PARAM_DEFINE_INT32(ADSB_MAX_SPEED, 0);
/**
* ADSB-In Special ICAO configuration
*
* This vehicle is always tracked. Use 0 to disable.
*
* @reboot_required false
* @min 0
* @max 16777215
* @group ADSB
*
*/
PARAM_DEFINE_INT32(ADSB_ICAO_SPECL, 0);
/**
* ADSB-Out Emergency State
*
* Sets the vehicle emergency state
*
* @reboot_required false
* @min 0
* @max 6
* @value 0 NoEmergency
* @value 1 General
* @value 2 Medical
* @value 3 LowFuel
* @value 4 NoCommunications
* @value 5 Interference
* @value 6 Downed
* @group ADSB
*/
PARAM_DEFINE_INT32(ADSB_EMERGC, 0);
/**
* ADSB-Out GPS Offset lat
*
* Sets GPS lataral offset encoding
*
* @reboot_required false
* @min 0
* @max 7
* @value 0 NoData
* @value 1 LatLeft2M
* @value 2 LatLeft4M
* @value 3 LatLeft6M
* @value 4 LatRight0M
* @value 5 LatRight2M
* @value 6 LatRight4M
* @value 7 LatRight6M
* @group ADSB
*/
PARAM_DEFINE_INT32(ADSB_GPS_OFF_LAT, 0);
/**
* ADSB-Out GPS Offset lon
*
* Sets GPS longitudinal offset encoding
*
* @reboot_required false
* @min 0
* @max 1
* @value 0 NoData
* @value 1 AppliedBySensor
* @group ADSB
*/
PARAM_DEFINE_INT32(ADSB_GPS_OFF_LON, 0);
src/lib/airspeed/airspeed.cpp
+11
View File
@@ -232,3 +232,14 @@ float calc_CAS_from_TAS(float speed_true, float pressure_ambient, float temperat
{
return speed_true * sqrtf(get_air_density(pressure_ambient, temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density)
{
return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density);
}
float calc_calibrated_from_true_airspeed(float speed_true, float air_density)
{
return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
src/lib/airspeed/airspeed.h
+18
View File
@@ -141,6 +141,24 @@ __EXPORT float get_air_density(float static_pressure, float temperature_celsius)
__EXPORT float calc_CAS_from_TAS(float speed_true, float pressure_ambient,
float temperature_celsius);
/**
* @brief Calculates true airspeed from calibrated airspeed and air density
*
* @param speed_calibrated calibrated airspeed [m/s]
* @param air_density air density [kg/m3]
* @return true airspeed [m/s]
*/
__EXPORT float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density);
/**
* @brief Calculates calibrated airspeed from true airspeed and air density
*
* @param speed_true true airspeed [m/s]
* @param air_density air density [kg/m3]
* @return calibrated airspeed [m/s]
*/
__EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density);
__END_DECLS
#endif
src/lib/avoidance/ObstacleAvoidance.cpp
+19 -10
View File
@@ -67,10 +67,11 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get();
const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get();
const bool avoidance_data_timeout =
hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US &&
hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(bezier_msg.control_points[bezier_msg.bezier_order -
1].delta * 1e6f);
const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(
bezier_msg.control_points[bezier_msg.bezier_order -
1].delta * 1e6f);
const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout;
const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
@@ -86,8 +87,12 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
}
if (avoidance_invalid) {
PX4_WARN("Obstacle Avoidance system failed, loitering");
_publishVehicleCmdDoLoiter();
if (_avoidance_activated) {
// Invalid point received: deactivate
PX4_WARN("Obstacle Avoidance system failed, loitering");
_publishVehicleCmdDoLoiter();
_avoidance_activated = false;
}
if (!PX4_ISFINITE(_failsafe_position(0)) || !PX4_ISFINITE(_failsafe_position(1))
|| !PX4_ISFINITE(_failsafe_position(2))) {
@@ -99,13 +104,18 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
vel_sp.setNaN();
yaw_sp = NAN;
yaw_speed_sp = NAN;
// Do nothing further - wait until activation
return;
} else {
} else if (!_avoidance_activated) {
// First setpoint has been received: activate
PX4_INFO("Obstacle Avoidance system activated");
_failsafe_position.setNaN();
_avoidance_activated = true;
}
if (avoidance_point_valid) {
if (avoidance_point_valid && !wp_msg_timeout) {
const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
pos_sp = Vector3f(point0.position);
vel_sp = Vector3f(point0.velocity);
@@ -116,8 +126,7 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
yaw_speed_sp = point0.yaw_speed;
}
} else if (avoidance_bezier_valid) {
} else if (avoidance_bezier_valid && !bezier_msg_timeout) {
float yaw = NAN, yaw_speed = NAN;
_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);
src/lib/avoidance/ObstacleAvoidance.hpp
+2
View File
@@ -130,6 +130,8 @@ protected:
matrix::Vector3f _position = {}; /**< current vehicle position */
matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */
systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
src/lib/battery/battery.cpp
+14 -1
View File
@@ -155,6 +155,7 @@ battery_status_s Battery::getBatteryStatus()
battery_status.id = static_cast<uint8_t>(_index);
battery_status.warning = _warning;
battery_status.timestamp = hrt_absolute_time();
battery_status.faults = determineFaults();
return battery_status;
}
@@ -215,7 +216,7 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a
cell_voltage += throttle * _params.v_load_drop;
}
_state_of_charge_volt_based = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
_state_of_charge_volt_based = math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
// choose which quantity we're using for final reporting
if (_params.capacity > 0.f && _battery_initialized) {
@@ -251,6 +252,18 @@ uint8_t Battery::determineWarning(float state_of_charge)
}
}
uint16_t Battery::determineFaults()
{
uint16_t faults{0};
if ((_params.n_cells > 0)
&& (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) {
faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES);
}
return faults;
}
void Battery::computeScale()
{
const float voltage_range = (_params.v_charged - _params.v_empty);
src/lib/battery/battery.h
+1
View File
@@ -146,6 +146,7 @@ private:
void sumDischarged(const hrt_abstime &timestamp, float current_a);
void estimateStateOfCharge(const float voltage_v, const float current_a);
uint8_t determineWarning(float state_of_charge);
uint16_t determineFaults();
void computeScale();
float computeRemainingTime(float current_a);
src/lib/cdev/posix/cdev_platform.cpp
+2
View File
@@ -40,6 +40,8 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <stdlib.h>
const cdev::px4_file_operations_t cdev::CDev::fops = {};
pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
src/lib/conversion/rotation.h
+6
View File
@@ -92,6 +92,9 @@ enum Rotation : uint8_t {
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_ROLL_270_YAW_225 = 41,
ROTATION_ROLL_270_YAW_315 = 42,
ROTATION_PITCH_90_YAW_45 = 43,
ROTATION_MAX
};
@@ -143,6 +146,9 @@ static constexpr rot_lookup_t rot_lookup[ROTATION_MAX] = {
{ 90, 68, 293 },
{ 0, 315, 0 },
{ 90, 315, 0 },
{270, 0, 225 },
{270, 0, 315 },
{ 0, 90, 45 },
};
/**
src/lib/drivers/smbus_sbs/SBS.hpp
+1 -1
View File
@@ -288,7 +288,7 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
// Read serial number.
ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, result);
data.serial_number = result;
sprintf(data.serial_number, "%d", result);
// Read battery temperature and covert to Celsius.
ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
src/lib/events/enums.json
+4
View File
@@ -243,6 +243,10 @@
"name": "auto_vtol_takeoff",
"description": "Vtol Takeoff"
},
"16": {
"name": "init",
"description": "Initialization"
},
"255": {
"name": "unknown",
"description": "[Unknown]"
src/lib/l1/ECL_L1_Pos_Controller.cpp
+13 -7
View File
@@ -213,10 +213,12 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
void
ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
int8_t loiter_direction, const Vector2f &ground_speed_vector)
const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
{
_has_guidance_updated = true;
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
/* the complete guidance logic in this section was proposed by [2] */
/* calculate the gains for the PD loop (circle tracking) */
@@ -274,7 +276,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
/* calculate velocity on circle / along tangent */
float tangent_vel = xtrack_vel_center * loiter_direction;
float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
/* prevent PD output from turning the wrong way when in circle mode */
const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
@@ -288,7 +290,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
(radius + xtrack_err_circle));
/* add PD control on circle and centripetal acceleration for total circle command */
float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
lateral_accel_sp_circle_centripetal);
/*
* Switch between circle (loiter) and capture (towards waypoint center) mode when
@@ -296,8 +299,10 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
*/
// XXX check switch over
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
&& xtrack_err_circle > 0.0f)
||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
_lateral_accel = lateral_accel_sp_center;
_circle_mode = false;
/* angle between requested and current velocity vector */
@@ -309,8 +314,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
_lateral_accel = lateral_accel_sp_circle;
_circle_mode = true;
_bearing_error = 0.0f;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
// once loitering, the bearing is tangentially to the loiter,
// so subtract PI/2 from the bearing from current position to loiter center
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) - M_PI_2_F;
}
update_roll_setpoint();
src/lib/l1/ECL_L1_Pos_Controller.hpp
+1 -1
View File
@@ -154,7 +154,7 @@ public:
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
/**
* Navigate on a fixed bearing.
src/lib/landing_slope/Landingslope.cpp
-132
View File
@@ -1,132 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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.
*
****************************************************************************/
/*
* @file landingslope.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "Landingslope.hpp"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
void
Landingslope::update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new)
{
_landing_slope_angle_rad = landing_slope_angle_rad_new;
_flare_relative_alt = flare_relative_alt_new;
_motor_lim_relative_alt = motor_lim_relative_alt_new;
_H1_virt = H1_virt_new;
calculateSlopeValues();
}
void
Landingslope::calculateSlopeValues()
{
_H0 = _flare_relative_alt + _H1_virt;
_d1 = _flare_relative_alt / tanf(_landing_slope_angle_rad);
_flare_constant = (_H0 * _d1) / _flare_relative_alt;
_flare_length = -logf(_H1_virt / _H0) * _flare_constant;
_horizontal_slope_displacement = (_flare_length - _d1);
}
float
Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
{
return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement,
_landing_slope_angle_rad);
}
float
Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
return getLandingSlopeRelativeAltitude(wp_landing_distance);
}
return 0.0f;
}
float
Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt;
}
return 0.0f;
}
/**
*
* @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
float landing_slope_angle_rad)
{
// flare_relative_alt is negative
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad);
}
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement,
landing_slope_angle_rad) + wp_landing_altitude;
}
/**
*
* @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
*/
float Landingslope::getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
}
src/lib/landing_slope/Landingslope.hpp
-118
View File
@@ -1,118 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 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.
*
****************************************************************************/
/*
* @file landingslope.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LANDINGSLOPE_H_
#define LANDINGSLOPE_H_
#include <math.h>
class Landingslope
{
private:
/* see Documentation/fw_landing.png for a plot of the landing slope */
float _landing_slope_angle_rad{0.0f}; /**< phi in the plot */
float _flare_relative_alt{0.0f}; /**< h_flare,rel in the plot */
float _motor_lim_relative_alt{0.0f};
float _H1_virt{0.0f}; /**< H1 in the plot */
float _H0{0.0f}; /**< h_flare,rel + H1 in the plot */
float _d1{0.0f}; /**< d1 in the plot */
float _flare_constant{0.0f};
float _flare_length{0.0f}; /**< d1 + delta d in the plot */
float _horizontal_slope_displacement{0.0f}; /**< delta d in the plot */
void calculateSlopeValues();
public:
Landingslope() = default;
~Landingslope() = default;
/**
*
* @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeRelativeAltitude(float wp_landing_distance);
/**
*
* @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
* Performs check if aircraft is in front of waypoint to avoid climbout
*/
float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
float bearing_airplane_currwp);
/**
*
* @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
float landing_slope_angle_rad);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
float horizontal_slope_displacement, float landing_slope_angle_rad);
/**
*
* @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
*/
static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
float horizontal_slope_displacement, float landing_slope_angle_rad);
float getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
float bearing_airplane_currwp);
void update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new);
float landing_slope_angle_rad() { return _landing_slope_angle_rad; }
float flare_relative_alt() { return _flare_relative_alt; }
float motor_lim_relative_alt() { return _motor_lim_relative_alt; }
float flare_length() { return _flare_length; }
float horizontal_slope_displacement() { return _horizontal_slope_displacement; }
};
#endif /* LANDINGSLOPE_H_ */
src/lib/mathlib/math/Functions.hpp
+37 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-2022 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
@@ -149,7 +149,7 @@ const T expo_deadzone(const T &value, const T &e, const T &dz)
* x_low x_high
*/
template<typename T>
const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
const T interpolate(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
{
if (value <= x_low) {
return y_low;
@@ -166,28 +166,47 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low,
}
/*
* Constant, linear, linear, constant function with the three corner points as parameters
* y_high -------
* /
* /
* y_middle /
* /
* Constant, piecewise linear, constant function with 1/N size intervalls and N corner points as parameters
* y[N-1] -------
* /
* /
* y[1] /
* /
* /
* y_low -------
* x_low x_middle x_high
* /
* y[0] -------
* 0 1/(N-1) 2/(N-1) ... 1
*/
template<typename T>
const T gradual3(const T &value,
const T &x_low, const T &x_middle, const T &x_high,
const T &y_low, const T &y_middle, const T &y_high)
template<typename T, size_t N>
const T interpolateN(const T &value, const T(&y)[N])
{
if (value < x_middle) {
return gradual(value, x_low, x_middle, y_low, y_middle);
size_t index = constrain((int)(value * (N - 1)), 0, (int)(N - 2));
return interpolate(value, (T)index / (T)(N - 1), (T)(index + 1) / (T)(N - 1), y[index], y[index + 1]);
}
} else {
return gradual(value, x_middle, x_high, y_middle, y_high);
/*
* Constant, piecewise linear, constant function with N corner points as parameters
* y[N-1] -------
* /
* /
* y[1] /
* /
* /
* /
* y[0] -------
* x[0] x[1] ... x[N-1]
* Note: x[N] corner coordinates have to be sorted in ascending order
*/
template<typename T, size_t N>
const T interpolateNXY(const T &value, const T(&x)[N], const T(&y)[N])
{
size_t index = 0;
while ((value > x[index + 1]) && (index < (N - 2))) {
index++;
}
return interpolate(value, x[index], x[index + 1], y[index], y[index + 1]);
}
/*
src/lib/mathlib/math/FunctionsTest.cpp
+39 -45
View File
@@ -140,63 +140,57 @@ TEST(FunctionsTest, expo_deadzone)
EXPECT_FLOAT_EQ(expo_deadzone(-.5f, .9f, .4f), -.020833336f);
}
TEST(FunctionsTest, gradual)
TEST(FunctionsTest, interpolate)
{
// factor of *2, offset +1
EXPECT_FLOAT_EQ(gradual(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual(0.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(gradual(.5f, 0.f, 1.f, 1.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(gradual(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(gradual(1.f, 0.f, 1.f, 1.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(gradual(12.f, 0.f, 1.f, 1.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolate(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(interpolate(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(interpolate(.5f, 0.f, 1.f, 1.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(interpolate(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(interpolate(1.f, 0.f, 1.f, 1.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolate(12.f, 0.f, 1.f, 1.f, 3.f), 3.f);
// factor of *2, offset +3
EXPECT_FLOAT_EQ(gradual(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(gradual(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f);
EXPECT_FLOAT_EQ(gradual(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f);
EXPECT_FLOAT_EQ(gradual(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f);
EXPECT_FLOAT_EQ(gradual(2.f, 1.f, 2.f, 4.f, 6.f), 6.f);
EXPECT_FLOAT_EQ(gradual(12.f, 1.f, 2.f, 4.f, 6.f), 6.f);
EXPECT_FLOAT_EQ(interpolate(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f);
EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f);
EXPECT_FLOAT_EQ(interpolate(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f);
EXPECT_FLOAT_EQ(interpolate(2.f, 1.f, 2.f, 4.f, 6.f), 6.f);
EXPECT_FLOAT_EQ(interpolate(12.f, 1.f, 2.f, 4.f, 6.f), 6.f);
// corner case when x_low == x_high == value
EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
// corner case when x_low > x_high
EXPECT_FLOAT_EQ(gradual(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f);
// corner case when y_low == y_high == value
EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
// corner case when y_low > y_high
EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 6.f, 4.f), 5.f);
// corner case when x_low == x_high == y_low == y_high == 0.f
EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 0.f, 0.f, 0.f), 0.f);
}
TEST(FunctionsTest, gradual3)
TEST(FunctionsTest, interpolateNXY)
{
float x[3] = {0.f, .5f, 1.5f};
float y[3] = {1.f, 2.f, 3.f};
// factor of *2, offset +1
EXPECT_FLOAT_EQ(gradual3(-12.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual3(0.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual3(.25f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(gradual3(.5f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(gradual3(.75f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.25f);
EXPECT_FLOAT_EQ(gradual3(1.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(gradual3(1.25f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.75f);
EXPECT_FLOAT_EQ(gradual3(1.5f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(gradual3(12.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolateNXY(-12.f, x, y), 1.f);
EXPECT_FLOAT_EQ(interpolateNXY(0.f, x, y), 1.f);
EXPECT_FLOAT_EQ(interpolateNXY(.25f, x, y), 1.5f);
EXPECT_FLOAT_EQ(interpolateNXY(.5f, x, y), 2.f);
EXPECT_FLOAT_EQ(interpolateNXY(.75f, x, y), 2.25f);
EXPECT_FLOAT_EQ(interpolateNXY(1.f, x, y), 2.5f);
EXPECT_FLOAT_EQ(interpolateNXY(1.25f, x, y), 2.75f);
EXPECT_FLOAT_EQ(interpolateNXY(1.5f, x, y), 3.f);
EXPECT_FLOAT_EQ(interpolateNXY(12.f, x, y), 3.f);
}
TEST(FunctionsTest, sqrt_linear)
src/lib/mathlib/math/WelfordMean.hpp
+11 -8
View File
@@ -42,22 +42,25 @@
namespace math
{
template<typename T>
template <typename Type, size_t N>
class WelfordMean
{
public:
// For a new value, compute the new count, new mean, the new M2.
void update(const T &new_value)
void update(const matrix::Vector<Type, N> &new_value)
{
_count++;
// mean accumulates the mean of the entire dataset
const T delta{new_value - _mean};
const matrix::Vector<Type, N> delta{new_value - _mean};
_mean += delta / _count;
// M2 aggregates the squared distance from the mean
// count aggregates the number of samples seen so far
_M2 += delta.emult(new_value - _mean);
// protect against floating point precision causing negative variances
_M2 = matrix::max(_M2, {});
}
bool valid() const { return _count > 2; }
@@ -71,12 +74,12 @@ public:
}
// Retrieve the mean, variance and sample variance
T mean() const { return _mean; }
T variance() const { return _M2 / _count; }
T sample_variance() const { return _M2 / (_count - 1); }
matrix::Vector<Type, N> mean() const { return _mean; }
matrix::Vector<Type, N> variance() const { return _M2 / _count; }
matrix::Vector<Type, N> sample_variance() const { return _M2 / (_count - 1); }
private:
T _mean{};
T _M2{};
matrix::Vector<Type, N> _mean{};
matrix::Vector<Type, N> _M2{};
unsigned _count{0};
};
src/lib/mathlib/math/WelfordMeanTest.cpp
+1 -1
View File
@@ -45,7 +45,7 @@ TEST(WelfordMeanTest, NoisySignal)
std::normal_distribution<float> standard_normal_distribution{0.f, std_dev};
std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
random_generator.seed(42);
WelfordMean<Vector3f> welford{};
WelfordMean<float, 3> welford{};
for (int i = 0; i < 50; i++) {
const float noisy_value = standard_normal_distribution(random_generator);
src/lib/matrix/matrix/Vector2.hpp
+26
View File
@@ -71,6 +71,32 @@ public:
return (*this).cross(b);
}
/**
* @brief rotate vector
*
* @param[in] angle [rad] The angle around the positive out of plane axis.
*/
void rotate(const Type angle)
{
Vector2 &v(*this);
Vector2 rotated;
Type sin_angle = Type(sin(angle));
Type cos_angle = Type(cos(angle));
rotated(0) = cos_angle * v(0) - sin_angle * v(1);
rotated(1) = sin_angle * v(0) + cos_angle * v(1);
v = rotated;
}
/**
* @brief Transform vector in new coordinate system
*
* @param[in] angle [rad] The rotation of new coordinate system around the positive out of plane axis.
*/
void transform(const Type angle)
{
this->rotate(-angle);
}
};
src/lib/matrix/test/MatrixVector2Test.cpp
+17
View File
@@ -34,6 +34,7 @@
#include <gtest/gtest.h>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
using namespace matrix;
@@ -64,4 +65,20 @@ TEST(MatrixVector2Test, Vector2)
Vector2f h(g);
EXPECT_FLOAT_EQ(h(0), 1.23f);
EXPECT_FLOAT_EQ(h(1), 423.4f);
// rotate 2D vector
Vector2f i(1.0f, 0.0f);
i.rotate(math::radians(45.0));
EXPECT_FLOAT_EQ(i(0), i(1));
i.rotate(math::radians(45.0));
EXPECT_FLOAT_EQ(i(0), 0.f);
EXPECT_FLOAT_EQ(i(1), 1.f);
// transform vector
Vector2f j(1.0f, 0.0f);
j.transform(math::radians(-45.0));
EXPECT_FLOAT_EQ(j(0), j(1));
j.transform(math::radians(-45.0));
EXPECT_FLOAT_EQ(j(0), 0.f);
EXPECT_FLOAT_EQ(j(1), 1.f);
}
src/lib/mixer_module/CMakeLists.txt
+13 -2
View File
@@ -44,14 +44,25 @@ add_custom_command(OUTPUT ${functions_header}
add_custom_target(output_functions_header DEPENDS ${functions_header})
px4_add_library(mixer_module
functions/FunctionActuatorSet.hpp
functions/FunctionConstantMax.hpp
functions/FunctionConstantMin.hpp
functions/FunctionGimbal.hpp
functions/FunctionLandingGear.hpp
functions/FunctionManualRC.hpp
functions/FunctionMotors.hpp
functions/FunctionParachute.hpp
functions/FunctionServos.hpp
actuator_test.cpp
actuator_test.hpp
mixer_module.cpp
functions.cpp
mixer_module.hpp
)
add_dependencies(mixer_module output_functions_header)
target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module mixer)
src/lib/mixer_module/actuator_test.cpp
+2
View File
@@ -33,6 +33,8 @@
#include "actuator_test.hpp"
#include "functions/FunctionMotors.hpp"
using namespace time_literals;
ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS])
src/lib/mixer_module/actuator_test.hpp
+1 -1
View File
@@ -33,7 +33,7 @@
#pragma once
#include "functions.hpp"
#include <mixer_module/output_functions.hpp>
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_test.h>
src/lib/mixer_module/functions.cpp
-135
View File
@@ -1,135 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
#include "functions.hpp"
FunctionMotors::FunctionMotors(const Context &context)
: _topic(&context.work_item, ORB_ID(actuator_motors)),
_thrust_factor(context.thrust_factor)
{
for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
_data.control[i] = NAN;
}
}
void FunctionMotors::update()
{
if (_topic.update(&_data)) {
updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
}
}
FunctionServos::FunctionServos(const Context &context)
: _topic(&context.work_item, ORB_ID(actuator_servos))
{
for (int i = 0; i < actuator_servos_s::NUM_CONTROLS; ++i) {
_data.control[i] = NAN;
}
}
FunctionActuatorSet::FunctionActuatorSet()
{
for (int i = 0; i < max_num_actuators; ++i) {
_data[i] = NAN;
}
}
void FunctionActuatorSet::update()
{
vehicle_command_s vehicle_command;
while (_topic.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) {
int index = (int)(vehicle_command.param7 + 0.5f);
if (index == 0) {
_data[0] = vehicle_command.param1;
_data[1] = vehicle_command.param2;
_data[2] = vehicle_command.param3;
_data[3] = vehicle_command.param4;
_data[4] = vehicle_command.param5;
_data[5] = vehicle_command.param6;
}
}
}
}
void FunctionLandingGear::update()
{
landing_gear_s landing_gear;
if (_topic.update(&landing_gear)) {
if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) {
_data = -1.f;
} else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) {
_data = 1.f;
}
}
}
FunctionManualRC::FunctionManualRC()
{
for (int i = 0; i < num_data_points; ++i) {
_data[i] = NAN;
}
}
void FunctionManualRC::update()
{
manual_control_setpoint_s manual_control_setpoint;
if (_topic.update(&manual_control_setpoint)) {
_data[0] = manual_control_setpoint.y; // roll
_data[1] = manual_control_setpoint.x; // pitch
_data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
_data[3] = manual_control_setpoint.r; // yaw
_data[4] = manual_control_setpoint.flaps;
_data[5] = manual_control_setpoint.aux1;
_data[6] = manual_control_setpoint.aux2;
_data[7] = manual_control_setpoint.aux3;
_data[8] = manual_control_setpoint.aux4;
_data[9] = manual_control_setpoint.aux5;
_data[10] = manual_control_setpoint.aux6;
}
}
void FunctionGimbal::update()
{
actuator_controls_s actuator_controls;
if (_topic.update(&actuator_controls)) {
_data[0] = actuator_controls.control[0];
_data[1] = actuator_controls.control[1];
_data[2] = actuator_controls.control[2];
}
}
src/lib/mixer_module/functions.hpp
-295
View File
@@ -1,295 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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.
*
****************************************************************************/
#pragma once
#include <limits.h>
#include <mixer_module/output_functions.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
class FunctionProviderBase
{
public:
struct Context {
px4::WorkItem &work_item;
const float &thrust_factor;
};
FunctionProviderBase() = default;
virtual ~FunctionProviderBase() = default;
virtual void update() = 0;
/**
* Get the current output value for a given function
* @return NAN (=disarmed) or value in range [-1, 1]
*/
virtual float value(OutputFunction func) = 0;
virtual float defaultFailsafeValue(OutputFunction func) const { return NAN; }
virtual bool allowPrearmControl() const { return true; }
virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; }
virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; }
/**
* Check whether the output (motor) is configured to be reversible
*/
virtual bool reversible(OutputFunction func) const { return false; }
};
/**
* Functions: Constant_Min
*/
class FunctionConstantMin : public FunctionProviderBase
{
public:
static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); }
float value(OutputFunction func) override { return -1.f; }
void update() override { }
float defaultFailsafeValue(OutputFunction func) const override { return -1.f; }
};
/**
* Functions: Constant_Max
*/
class FunctionConstantMax : public FunctionProviderBase
{
public:
static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); }
float value(OutputFunction func) override { return 1.f; }
void update() override { }
float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
};
/**
* Functions: Motor1 ... MotorMax
*/
class FunctionMotors : public FunctionProviderBase
{
public:
static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
"Unexpected num motors");
static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
FunctionMotors(const Context &context);
static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }
void update() override;
float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; }
bool allowPrearmControl() const override { return false; }
uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values);
bool reversible(OutputFunction func) const override
{ return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); }
private:
uORB::SubscriptionCallbackWorkItem _topic;
actuator_motors_s _data{};
const float &_thrust_factor;
};
void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
{
if (thrust_factor > 0.f && thrust_factor <= 1.f) {
// thrust factor
// rel_thrust = factor * x^2 + (1-factor) * x,
const float a = thrust_factor;
const float b = (1.f - thrust_factor);
// don't recompute for all values (ax^2+bx+c=0)
const float tmp1 = b / (2.f * a);
const float tmp2 = b * b / (4.f * a * a);
for (int i = 0; i < num_values; ++i) {
float control = values[i];
if (control > 0.f) {
values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
} else if (control < -0.f) {
values[i] = tmp1 - sqrtf(tmp2 - (control / a));
} else {
values[i] = 0.f;
}
}
}
for (int i = 0; i < num_values; ++i) {
if ((reversible & (1u << i)) == 0) {
if (values[i] < -FLT_EPSILON) {
values[i] = NAN;
} else {
// remap from [0, 1] to [-1, 1]
values[i] = values[i] * 2.f - 1.f;
}
}
}
}
/**
* Functions: Servo1 ... ServoMax
*/
class FunctionServos : public FunctionProviderBase
{
public:
static_assert(actuator_servos_s::NUM_CONTROLS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1,
"Unexpected num servos");
FunctionServos(const Context &context);
static FunctionProviderBase *allocate(const Context &context) { return new FunctionServos(context); }
void update() override { _topic.update(&_data); }
float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Servo1]; }
uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
float defaultFailsafeValue(OutputFunction func) const override { return 0.f; }
private:
uORB::SubscriptionCallbackWorkItem _topic;
actuator_servos_s _data{};
};
/**
* Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
*/
class FunctionActuatorSet : public FunctionProviderBase
{
public:
FunctionActuatorSet();
static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); }
void update() override;
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }
private:
static constexpr int max_num_actuators = 6;
uORB::Subscription _topic{ORB_ID(vehicle_command)};
float _data[max_num_actuators];
};
/**
* Functions: Landing_Gear
*/
class FunctionLandingGear : public FunctionProviderBase
{
public:
FunctionLandingGear() = default;
static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); }
void update() override;
float value(OutputFunction func) override { return _data; }
private:
uORB::Subscription _topic{ORB_ID(landing_gear)};
float _data{-1.f};
};
/**
* Functions: Parachute
*/
class FunctionParachute : public FunctionProviderBase
{
public:
FunctionParachute() = default;
static FunctionProviderBase *allocate(const Context &context) { return new FunctionParachute(); }
void update() override {}
float value(OutputFunction func) override { return -1.f; }
float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
};
/**
* Functions: RC_Roll .. RCAUX_Max
*/
class FunctionManualRC : public FunctionProviderBase
{
public:
FunctionManualRC();
static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); }
void update() override;
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; }
private:
static constexpr int num_data_points = 11;
static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1,
"number of functions mismatch");
uORB::Subscription _topic{ORB_ID(manual_control_setpoint)};
float _data[num_data_points];
};
/**
* Functions: Gimbal_Roll .. Gimbal_Yaw
*/
class FunctionGimbal : public FunctionProviderBase
{
public:
FunctionGimbal() = default;
static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); }
void update() override;
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; }
private:
uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
float _data[3] { NAN, NAN, NAN };
};
src/lib/mixer_module/functions/FunctionActuatorSet.hpp
+82
View File
@@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
#include <uORB/topics/vehicle_command.h>
/**
* Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
*/
class FunctionActuatorSet : public FunctionProviderBase
{
public:
FunctionActuatorSet()
{
for (int i = 0; i < max_num_actuators; ++i) {
_data[i] = NAN;
}
}
static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); }
void update() override
{
vehicle_command_s vehicle_command;
while (_topic.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) {
int index = (int)(vehicle_command.param7 + 0.5f);
if (index == 0) {
_data[0] = vehicle_command.param1;
_data[1] = vehicle_command.param2;
_data[2] = vehicle_command.param3;
_data[3] = vehicle_command.param4;
_data[4] = vehicle_command.param5;
_data[5] = vehicle_command.param6;
}
}
}
}
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }
private:
static constexpr int max_num_actuators = 6;
uORB::Subscription _topic{ORB_ID(vehicle_command)};
float _data[max_num_actuators] {};
};
src/lib/mixer_module/functions/FunctionConstantMax.hpp
+50
View File
@@ -0,0 +1,50 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
/**
* Functions: Constant_Max
*/
class FunctionConstantMax : public FunctionProviderBase
{
public:
static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); }
float value(OutputFunction func) override { return 1.f; }
void update() override { }
float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
};
src/lib/mixer_module/functions/FunctionConstantMin.hpp
+50
View File
@@ -0,0 +1,50 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
/**
* Functions: Constant_Min
*/
class FunctionConstantMin : public FunctionProviderBase
{
public:
static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); }
float value(OutputFunction func) override { return -1.f; }
void update() override { }
float defaultFailsafeValue(OutputFunction func) const override { return -1.f; }
};
src/lib/mixer_module/functions/FunctionGimbal.hpp
+65
View File
@@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
#include <uORB/topics/actuator_controls.h>
/**
* Functions: Gimbal_Roll .. Gimbal_Yaw
*/
class FunctionGimbal : public FunctionProviderBase
{
public:
FunctionGimbal() = default;
static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); }
void update() override
{
actuator_controls_s actuator_controls;
if (_topic.update(&actuator_controls)) {
_data[0] = actuator_controls.control[0];
_data[1] = actuator_controls.control[1];
_data[2] = actuator_controls.control[2];
}
}
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; }
private:
uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
float _data[3] { NAN, NAN, NAN };
};
src/lib/mixer_module/functions/FunctionGripper.hpp
+69
View File
@@ -0,0 +1,69 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
#include <uORB/topics/gripper.h>
/**
* @brief Function: Gripper (Used for actuating a Gripper)
*/
class FunctionGripper : public FunctionProviderBase
{
public:
FunctionGripper() = default;
static FunctionProviderBase *allocate(const Context &context) { return new FunctionGripper(); }
void update() override
{
gripper_s gripper;
if (_gripper_sub.update(&gripper)) {
if (gripper.command == gripper_s::COMMAND_RELEASE) {
_data = -1.f; // Minimum command for release
} else if (gripper.command == gripper_s::COMMAND_GRAB) {
_data = 1.f; // Maximum command for grab
}
}
}
float value(OutputFunction func) override { return _data; }
private:
uORB::Subscription _gripper_sub{ORB_ID(gripper)};
float _data{-1.f};
};
src/lib/mixer_module/functions/FunctionLandingGear.hpp
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
#include <uORB/topics/landing_gear.h>
/**
* Functions: Landing_Gear
*/
class FunctionLandingGear : public FunctionProviderBase
{
public:
FunctionLandingGear() = default;
static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); }
void update() override
{
landing_gear_s landing_gear;
if (_topic.update(&landing_gear)) {
if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) {
_data = -1.f;
} else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) {
_data = 1.f;
}
}
}
float value(OutputFunction func) override { return _data; }
private:
uORB::Subscription _topic{ORB_ID(landing_gear)};
float _data{-1.f};
};
src/lib/mixer_module/functions/FunctionManualRC.hpp
+84
View File
@@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
#include <uORB/topics/manual_control_setpoint.h>
/**
* Functions: RC_Roll .. RCAUX_Max
*/
class FunctionManualRC : public FunctionProviderBase
{
public:
FunctionManualRC()
{
for (int i = 0; i < num_data_points; ++i) {
_data[i] = NAN;
}
}
static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); }
void update() override
{
manual_control_setpoint_s manual_control_setpoint;
if (_topic.update(&manual_control_setpoint)) {
_data[0] = manual_control_setpoint.y; // roll
_data[1] = manual_control_setpoint.x; // pitch
_data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
_data[3] = manual_control_setpoint.r; // yaw
_data[4] = manual_control_setpoint.flaps;
_data[5] = manual_control_setpoint.aux1;
_data[6] = manual_control_setpoint.aux2;
_data[7] = manual_control_setpoint.aux3;
_data[8] = manual_control_setpoint.aux4;
_data[9] = manual_control_setpoint.aux5;
_data[10] = manual_control_setpoint.aux6;
}
}
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; }
private:
static constexpr int num_data_points = 11;
static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1,
"number of functions mismatch");
uORB::Subscription _topic{ORB_ID(manual_control_setpoint)};
float _data[num_data_points] {};
};
src/lib/mixer_module/functions/FunctionMotors.hpp
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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.
*
****************************************************************************/
#pragma once
#include "FunctionProviderBase.hpp"
#include <uORB/topics/actuator_servos.h>
/**
* Functions: Motor1 ... MotorMax
*/
class FunctionMotors : public FunctionProviderBase
{
public:
static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
"Unexpected num motors");
static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
FunctionMotors(const Context &context) :
_topic(&context.work_item, ORB_ID(actuator_motors)),
_thrust_factor(context.thrust_factor)
{
for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
_data.control[i] = NAN;
}
}
static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }
void update() override
{
if (_topic.update(&_data)) {
updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
}
}
float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; }
bool allowPrearmControl() const override { return false; }
uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
{
if (thrust_factor > 0.f && thrust_factor <= 1.f) {
// thrust factor
// rel_thrust = factor * x^2 + (1-factor) * x,
const float a = thrust_factor;
const float b = (1.f - thrust_factor);
// don't recompute for all values (ax^2+bx+c=0)
const float tmp1 = b / (2.f * a);
const float tmp2 = b * b / (4.f * a * a);
for (int i = 0; i < num_values; ++i) {
float control = values[i];
if (control > 0.f) {
values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
} else if (control < -0.f) {
values[i] = tmp1 - sqrtf(tmp2 - (control / a));
} else {
values[i] = 0.f;
}
}
}
for (int i = 0; i < num_values; ++i) {
if ((reversible & (1u << i)) == 0) {
if (values[i] < -FLT_EPSILON) {
values[i] = NAN;
} else {
// remap from [0, 1] to [-1, 1]
values[i] = values[i] * 2.f - 1.f;
}
}
}
}
bool reversible(OutputFunction func) const override { return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); }
private:
uORB::SubscriptionCallbackWorkItem _topic;
actuator_motors_s _data{};
const float &_thrust_factor;
};

Some files were not shown because too many files have changed in this diff Show More