fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 02:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into beta

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-17 13:04:49 +01:00
parent 5e57553615 092a51426f
commit 10430d430c
107 changed files with 1697 additions and 1558 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+3
View File
@@ -25,3 +25,6 @@
[submodule "unittests/googletest"]
path = unittests/googletest
url = https://github.com/google/googletest.git
[submodule "src/lib/matrix"]
path = src/lib/matrix
url = https://github.com/PX4/Matrix.git
CMakeLists.txt
+1
View File
@@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
add_custom_target(submodule_clean
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
ROMFS/px4fmu_common/init.d/4009_qav250
+5 -6
View File
@@ -16,9 +16,9 @@ sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_P 0.05
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_D 0.002
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.1
@@ -27,8 +27,7 @@ then
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set PWM_MIN 1075
param set PWM_THR_MIN 0.06
param set PWM_MANTHR_MIN 0.06
param set PWM_MIN 980
param set MPC_THR_MIN 0.06
param set MPC_MANTHR_MIN 0.06
fi
Tools/jMAVSim
+1 -1
Submodule Tools/jMAVSim updated: fd9ed5df04...96029523d1
Tools/parameters_injected.xml
+138
View File
@@ -0,0 +1,138 @@
<?xml version='1.0' encoding='UTF-8'?>
<parameters>
<version>3</version>
<group name="UAVCAN Motor Parameters" no_code_generation="true">
<parameter default="75" name="ctl_bw" type="INT32">
<short_desc>Speed controller bandwidth</short_desc>
<long_desc>Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness.</long_desc>
<unit>Hertz</unit>
<min>10</min>
<max>250</max>
</parameter>
<parameter default="1" name="ctl_dir" type="INT32">
<short_desc>Reverse direction</short_desc>
<long_desc>Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction.</long_desc>
<min>0</min>
<max>1</max>
</parameter>
<parameter default="1" name="ctl_gain" type="FLOAT">
<short_desc>Speed (RPM) controller gain</short_desc>
<long_desc>Speed (RPM) controller gain. Determines controller
aggressiveness; units are amp-seconds per radian. Systems with
higher rotational inertia (large props) will need gain increased;
systems with low rotational inertia (small props) may need gain
decreased. Higher values result in faster response, but may result
in oscillation and excessive overshoot. Lower values result in a
slower, smoother response.</long_desc>
<unit>amp-seconds per radian</unit>
<decimal>3</decimal>
<min>0.00</min>
<max>1.00</max>
</parameter>
<parameter default="3.5" name="ctl_hz_idle" type="FLOAT">
<short_desc>Idle speed (e Hz)</short_desc>
<long_desc>Idle speed (e Hz)</long_desc>
<unit>Hertz</unit>
<decimal>3</decimal>
<min>0.0</min>
<max>100.0</max>
</parameter>
<parameter default="25" name="ctl_start_rate" type="INT32">
<short_desc>Spin-up rate (e Hz/s)</short_desc>
<long_desc>Spin-up rate (e Hz/s)</long_desc>
<unit>Hz/s</unit>
<min>5</min>
<max>1000</max>
</parameter>
<parameter default="0" name="esc_index" type="INT32">
<short_desc>Index of this ESC in throttle command messages.</short_desc>
<long_desc>Index of this ESC in throttle command messages.</long_desc>
<unit>Index</unit>
<min>0</min>
<max>15</max>
</parameter>
<parameter default="20034" name="id_ext_status" type="INT32">
<short_desc>Extended status ID</short_desc>
<long_desc>Extended status ID</long_desc>
<min>1</min>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_ext_status" type="INT32">
<short_desc>Extended status interval (µs)</short_desc>
<long_desc>Extended status interval (µs)</long_desc>
<unit>µs</unit>
<min>0</min>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_status" type="INT32">
<short_desc>ESC status interval (µs)</short_desc>
<long_desc>ESC status interval (µs)</long_desc>
<unit>µs</unit>
<max>1000000</max>
</parameter>
<parameter default="12" name="mot_i_max" type="FLOAT">
<short_desc>Motor current limit in amps</short_desc>
<long_desc>Motor current limit in amps. This determines the maximum
current controller setpoint, as well as the maximum allowable
current setpoint slew rate. This value should generally be set to
the continuous current rating listed in the motor’s specification
sheet, or set equal to the motor’s specified continuous power
divided by the motor voltage limit.</long_desc>
<unit>Amps</unit>
<decimal>3</decimal>
<min>1</min>
<max>80</max>
</parameter>
<parameter default="2300" name="mot_kv" type="INT32">
<short_desc>Motor Kv in RPM per volt</short_desc>
<long_desc>Motor Kv in RPM per volt. This can be taken from the motor’s
specification sheet; accuracy will help control performance but
some deviation from the specified value is acceptable.</long_desc>
<unit>RPM/v</unit>
<min>0</min>
<max>4000</max>
</parameter>
<parameter default="0.0" name="mot_ls" type="FLOAT">
<short_desc>READ ONLY: Motor inductance in henries.</short_desc>
<long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
<unit>henries</unit>
<decimal>3</decimal>
</parameter>
<parameter default="14" name="mot_num_poles" type="INT32">
<short_desc>Number of motor poles.</short_desc>
<long_desc>Number of motor poles. Used to convert mechanical speeds to
electrical speeds. This number should be taken from the motor’s
specification sheet.</long_desc>
<unit>Poles</unit>
<min>2</min>
<max>40</max>
</parameter>
<parameter default="0.0" name="mot_rs" type="FLOAT">
<short_desc>READ ONLY: Motor resistance in ohms</short_desc>
<long_desc>READ ONLY: Motor resistance in ohms. This is measured on start-up. When
tuning a new motor, check that this value is approximately equal
to the value shown in the motor’s specification sheet.</long_desc>
<unit>Ohms</unit>
<decimal>3</decimal>
</parameter>
<parameter default="0.5" name="mot_v_accel" type="FLOAT">
<short_desc>Acceleration limit (V)</short_desc>
<long_desc>Acceleration limit (V)</long_desc>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0.01</min>
<max>1.00</max>
</parameter>
<parameter default="14.8" name="mot_v_max" type="FLOAT">
<short_desc>Motor voltage limit in volts</short_desc>
<long_desc>Motor voltage limit in volts. The current controller’s
commanded voltage will never exceed this value. Note that this may
safely be above the nominal voltage of the motor; to determine the
actual motor voltage limit, divide the motor’s rated power by the
motor current limit.</long_desc>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0</min>
</parameter>
</group>
</parameters>
Tools/px4params/xmlout.py
+5 -1
View File
@@ -18,10 +18,14 @@ def indent(elem, level=0):
class XMLOutput():
def __init__(self, groups, board):
def __init__(self, groups, board, inject_xml_file_name):
xml_parameters = ET.Element("parameters")
xml_version = ET.SubElement(xml_parameters, "version")
xml_version.text = "3"
importtree = ET.parse(inject_xml_file_name)
injectgroups = importtree.getroot().findall("group")
for igroup in injectgroups:
xml_parameters.append(igroup)
last_param_name = ""
board_specific_param_set = False
for group in groups:
Tools/px_generate_params.py
+3 -2
View File
@@ -29,7 +29,7 @@ start_name = ""
end_name = ""
for group in root:
if group.tag == "group":
if group.tag == "group" and "no_code_generation" not in group.attrib:
header += """
/*****************************************************************
* %s
@@ -62,7 +62,8 @@ struct px4_parameters_t px4_parameters = {
"""
i=0
for group in root:
if group.tag == "group":
if group.tag == "group" and "no_code_generation" not in group.attrib:
src += """
/*****************************************************************
* %s
Tools/px_process_params.py
+7 -1
View File
@@ -65,6 +65,12 @@ def main():
metavar="FILENAME",
help="Create XML file"
" (default FILENAME: parameters.xml)")
parser.add_argument("-i", "--inject-xml",
nargs='?',
const="../Tools/parameters_injected.xml",
metavar="FILENAME",
help="Inject additional param XML file"
" (default FILENAME: ../Tools/parameters_injected.xml)")
parser.add_argument("-b", "--board",
nargs='?',
const="",
@@ -124,7 +130,7 @@ def main():
# Output to XML file
if args.xml:
print("Creating XML file " + args.xml)
out = xmlout.XMLOutput(param_groups, args.board)
out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml)
out.Save(args.xml)
# Output to DokuWiki tables
Tools/px_romfs_pruner.py
+3 -2
View File
@@ -40,7 +40,7 @@ Delete all comments and newlines before ROMFS is converted to an image
"""
from __future__ import print_function
import argparse
import argparse, re
import os
@@ -64,7 +64,7 @@ def main():
# read file line by line
pruned_content = ""
with open(file_path, "r") as f:
with open(file_path, "rU") as f:
for line in f:
# handle mixer files differently than startup files
@@ -76,6 +76,7 @@ def main():
pruned_content += line
# overwrite old scratch file
with open(file_path, "wb") as f:
pruned_content = re.sub("\r\n", "\n", pruned_content)
f.write(pruned_content.encode("ascii", errors='strict'))
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: 7d855cd385...d362e661b4
Tools/sitl_run.sh
+8 -2
View File
@@ -56,8 +56,8 @@ then
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build
# Set the model path so Gazebo finds the airframes
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
# Disable online model lookup since this is quite experimental and unstable
export GAZEBO_MODEL_DATABASE_URI=""
# The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
# export GAZEBO_MODEL_DATABASE_URI=""
export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
mkdir -p Tools/sitl_gazebo/Build
cd Tools/sitl_gazebo/Build
@@ -83,6 +83,12 @@ then
elif [ "$debugger" == "gdb" ]
then
gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "ddd" ]
then
ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "valgrind" ]
then
valgrind ./mainapp ../../../../${rc_script}_${program}
else
./mainapp ../../../../${rc_script}_${program}
fi
cmake/common/px4_base.cmake
+2 -2
View File
@@ -622,7 +622,7 @@ function(px4_add_common_flags)
)
list(APPEND added_include_dirs
src/lib/eigen
src/lib/matrix
)
set(added_link_dirs) # none used currently
@@ -741,7 +741,7 @@ function(px4_generate_parameters_xml)
)
add_custom_command(OUTPUT ${OUT}
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py
-s ${path} --board CONFIG_ARCH_${BOARD} --xml
-s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml
DEPENDS ${param_src_files}
)
set(${OUT} ${${OUT}} PARENT_SCOPE)
cmake/configs/posix_sitl_simple.cmake
+1
View File
@@ -36,6 +36,7 @@ set(config_module_list
modules/ekf_att_pos_estimator
modules/position_estimator_inav
modules/navigator
modules/vtol_att_control
modules/mc_pos_control
modules/mc_att_control
modules/mc_pos_control_multiplatform
cmake/ros-CMakeLists.txt
+2
View File
@@ -81,6 +81,7 @@ add_message_files(
offboard_control_mode.msg
vehicle_force_setpoint.msg
distance_sensor.msg
control_state.msg
)
## Generate services in the 'srv' folder
@@ -135,6 +136,7 @@ include_directories(
${CMAKE_BINARY_DIR}/src/modules
src/
src/lib
src/lib/matrix
${EIGEN_INCLUDE_DIRS}
integrationtests
)
msg/control_state.msg
+19
View File
@@ -0,0 +1,19 @@
# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
uint64 timestamp # in microseconds since system start
float32 x_acc # X acceleration in body frame
float32 y_acc # Y acceleration in body frame
float32 z_acc # Z acceleration in body frame
float32 x_vel # X velocity in body frame
float32 y_vel # Y velocity in body frame
float32 z_vel # Z velocity in body frame
float32 x_pos # X position in local frame
float32 y_pos # Y position in local frame
float32 z_pos # z position in local frame
float32 airspeed # Airspeed, estimated
float32[3] vel_variance # Variance in body velocity estimate
float32[3] pos_variance # Variance in local position estimate
float32[4] q # Attitude Quaternion
float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down)
float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down)
float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down)
float32 horz_acc_mag # Magnitude of the horizontal acceleration
msg/vehicle_status.msg
+4
View File
@@ -120,6 +120,8 @@ bool in_transition_mode
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
bool condition_system_sensors_initialized
bool condition_system_prearm_error_reported # true if errors have already been reported
bool condition_system_hotplug_timeout # true if the hotplug sensor search is over
bool condition_system_returned_to_home
bool condition_auto_mission_available
bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
@@ -139,6 +141,7 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded
bool rc_input_blocked # set if RC input should be ignored temporarily
uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.
bool data_link_found_new # new datalink to GCS found
bool data_link_lost # datalink to GCS lost
bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
@@ -179,3 +182,4 @@ bool circuit_breaker_engaged_power_check
bool circuit_breaker_engaged_airspd_check
bool circuit_breaker_engaged_enginefailure_check
bool circuit_breaker_engaged_gpsfailure_check
bool cb_usb
src/drivers/boards/aerocore/board_config.h
+3
View File
@@ -82,6 +82,9 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
/* SPI4--Ramtron */
#define PX4_SPI_BUS_RAMTRON 4
/* Nominal chip selects for devices on SPI bus #3 */
#define PX4_SPIDEV_ACCEL_MAG 0
#define PX4_SPIDEV_GYRO 1
src/drivers/boards/px4fmu-v2/board_config.h
+1
View File
@@ -113,6 +113,7 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define PX4_SPI_BUS_SENSORS 1
#define PX4_SPI_BUS_RAMTRON 2
#define PX4_SPI_BUS_EXT 4
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
src/drivers/px4fmu/fmu.cpp
+4
View File
@@ -1469,6 +1469,10 @@ PX4FMU::sensor_reset(int ms)
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
// // XXX bring up the EXTI pins again
// stm32_configgpio(GPIO_GYRO_DRDY);
// stm32_configgpio(GPIO_MAG_DRDY);
src/drivers/px4io/px4io.cpp
+2 -2
View File
@@ -1780,8 +1780,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
/* get RSSI from input channel */
if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) /
((_rssi_pwm_max - _rssi_pwm_min) / 100);
int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) /
(_rssi_pwm_max - _rssi_pwm_min);
rssi = rssi > 100 ? 100 : rssi;
rssi = rssi < 0 ? 0 : rssi;
input_rc.rssi = rssi;
src/firmware/posix/CMakeLists.txt
+1 -1
View File
@@ -33,7 +33,7 @@ add_custom_target(run_config
add_dependencies(run_config mainapp)
foreach(viewer none jmavsim gazebo)
foreach(debugger none gdb lldb)
foreach(debugger none gdb lldb ddd valgrind)
foreach(model none iris vtol)
if (debugger STREQUAL "none")
if (model STREQUAL "none")
src/lib/ecl/validation/data_validator.cpp
+26 -19
View File
@@ -43,6 +43,7 @@
#include <ecl/ecl.h>
DataValidator::DataValidator(DataValidator *prev_sibling) :
_error_mask(ERROR_FLAG_NO_ERROR),
_time_last(0),
_timeout_interval(20000),
_event_count(0),
@@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
float
DataValidator::confidence(uint64_t timestamp)
{
float ret = 1.0f;
/* check if we have any data */
if (_time_last == 0) {
return 0.0f;
_error_mask |= ERROR_FLAG_NO_DATA;
ret = 0.0f;
}
/* check error count limit */
if (_error_count > NORETURN_ERRCOUNT) {
return 0.0f;
/* timed out - that's it */
if (timestamp - _time_last > _timeout_interval) {
_error_mask |= ERROR_FLAG_TIMEOUT;
ret = 0.0f;
}
/* we got the exact same sensor value N times in a row */
if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
return 0.0f;
_error_mask |= ERROR_FLAG_STALE_DATA;
ret = 0.0f;
}
/* timed out - that's it */
if (timestamp - _time_last > _timeout_interval) {
return 0.0f;
/* check error count limit */
if (_error_count > NORETURN_ERRCOUNT) {
_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
ret = 0.0f;
}
/* cap error density counter at window size */
if (_error_density > ERROR_DENSITY_WINDOW) {
_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
_error_density = ERROR_DENSITY_WINDOW;
}
/* return local error density for last N measurements */
return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
}
int
DataValidator::priority()
{
return _priority;
/* no critical errors */
if(ret > 0.0f) {
/* return local error density for last N measurements */
ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
}
return ret;
}
void
src/lib/ecl/validation/data_validator.h
+23 -1
View File
@@ -91,7 +91,18 @@ public:
* Get the priority of this validator
* @return the stored priority
*/
int priority();
int priority() { return (_priority); }
/**
* Get the error state of this validator
* @return the bitmask with the error status
*/
uint32_t state() { return (_error_mask); }
/**
* Reset the error state of this validator
*/
void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
/**
* Get the RMS values of this validator
@@ -111,9 +122,20 @@ public:
* @param timeout_interval_us The timeout interval in microseconds
*/
void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
/**
* Data validator error states
*/
static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U);
static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U);
static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1);
static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2);
static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3);
static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4);
private:
static const unsigned _dimensions = 3;
uint32_t _error_mask; /**< sensor error state */
uint64_t _time_last; /**< last timestamp */
uint64_t _timeout_interval; /**< interval in which the datastream times out in us */
uint64_t _event_count; /**< total data counter */
src/lib/ecl/validation/data_validator_group.cpp
+46 -4
View File
@@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
bool true_failsafe = true;
/* check wether the switch was a failsafe or preferring a higher priority sensor */
/* check whether the switch was a failsafe or preferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio < max_priority &&
fabsf(pre_check_confidence - max_confidence) < 0.1f) {
/* this is not a failover */
true_failsafe = false;
/* reset error flags, this is likely a hotplug sensor coming online late */
best->reset_state();
}
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@@ -199,17 +201,25 @@ void
DataValidatorGroup::print()
{
/* print the group's state */
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used()) {
ECL_INFO("sensor #%u, prio: %d", i, next->priority());
uint32_t flags = next->state();
ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""),
((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
next->print();
}
next = next->sibling();
@@ -222,3 +232,35 @@ DataValidatorGroup::failover_count()
{
return _toggle_count;
}
int
DataValidatorGroup::failover_index()
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return i;
}
next = next->sibling();
i++;
}
return -1;
}
uint32_t
DataValidatorGroup::failover_state()
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return next->state();
}
next = next->sibling();
i++;
}
return DataValidator::ERROR_FLAG_NO_ERROR;
}
src/lib/ecl/validation/data_validator_group.h
+14
View File
@@ -80,6 +80,20 @@ public:
* @return the number of failovers
*/
unsigned failover_count();
/**
* Get the index of the failed sensor in the group
*
* @return index of the failed sensor
*/
int failover_index();
/**
* Get the error state of the failed sensor in the group
*
* @return bitmask with erro states of the failed sensor
*/
uint32_t failover_state();
/**
* Print the validator value
src/lib/launchdetection/CMakeLists.txt
-1
View File
@@ -37,7 +37,6 @@ px4_add_module(
SRCS
LaunchDetector.cpp
CatapultLaunchMethod.cpp
launchdetection_params.c
DEPENDS
platforms__common
)
src/lib/launchdetection/launchdetection_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Catapult launch detection parameters, accessible via MAVLink
*
src/lib/mathlib/math/Matrix.hpp
+3 -4
View File
@@ -49,7 +49,7 @@
#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
#else
#include "modules/local_position_estimator/matrix/src/Matrix.hpp"
#include "matrix/math.hpp"
#endif
#include <platforms/px4_defines.h>
@@ -339,9 +339,8 @@ public:
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
#else
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, M, N> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
Matrix<M, N> res(MyInverse.data());
matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
Matrix<M, N> res(Me.I().data());
return res;
#endif
}
src/lib/matrix
Submodule
+1
Submodule src/lib/matrix added at 7656385ea1
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+178 -22
View File
@@ -54,8 +54,11 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -117,22 +120,28 @@ private:
int _sensors_sub = -1;
int _params_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
struct {
param_t w_acc;
param_t w_mag;
param_t w_ext_hdg;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
param_t vibe_thresh;
param_t ext_hdg_mode;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_ext_hdg = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
@@ -140,11 +149,18 @@ private:
float _bias_max = 0.0f;
float _vibration_warning_threshold = 1.0f;
hrt_abstime _vibration_warning_timestamp = 0;
int _ext_hdg_mode = 0;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
vision_position_estimate_s _vision = {};
Vector<3> _vision_hdg;
att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
@@ -168,6 +184,7 @@ private:
bool _data_good = false;
bool _failsafe = false;
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
@@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_mag(3),
_lp_roll_rate(250.0f, 18.0f),
_lp_pitch_rate(250.0f, 18.0f),
_lp_yaw_rate(250, 10.0f)
_lp_yaw_rate(250.0f, 10.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
}
/**
@@ -269,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
void AttitudeEstimatorQ::task_main()
{
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -325,10 +348,17 @@ void AttitudeEstimatorQ::task_main()
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
/* ignore empty fields */
if (sensors.accelerometer_timestamp[i] > 0) {
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
}
/* ignore empty fields */
if (sensors.magnetometer_timestamp[i] > 0) {
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
}
}
int best_gyro, best_accel, best_mag;
@@ -346,12 +376,48 @@ void AttitudeEstimatorQ::task_main()
_data_good = true;
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
if (!_failsafe) {
uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
flags = _voter_gyro.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_accel.failover_count() > 0) {
_failsafe = true;
flags = _voter_accel.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_mag.failover_count() > 0) {
_failsafe = true;
flags = _voter_mag.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_failsafe) {
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
@@ -374,6 +440,47 @@ void AttitudeEstimatorQ::task_main()
}
}
// Update vision and motion capture heading
bool vision_updated = false;
orb_check(_vision_sub, &vision_updated);
bool mocap_updated = false;
orb_check(_mocap_sub, &mocap_updated);
if (vision_updated) {
orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
math::Quaternion q(_vision.q);
math::Matrix<3, 3> Rvis = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transposed() * v;
}
if (mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
math::Quaternion q(_mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transposed() * v;
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
} else if (_ext_hdg_mode == 2) {
_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
@@ -431,12 +538,9 @@ void AttitudeEstimatorQ::task_main()
att.pitch = euler(1);
att.yaw = euler(2);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_rates(2));
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -461,6 +565,34 @@ void AttitudeEstimatorQ::task_main()
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* Attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* Attitude rates for control state */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
}
}
}
@@ -478,6 +610,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -490,6 +623,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
}
}
@@ -545,12 +679,34 @@ bool AttitudeEstimatorQ::update(float dt)
// Angular rate of correction
Vector<3> corr;
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
if (_ext_hdg_mode > 0 && _ext_hdg_good) {
if (_ext_hdg_mode == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
}
if (_ext_hdg_mode == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
}
}
if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+20
View File
@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
/**
* Complimentary filter external heading weight
*
* @group Attitude Q estimator
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
/**
* Complimentary filter gyroscope bias weight
*
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
*/
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
/**
* External heading usage mode (from Motion capture/Vision)
* Set to 1 to use heading estimate from vision.
* Set to 2 to use heading from motion capture.
*
* @group Attitude Q estimator
* @min 0
* @max 2
*/
PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
/**
* Enable acceleration compensation based on GPS
* velocity.
src/modules/commander/PreflightCheck.cpp
+61 -35
View File
@@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid)
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
@@ -134,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
goto out;
@@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
success = false;
goto out;
@@ -154,7 +154,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
@@ -174,7 +174,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
goto out;
@@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
if(report_fail) mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
success = false;
goto out;
@@ -200,13 +200,15 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
/* this is frickin' fatal */
success = false;
goto out;
}
} else {
mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
goto out;
@@ -219,7 +221,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -229,8 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
return false;
@@ -239,8 +243,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
@@ -248,8 +254,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
@@ -259,7 +267,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -269,8 +277,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
return false;
@@ -294,7 +304,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success;
}
static bool airspeedCheck(int mavlink_fd, bool optional)
static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
{
bool success = true;
int ret;
@@ -304,13 +314,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
}
// XXX do not make this fatal yet
}
@@ -319,7 +333,7 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd)
static bool gnssCheck(int mavlink_fd, bool report_fail)
{
bool success = true;
@@ -341,8 +355,10 @@ static bool gnssCheck(int mavlink_fd)
}
//Report failure to detect module
if(!success) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
if (!success) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
}
px4_close(gpsSub);
@@ -350,7 +366,7 @@ static bool gnssCheck(int mavlink_fd)
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
@@ -365,7 +381,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -376,7 +392,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
}
failed = true;
}
}
@@ -392,7 +410,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
@@ -403,7 +421,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
}
failed = true;
}
}
@@ -419,7 +439,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -430,7 +450,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
}
failed = true;
}
}
@@ -446,7 +468,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -458,29 +480,33 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
}
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true)) {
if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed");
if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
}
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
if(!gnssCheck(mavlink_fd, reportFailures)) {
failed = true;
}
}
src/modules/commander/PreflightCheck.h
+1 -1
View File
@@ -67,7 +67,7 @@ namespace Commander
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);
const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;
src/modules/commander/airspeed_calibration.cpp
+20 -20
View File
@@ -68,7 +68,7 @@ static const char *sensor_name = "dpress";
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
@@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd)
const unsigned maxcount = 2400;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
@@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
@@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
@@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
@@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
@@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd)
goto error_return;
}
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
goto error_return;
} else {
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
@@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd)
goto error_return;
}
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
src/modules/commander/commander.cpp
+71 -25
View File
@@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000
@@ -360,6 +362,8 @@ int commander_main(int argc, char *argv[])
calib_ret = do_level_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "esc")) {
calib_ret = do_esc_calibration(mavlink_fd, &armed);
} else if (!strcmp(argv[2], "airspeed")) {
calib_ret = do_airspeed_calibration(mavlink_fd);
} else {
warnx("argument %s unsupported.", argv[2]);
}
@@ -377,23 +381,31 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
int checkres = prearm_check(&status, mavlink_fd_local);
int checkres = 0;
checkres = preflight_check(&status, mavlink_fd_local, false);
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, mavlink_fd_local, true);
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
px4_close(mavlink_fd_local);
warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
return 0;
}
if (!strcmp(argv[1], "arm")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(true, mavlink_fd_local, "command line");
warnx("note: not updating home position on commandline arming!");
if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) {
warnx("note: not updating home position on commandline arming!");
} else {
warnx("arming failed");
}
px4_close(mavlink_fd_local);
return 0;
}
if (!strcmp(argv[1], "disarm")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(false, mavlink_fd_local, "command line");
if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) {
warnx("rejected disarm");
}
px4_close(mavlink_fd_local);
return 0;
}
@@ -891,6 +903,7 @@ int commander_thread_main(int argc, char *argv[])
/* not yet initialized */
commander_initialized = false;
bool sensor_fail_tune_played = false;
bool arm_tune_played = false;
bool was_landed = true;
bool was_armed = false;
@@ -1020,6 +1033,9 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
status.condition_system_prearm_error_reported = true;
status.condition_system_hotplug_timeout = false;
status.counter++;
status.timestamp = hrt_absolute_time();
@@ -1101,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false;
rc_calibration_check(mavlink_fd);
rc_calibration_check(mavlink_fd, true);
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -1243,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
// Run preflight check
int32_t rc_in_off = 0;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
param_get(_param_autostart_id, &autostart_id);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
@@ -1251,14 +1268,10 @@ int commander_thread_main(int argc, char *argv[])
status.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
else {
// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
}
commander_boot_timestamp = hrt_absolute_time();
@@ -1354,11 +1367,11 @@ int commander_thread_main(int argc, char *argv[])
if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
/* re-check RC calibration */
rc_calibration_check(mavlink_fd);
rc_calibration_check(mavlink_fd, true);
}
/* Safety parameters */
@@ -1446,6 +1459,8 @@ int commander_thread_main(int argc, char *argv[])
!armed.armed) {
bool chAirspeed = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@@ -1456,11 +1471,11 @@ int commander_thread_main(int argc, char *argv[])
if (is_hil_setup(autostart_id)) {
/* HIL configuration: check only RC input */
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true);
} else {
/* check sensors also */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
}
@@ -1686,10 +1701,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "LANDING DETECTED");
mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED");
} else {
mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED");
}
}
@@ -2177,9 +2192,13 @@ int commander_thread_main(int argc, char *argv[])
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
/* only report a regain */
/* report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
} else if (telemetry_last_dl_loss[i] == 0){
/* do not report a new data link in order to not spam the user */
status.data_link_found_new = true;
status_changed = true;
}
telemetry_lost[i] = false;
@@ -2189,6 +2208,10 @@ int commander_thread_main(int argc, char *argv[])
/* telemetry was healthy also in last iteration
* we don't have to check a timeout */
have_link = true;
if(status.data_link_found_new) {
status.data_link_found_new = false;
status_changed = true;
}
}
} else {
@@ -2414,7 +2437,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
set_tune(TONE_STOP_TUNE);
}
/* reset arm_tune_played when disarmed */
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
@@ -2425,6 +2448,21 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
set_tune_override(TONE_GPS_WARNING_TUNE);
sensor_fail_tune_played = true;
status_changed = true;
}
/* update timeout flag */
if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
status.condition_system_hotplug_timeout = hotplug_timeout;
status_changed = true;
}
counter++;
@@ -2482,6 +2520,8 @@ get_circuit_breaker_params()
{
status.circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.cb_usb =
circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
@@ -2508,19 +2548,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* driving rgbled */
if (changed) {
bool set_normal_color = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* set mode */
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
} else { // STANDBY_ERROR and other states
rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
@@ -2536,7 +2581,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
rgbled_set_color(RGBLED_COLOR_RED);
} else {
if (status_local->condition_home_position_valid) {
if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN);
} else {
@@ -3197,6 +3242,7 @@ void *commander_low_prio_loop(void *arg)
// so this would be prone to false negatives.
bool checkAirspeed = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@@ -3204,7 +3250,7 @@ void *commander_low_prio_loop(void *arg)
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
src/modules/commander/state_machine_helper.cpp
+38 -24
View File
@@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_IN_AIR_RESTORE",
};
static bool sensor_feedback_provided = false;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
@@ -122,10 +120,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
*/
int prearm_ret = OK;
/* only perform the check if we have to */
/* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = prearm_check(status, mavlink_fd);
prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED ||
new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
status->condition_system_sensors_initialized = !prearm_ret;
}
/*
@@ -171,7 +177,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -196,10 +202,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
@@ -243,9 +249,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
if ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
sensor_feedback_provided = true;
status->condition_system_prearm_error_reported = true;
}
feedback_provided = true;
valid_transition = false;
@@ -262,7 +270,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* reset feedback state */
if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
valid_transition) {
sensor_feedback_provided = false;
status->condition_system_prearm_error_reported = false;
}
if(status->data_link_found_new == true)
{
status->condition_system_prearm_error_reported = false;
}
/* end of atomic state update */
@@ -380,7 +392,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
@@ -453,7 +465,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
@@ -490,11 +502,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
}
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
#endif
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
@@ -731,24 +743,26 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old;
}
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm)
{
/* at this point this equals the preflight check, but might add additional
* quantities later.
/*
*/
bool reportFailures = false;
reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout;
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
checkAirspeed = true;
}
bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
if (status->usb_connected) {
prearm_ok = false;
mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
if (!status->cb_usb && status->usb_connected && prearm) {
preflight_ok = false;
if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
}
return !prearm_ok;
return !preflight_ok;
}
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+9
View File
@@ -49,6 +49,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
@@ -154,12 +155,14 @@ private:
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
@@ -320,6 +323,12 @@ private:
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+79 -4
View File
@@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* publications */
_att_pub(nullptr),
_ctrl_state_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att{},
_ctrl_state{},
_gyro{},
_accel{},
_mag{},
@@ -698,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
// Publish attitude estimations
publishAttitude();
// publish control state
publishControlState();
// Publish Local Position estimations
publishLocalPosition();
@@ -822,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -833,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
/* publish the attitude setpoint */
/* publish the attitude */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
@@ -842,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
}
}
void AttitudePositionEstimatorEKF::publishControlState()
{
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
Vector3f v_b = _ekf->Tnb * v_n;
Vector3f v_b_var = _ekf->Tnb * v_n_var;
_ctrl_state.x_vel = v_b.x;
_ctrl_state.y_vel = v_b.y;
_ctrl_state.z_vel = v_b.z;
_ctrl_state.vel_variance[0] = v_b_var.x;
_ctrl_state.vel_variance[1] = v_b_var.y;
_ctrl_state.vel_variance[2] = v_b_var.z;
/* Local Position */
_ctrl_state.x_pos = _ekf->states[7];
_ctrl_state.y_pos = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
_ctrl_state.pos_variance[2] = _ekf->P[9][9];
/* Attitude */
_ctrl_state.timestamp = _last_sensor_timestamp;
_ctrl_state.q[0] = _ekf->states[0];
_ctrl_state.q[1] = _ekf->states[1];
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos))
{
// bad data, abort publication
return;
}
/* lazily publish the control state only once available */
if (_ctrl_state_pub != nullptr) {
/* publish the control state */
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
} else {
/* advertise and publish */
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
}
}
void AttitudePositionEstimatorEKF::publishLocalPosition()
{
_local_pos.timestamp = _last_sensor_timestamp;
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+4 -2
View File
@@ -48,6 +48,8 @@
#define M_PI_F static_cast<float>(M_PI)
#endif
#define MIN_AIRSPEED_MEAS 5.0f
constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
AttPosEKF::AttPosEKF() :
@@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the predicted airspeed
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
P[13][13] = sq(0.2f*dtIMU);
//Wind velocities
P[14][14] = 0.0f;
P[14][14] = 0.01f;
P[15][15] = P[14][14];
//Earth magnetic field
src/modules/fw_att_control/fw_att_control_main.cpp
+52 -86
View File
@@ -58,7 +58,6 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -67,7 +66,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -125,11 +124,10 @@ private:
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
@@ -144,12 +142,11 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
@@ -242,6 +239,11 @@ private:
} _parameter_handles; /**< handles for interesting parameters */
// Rotation matrix and euler angles to extract from control state
math::Matrix<3, 3> _R;
float _roll;
float _pitch;
float _yaw;
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
@@ -269,12 +271,6 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for airspeed updates.
*/
void vehicle_airspeed_poll();
/**
* Check for accel updates.
*/
@@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_accel_sub(-1),
_airspeed_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
@@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_debug(false)
{
/* safely initialize structs */
_att = {};
_ctrl_state = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
_actuators = {};
_actuators_airframe = {};
@@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
}
void
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
}
void
FixedwingAttitudeControl::vehicle_accel_poll()
{
@@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
@@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
@@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
@@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
float helper = _ctrl_state.roll_rate;
_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
_ctrl_state.yaw_rate = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
@@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
airspeed = math::max(0.5f, _ctrl_state.airspeed);
}
/*
@@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
@@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
@@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+53 -71
View File
@@ -68,14 +68,13 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
@@ -153,8 +152,7 @@ private:
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -165,11 +163,10 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
@@ -222,6 +219,9 @@ private:
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
float _roll;
float _pitch;
float _yaw;
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@@ -356,14 +356,9 @@ private:
bool vehicle_manual_control_setpoint_poll();
/**
* Check for airspeed updates.
* Check for changes in control state.
*/
bool vehicle_airspeed_poll();
/**
* Check for position updates.
*/
void vehicle_attitude_poll();
void control_state_poll();
/**
* Check for accel updates.
@@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
@@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_nav_capabilities_pub(nullptr),
/* states */
_att(),
_ctrl_state(),
_att_sp(),
_nav_capabilities(),
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
@@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
/* check if there is an airspeed update or if it timed out */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
@@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
return manual_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
FixedwingPositionControl::control_state_poll()
{
/* check if there is a new position */
bool att_updated;
orb_check(_att_sub, &att_updated);
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_R_nb(i, j) = PX4_R(_att.R, i, j);
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* set rotation matrix and euler angles */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R_nb = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R_nb.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
}
void
@@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
float airspeed;
if (_airspeed_valid) {
airspeed = _airspeed.true_airspeed_m_s;
airspeed = _ctrl_state.airspeed;
} else {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
@@ -1117,7 +1101,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@@ -1127,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -1221,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (pos_sp_triplet.previous.valid) {
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
land_noreturn_horizontal = true;
@@ -1469,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
@@ -1478,7 +1462,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@@ -1524,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
@@ -1543,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
@@ -1591,7 +1575,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@@ -1704,11 +1688,10 @@ FixedwingPositionControl::task_main()
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1785,10 +1768,9 @@ FixedwingPositionControl::task_main()
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
control_state_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
vehicle_manual_control_setpoint_poll();
// vehicle_baro_poll();
@@ -1894,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
limitOverride);
} else {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@@ -1907,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
_ctrl_state.airspeed, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+24 -24
View File
@@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
Q(X_bz, X_bz) = _pn_b_noise_power.get();
// continuous time kalman filter prediction
Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
// only predict for components we have
// valid measurements for
@@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
_flowY += global_speed[1] * dt;
// measurement
Vector2f y;
Vector<float, 2> y;
y(0) = _flowX;
y(1) = _flowY;
// residual
Vector2f r = y - C * _x;
Vector<float, 2> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
}
// measurement
Matrix<float, n_y_sonar, 1> y;
Vector<float, n_y_sonar> y;
y(0) = (d - _sonarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_sonar, 1> r = y - C * _x;
Vector<float, n_y_sonar> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_sonar, n_y_sonar> S_I =
(C * _P * C.transpose() + R).inverse();
inv<float, n_y_sonar>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
void BlockLocalPositionEstimator::correctBaro()
{
Matrix<float, n_y_baro, 1> y;
Vector<float, n_y_baro> y;
y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;
// baro measurement matrix
@@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_baro, 1> r = y - (C * _x);
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
}
// lower baro trust
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
} else if (_baroFault) {
_baroFault = FAULT_NONE;
@@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
R(0, 0) = cov;
}
Matrix<float, n_y_lidar, 1> y;
Vector<float, n_y_lidar> y;
y.setZero();
y(0) = (d - _lidarAltHome) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// residual
Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_lidar, 1> r = y - C * _x;
Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
Vector<float, n_y_lidar> r = y - C * _x;
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
Matrix<float, 6, 1> y;
Vector<float, 6> y;
y.setZero();
y(0) = px;
y(1) = py;
@@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
R(5, 5) = var_vz;
// residual
Matrix<float, 6, 1> r = y - C * _x;
Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
Vector<float, 6> r = y - C * _x;
Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
}
// trust GPS less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
@@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met
void BlockLocalPositionEstimator::correctVision()
{
Matrix<float, 3, 1> y;
Vector<float, 3> y;
y.setZero();
y(0) = _sub_vision_pos.get().x - _visionHome(0);
y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
// residual
Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
Matrix<float, n_y_vision, 1> r = y - C * _x;
// fault detection
@@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
} else if (_visionFault) {
_visionFault = FAULT_NONE;
@@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
void BlockLocalPositionEstimator::correctmocap()
{
Matrix<float, n_y_mocap, 1> y;
Vector<float, n_y_mocap> y;
y.setZero();
y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
// residual
Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
Matrix<float, n_y_mocap, 1> r = y - C * _x;
// fault detection
@@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
}
// trust less
S_I = ((C * _P * C.transpose()) + R * 10).inverse();
S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+3 -3
View File
@@ -6,7 +6,7 @@
#include <lib/geo/geo.h>
#ifdef USE_MATRIX_LIB
#include "matrix/src/Matrix.hpp"
#include "matrix/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
perf_counter_t _err_perf;
// state space
Matrix<float, n_x, 1> _x; // state vector
Matrix<float, n_u, 1> _u; // input vector
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
};
src/modules/local_position_estimator/CMakeLists.txt
-2
View File
@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error)
# add matrix tests
add_subdirectory(matrix)
endif()
# use custom matrix lib instead of Eigen
src/modules/local_position_estimator/matrix/.gitignore
-1
View File
@@ -1 +0,0 @@
build*/
src/modules/local_position_estimator/matrix/CMakeLists.txt
-13
View File
@@ -1,13 +0,0 @@
cmake_minimum_required(VERSION 2.8)
project(matrix CXX)
set(CMAKE_BUILD_TYPE "RelWithDebInfo")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
enable_testing()
include_directories(src)
add_subdirectory(test)
src/modules/local_position_estimator/matrix/src/Matrix.hpp
-464
View File
@@ -1,464 +0,0 @@
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
namespace matrix
{
template<typename T, size_t M, size_t N>
class Matrix
{
private:
T _data[M][N];
size_t _rows;
size_t _cols;
public:
Matrix() :
_data(),
_rows(M),
_cols(N)
{
}
Matrix(const T *data) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, data, sizeof(_data));
}
Matrix(const Matrix &other) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, other._data, sizeof(_data));
}
Matrix(T x, T y, T z) :
_data(),
_rows(M),
_cols(N)
{
// TODO, limit to only 3x1 matrices
_data[0][0] = x;
_data[1][0] = y;
_data[2][0] = z;
}
/**
* Accessors/ Assignment etc.
*/
T *data()
{
return _data[0];
}
inline size_t rows() const
{
return _rows;
}
inline size_t cols() const
{
return _rows;
}
inline T operator()(size_t i) const
{
return _data[i][0];
}
inline T operator()(size_t i, size_t j) const
{
return _data[i][j];
}
inline T &operator()(size_t i)
{
return _data[i][0];
}
inline T &operator()(size_t i, size_t j)
{
return _data[i][j];
}
/**
* Matrix Operations
*/
// this might use a lot of programming memory
// since it instantiates a class for every
// required mult pair, but it provides
// compile time size_t checking
template<size_t P>
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
{
const Matrix<T, M, N> &self = *this;
Matrix<T, M, P> res;
res.setZero();
for (size_t i = 0; i < M; i++) {
for (size_t k = 0; k < P; k++) {
for (size_t j = 0; j < N; j++) {
res(i, k) += self(i, j) * other(j, k);
}
}
}
return res;
}
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + other(i, j);
}
}
return res;
}
bool operator==(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
return false;
}
}
}
return true;
}
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) - other(i, j);
}
}
return res;
}
void operator+=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self + other;
}
void operator-=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self - other;
}
void operator*=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self * other;
}
/**
* Scalar Operations
*/
Matrix<T, M, N> operator*(T scalar) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) * scalar;
}
}
return res;
}
Matrix<T, M, N> operator+(T scalar) const
{
Matrix<T, M, N> res;
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + scalar;
}
}
return res;
}
void operator*=(T scalar)
{
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = self(i, j) * scalar;
}
}
}
void operator/=(T scalar)
{
Matrix<T, M, N> &self = *this;
self = self * (1.0f / scalar);
}
/**
* Misc. Functions
*/
void print()
{
Matrix<T, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {
printf("[");
for (size_t j = 0; j < N; j++) {
printf("%10g\t", double(self(i, j)));
}
printf("]\n");
}
}
Matrix<T, N, M> transpose() const
{
Matrix<T, N, M> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(j, i) = self(i, j);
}
}
return res;
}
Matrix<T, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) { break; }
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Matrix<T, M, 1> diagonal() const
{
Matrix<T, M, 1> res;
// force square for now
const Matrix<T, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
}
void setIdentity()
{
setZero();
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M and i < N; i++) {
self(i, i) = 1;
}
}
inline void swapRows(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t j = 0; j < cols(); j++) {
T tmp = self(a, j);
self(a, j) = self(b, j);
self(b, j) = tmp;
}
}
inline void swapCols(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < rows(); i++) {
T tmp = self(i, a);
self(i, a) = self(i, b);
self(i, b) = tmp;
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <T, M, M> inverse() const
{
Matrix<T, M, M> L;
L.setIdentity();
const Matrix<T, M, M> &A = (*this);
Matrix<T, M, M> U = A;
Matrix<T, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) { continue; }
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<T, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<T, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<T, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
};
typedef Matrix<float, 2, 1> Vector2f;
typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix
src/modules/local_position_estimator/matrix/test/CMakeLists.txt
-15
View File
@@ -1,15 +0,0 @@
set(tests
setIdentity
inverse
matrixMult
vectorAssignment
matrixAssignment
matrixScalarMult
transpose
)
foreach(test ${tests})
add_executable(${test}
${test}.cpp)
add_test(${test} ${test})
endforeach()
src/modules/local_position_estimator/matrix/test/inverse.cpp
-30
View File
@@ -1,30 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
Matrix3f A_I = A.inverse();
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I_check(data_check);
(void)A_I;
assert(A_I == A_I_check);
// stess test
static const size_t n = 100;
Matrix<float, n, n> A_large;
A_large.setIdentity();
Matrix<float, n, n> A_large_I;
A_large_I.setZero();
for (size_t i = 0; i < 100; i++) {
A_large_I = A_large.inverse();
assert(A_large == A_large_I);
}
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp
-29
View File
@@ -1,29 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
m.print();
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
m2.print();
assert(m == m2);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixMult.cpp
-26
View File
@@ -1,26 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
A.print();
A_I.print();
Matrix3f R = A * A_I;
R.print();
assert(R == I);
Matrix3f R2 = A;
R2 *= A_I;
R2.print();
assert(R2 == I);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp
-18
View File
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
A.print();
A_check.print();
assert(A == A_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/setIdentity.cpp
-25
View File
@@ -1,25 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f A;
A.setIdentity();
assert(A.rows() == 3);
assert(A.cols() == 3);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
assert(A(i, j) == 1);
} else {
assert(A(i, j) == 0);
}
}
}
return 0;
}
src/modules/local_position_estimator/matrix/test/transpose.cpp
-18
View File
@@ -1,18 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[9] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
A_T.print();
A_T_check.print();
assert(A_T == A_T_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp
-28
View File
@@ -1,28 +0,0 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
v.print();
assert(v(0) == 1);
assert(v(1) == 2);
assert(v(2) == 3);
Vector3f v2(4, 5, 6);
v2.print();
assert(v2(0) == 4);
assert(v2(1) == 5);
assert(v2(2) == 6);
return 0;
}
src/modules/mavlink/CMakeLists.txt
-1
View File
@@ -56,7 +56,6 @@ px4_add_module(
mavlink_rate_limiter.cpp
mavlink_receiver.cpp
mavlink_ftp.cpp
mavlink_params.c
DEPENDS
platforms__common
)
src/modules/mavlink/mavlink_params.c
-2
View File
@@ -31,8 +31,6 @@
*
****************************************************************************/
#include <systemlib/param/param.h>
/**
* MAVLink system ID
* @group MAVLink
src/modules/mc_att_control/mc_att_control_main.cpp
+16 -16
View File
@@ -74,7 +74,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
@@ -126,7 +126,7 @@ private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@@ -145,7 +145,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@@ -297,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
@@ -319,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@@ -599,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
@@ -693,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
@@ -733,7 +733,7 @@ MulticopterAttitudeControl::task_main()
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -747,7 +747,7 @@ MulticopterAttitudeControl::task_main()
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _v_att_sub;
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -783,8 +783,8 @@ MulticopterAttitudeControl::task_main()
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* copy attitude and control state topics */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */
parameter_update_poll();
@@ -862,7 +862,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
src/modules/mc_att_control_multiplatform/CMakeLists.txt
-1
View File
@@ -38,7 +38,6 @@ px4_add_module(
mc_att_control_start_nuttx.cpp
mc_att_control.cpp
mc_att_control_base.cpp
mc_att_control_params.c
DEPENDS
platforms__common
)
src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+1 -5
View File
@@ -36,14 +36,10 @@
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
* Roll P gain
*
src/modules/mc_pos_control/mc_pos_control_main.cpp
+31 -17
View File
@@ -68,7 +68,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@@ -119,14 +119,14 @@ public:
int start();
private:
const float alt_ctl_dz = 0.2f;
const float alt_ctl_dz = 0.1f;
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
@@ -142,7 +142,7 @@ private:
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@@ -230,6 +230,9 @@ private:
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
/**
* Update our local parameter cache.
*/
@@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_mavlink_fd(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
@@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true)
_run_alt_control(true),
_yaw(0.0f)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
@@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_prev.zero();
_vel_ff.zero();
_R.identity();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P");
@@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
orb_check(_ctrl_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
orb_check(_att_sp_sub, &updated);
@@ -1009,7 +1015,7 @@ MulticopterPositionControl::task_main()
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1064,6 +1070,14 @@ MulticopterPositionControl::task_main()
}
poll_subscriptions();
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
@@ -1139,7 +1153,7 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
@@ -1324,11 +1338,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1515,7 +1529,7 @@ MulticopterPositionControl::task_main()
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while arming */
@@ -1525,7 +1539,7 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it
src/modules/navigator/mission_params.c
+1 -1
View File
@@ -52,7 +52,7 @@
* @unit meters
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
/**
* Enable persistent onboard mission storage
src/modules/navigator/navigator.h
+2 -4
View File
@@ -135,7 +135,7 @@ public:
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
bool home_position_valid() { return _home_position_set; }
bool home_position_valid() { return (_home_pos.timestamp > 0); }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
@@ -205,8 +205,6 @@ private:
geofence_result_s _geofence_result;
vehicle_attitude_setpoint_s _att_sp;
bool _home_position_set;
bool _mission_item_valid; /**< flags if the current mission item is valid */
int _mission_instance_count; /**< instance count for the current mission */
@@ -257,7 +255,7 @@ private:
/**
* Retrieve home position
*/
void home_position_update();
void home_position_update(bool force=false);
/**
* Retreive navigation capabilities
src/modules/navigator/navigator_main.cpp
+5 -10
View File
@@ -126,7 +126,6 @@ Navigator::Navigator() :
_pos_sp_triplet{},
_mission_result{},
_att_sp{},
_home_position_set(false),
_mission_item_valid(false),
_mission_instance_count(0),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -206,17 +205,13 @@ Navigator::sensor_combined_update()
}
void
Navigator::home_position_update()
Navigator::home_position_update(bool force)
{
bool updated = false;
orb_check(_home_pos_sub, &updated);
if (updated) {
if (updated || force) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
if (_home_pos.timestamp > 0) {
_home_position_set = true;
}
}
}
@@ -298,7 +293,7 @@ Navigator::task_main()
global_position_update();
gps_position_update();
sensor_combined_update();
home_position_update();
home_position_update(true);
navigation_capabilities_update();
params_update();
@@ -408,7 +403,7 @@ Navigator::task_main()
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
@@ -489,7 +484,7 @@ Navigator::task_main()
}
/* iterate through navigation modes and set active/inactive for each */
for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
src/modules/sdlog2/sdlog2.c
+37 -17
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
@@ -155,7 +156,9 @@ struct logbuffer_s lb;
static pthread_mutex_t logbuffer_mutex;
static pthread_cond_t logbuffer_cond;
static char log_dir[32];
#define LOG_BASE_PATH_LEN 64
static char log_dir[LOG_BASE_PATH_LEN];
/* statistics counters */
static uint64_t start_time = 0;
@@ -422,7 +425,6 @@ int create_log_dir()
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
warnx("log dir created: %s", log_dir);
break;
} else if (errno != EEXIST) {
@@ -443,15 +445,15 @@ int create_log_dir()
}
/* print logging path, important to find log file later */
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir);
return 0;
}
int open_log_file()
{
/* string to hold the path to the log */
char log_file_name[32] = "";
char log_file_path[64] = "";
char log_file_name[64] = "";
char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = "";
struct tm tt;
bool time_ok = get_log_time_utc_tt(&tt, false);
@@ -479,7 +481,7 @@ int open_log_file()
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@@ -491,10 +493,10 @@ int open_log_file()
#endif
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name);
} else {
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name);
mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name);
}
return fd;
@@ -503,8 +505,8 @@ int open_log_file()
int open_perf_file(const char* str)
{
/* string to hold the path to the log */
char log_file_name[32] = "";
char log_file_path[64] = "";
char log_file_name[64] = "";
char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = "";
struct tm tt;
bool time_ok = get_log_time_utc_tt(&tt, false);
@@ -531,7 +533,7 @@ int open_perf_file(const char* str)
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@@ -543,7 +545,7 @@ int open_perf_file(const char* str)
#endif
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name);
}
@@ -670,7 +672,7 @@ void sdlog2_start_log()
/* create log dir if needed */
if (create_log_dir() != 0) {
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir");
return;
}
@@ -760,7 +762,7 @@ void sdlog2_stop_log()
/* free log writer performance counter */
perf_free(perf_write);
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped");
mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped");
sdlog2_status();
}
@@ -1089,6 +1091,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vtol_vehicle_status_s vtol_status;
struct time_offset_s time_offset;
struct mc_att_ctrl_status_s mc_att_ctrl_status;
struct control_state_s ctrl_state;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -1137,6 +1140,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ENCD_s log_ENCD;
struct log_TSYN_s log_TSYN;
struct log_MACS_s log_MACS;
struct log_CTS_s log_CTS;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1179,6 +1183,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int encoders_sub;
int tsync_sub;
int mc_att_ctrl_status_sub;
int ctrl_state_sub;
} subs;
subs.cmd_sub = -1;
@@ -1212,6 +1217,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.wind_sub = -1;
subs.tsync_sub = -1;
subs.mc_att_ctrl_status_sub = -1;
subs.ctrl_state_sub = -1;
subs.encoders_sub = -1;
/* add new topics HERE */
@@ -1670,6 +1676,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.rssi = buf.rc.rssi;
log_msg.body.log_RC.channel_count = buf.rc.channel_count;
log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
LOGBUFFER_WRITE_AND_COUNT(RC);
@@ -1856,6 +1863,19 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(MACS);
}
/* --- CONTROL STATE --- */
if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) {
log_msg.msg_type = LOG_CTS_MSG;
log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel;
log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel;
log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel;
log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed;
log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate;
log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate;
log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate;
LOGBUFFER_WRITE_AND_COUNT(CTS);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
@@ -1893,7 +1913,7 @@ void sdlog2_status()
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}
}
@@ -1918,7 +1938,7 @@ int check_free_space()
/* use a threshold of 50 MiB */
if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
mavlink_and_console_log_critical(mavlink_fd,
"[sdlog2] no space on MicroSD: %u MiB",
"[log] no space on MicroSD: %u MiB",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
/* we do not need a flag to remember that we sent this warning because we will exit anyway */
return PX4_ERROR;
@@ -1926,7 +1946,7 @@ int check_free_space()
/* use a threshold of 100 MiB to send a warning */
} else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
mavlink_and_console_log_critical(mavlink_fd,
"[sdlog2] space on MicroSD low: %u MiB",
"[log] space on MicroSD low: %u MiB",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
/* we don't want to flood the user with warnings */
space_warning_sent = true;
src/modules/sdlog2/sdlog2_messages.h
+15 -1
View File
@@ -189,6 +189,7 @@ struct log_STAT_s {
#define LOG_RC_MSG 11
struct log_RC_s {
float channel[8];
uint8_t rssi;
uint8_t channel_count;
uint8_t signal_lost;
};
@@ -477,6 +478,18 @@ struct log_MACS_s {
float yaw_rate_integ;
};
/* --- CONTROL STATE --- */
#define LOG_CTS_MSG 47
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* WARNING: ID 46 is already in use for ATTC1 */
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -519,7 +532,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(VTOL, "f", "Arsp"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
src/modules/sensors/sensors.cpp
+3 -3
View File
@@ -850,7 +850,7 @@ Sensors::parameters_update()
_parameters.battery_voltage_scaling = 0.0082f;
#elif CONFIG_ARCH_BOARD_AEROCORE
_parameters.battery_voltage_scaling = 0.0063f;
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
#elif CONFIG_ARCH_BOARD_PX4FMU_V1
_parameters.battery_voltage_scaling = 0.00459340659f;
#else
/* ensure a missing default trips a low voltage lockdown */
@@ -1250,7 +1250,7 @@ Sensors::vehicle_control_mode_poll()
void
Sensors::parameter_update_poll(bool forced)
{
bool param_updated;
bool param_updated = false;
/* Check if any parameter has changed */
orb_check(_params_sub, &param_updated);
@@ -2146,7 +2146,7 @@ Sensors::task_main()
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];
px4_pollfd_struct_t fds[1] = {};
/* use the gyro to pace output */
fds[0].fd = _gyro_sub[0];
src/modules/simulator/simulator_mavlink.cpp
+2
View File
@@ -463,10 +463,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
char serial_buf[1024];
struct pollfd fds[2];
memset(fds, 0, sizeof(fds));
unsigned fd_count = 1;
fds[0].fd = _fd;
fds[0].events = POLLIN;
if (serial_fd >= 0) {
fds[1].fd = serial_fd;
fds[1].events = POLLIN;
src/modules/systemlib/circuit_breaker.h
+1
View File
@@ -56,6 +56,7 @@
#define CBRK_FLIGHTTERM_KEY 121212
#define CBRK_ENGINEFAIL_KEY 284953
#define CBRK_GPSFAIL_KEY 240024
#define CBRK_USB_CHK_KEY 197848
#include <stdbool.h>
src/modules/systemlib/circuit_breaker_params.c
+13
View File
@@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_BUZZER, 0);
/**
* Circuit breaker for USB link check
*
* Setting this parameter to 197848 will disable the USB connected
* checks in the commander.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 197848
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_USB_CHK, 0);
src/modules/systemlib/rc_check.c
+28 -11
View File
@@ -52,7 +52,7 @@
#define RC_INPUT_MAP_UNMAPPED 0
int rc_calibration_check(int mavlink_fd)
int rc_calibration_check(int mavlink_fd, bool report_fail)
{
char nbuf[20];
@@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd)
param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) {
mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
@@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd)
param_get(map_parm, &mapping);
if (mapping > RC_INPUT_MAX_CHANNELS) {
mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
}
if (mapping == 0) {
mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
@@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd)
/* assert min..center..max ordering */
if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US);
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
@@ -187,8 +199,13 @@ int rc_calibration_check(int mavlink_fd)
if (channels_failed) {
sleep(2);
mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.",
total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
}
usleep(100000);
}
src/modules/systemlib/rc_check.h
+1 -1
View File
@@ -47,6 +47,6 @@ __BEGIN_DECLS
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
__EXPORT int rc_calibration_check(int mavlink_fd);
__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail);
__END_DECLS
src/modules/systemlib/system_params.c
+2 -2
View File
@@ -40,7 +40,7 @@
/**
* Auto-start script index.
*
* Defines the auto-start script used to bootstrap the system.
* CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system.
*
* @group System
*/
@@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
/**
* Companion computer interface
*
* Configures the baud rate of the companion computer interface.
* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface.
* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
* 157600: enables OSD mode at 57600 baud, 8N1.
src/modules/uORB/objects_common.cpp
+3
View File
@@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s);
#include "topics/vehicle_attitude.h"
ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
#include "topics/control_state.h"
ORB_DEFINE(control_state, struct control_state_s);
#include "topics/sensor_combined.h"
ORB_DEFINE(sensor_combined, struct sensor_combined_s);
src/modules/uavcan/CMakeLists.txt
-3
View File
@@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
string(TOUPPER "${OS}" OS_UPPER)
add_definitions(
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
-DUAVCAN_NO_ASSERTIONS
-DUAVCAN_PLATFORM=stm32
-DUAVCAN_STM32_${OS_UPPER}=1
-DUAVCAN_STM32_NUM_IFACES=2
-DUAVCAN_STM32_TIMER_NUMBER=5
-DUAVCAN_USE_CPP03=ON
-DUAVCAN_USE_EXTERNAL_SNPRINT
)
add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
src/modules/uavcan/allocator.hpp
+73
View File
@@ -0,0 +1,73 @@
/****************************************************************************
*
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <systemlib/err.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step.
namespace uavcan_node
{
struct AllocatorSynchronizer
{
const ::irqstate_t state = ::irqsave();
~AllocatorSynchronizer() { ::irqrestore(state); }
};
struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>
{
static constexpr unsigned CapacitySoftLimit = 250;
static constexpr unsigned CapacityHardLimit = 500;
Allocator() :
uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>(CapacitySoftLimit, CapacityHardLimit)
{ }
~Allocator()
{
if (getNumAllocatedBlocks() > 0)
{
warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
}
}
};
}
src/modules/uavcan/libuavcan
+1 -1
Submodule src/modules/uavcan/libuavcan updated: 0643879922...ed1d71e639
src/modules/uavcan/sensors/baro.cpp
+4 -11
View File
@@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
_sub_air_pressure_data(node),
_sub_air_temperature_data(node),
_reports(nullptr)
_reports(2, sizeof(baro_report))
{ }
int UavcanBarometerBridge::init()
@@ -55,13 +55,6 @@ int UavcanBarometerBridge::init()
return res;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
return -1;
}
res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
if (res < 0) {
@@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
}
while (count--) {
if (_reports->get(baro_buf)) {
if (_reports.get(baro_buf)) {
ret += sizeof(*baro_buf);
baro_buf++;
}
@@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
if (!_reports.resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
@@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// add to the ring buffer
_reports->force(&report);
_reports.force(&report);
publish(msg.getSrcNodeID().get(), &report);
}
src/modules/uavcan/sensors/baro.hpp
+3 -1
View File
@@ -76,8 +76,10 @@ private:
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
ringbuffer::RingBuffer _reports;
unsigned _msl_pressure = 101325;
ringbuffer::RingBuffer *_reports;
float last_temperature_kelvin = 0.0f;
};
src/modules/uavcan/uavcan_main.cpp
+50 -6
View File
@@ -77,7 +77,7 @@
UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_node(can_driver, system_clock, _pool_allocator),
_node_mutex(),
_esc_controller(_node),
_time_sync_master(_node),
@@ -144,10 +144,9 @@ UavcanNode::~UavcanNode()
} while (_task != -1);
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
(void)::close(_armed_sub);
(void)::close(_test_motor_sub);
(void)::close(_actuator_direct_sub);
// Removing the sensor bridges
auto br = _sensor_bridges.getHead();
@@ -166,6 +165,10 @@ UavcanNode::~UavcanNode()
pthread_mutex_destroy(&_node_mutex);
px4_sem_destroy(&_server_command_sem);
// Is it allowed to delete it like that?
if (_mixers != nullptr) {
delete _mixers;
}
}
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
@@ -965,6 +968,8 @@ int UavcanNode::run()
}
}
(void)::close(busevent_fd);
teardown();
warnx("exiting.");
@@ -1119,6 +1124,33 @@ UavcanNode::print_info()
(void)pthread_mutex_lock(&_node_mutex);
// Memory status
printf("Pool allocator status:\n");
printf("\tCapacity hard/soft: %u/%u blocks\n",
_pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity());
printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks());
printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
// UAVCAN node perfcounters
printf("UAVCAN node status:\n");
printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount());
printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
// CAN driver status
for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
printf("CAN%u status:\n", unsigned(i + 1));
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
printf("\tHW errors: %llu\n", iface->getErrorCount());
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
printf("\tIO errors: %llu\n", iface_perf_cnt.errors);
printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx);
printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
}
// ESC mixer status
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
@@ -1169,13 +1201,20 @@ UavcanNode::print_info()
(void)pthread_mutex_unlock(&_node_mutex);
}
void UavcanNode::shrink()
{
(void)pthread_mutex_lock(&_node_mutex);
_pool_allocator.shrink();
(void)pthread_mutex_unlock(&_node_mutex);
}
/*
* App entry point
*/
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
"\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -1251,6 +1290,11 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
if (!std::strcmp(argv[1], "shrink")) {
inst->shrink();
::exit(0);
}
if (!std::strcmp(argv[1], "arm")) {
inst->arm_actuators(true);
::exit(0);
src/modules/uavcan/uavcan_main.hpp
+10 -6
View File
@@ -36,6 +36,7 @@
#include <px4_config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
#include <uavcan/protocol/global_time_sync_master.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
@@ -55,6 +56,7 @@
#include "sensors/sensor_bridge.hpp"
#include "uavcan_servers.hpp"
#include "allocator.hpp"
/**
* @file uavcan_main.hpp
@@ -78,9 +80,8 @@ class UavcanNode : public device::CDev
static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame;
static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1);
static constexpr unsigned PollTimeoutMs = 10;
static constexpr unsigned PollTimeoutMs = 3;
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
/*
* This memory is reserved for uavcan to use for queuing CAN frames.
* At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -97,7 +98,6 @@ class UavcanNode : public device::CDev
static constexpr unsigned StackSize = 1800;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
enum eServerAction {None, Start, Stop, CheckFW , Busy};
@@ -109,7 +109,7 @@ public:
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node &get_node() { return _node; }
uavcan::Node<> &get_node() { return _node; }
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
@@ -121,6 +121,8 @@ public:
void print_info();
void shrink();
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
@@ -130,8 +132,8 @@ public:
int set_param(int remote_node_id, const char *name, char *value);
int get_param(int remote_node_id, const char *name);
int reset_node(int remote_node_id);
private:
private:
void fill_node_info();
int init(uavcan::NodeID node_id);
void node_spin_once();
@@ -167,7 +169,9 @@ private:
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
uavcan_node::Allocator _pool_allocator;
uavcan::Node<> _node; ///< library instance
pthread_mutex_t _node_mutex;
px4_sem_t _server_command_sem;
UavcanEscController _esc_controller;
src/modules/uavcan/uavcan_servers.cpp
+12 -9
View File
@@ -85,8 +85,8 @@
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()),
_main_node(main_node),
_tracer(),
_storage_backend(),
@@ -141,13 +141,14 @@ int UavcanServers::stop()
return -1;
}
_instance = nullptr;
if (server->_subnode_thread != -1) {
pthread_cancel(server->_subnode_thread);
pthread_join(server->_subnode_thread, NULL);
if (server->_subnode_thread) {
warnx("stopping fw srv thread...");
server->_subnode_thread_should_exit = true;
(void)pthread_join(server->_subnode_thread, NULL);
}
_instance = nullptr;
server->_main_node.getDispatcher().removeRxFrameListener();
delete server;
@@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
_esc_enumeration_index = 0;
while (1) {
while (!_subnode_thread_should_exit) {
if (_check_fw == true) {
_check_fw = false;
@@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
}
}
warnx("exiting.");
_subnode_thread_should_exit = false;
warnx("exiting");
return (pthread_addr_t) 0;
}
src/modules/uavcan/uavcan_servers.hpp
+6 -21
View File
@@ -81,24 +81,10 @@ class UavcanServers
{
static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
static constexpr unsigned MaxCanFramesPerTransfer = 63;
/**
* This number is based on the worst case max number of frames per interface. With
* MemPoolBlockSize set at 48 this is 6048 Bytes.
*
* The servers can be forced to use the primary interface only, this can be achieved simply by passing
* 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
*/
static constexpr unsigned QueuePoolSize =
(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
static constexpr unsigned StackSize = 6000;
static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode;
static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80;
public:
UavcanServers(uavcan::INode &main_node);
@@ -108,7 +94,7 @@ public:
static int start(uavcan::INode &main_node);
static int stop();
SubNode &get_node() { return _subnode; }
uavcan::SubNode<> &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
@@ -124,6 +110,7 @@ public:
private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
volatile bool _subnode_thread_should_exit = false;
int init();
@@ -131,12 +118,10 @@ private:
static UavcanServers *_instance; ///< singleton pointer
typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
VirtualCanDriver _vdriver;
vCanDriver _vdriver;
uavcan::SubNode<MemPoolSize> _subnode; ///< library instance
uavcan::INode &_main_node; ///< library instance
uavcan::SubNode<> _subnode;
uavcan::INode &_main_node;
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
src/modules/uavcan/uavcan_virtual_can_driver.hpp
+17 -14
View File
@@ -114,6 +114,14 @@ public:
uavcan::IsDynamicallyAllocatable<Item>::check();
}
~Queue()
{
while (!isEmpty())
{
pop();
}
}
bool isEmpty() const { return list_.isEmpty(); }
/**
@@ -329,11 +337,7 @@ public:
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
* @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
* memory pool that will be shared across all interfaces for RX/TX queues.
* Typically this value should be no less than 4K per interface.
*/
template <unsigned SharedMemoryPoolSize>
class VirtualCanDriver : public uavcan::ICanDriver,
public uavcan::IRxFrameListener,
public ITxQueueInjector,
@@ -400,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver,
}
};
Event event_; ///< Used to unblock the select() call when IO happens.
Event event_; ///< Used to unblock the select() call when IO happens.
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
const unsigned num_ifaces_;
uavcan::ISystemClock &clock_;
@@ -476,7 +479,10 @@ class VirtualCanDriver : public uavcan::ICanDriver,
}
public:
VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
VirtualCanDriver(unsigned arg_num_ifaces,
uavcan::ISystemClock &system_clock,
uavcan::IPoolAllocator& allocator,
unsigned virtual_iface_block_allocation_quota) :
num_ifaces_(arg_num_ifaces),
clock_(system_clock)
{
@@ -485,15 +491,12 @@ public:
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &,
pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue);
ifaces_[i].template
construct<uavcan::IPoolAllocator&, uavcan::ISystemClock&, pthread_mutex_t&, unsigned>
(allocator, clock_, driver_mutex_, quota_per_queue);
}
}
src/modules/vtol_att_control/CMakeLists.txt
-3
View File
@@ -38,12 +38,9 @@ px4_add_module(
SRCS
vtol_att_control_main.cpp
vtol_att_control_params.c
tiltrotor_params.c
tiltrotor.cpp
vtol_type.cpp
tailsitter.cpp
standard_params.c
standard.cpp
DEPENDS
platforms__common
src/modules/vtol_att_control/standard.cpp
+56 -40
View File
@@ -60,7 +60,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
}
}
Standard::~Standard()
{
@@ -98,12 +98,12 @@ Standard::parameters_update()
void Standard::update_vtol_state()
{
parameters_update();
parameters_update();
/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* forward speed. After the vehicle has picked up enough speed the rotors shutdown.
* For the back transition the pusher motor is immediately stopped and rotors reactivated.
*/
*/
if (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
@@ -130,7 +130,7 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
(_params_standard.back_trans_dur * 1000000.0f)) {
(_params_standard.back_trans_dur * 1000000.0f)) {
_vtol_schedule.flight_mode = MC_MODE;
}
}
@@ -154,7 +154,7 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) {
if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) {
_vtol_schedule.flight_mode = FW_MODE;
// we can turn off the multirotor motors now
_flag_enable_mc_motors = false;
@@ -168,17 +168,19 @@ void Standard::update_vtol_state()
}
// map specific control phases to simple control modes
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_TO_FW:
case TRANSITION_TO_MC:
_vtol_mode = TRANSITION;
break;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_TO_FW:
case TRANSITION_TO_MC:
_vtol_mode = TRANSITION;
break;
}
}
@@ -188,18 +190,21 @@ void Standard::update_transition_state()
if (_params_standard.front_trans_dur <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params_standard.pusher_trans;
} else if (_pusher_throttle <= _params_standard.pusher_trans) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls if a blending airspeed has been provided
if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed_trans_blend_margin;
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
@@ -210,17 +215,19 @@ void Standard::update_transition_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_dur > 0.0f) {
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
1000000.0f);
_mc_roll_weight = weight;
_mc_pitch_weight = weight;
_mc_yaw_weight = weight;
} else {
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
@@ -238,15 +245,15 @@ void Standard::update_mc_state()
// do nothing
}
void Standard::update_fw_state()
void Standard::update_fw_state()
{
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw(); // force them to stop, not just idle
_flag_enable_mc_motors = true;
}
}
}
void Standard::update_external_state()
{
@@ -259,22 +266,31 @@ void Standard::update_external_state()
void Standard::fill_actuator_outputs()
{
/* multirotor controls */
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight; // roll
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight; // yaw
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
/* fixed wing controls */
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight); //roll
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_weight); // yaw
// set the fixed wing throttle control
if (_vtol_schedule.flight_mode == FW_MODE) {
// take the throttle value commanded by the fw controller
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
// otherwise we may be ramping up the throttle during the transition to fw mode
// otherwise we may be ramping up the throttle during the transition to fw mode
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
}
}
@@ -288,11 +304,11 @@ Standard::set_max_mc(unsigned pwm_value)
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
@@ -301,9 +317,9 @@ Standard::set_max_mc(unsigned pwm_value)
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
if (ret != OK) {PX4_WARN("failed setting max values");}
close(fd);
px4_close(fd);
}
src/modules/vtol_att_control/standard.h
+11 -11
View File
@@ -31,15 +31,15 @@
*
****************************************************************************/
/**
* @file standard.h
* VTOL with fixed multirotor motor configurations (such as quad) and a pusher
* (or puller aka tractor) motor for forward flight.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file standard.h
* VTOL with fixed multirotor motor configurations (such as quad) and a pusher
* (or puller aka tractor) motor for forward flight.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef STANDARD_H
#define STANDARD_H
@@ -52,7 +52,7 @@ class Standard : public VtolType
public:
Standard(VtolAttitudeControl * _att_controller);
Standard(VtolAttitudeControl *_att_controller);
~Standard();
void update_vtol_state();
@@ -89,7 +89,7 @@ private:
struct {
vtol_mode flight_mode; // indicates in which mode the vehicle is in
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
}_vtol_schedule;
} _vtol_schedule;
bool _flag_enable_mc_motors;
float _pusher_throttle;
src/modules/vtol_att_control/standard_params.c
+1 -3
View File
@@ -36,11 +36,9 @@
* Parameters for the standard VTOL controller.
*
* @author Simon Wilks <simon@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Target throttle value for pusher/puller motor during the transition to fw mode
*
src/modules/vtol_att_control/tailsitter.cpp
+75 -56
View File
@@ -31,21 +31,21 @@
*
****************************************************************************/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
}
@@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state()
// simply switch between the two modes
if (!_attc->is_fixed_wing_requested()) {
_vtol_mode = ROTARY_WING;
} else {
_vtol_mode = FIXED_WING;
}
@@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state()
void Tailsitter::update_mc_state()
{
if (!flag_idle_mc) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
@@ -91,18 +92,18 @@ void Tailsitter::update_external_state()
}
void Tailsitter::calc_tot_airspeed()
{
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P,1.0f,_params->power_max);
P = math::constrain(P, 1.0f, _params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params->prop_eff/_params->power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
float power_factor = 1.0f - P * _params->prop_eff / _params->power_max;
float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f;
eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
float v_ind = (airspeed / eta - airspeed) * 2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
@@ -115,16 +116,19 @@ Tailsitter::scale_mc_output()
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max);
airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
@@ -135,8 +139,10 @@ Tailsitter::scale_mc_output()
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
-1.0f, 1.0f);
}
/**
@@ -144,37 +150,50 @@ Tailsitter::scale_mc_output()
*/
void Tailsitter::fill_actuator_outputs()
{
switch(_vtol_mode) {
case ROTARY_WING:
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
switch (_vtol_mode) {
case ROTARY_WING:
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
} else {
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
case FIXED_WING:
// in fixed wing mode we use engines only for providing thrust, no moments are generated
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
break;
case TRANSITION:
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
} else {
// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
_actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
}
break;
case FIXED_WING:
// in fixed wing mode we use engines only for providing thrust, no moments are generated
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
break;
case TRANSITION:
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
}
}
src/modules/vtol_att_control/tailsitter.h
+7 -7
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TAILSITTER_H
#define TAILSITTER_H
@@ -48,7 +48,7 @@ class Tailsitter : public VtolType
{
public:
Tailsitter(VtolAttitudeControl * _att_controller);
Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter();
void update_vtol_state();
src/modules/vtol_att_control/tiltrotor.cpp
+144 -102
View File
@@ -44,12 +44,10 @@
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc),
_rear_motors(ENABLED),
_tilt_control(0.0f),
_roll_weight_mc(1.0f),
_yaw_weight_mc(1.0f),
_min_front_trans_dur(0.5f)
VtolType(attc),
_rear_motors(ENABLED),
_tilt_control(0.0f),
_min_front_trans_dur(0.5f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -88,11 +86,11 @@ Tiltrotor::parameters_update()
/* vtol duration of a front transition */
param_get(_params_handles_tiltrotor.front_trans_dur, &v);
_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
_params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
/* vtol duration of a back transition */
param_get(_params_handles_tiltrotor.back_trans_dur, &v);
_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
_params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
/* vtol tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
@@ -125,22 +123,26 @@ Tiltrotor::parameters_update()
/* avoid parameters which will lead to zero division in the transition code */
_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) {
_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
}
return OK;
}
int Tiltrotor::get_motor_off_channels(int channels) {
int Tiltrotor::get_motor_off_channels(int channels)
{
int channel_bitmap = 0;
int channel;
for (int i = 0; i < _params->vtol_motor_count; ++i) {
channel = channels % 10;
if (channel == 0) {
break;
}
channel_bitmap |= 1 << channel;
channels = channels / 10;
}
@@ -150,82 +152,99 @@ int Tiltrotor::get_motor_off_channels(int channels) {
void Tiltrotor::update_vtol_state()
{
parameters_update();
parameters_update();
/* simple logic using a two way switch to perform transitions.
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
*/
if (!_attc->is_fixed_wing_requested()) {
// plane is in multicopter mode
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_FRONT_P2:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
break;
}
} else {
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
// also allow switch if we are not armed for the sake of bench testing
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1:
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
_vtol_schedule.transition_start = hrt_absolute_time();
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
}
break;
case TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
break;
_tilt_control = _params_tiltrotor.tilt_fw;
}
break;
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
break;
}
}
// map tiltrotor specific control phases to simple control modes
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
}
}
@@ -250,7 +269,7 @@ void Tiltrotor::update_mc_state()
_mc_yaw_weight = 1.0f;
}
void Tiltrotor::update_fw_state()
void Tiltrotor::update_fw_state()
{
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
@@ -278,15 +297,18 @@ void Tiltrotor::update_transition_state()
if (_rear_motors != ENABLED) {
set_rear_motor_state(ENABLED);
}
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc +
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f);
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
}
// do blending of mc and fw controls
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {
_mc_roll_weight = 0.0f;
} else {
// at low speeds give full weight to mc
_mc_roll_weight = 1.0f;
@@ -294,6 +316,7 @@ void Tiltrotor::update_transition_state()
// disable mc yaw control once the plane has picked up speed
_mc_yaw_weight = 1.0f;
if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
}
@@ -301,8 +324,10 @@ void Tiltrotor::update_transition_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
_mc_roll_weight = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_rear_motors != IDLE) {
set_rear_motor_state(IDLE);
@@ -312,10 +337,12 @@ void Tiltrotor::update_transition_state()
set_idle_mc();
flag_idle_mc = true;
}
// tilt rotors back
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw -
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
}
// set zero throttle for backtransition otherwise unwanted moments will be created
@@ -339,19 +366,28 @@ void Tiltrotor::update_external_state()
*/
void Tiltrotor::fill_actuator_outputs()
{
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight;
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
}
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
* (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[4] = _tilt_control;
}
@@ -360,52 +396,58 @@ void Tiltrotor::fill_actuator_outputs()
* Set state of rear motors.
*/
void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
void Tiltrotor::set_rear_motor_state(rear_motor_state state)
{
int pwm_value = PWM_DEFAULT_MAX;
// map desired rear rotor state to max allowed pwm signal
switch (state) {
case ENABLED:
pwm_value = PWM_DEFAULT_MAX;
_rear_motors = ENABLED;
break;
case DISABLED:
pwm_value = PWM_LOWEST_MAX;
_rear_motors = DISABLED;
break;
case IDLE:
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
case ENABLED:
pwm_value = PWM_DEFAULT_MAX;
_rear_motors = ENABLED;
break;
case DISABLED:
pwm_value = PWM_LOWEST_MAX;
_rear_motors = DISABLED;
break;
case IDLE:
pwm_value = _params->idle_pwm_mc;
_rear_motors = IDLE;
break;
}
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
pwm_values.values[i] = pwm_value;
} else {
pwm_values.values[i] = PWM_DEFAULT_MAX;
}
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
if (ret != OK) {PX4_WARN("failed setting max values");}
close(fd);
px4_close(fd);
}
bool Tiltrotor::is_motor_off_channel(const int channel) {
bool Tiltrotor::is_motor_off_channel(const int channel)
{
return (_params_tiltrotor.fw_motors_off >> channel) & 1;
}
src/modules/vtol_att_control/tiltrotor.h
+8 -10
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TILTROTOR_H
#define TILTROTOR_H
@@ -49,7 +49,7 @@ class Tiltrotor : public VtolType
public:
Tiltrotor(VtolAttitudeControl * _att_controller);
Tiltrotor(VtolAttitudeControl *_att_controller);
~Tiltrotor();
/**
@@ -127,11 +127,9 @@ private:
struct {
vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */
hrt_abstime transition_start; /**< absoulte time at which front transition started */
}_vtol_schedule;
} _vtol_schedule;
float _tilt_control; /**< actuator value for the tilt servo */
float _roll_weight_mc; /**< multicopter desired roll moment weight */
float _yaw_weight_mc; /**< multicopter desired yaw moment weight */
const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
src/modules/vtol_att_control/tiltrotor_params.c
+1 -1
View File
@@ -35,7 +35,7 @@
* @file tiltrotor_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
src/modules/vtol_att_control/vtol_att_control_main.cpp
+38 -27
View File
@@ -93,10 +93,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
memset(&_armed, 0, sizeof(_armed));
memset(&_local_pos,0,sizeof(_local_pos));
memset(&_airspeed,0,sizeof(_airspeed));
memset(&_batt_status,0,sizeof(_batt_status));
memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_airspeed, 0, sizeof(_airspeed));
memset(&_batt_status, 0, sizeof(_batt_status));
memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
_params.idle_pwm_mc = PWM_LOWEST_MIN;
_params.vtol_motor_count = 0;
@@ -121,12 +121,15 @@ VtolAttitudeControl::VtolAttitudeControl() :
if (_params.vtol_type == 0) {
_tailsitter = new Tailsitter(this);
_vtol_type = _tailsitter;
} else if (_params.vtol_type == 1) {
_tiltrotor = new Tiltrotor(this);
_vtol_type = _tiltrotor;
} else if (_params.vtol_type == 2) {
_standard = new Standard(this);
_vtol_type = _standard;
} else {
_task_should_exit = true;
}
@@ -150,7 +153,7 @@ VtolAttitudeControl::~VtolAttitudeControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -258,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
* Check for airspeed updates.
*/
void
VtolAttitudeControl::vehicle_airspeed_poll() {
VtolAttitudeControl::vehicle_airspeed_poll()
{
bool updated;
orb_check(_airspeed_sub, &updated);
@@ -271,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() {
* Check for battery updates.
*/
void
VtolAttitudeControl::vehicle_battery_poll() {
VtolAttitudeControl::vehicle_battery_poll()
{
bool updated;
orb_check(_battery_status_sub, &updated);
@@ -442,7 +447,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
void VtolAttitudeControl::task_main()
{
warnx("started");
PX4_WARN("started");
fflush(stdout);
/* do subscriptions */
@@ -471,7 +476,7 @@ void VtolAttitudeControl::task_main()
_vtol_type->set_idle_mc();
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
@@ -491,7 +496,7 @@ void VtolAttitudeControl::task_main()
}
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
@@ -633,7 +638,7 @@ void VtolAttitudeControl::task_main()
warnx("exit");
_control_task = -1;
_exit(0);
return;
}
int
@@ -643,14 +648,14 @@ VtolAttitudeControl::start()
/* start the task */
_control_task = px4_task_spawn_cmd("vtol_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
(px4_main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
PX4_WARN("task start failed");
return -errno;
}
@@ -661,49 +666,55 @@ VtolAttitudeControl::start()
int vtol_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: vtol_att_control {start|stop|status}");
PX4_WARN("usage: vtol_att_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (VTOL_att_control::g_control != nullptr) {
errx(1, "already running");
PX4_WARN("already running");
return 0;
}
VTOL_att_control::g_control = new VtolAttitudeControl;
if (VTOL_att_control::g_control == nullptr) {
errx(1, "alloc failed");
PX4_WARN("alloc failed");
return 1;
}
if (OK != VTOL_att_control::g_control->start()) {
delete VTOL_att_control::g_control;
VTOL_att_control::g_control = nullptr;
err(1, "start failed");
PX4_WARN("start failed");
return 1;
}
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (VTOL_att_control::g_control == nullptr) {
errx(1, "not running");
PX4_WARN("not running");
return 0;
}
delete VTOL_att_control::g_control;
VTOL_att_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (VTOL_att_control::g_control) {
errx(0, "running");
PX4_WARN("running");
} else {
errx(1, "not running");
PX4_WARN("not running");
}
return 0;
}
warnx("unrecognized command");
PX4_WARN("unrecognized command");
return 1;
}
src/modules/vtol_att_control/vtol_att_control_main.h
+25 -27
View File
@@ -46,6 +46,10 @@
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -80,7 +84,6 @@
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
#include "tiltrotor.h"
@@ -101,24 +104,24 @@ public:
int start(); /* start the task and return OK on success */
bool is_fixed_wing_requested();
struct vehicle_attitude_s* get_att () {return &_v_att;}
struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;}
struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;}
struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;}
struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;}
struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;}
struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;}
struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;}
struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;}
struct actuator_armed_s* get_armed () {return &_armed;}
struct vehicle_local_position_s* get_local_pos () {return &_local_pos;}
struct airspeed_s* get_airspeed () {return &_airspeed;}
struct battery_status_s* get_batt_status () {return &_batt_status;}
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_armed_s *get_armed() {return &_armed;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct battery_status_s *get_batt_status() {return &_batt_status;}
struct Params* get_params () {return &_params;}
struct Params *get_params() {return &_params;}
private:
@@ -184,17 +187,12 @@ private:
param_t elevons_mc_lock;
} _params_handles;
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
unsigned _motor_count; // number of motors
float _airspeed_tot;
int _transition_command;
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
Standard * _standard; // standard vtol type
VtolType *_vtol_type; // base class for different vtol types
Tiltrotor *_tiltrotor; // tailsitter vtol type
Tailsitter *_tailsitter; // tiltrotor vtol type
Standard *_standard; // standard vtol type
//*****************Member functions***********************************************************************
src/modules/vtol_att_control/vtol_att_control_params.c
+1 -3
View File
@@ -35,11 +35,9 @@
* @file vtol_att_control_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstr@ethz.ch>
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* VTOL number of engines
*
src/modules/vtol_att_control/vtol_type.cpp
+21 -21
View File
@@ -31,21 +31,21 @@
*
****************************************************************************/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "vtol_type.h"
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include <px4_defines.h>
#include "vtol_att_control_main.h"
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(ROTARY_WING)
_attc(att_controller),
_vtol_mode(ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
@@ -70,7 +70,7 @@ _vtol_mode(ROTARY_WING)
VtolType::~VtolType()
{
}
/**
@@ -81,11 +81,11 @@ void VtolType::set_idle_mc()
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
@@ -95,11 +95,11 @@ void VtolType::set_idle_mc()
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
if (ret != OK) {PX4_WARN("failed setting min values");}
close(fd);
px4_close(fd);
flag_idle_mc = true;
}
@@ -111,9 +111,9 @@ void VtolType::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
int fd = px4_open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
if (fd < 0) {PX4_WARN("can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
@@ -125,9 +125,9 @@ void VtolType::set_idle_fw()
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
if (ret != OK) {PX4_WARN("failed setting min values");}
close(fd);
px4_close(fd);
}
src/modules/vtol_att_control/vtol_type.h
+7 -7
View File
@@ -31,12 +31,12 @@
*
****************************************************************************/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef VTOL_TYPE_H
#define VTOL_TYPE_H
@@ -85,7 +85,7 @@ public:
void set_idle_mc();
void set_idle_fw();
mode get_mode () {return _vtol_mode;};
mode get_mode() {return _vtol_mode;};
protected:
VtolAttitudeControl *_attc;
src/platforms/common/CMakeLists.txt
+1
View File
@@ -35,6 +35,7 @@ set(depends
prebuild_targets
git_mavlink
git_uavcan
git_matrix
)
px4_add_module(

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