mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-27 08:30:05 +08:00
Compare commits
9 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 749015288d | |||
| 2196ab69ea | |||
| 9fbabefcbd | |||
| 473ef5fd06 | |||
| 46d9b14ba0 | |||
| 66e21497a6 | |||
| de49edc428 | |||
| b5846fd8c2 | |||
| ec6dd286fc |
@@ -3,92 +3,45 @@ description: Create a report to help us improve
|
||||
title: "[Bug] "
|
||||
labels: ["bug-report"]
|
||||
body:
|
||||
- type: markdown
|
||||
attributes:
|
||||
value: |
|
||||
**Tips for a great bug report:**
|
||||
- Describe what went wrong and what you expected
|
||||
- Include a flight log link from [logs.px4.io](http://logs.px4.io/) if possible
|
||||
- Mention your PX4 version, flight controller, and vehicle type if relevant
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: Describe the bug
|
||||
description: A clear and concise description of the bug.
|
||||
description: A clear description of the bug and what you expected to happen.
|
||||
placeholder: |
|
||||
What happened and what did you expect instead?
|
||||
|
||||
Steps to reproduce (if applicable):
|
||||
1.
|
||||
2.
|
||||
3.
|
||||
validations:
|
||||
required: true
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: To Reproduce
|
||||
label: Flight Log / Additional Information
|
||||
description: |
|
||||
Steps to reproduce the behavior.
|
||||
1. Drone switched on '...'
|
||||
2. Uploaded mission '....' (attach QGC mission file)
|
||||
3. Took off '....'
|
||||
4. See error
|
||||
validations:
|
||||
required: false
|
||||
**Flight log** (highly recommended for flight-related issues):
|
||||
- Upload to [PX4 Flight Review](http://logs.px4.io/) and paste the link
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: Expected behavior
|
||||
description: A clear and concise description of what you expected to happen.
|
||||
validations:
|
||||
required: false
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: Screenshot / Media
|
||||
description: Add screenshot / media if you have them
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: Flight Log
|
||||
description: |
|
||||
*Always* provide a link to the flight log file:
|
||||
- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)).
|
||||
- Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
|
||||
- Share the link to the log (Copy and paste the URL of the log)
|
||||
**Additional details** (if relevant):
|
||||
- PX4 version (output of `ver all` in MAVLink Shell)
|
||||
- Flight controller model
|
||||
- Vehicle type (multicopter, fixed-wing, VTOL, etc.)
|
||||
- Screenshots or media
|
||||
placeholder: |
|
||||
# PASTE HERE THE LINK TO THE LOG
|
||||
Flight log link:
|
||||
|
||||
Version:
|
||||
|
||||
Hardware:
|
||||
validations:
|
||||
required: false
|
||||
|
||||
- type: markdown
|
||||
attributes:
|
||||
value: |
|
||||
## Setup
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: Software Version
|
||||
description: |
|
||||
Which version of PX4 are you using?
|
||||
placeholder: |
|
||||
# If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC
|
||||
validations:
|
||||
required: false
|
||||
|
||||
- type: input
|
||||
attributes:
|
||||
label: Flight controller
|
||||
description: Specify your flight controller model (what type is it, where was it bought from, ...).
|
||||
validations:
|
||||
required: false
|
||||
|
||||
- type: dropdown
|
||||
attributes:
|
||||
label: Vehicle type
|
||||
options:
|
||||
- Multicopter
|
||||
- Helicopter
|
||||
- Fixed Wing
|
||||
- Hybrid VTOL
|
||||
- Airship/Balloon
|
||||
- Rover
|
||||
- Boat
|
||||
- Submarine
|
||||
- Other
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: How are the different components wired up (including port information)
|
||||
description: Details about how all is wired.
|
||||
|
||||
- type: textarea
|
||||
attributes:
|
||||
label: Additional context
|
||||
description: Add any other context about the problem here.
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
blank_issues_enabled: false
|
||||
blank_issues_enabled: true
|
||||
contact_links:
|
||||
- name: Support Question
|
||||
url: https://docs.px4.io/main/en/contribute/support.html#forums-and-chat
|
||||
|
||||
@@ -237,6 +237,12 @@ then
|
||||
tla2528 start -X
|
||||
fi
|
||||
|
||||
# Start TMP102 temperature sensor
|
||||
if param compare SENS_EN_TMP102 1
|
||||
then
|
||||
tmp102 start -X
|
||||
fi
|
||||
|
||||
# probe for optional external I2C devices
|
||||
if param compare SENS_EXT_I2C_PRB 1
|
||||
then
|
||||
|
||||
@@ -121,21 +121,22 @@ PX4 and the receiver may also need to be configured in order to _detect RC loss_
|
||||
|
||||

|
||||
|
||||
The QGCroundControl Safety UI allows you to set the [failsafe action](#failsafe-actions) and [RC Loss timeout](#COM_RC_LOSS_T).
|
||||
Users that want to disable the RC loss failsafe in specific automatic modes (mission, hold, offboard) can do so using the parameter [COM_RCL_EXCEPT](#COM_RCL_EXCEPT).
|
||||
The QGCroundControl Safety UI allows you to set the [failsafe action](#failsafe-actions) and [manual control loss timeout](#COM_RC_LOSS_T).
|
||||
Users that want to disable this failsafe in specific modes can do so using the parameter [COM_RCL_EXCEPT](#COM_RCL_EXCEPT).
|
||||
|
||||
Additional (and underlying) parameter settings are shown below.
|
||||
|
||||
| Parameter | Setting | Description |
|
||||
| ----------------------------------------------------------------------------------------------------- | --------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
|
||||
| <a id="COM_RC_LOSS_T"></a>[COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T) | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the old manual control setpoint until the timeout triggers. |
|
||||
| <a id="COM_RC_LOSS_T"></a>[COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T) | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the last known stick position until the timeout triggers. |
|
||||
| <a id="COM_FAIL_ACT_T"></a>[COM_FAIL_ACT_T](../advanced_config/parameter_reference.md#COM_FAIL_ACT_T) | Failsafe Reaction Delay | Delay in seconds between failsafe condition being triggered (`COM_RC_LOSS_T`) and failsafe action (RTL, Land, Hold). In this state the vehicle waits in hold mode for the manual control source to reconnect. This might be set longer for long-range flights so that intermittent connection loss doesn't immediately invoke the failsafe. It can be to zero so that the failsafe triggers immediately. |
|
||||
| <a id="NAV_RCL_ACT"></a>[NAV_RCL_ACT](../advanced_config/parameter_reference.md#NAV_RCL_ACT) | Failsafe Action | Disabled, Loiter, Return, Land, Disarm, Terminate. |
|
||||
| <a id="COM_RCL_EXCEPT"></a>[COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions | Set the modes in which manual control loss is ignored: Mission, Hold, Offboard. |
|
||||
| <a id="COM_RCL_EXCEPT"></a>[COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions | Set modes in which manual control loss is ignored. |
|
||||
|
||||
## Data Link Loss Failsafe
|
||||
|
||||
The Data Link Loss failsafe is triggered if a telemetry link (connection to ground station) is lost.
|
||||
The Data Link Loss failsafe is triggered if the connection to the last MAVLink ground station like QGroundControl is lost.
|
||||
Users that want to disable this failsafe in specific modes can do so using the parameter [COM_DLL_EXCEPT](#COM_DLL_EXCEPT).
|
||||
|
||||

|
||||
|
||||
@@ -145,12 +146,7 @@ The settings and underlying parameters are shown below.
|
||||
| ---------------------- | ------------------------------------------------------------------------ | --------------------------------------------------------------------------------- |
|
||||
| Data Link Loss Timeout | [COM_DL_LOSS_T](../advanced_config/parameter_reference.md#COM_DL_LOSS_T) | Amount of time after losing the data connection before the failsafe will trigger. |
|
||||
| Failsafe Action | [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT) | Disabled, Hold mode, Return mode, Land mode, Disarm, Terminate. |
|
||||
|
||||
The following settings also apply, but are not displayed in the QGC UI.
|
||||
|
||||
| Setting | Parameter | Description |
|
||||
| ----------------------------------------------------------- | -------------------------------------------------------------------------- | ---------------------------------------------------- |
|
||||
| <a id="COM_DLL_EXCEPT"></a>Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes where DL loss will not trigger a failsafe. |
|
||||
| <a id="COM_DLL_EXCEPT"></a>Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes in which data link loss is ignored. |
|
||||
|
||||
## Geofence Failsafe
|
||||
|
||||
|
||||
@@ -267,6 +267,7 @@
|
||||
|
||||
|
||||
#define DRV_TEMP_DEVTYPE_MCP9808 0xEE
|
||||
#define DRV_TEMP_DEVTYPE_TMP102 0xF0
|
||||
|
||||
#define DRV_DEVTYPE_UNUSED 0xff
|
||||
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2026 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__temperature_sensor__tmp102
|
||||
MAIN tmp102
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
tmp102_main.cpp
|
||||
tmp102.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_TEMPERATURE_SENSOR_TMP102
|
||||
bool "TMP102 temperature sensor"
|
||||
default n
|
||||
---help---
|
||||
Enable support for the TMP102 temperature sensor
|
||||
@@ -0,0 +1,15 @@
|
||||
__max_num_config_instances: &max_num_config_instances 1
|
||||
|
||||
module_name: TMP102
|
||||
|
||||
parameters:
|
||||
- group: Sensors
|
||||
definitions:
|
||||
SENS_EN_TMP102:
|
||||
description:
|
||||
short: Enable TMP102
|
||||
long: |
|
||||
Enable the driver for the TMP102 temperature sensor
|
||||
type: boolean
|
||||
reboot_required: true
|
||||
default: 0
|
||||
@@ -0,0 +1,129 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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 "tmp102.h"
|
||||
|
||||
TMP102::TMP102(const I2CSPIDriverConfig &config) :
|
||||
I2C(config),
|
||||
I2CSPIDriver(config),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
|
||||
{
|
||||
}
|
||||
|
||||
TMP102::~TMP102()
|
||||
{
|
||||
ScheduleClear();
|
||||
perf_free(_cycle_perf);
|
||||
perf_free(_comms_errors);
|
||||
}
|
||||
|
||||
void TMP102::RunImpl()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
float temperature = read_temperature();
|
||||
|
||||
if (std::isnan(temperature)) {
|
||||
perf_count(_comms_errors);
|
||||
|
||||
} else {
|
||||
sensor_temp_s _sensor_temp{};
|
||||
_sensor_temp.timestamp = hrt_absolute_time();
|
||||
_sensor_temp.temperature = temperature;
|
||||
_sensor_temp.device_id = get_device_id();
|
||||
_sensor_temp_pub.publish(_sensor_temp);
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
int TMP102::probe()
|
||||
{
|
||||
uint16_t conf_reg;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (read_reg(TMP102_CONFIG_REG, conf_reg) == PX4_OK && (conf_reg | 0x0020) == DEFAULT_CONFIG_REG) { // Mask the AL bit
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
px4_sleep(1);
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int TMP102::init()
|
||||
{
|
||||
int ret = I2C::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
PX4_ERR("TMP102, I2C init failed");
|
||||
return ret;
|
||||
}
|
||||
|
||||
_sensor_temp_pub.advertise();
|
||||
ScheduleOnInterval(250_ms); // DEFAULT SAMPLE RATE IS 4HZ => 250ms INTERVAL
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
float TMP102::read_temperature()
|
||||
{
|
||||
uint16_t tmp_data;
|
||||
|
||||
if (read_reg(TMP102_TEMP_REG, tmp_data) != PX4_OK) {
|
||||
return NAN;
|
||||
}
|
||||
|
||||
float temperature = ((int16_t)(tmp_data) >> 4) * 0.0625f;
|
||||
return temperature;
|
||||
}
|
||||
|
||||
int TMP102::read_reg(uint8_t reg, uint16_t &data)
|
||||
{
|
||||
uint8_t tmp_data[2];
|
||||
tmp_data[0] = reg;
|
||||
|
||||
if (transfer(tmp_data, 1, nullptr, 0) != PX4_OK) { // Set the PR to the desired register
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int ret = transfer(nullptr, 0, tmp_data, 2); // Read the data from the desired register
|
||||
data = (tmp_data[0] << 8) | tmp_data[1];
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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 <stdint.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <uORB/topics/sensor_temp.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#define TMP102_TEMP_REG 0x00
|
||||
#define TMP102_CONFIG_REG 0x01
|
||||
#define TMP102_TLOW_REG 0x02
|
||||
#define TMP102_THIGH_REG 0x03
|
||||
|
||||
constexpr uint16_t DEFAULT_CONFIG_REG = 0x60A0; // 12-bit resolution, comparator mode, active low, 4Hz
|
||||
|
||||
class TMP102 : public device::I2C, public I2CSPIDriver<TMP102>
|
||||
{
|
||||
public:
|
||||
TMP102(const I2CSPIDriverConfig &config);
|
||||
~TMP102() override;
|
||||
|
||||
int init() override;
|
||||
int probe() override;
|
||||
void RunImpl();
|
||||
static void print_usage();
|
||||
|
||||
protected:
|
||||
void print_status();
|
||||
|
||||
private:
|
||||
uORB::PublicationMulti<sensor_temp_s> _sensor_temp_pub{ORB_ID(sensor_temp)};
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
|
||||
float read_temperature();
|
||||
int read_reg(uint8_t address, uint16_t &data);
|
||||
};
|
||||
@@ -0,0 +1,90 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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 tmp102_main.cpp
|
||||
* @author Philipp Engljaehringer
|
||||
*
|
||||
* Driver for the TMP102 Temperature Sensor connected via I2C.
|
||||
*/
|
||||
|
||||
#include "tmp102.h"
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void TMP102::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("tmp102", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x48);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
void TMP102::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_cycle_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
}
|
||||
|
||||
extern "C" int tmp102_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = TMP102;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.default_i2c_frequency = 400000;
|
||||
cli.i2c_address = 0x48;
|
||||
|
||||
const char *verb = cli.parseDefaultArguments(argc, argv);
|
||||
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_TEMP_DEVTYPE_TMP102);
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -43,8 +43,6 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/atmosphere/atmosphere.h>
|
||||
|
||||
#define MOTOR_BIT(x) (1<<(x))
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
UavcanEscController::UavcanEscController(uavcan::INode &node) :
|
||||
|
||||
@@ -47,13 +47,9 @@
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/esc/Status.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/uavcan/node_info.hpp>
|
||||
#include <parameters/param.h>
|
||||
#include "../node_info.hpp"
|
||||
|
||||
class UavcanEscController
|
||||
{
|
||||
|
||||
@@ -66,10 +66,9 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
|
||||
if (_actuator_armed_sub.update(&actuator_armed)) {
|
||||
uavcan::equipment::safety::ArmingStatus cmd;
|
||||
|
||||
if (actuator_armed.lockdown || actuator_armed.kill) {
|
||||
cmd.status = cmd.STATUS_DISARMED;
|
||||
bool lockdown_active = actuator_armed.lockdown || actuator_armed.termination || actuator_armed.kill;
|
||||
|
||||
} else if (actuator_armed.armed) {
|
||||
if (!lockdown_active && (actuator_armed.armed || _is_actuator_test_running)) {
|
||||
cmd.status = cmd.STATUS_FULLY_ARMED;
|
||||
|
||||
} else {
|
||||
|
||||
@@ -57,6 +57,8 @@ public:
|
||||
*/
|
||||
int init();
|
||||
|
||||
void setActuatorTestRunning(bool running) {_is_actuator_test_running = running;}
|
||||
|
||||
private:
|
||||
/*
|
||||
* Max update rate to avoid exessive bus traffic
|
||||
@@ -80,4 +82,5 @@ private:
|
||||
|
||||
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
|
||||
|
||||
bool _is_actuator_test_running = false;
|
||||
};
|
||||
|
||||
@@ -969,6 +969,10 @@ UavcanNode::Run()
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
|
||||
_arming_status_controller.setActuatorTestRunning(_mixing_interface_esc.isActuatorTestRunning());
|
||||
#endif
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
@@ -136,6 +136,8 @@ public:
|
||||
|
||||
MixingOutput &mixingOutput() { return _mixing_output; }
|
||||
|
||||
bool isActuatorTestRunning() const { return _mixing_output.isActuatorTestRunning(); }
|
||||
|
||||
protected:
|
||||
void Run() override;
|
||||
private:
|
||||
|
||||
@@ -163,6 +163,7 @@ public:
|
||||
void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us);
|
||||
|
||||
const actuator_armed_s &armed() const { return _armed; }
|
||||
bool isActuatorTestRunning() const { return _actuator_test.inTestMode(); }
|
||||
|
||||
void setAllFailsafeValues(uint16_t value);
|
||||
void setAllDisarmedValues(uint16_t value);
|
||||
|
||||
@@ -607,13 +607,14 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
|
||||
/**
|
||||
* Manual control loss exceptions
|
||||
*
|
||||
* Specify modes where manual control loss is ignored and no failsafe is triggered.
|
||||
* Specify modes in which stick input is ignored and no failsafe action is triggered.
|
||||
* External modes requiring stick input will still failsafe.
|
||||
* Auto modes are: Hold, Takeoff, Land, RTL, Descend, Follow Target, Precland, Orbit.
|
||||
*
|
||||
* @min 0
|
||||
* @max 31
|
||||
* @bit 0 Mission
|
||||
* @bit 1 Hold
|
||||
* @bit 1 Auto modes
|
||||
* @bit 2 Offboard
|
||||
* @bit 3 External Mode
|
||||
* @bit 4 Altitude Cruise
|
||||
@@ -624,13 +625,16 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
|
||||
/**
|
||||
* Datalink loss exceptions
|
||||
*
|
||||
* Specify modes in which datalink loss is ignored and the failsafe action not triggered.
|
||||
* Specify modes in which ground control station connection loss is ignored and no failsafe action is triggered.
|
||||
* See also COM_RCL_EXCEPT.
|
||||
*
|
||||
* @min 0
|
||||
* @max 7
|
||||
* @max 31
|
||||
* @bit 0 Mission
|
||||
* @bit 1 Hold
|
||||
* @bit 1 Auto modes
|
||||
* @bit 2 Offboard
|
||||
* @bit 3 External Mode
|
||||
* @bit 4 Altitude Cruise
|
||||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0);
|
||||
|
||||
@@ -182,21 +182,23 @@
|
||||
<tr>
|
||||
<td><label for="intended_nav_state">Intended Mode: </label></td>
|
||||
<td><select id="intended_nav_state">
|
||||
<option value="0">MANUAL</option>
|
||||
<option value="10">ACRO</option>
|
||||
<option value="15">STAB</option>
|
||||
<option value="1">ALTCTL</option>
|
||||
<option value="2" selected>POSCTL</option>
|
||||
<option value="3">AUTO_MISSION</option>
|
||||
<option value="4">AUTO_LOITER</option>
|
||||
<option value="5">AUTO_RTL</option>
|
||||
<option value="17">AUTO_TAKEOFF</option>
|
||||
<option value="18">AUTO_LAND</option>
|
||||
<option value="19">AUTO_FOLLOW_TARGET</option>
|
||||
<option value="20">AUTO_PRECLAND</option>
|
||||
<option value="22">AUTO_VTOL_TAKEOFF</option>
|
||||
<option value="14">OFFBOARD</option>
|
||||
<option value="21">ORBIT</option>
|
||||
<option value="0">Manual</option>
|
||||
<option value="10">Acro</option>
|
||||
<option value="15">Stabilized</option>
|
||||
<option value="1">Altitude</option>
|
||||
<option value="8">Altitude Cruise</option>
|
||||
<option value="2" selected>Position</option>
|
||||
<option value="3">Mission</option>
|
||||
<option value="4">Hold</option>
|
||||
<option value="5">Return</option>
|
||||
<option value="17">Takeoff</option>
|
||||
<option value="18">Land</option>
|
||||
<option value="19">Follow</option>
|
||||
<option value="20">Precision Land</option>
|
||||
<option value="22">VTOL Takeoff</option>
|
||||
<option value="14">Offboard</option>
|
||||
<option value="21">Orbit</option>
|
||||
<option value="23">External 1</option>
|
||||
</select>
|
||||
</td>
|
||||
<td></td>
|
||||
|
||||
@@ -442,18 +442,53 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par
|
||||
return options;
|
||||
}
|
||||
|
||||
bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter)
|
||||
{
|
||||
switch (user_intended_mode) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::Mission;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::Offboard;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::ExternalMode;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::AltitudeCruise;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||
const failsafe_flags_s &status_flags)
|
||||
{
|
||||
updateArmingState(time_us, state.armed, status_flags);
|
||||
|
||||
const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
|
||||
|| state.vtol_in_transition_mode;
|
||||
|
||||
// Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
|
||||
// altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
|
||||
const bool ignore_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode ==
|
||||
vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
|
||||
const bool in_forward_flight = (state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || state.vtol_in_transition_mode;
|
||||
const bool ignore_any_link_loss_vtol_takeoff_fixedwing = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
|
||||
&& in_forward_flight && !state.mission_finished;
|
||||
|
||||
// Manual control (RC or joystick) loss
|
||||
@@ -462,59 +497,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||
_manual_control_lost_at_arming = false;
|
||||
}
|
||||
|
||||
const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission);
|
||||
const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
|
||||
const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard);
|
||||
const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
|
||||
const bool rc_loss_ignored_altitude_cruise = (state.user_intended_mode ==
|
||||
vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AltitudeCruise));
|
||||
|
||||
const bool rc_loss_ignored_external_mode =
|
||||
(state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL2 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL3 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL4 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL5 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL6 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL7 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8)
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode);
|
||||
|
||||
const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
|
||||
rc_loss_ignored_takeoff || rc_loss_ignored_external_mode || ignore_any_link_loss_vtol_takeoff_fixedwing
|
||||
|| _manual_control_lost_at_arming || rc_loss_ignored_altitude_cruise;
|
||||
const bool rc_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get())
|
||||
|| ignore_any_link_loss_vtol_takeoff_fixedwing || _manual_control_lost_at_arming;
|
||||
|
||||
if (_param_com_rc_in_mode.get() != int32_t(RcInMode::DisableManualControl) && !rc_loss_ignored) {
|
||||
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
|
||||
fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
|
||||
}
|
||||
|
||||
// GCS connection loss
|
||||
// Ground control station connection loss
|
||||
const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|
||||
|| state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
||||
|
||||
const bool dll_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Mission);
|
||||
const bool dll_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
|
||||
const bool dll_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Offboard);
|
||||
const bool dll_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
|
||||
|
||||
const bool dll_loss_ignored = dll_loss_ignored_mission || dll_loss_ignored_loiter || dll_loss_ignored_offboard ||
|
||||
dll_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;
|
||||
const bool dll_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_dll_except.get())
|
||||
|| ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;
|
||||
|
||||
if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) {
|
||||
CHECK_FAILSAFE(status_flags, gcs_connection_lost,
|
||||
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
|
||||
CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
|
||||
}
|
||||
|
||||
// VTOL transition failure (quadchute)
|
||||
@@ -531,7 +530,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||
|
||||
// If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished,
|
||||
// trigger RTL to avoid losing the vehicle
|
||||
if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) || rc_loss_ignored_mission)
|
||||
if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl)
|
||||
|| isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get()))
|
||||
&& _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled)
|
||||
&& state.mission_finished) {
|
||||
_last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost,
|
||||
|
||||
@@ -53,20 +53,14 @@ protected:
|
||||
private:
|
||||
void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags);
|
||||
|
||||
enum class ManualControlLossExceptionBits : int32_t {
|
||||
enum class LinkLossExceptionBits : int32_t {
|
||||
Mission = (1 << 0),
|
||||
Hold = (1 << 1),
|
||||
AutoModes = (1 << 1),
|
||||
Offboard = (1 << 2),
|
||||
ExternalMode = (1 << 3),
|
||||
AltitudeCruise = (1 << 4)
|
||||
};
|
||||
|
||||
enum class DatalinkLossExceptionBits : int32_t {
|
||||
Mission = (1 << 0),
|
||||
Hold = (1 << 1),
|
||||
Offboard = (1 << 2)
|
||||
};
|
||||
|
||||
// COM_LOW_BAT_ACT parameter values
|
||||
enum class LowBatteryAction : int32_t {
|
||||
Warning = 0, // Warning
|
||||
@@ -175,6 +169,8 @@ private:
|
||||
static ActionOptions fromPosLowActParam(int param_value);
|
||||
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
|
||||
|
||||
static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
|
||||
|
||||
const int _caller_id_mode_fallback{genCallerId()};
|
||||
bool _last_state_mode_fallback{false};
|
||||
const int _caller_id_mission_control_lost{genCallerId()};
|
||||
|
||||
@@ -166,6 +166,8 @@ void FwLateralLongitudinalControl::Run()
|
||||
_landed = landed.landed;
|
||||
}
|
||||
|
||||
uint8_t current_flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
|
||||
|
||||
_vehicle_status_sub.update();
|
||||
_control_mode_sub.update();
|
||||
|
||||
@@ -209,18 +211,18 @@ void FwLateralLongitudinalControl::Run()
|
||||
// If the both altitude and height rate are set, set altitude setpoint to NAN
|
||||
const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude;
|
||||
|
||||
tecs_update_pitch_throttle(control_interval, altitude_sp,
|
||||
airspeed_sp_eas,
|
||||
_long_configuration.pitch_min,
|
||||
_long_configuration.pitch_max,
|
||||
_long_configuration.throttle_min,
|
||||
_long_configuration.throttle_max,
|
||||
_long_configuration.sink_rate_target,
|
||||
_long_configuration.climb_rate_target,
|
||||
_long_configuration.disable_underspeed_protection,
|
||||
_long_control_sp.height_rate,
|
||||
now
|
||||
);
|
||||
current_flight_phase = tecs_update_pitch_throttle(control_interval, altitude_sp,
|
||||
airspeed_sp_eas,
|
||||
_long_configuration.pitch_min,
|
||||
_long_configuration.pitch_max,
|
||||
_long_configuration.throttle_min,
|
||||
_long_configuration.throttle_max,
|
||||
_long_configuration.sink_rate_target,
|
||||
_long_configuration.climb_rate_target,
|
||||
_long_configuration.disable_underspeed_protection,
|
||||
_long_control_sp.height_rate,
|
||||
now
|
||||
);
|
||||
|
||||
pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
|
||||
throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct :
|
||||
@@ -317,6 +319,16 @@ void FwLateralLongitudinalControl::Run()
|
||||
|
||||
}
|
||||
|
||||
// Publish flight phase with low rate, but immediately if updated
|
||||
const bool flight_phase_updated = current_flight_phase != _flight_phase_estimation_pub.get().flight_phase;
|
||||
const hrt_abstime time_since_last_flightphase_pub = now - _flight_phase_estimation_pub.get().timestamp;
|
||||
|
||||
if (flight_phase_updated || time_since_last_flightphase_pub >= 1_s) {
|
||||
_flight_phase_estimation_pub.get().timestamp = now;
|
||||
_flight_phase_estimation_pub.get().flight_phase = current_flight_phase;
|
||||
_flight_phase_estimation_pub.update();
|
||||
}
|
||||
|
||||
_z_reset_counter = _local_pos.z_reset_counter;
|
||||
}
|
||||
|
||||
@@ -356,7 +368,7 @@ void FwLateralLongitudinalControl::updateControllerConfiguration(hrt_abstime tim
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
uint8_t
|
||||
FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
|
||||
float pitch_min_rad, float pitch_max_rad, float throttle_min,
|
||||
float throttle_max, const float desired_max_sinkrate,
|
||||
@@ -370,8 +382,7 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
|
||||
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
|| _vehicle_status_sub.get().in_transition_mode)) {
|
||||
tecs_is_running = false;
|
||||
return;
|
||||
|
||||
return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
|
||||
}
|
||||
|
||||
const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
|
||||
@@ -409,25 +420,23 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
|
||||
// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
|
||||
if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
|
||||
fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
|
||||
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
|
||||
return flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
|
||||
|
||||
} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
|
||||
(tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
|
||||
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
|
||||
return flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
|
||||
|
||||
} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
|
||||
(tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
|
||||
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
|
||||
return flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
|
||||
|
||||
} else {
|
||||
// We can't infer the flight phase , do nothing, estimation is reset at each step
|
||||
_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
|
||||
|
||||
// We can't infer the flight phase
|
||||
return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
|
||||
}
|
||||
|
||||
_flight_phase_estimation_pub.get().timestamp = now;
|
||||
_flight_phase_estimation_pub.update();
|
||||
}
|
||||
|
||||
return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@@ -213,11 +213,12 @@ private:
|
||||
|
||||
void parameters_update();
|
||||
void update_control_state(hrt_abstime now);
|
||||
void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
|
||||
float pitch_min_rad, float pitch_max_rad, float throttle_min,
|
||||
float throttle_max, const float desired_max_sinkrate,
|
||||
const float desired_max_climbrate,
|
||||
bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);
|
||||
|
||||
uint8_t tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
|
||||
float pitch_min_rad, float pitch_max_rad, float throttle_min,
|
||||
float throttle_max, const float desired_max_sinkrate,
|
||||
const float desired_max_climbrate,
|
||||
bool disable_underspeed_detection, float hgt_rate_sp, hrt_abstime now);
|
||||
|
||||
void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
|
||||
float throttle_trim, hrt_abstime now);
|
||||
|
||||
@@ -125,7 +125,6 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("sensor_gyro_fft", 50);
|
||||
add_topic("sensor_selection");
|
||||
add_topic("sensors_status_imu", 200);
|
||||
add_optional_topic("sensor_temp", 100);
|
||||
add_optional_topic("spoilers_setpoint", 1000);
|
||||
add_topic("system_power", 500);
|
||||
add_optional_topic("takeoff_status", 1000);
|
||||
@@ -167,6 +166,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic_multi("control_allocator_status", 200, 2);
|
||||
add_optional_topic_multi("rate_ctrl_status", 200, 2);
|
||||
add_optional_topic_multi("sensor_hygrometer", 500, 4);
|
||||
add_optional_topic_multi("sensor_temp", 100, 4);
|
||||
add_optional_topic_multi("rpm", 200);
|
||||
add_topic_multi("timesync_status", 1000, 3);
|
||||
add_optional_topic_multi("telemetry_status", 1000, 4);
|
||||
|
||||
Reference in New Issue
Block a user