Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-20 09:49:20 +08:00
Code Issues Actions 11 Packages Projects Releases Wiki Activity

Merged master into driver_framework

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-20 09:14:37 +01:00
parent c9c5f59bce 71320ac0fe
commit 75e962ee7c
102 changed files with 2985 additions and 474 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+2 -4
View File
@@ -9,7 +9,7 @@ matrix:
- os: linux
sudo: false
- os: osx
osx_image: beta-xcode6.3
osx_image: xcode7
sudo: true
cache:
@@ -60,11 +60,9 @@ before_install:
elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
brew tap PX4/homebrew-px4
&& brew update
&& brew install astyle
&& brew install gcc-arm-none-eabi
&& brew install cmake ninja astyle gcc-arm-none-eabi
&& brew install genromfs
&& brew install kconfig-frontends
&& brew install ninja
&& sudo easy_install pip
&& sudo pip install pyserial empy
;
Makefile
+1 -1
View File
@@ -184,7 +184,7 @@ clean:
# targets handled by cmake
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \
jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol
jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter
$(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
.PHONY: clean
ROMFS/px4fmu_common/init.d/2101_fw_AERT
+5 -3
View File
@@ -6,8 +6,8 @@
#
# @output MAIN1 aileron
# @output MAIN2 elevator
# @output MAIN4 rudder
# @output MAIN3 throttle
# @output MAIN3 rudder
# @output MAIN4 throttle
# @output MAIN5 flaps
#
# @output AUX1 feed-through of RC AUX1 channel
@@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults
set MIXER AERT
# The ESC requires a specific pulse to arm.
# use PWM parameters for throttle channel
set PWM_OUT 4
set PWM_DISARMED p:PWM_DISARMED
set PWM_MIN p:PWM_MIN
set PWM_MAX p:PWM_MAX
ROMFS/px4fmu_common/init.d/2104_fw_AETR
+3 -1
View File
@@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults
set MIXER AETR
# The ESC requires a specific pulse to arm.
# use PWM parameters for throttle channel
set PWM_OUT 3
set PWM_DISARMED p:PWM_DISARMED
set PWM_MIN p:PWM_MIN
set PWM_MAX p:PWM_MAX
ROMFS/px4fmu_common/init.d/2105_maja
+50
View File
@@ -0,0 +1,50 @@
#!nsh
#
# @name Bormatec Maja
#
# @type Standard Plane
#
# @output MAIN1 aileron
# @output MAIN2 aileron
# @output MAIN3 elevator
# @output MAIN4 rudder
# @output MAIN5 throttle
# @output MAIN6 wheel
# @output MAIN7 flaps
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Andreas Antener <andreas@uaventure.com>
#
sh /etc/init.d/rc.fw_defaults
if [ $AUTOCNF == yes ]
then
param set FW_AIRSPD_MIN 10
param set FW_AIRSPD_TRIM 15
param set FW_AIRSPD_MAX 20
param set FW_MAN_P_MAX 55
param set FW_MAN_R_MAX 55
param set FW_R_LIM 55
param set FW_WR_FF 0.2
param set FW_WR_I 0.2
param set FW_WR_IMAX 0.8
param set FW_WR_P 1
param set FW_W_RMAX 0
# set disarmed value for the ESC
param set PWM_DISARMED 1000
fi
set MIXER AAERTWF
# use PWM parameters for throttle channel
set PWM_OUT 5
set PWM_DISARMED p:PWM_DISARMED
set PWM_MIN p:PWM_MIN
set PWM_MAX p:PWM_MAX
ROMFS/px4fmu_common/init.d/2106_albatross
+51
View File
@@ -0,0 +1,51 @@
#!nsh
#
# @name Applied Aeronautics Albatross
#
# @type Standard Plane
#
# @output MAIN1 aileron right
# @output MAIN2 aileron left
# @output MAIN3 v-tail right
# @output MAIN4 v-tail left
# @output MAIN5 throttle
# @output MAIN6 wheel
# @output MAIN7 flaps right
# @output MAIN8 flaps left
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Andreas Antener <andreas@uaventure.com>
#
sh /etc/init.d/rc.fw_defaults
if [ $AUTOCNF == yes ]
then
param set FW_AIRSPD_MIN 10
param set FW_AIRSPD_TRIM 15
param set FW_AIRSPD_MAX 20
param set FW_MAN_P_MAX 55
param set FW_MAN_R_MAX 55
param set FW_R_LIM 55
param set FW_WR_FF 0.2
param set FW_WR_I 0.2
param set FW_WR_IMAX 0.8
param set FW_WR_P 1
param set FW_W_RMAX 0
# set disarmed value for the ESC
param set PWM_DISARMED 1000
fi
set MIXER AAVVTWFF
# use PWM parameters for throttle channel
set PWM_OUT 5
set PWM_DISARMED p:PWM_DISARMED
set PWM_MIN p:PWM_MIN
set PWM_MAX p:PWM_MAX
ROMFS/px4fmu_common/init.d/rcS
+4
View File
@@ -564,6 +564,10 @@ then
then
mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
fi
if param compare SYS_COMPANION 257600
then
mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
fi
# Sensors on the PWM interface bank
# clear pins 5 and 6
if param compare SENS_EN_LL40LS 1
ROMFS/px4fmu_common/mixers/AAERTWF.main.mix
+96
View File
@@ -0,0 +1,96 @@
Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU
=======================================================
This file defines mixers suitable for controlling a fixed wing aircraft with
aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU.
The configuration assumes the aileron servo(s) are connected to PX4FMU servo
output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle
to output 4 and the wheel to output 5.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
Aileron mixer (roll + flaperon)
---------------------------------
This mixer assumes that the aileron servos are set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
S: 0 6 -10000 -10000 0 -10000 10000
Elevator mixer
------------
Two scalers total (output, roll).
This mixer assumes that the elevator servo is set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 -10000 -10000 0 -10000 10000
Rudder mixer
------------
Two scalers total (output, yaw).
This mixer assumes that the rudder servo is set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Wheel mixer
------------
Two scalers total (output, yaw).
This mixer assumes that the wheel servo is set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Flaps / gimbal / payload mixer for last three channels,
using the payload control group
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 2 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 2 2 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix
+84
View File
@@ -0,0 +1,84 @@
Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU
=======================================================
This file defines mixers suitable for controlling a fixed wing aircraft with
aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps
using PX4FMU.
The configuration assumes the aileron servos are connected to PX4FMU servo
output 0 and 1, the tail servos to output 2 and 3, the throttle
to output 4, the wheel to output 5 and the flaps to output 6 and 7.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
Aileron mixer (roll + flaperon)
---------------------------------
This mixer assumes that the aileron servos are set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
S: 0 6 -10000 -10000 0 -10000 10000
V-tail mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two tail servos are physically reversed, the pitch
input is inverted between the two servos.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 2 -7000 -7000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 2 -7000 -7000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Wheel mixer
------------
Two scalers total (output, yaw).
This mixer assumes that the wheel servo is set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 -10000 -10000 0 -10000 10000
Flaps mixer
------------
Flap servos are physically reversed.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 0 5000 -10000 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 0 -5000 10000 -10000 10000
Tools/generate_listener.py
+5 -3
View File
@@ -17,7 +17,7 @@ for index,m in enumerate(raw_messages):
temp_list_floats = []
temp_list_uint64 = []
temp_list_bool = []
if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m):
if("pwm_input" not in m and "position_setpoint" not in m):
temp_list = []
f = open(m,'r')
for line in f.readlines():
@@ -55,9 +55,11 @@ for index,m in enumerate(raw_messages):
f.close()
(m_head, m_tail) = os.path.split(m)
messages.append(m_tail.split('.')[0])
message = m_tail.split('.')[0]
if message != "actuator_controls":
messages.append(message)
message_elements.append(temp_list)
#messages.append(m.split('/')[-1].split('.')[0])
message_elements.append(temp_list)
num_messages = len(messages);
Tools/parameters_injected.xml
+35 -43
View File
@@ -15,7 +15,7 @@
<min>0</min>
<max>1</max>
</parameter>
<parameter default="1" name="ctl_gain" type="FLOAT">
<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
@@ -24,59 +24,53 @@
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>
<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">
<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>
<unit>Hertz</unit>
<decimal>3</decimal>
<min>0.0</min>
<max>100.0</max>
</parameter>
<parameter default="25" name="ctl_start_rate" type="INT32">
<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>
<decimal></decimal>
<unit>Hz/s</unit>
<min>5</min>
<max>1000</max>
</parameter>
<parameter default="0" name="esc_index" type="INT32">
<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>
<decimal></decimal>
<unit>Index</unit>
<min>0</min>
<max>15</max>
</parameter>
<parameter default="20034" name="id_ext_status" type="INT32">
<parameter default="20034" name="id_ext_status" type="INT32">
<short_desc>Extended status ID</short_desc>
<long_desc>Extended status ID</long_desc>
<unit></unit>
<decimal></decimal>
<min>1</min>
<max>1023</max>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_ext_status" type="INT32">
<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>
<decimal></decimal>
<unit>µs</unit>
<min>0</min>
<max>1000000</max>
</parameter>
<parameter default="50000" name="int_status" type="INT32">
<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>
<decimal></decimal>
<unit>µs</unit>
<max>1000000</max>
</parameter>
<parameter default="12" name="mot_i_max" type="FLOAT">
<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
@@ -84,62 +78,60 @@
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>
<unit>Amps</unit>
<decimal>3</decimal>
<min>1</min>
<max>80</max>
</parameter>
<parameter default="2300" name="mot_kv" type="INT32">
<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>
<decimal>0</decimal>
<unit>RPM/v</unit>
<min>0</min>
<max>100</max>
<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>
<unit>henries</unit>
<decimal>3</decimal>
</parameter>
<parameter default="14" name="mot_num_poles" type="INT32">
<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>
<decimal></decimal>
<unit>Poles</unit>
<min>2</min>
<max>40</max>
</parameter>
<parameter default="0.0" name="mot_rs" type="FLOAT">
<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>
<unit>Ohms</unit>
<decimal>3</decimal>
</parameter>
<parameter default=".5" name="mot_v_accel" type="INT32">
<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>
<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">
<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>
<unit>Volts</unit>
<decimal>3</decimal>
<min>0</min>
</parameter>
</group>
Tools/px_process_params.py
+1 -1
View File
@@ -130,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, args.inject_xml)
out = xmlout.XMLOutput(param_groups, args.board, os.path.join(args.src_path, args.inject_xml))
out.Save(args.xml)
# Output to DokuWiki tables
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: d362e661b4...6072426bbc
Tools/sitl_run.sh
+4 -4
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
@@ -88,9 +88,9 @@ then
ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}
elif [ "$debugger" == "valgrind" ]
then
valgrind ./mainapp ../../../../${rc_script}_${program}
valgrind ./mainapp ../../../../${rc_script}_${program}_${model}
else
./mainapp ../../../../${rc_script}_${program}
./mainapp ../../../../${rc_script}_${program}_${model}
fi
if [ "$program" == "jmavsim" ]
cmake/configs/nuttx_px4fmu-v1_default.cmake
+2
View File
@@ -111,6 +111,8 @@ set(config_module_list
lib/geo_lookup
lib/conversion
lib/launchdetection
lib/terrain_estimation
lib/runway_takeoff
platforms/nuttx
# had to add for cmake, not sure why wasn't in original config
cmake/configs/nuttx_px4fmu-v2_default.cmake
+2
View File
@@ -118,6 +118,8 @@ set(config_module_list
lib/geo_lookup
lib/conversion
lib/launchdetection
lib/terrain_estimation
lib/runway_takeoff
platforms/nuttx
# had to add for cmake, not sure why wasn't in original config
cmake/configs/posix_eagle_default.cmake
+2
View File
@@ -38,6 +38,8 @@ set(config_module_list
lib/geo
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
platforms/common
platforms/posix/px4_layer
cmake/configs/posix_sitl_simple.cmake
+2
View File
@@ -56,6 +56,8 @@ set(config_module_list
lib/geo
lib/geo_lookup
lib/launchdetection
lib/terrain_estimation
lib/runway_takeoff
)
set(config_extra_builtin_cmds
cmake/configs/qurt_eagle_hil.cmake
+2
View File
@@ -47,6 +47,8 @@ set(config_module_list
lib/geo
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
modules/controllib
#
cmake/configs/qurt_eagle_release.cmake
+2
View File
@@ -70,6 +70,8 @@ set(config_module_list
lib/geo
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
#
# QuRT port
cmake/configs/qurt_eagle_travis.cmake
+2
View File
@@ -52,6 +52,8 @@ set(config_module_list
lib/geo_lookup
lib/conversion
lib/ecl
lib/terrain_estimation
lib/runway_takeoff
#
# QuRT port
msg/actuator_controls.msg
+3
View File
@@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1
uint8 INDEX_YAW = 2
uint8 INDEX_THROTTLE = 3
uint8 INDEX_FLAPS = 4
uint8 INDEX_SPOILERS = 5
uint8 INDEX_AIRBRAKES = 6
uint8 INDEX_LANDING_GEAR = 7
uint8 GROUP_INDEX_ATTITUDE = 0
uint64 timestamp
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
msg/control_state.msg
+1
View File
@@ -16,3 +16,4 @@ 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/navigation_capabilities.msg
+1
View File
@@ -3,3 +3,4 @@ float32 turn_distance # the optimal distance to a waypoint to switch to the nex
float32 landing_horizontal_slope_displacement
float32 landing_slope_angle_rad
float32 landing_flare_length
bool abort_landing
msg/vehicle_attitude_setpoint.msg
+4
View File
@@ -21,3 +21,7 @@ float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool apply_flaps
msg/vehicle_status.msg
+3
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
@@ -179,3 +181,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
posix-configs/SITL/init/rcS_gazebo → posix-configs/SITL/init/rcS_gazebo_iris
View File
posix-configs/SITL/init/rcS_gazebo_tailsitter
+64
View File
@@ -0,0 +1,64 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 20
param set MC_PITCHRATE_P 0.3
param set MC_ROLLRATE_P 0.3
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set VT_TYPE 0
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1376256
param set CAL_ACC1_ID 1310720
param set CAL_MAG0_ID 196608
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
param set COM_RC_IN_MODE 2
rgbled start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
measairspeedsim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
position_estimator_inav start
vtol_att_control start
mc_pos_control start
mc_att_control start
fw_pos_control_l1 start
fw_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
mavlink start -u 14556 -r 2000000
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
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/pwm_out_sim/pwm_out_sim.cpp
+1 -1
View File
@@ -548,7 +548,7 @@ PWMSim::control_callback(uintptr_t handle,
const actuator_controls_s *controls = (actuator_controls_s *)handle;
if (_armed) {
input = controls->control[control_index];
input = controls[control_group].control[control_index];
} else {
/* clamp actuator to zero if not armed */
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
@@ -36,7 +36,7 @@ add_dependencies(run_config mainapp)
foreach(viewer none jmavsim gazebo)
foreach(debugger none gdb lldb ddd valgrind)
foreach(model none iris vtol)
foreach(model none iris tailsitter)
if (debugger STREQUAL "none")
if (model STREQUAL "none")
set(_targ_name "${viewer}")
src/include/mavlink/mavlink_log.h
+12 -12
View File
@@ -106,10 +106,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n");
#define mavlink_and_console_log_emergency(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n"); } while(0);
/**
* Send a mavlink critical message and print to console.
@@ -117,10 +117,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n");
#define mavlink_and_console_log_critical(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n"); } while(0);
/**
* Send a mavlink emergency message and print to console.
@@ -128,10 +128,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n");
#define mavlink_and_console_log_info(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n"); } while(0);
struct mavlink_logmessage {
char text[MAVLINK_LOG_MAXLEN + 1];
src/lib/ecl/CMakeLists.txt
+1
View File
@@ -39,6 +39,7 @@ px4_add_module(
attitude_fw/ecl_pitch_controller.cpp
attitude_fw/ecl_roll_controller.cpp
attitude_fw/ecl_yaw_controller.cpp
attitude_fw/ecl_wheel_controller.cpp
l1/ecl_l1_pos_controller.cpp
validation/data_validator.cpp
validation/data_validator_group.cpp
src/lib/ecl/attitude_fw/ecl_controller.h
+2
View File
@@ -75,6 +75,8 @@ struct ECL_ControlData {
float airspeed_max;
float airspeed;
float scaler;
float groundspeed;
float groundspeed_scaler;
bool lock_integrator;
};
src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
+153
View File
@@ -0,0 +1,153 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_wheel_controller.cpp
* Implementation of a simple PID wheel controller for heading tracking.
*
* Authors and acknowledgements in header.
*/
#include "ecl_wheel_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <ecl/ecl.h>
ECL_WheelController::ECL_WheelController() :
ECL_Controller("wheel")
{
}
ECL_WheelController::~ECL_WheelController()
{
}
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
PX4_ISFINITE(ctl_data.groundspeed) &&
PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* input conditioning */
float min_speed = 1.0f;
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
float id = _rate_error * dt * ctl_data.groundspeed_scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
_rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
integrator_constrained;
/*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
(double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
PX4_ISFINITE(ctl_data.yaw))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling wheel");
return _rate_setpoint;
}
/* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */
yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/*warnx("yaw_error: %.4f", (double)yaw_error);*/
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
}
return _rate_setpoint;
}
src/lib/ecl/attitude_fw/ecl_wheel_controller.h
+70
View File
@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_wheel_controller.h
* Definition of a simple orthogonal coordinated turn yaw PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Andreas Antener <andreas@uaventure.com>
*
* Acknowledgements:
*
* The control design is based on a design
* by Paul Riseborough and Andrew Tridgell, 2013,
* which in turn is based on initial work of
* Jonathan Challinger, 2012.
*/
#ifndef ECL_HEADING_CONTROLLER_H
#define ECL_HEADING_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
#include "ecl_controller.h"
class __EXPORT ECL_WheelController :
public ECL_Controller
{
public:
ECL_WheelController();
~ECL_WheelController();
float control_attitude(const struct ECL_ControlData &ctl_data);
float control_bodyrate(const struct ECL_ControlData &ctl_data);
};
#endif // ECL_HEADING_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+5 -4
View File
@@ -76,14 +76,15 @@ public:
_coordinated_method = coordinated_method;
}
protected:
float _coordinated_min_speed;
enum {
COORD_METHOD_OPEN = 0,
COORD_METHOD_CLOSEACC = 1,
COORD_METHOD_CLOSEACC = 1
};
protected:
float _coordinated_min_speed;
float _max_rate;
int32_t _coordinated_method;
float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
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/geo/geo.c
+35
View File
@@ -298,6 +298,41 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
return CONSTANTS_RADIUS_OF_EARTH * c;
}
__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target)
{
if (fabsf(dist) < FLT_EPSILON) {
*lat_target = lat_A;
*lon_target = lon_A;
} else if (dist >= FLT_EPSILON) {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
} else {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
heading = _wrap_2pi(heading + M_PI_F);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
}
}
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target)
{
bearing = _wrap_2pi(bearing);
double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
double lat_start_rad = lat_start * M_DEG_TO_RAD;
double lon_start_rad = lon_start * M_DEG_TO_RAD;
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
double)bearing));
*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
*lat_target *= M_RAD_TO_DEG;
*lon_target *= M_RAD_TO_DEG;
}
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
src/lib/geo/geo.h
+30
View File
@@ -236,6 +236,36 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al
*/
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
/**
* Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
* from waypoint A
*
* @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
* @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
* @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
* @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
* @param dist distance of target waypoint from waypoint A in meters (can be negative)
* @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
*/
__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target);
/**
* Creates a waypoint from given waypoint, distance and bearing
* see http://www.movable-type.co.uk/scripts/latlong.html
*
* @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
* @param bearing in rad
* @param distance in meters
* @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
*/
__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target);
/**
* Returns the bearing to the next waypoint in radians.
*
src/lib/matrix
+1 -1
Submodule src/lib/matrix updated: 2c7a375e3d...7656385ea1
src/lib/runway_takeoff/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_module(
MODULE lib__runway_takeoff
COMPILE_FLAGS
-Os
SRCS
RunwayTakeoff.cpp
runway_takeoff_params.c
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/lib/runway_takeoff/RunwayTakeoff.cpp
+286
View File
@@ -0,0 +1,286 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file RunwayTakeoff.cpp
* Runway takeoff handling for fixed-wing UAVs with steerable wheels.
*
* @author Roman Bapst <roman@px4.io>
* @author Andreas Antener <andreas@uaventure.com>
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "RunwayTakeoff.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <mavlink/mavlink_log.h>
#include <mathlib/mathlib.h>
namespace runwaytakeoff
{
RunwayTakeoff::RunwayTakeoff() :
SuperBlock(NULL, "RWTO"),
_state(),
_initialized(false),
_initialized_time(0),
_init_yaw(0),
_climbout(false),
_throttle_ramp_time(2 * 1e6),
_start_wp(),
_runway_takeoff_enabled(this, "TKOFF"),
_heading_mode(this, "HDG"),
_nav_alt(this, "NAV_ALT"),
_takeoff_throttle(this, "MAX_THR"),
_runway_pitch_sp(this, "PSP"),
_max_takeoff_pitch(this, "MAX_PITCH"),
_max_takeoff_roll(this, "MAX_ROLL"),
_min_airspeed_scaling(this, "AIRSPD_SCL"),
_airspeed_min(this, "FW_AIRSPD_MIN", false),
_climbout_diff(this, "FW_CLMBOUT_DIFF", false)
{
updateParams();
}
RunwayTakeoff::~RunwayTakeoff()
{
}
void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
{
_init_yaw = yaw;
_initialized = true;
_state = RunwayTakeoffState::THROTTLE_RAMP;
_initialized_time = hrt_absolute_time();
_climbout = true; // this is true until climbout is finished
_start_wp(0) = (float)current_lat;
_start_wp(1) = (float)current_lon;
}
void RunwayTakeoff::update(float airspeed, float alt_agl,
double current_lat, double current_lon, int mavlink_fd)
{
switch (_state) {
case RunwayTakeoffState::THROTTLE_RAMP:
if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) {
_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
}
break;
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) {
_state = RunwayTakeoffState::TAKEOFF;
mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached");
}
break;
case RunwayTakeoffState::TAKEOFF:
if (alt_agl > _nav_alt.get()) {
_state = RunwayTakeoffState::CLIMBOUT;
/*
* If we started in heading hold mode, move the navigation start WP to the current location now.
* The navigator will take this as starting point to navigate towards the takeoff WP.
*/
if (_heading_mode.get() == 0) {
_start_wp(0) = (float)current_lat;
_start_wp(1) = (float)current_lon;
}
mavlink_log_info(mavlink_fd, "#Climbout");
}
break;
case RunwayTakeoffState::CLIMBOUT:
if (alt_agl > _climbout_diff.get()) {
_climbout = false;
_state = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_fd, "#Navigating to waypoint");
}
break;
default:
break;
}
}
/*
* Returns true as long as we're below navigation altitude
*/
bool RunwayTakeoff::controlYaw()
{
// keep controlling yaw directly until we start navigation
return _state < RunwayTakeoffState::CLIMBOUT;
}
/*
* Returns pitch setpoint to use.
*
* Limited (parameter) as long as the plane is on runway. Otherwise
* use the one from TECS
*/
float RunwayTakeoff::getPitch(float tecsPitch)
{
if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
return math::radians(_runway_pitch_sp.get());
}
return tecsPitch;
}
/*
* Returns the roll setpoint to use.
*/
float RunwayTakeoff::getRoll(float navigatorRoll)
{
// until we have enough ground clearance, set roll to 0
if (_state < RunwayTakeoffState::CLIMBOUT) {
return 0.0f;
}
// allow some roll during climbout
else if (_state < RunwayTakeoffState::FLY) {
return math::constrain(navigatorRoll,
math::radians(-_max_takeoff_roll.get()),
math::radians(_max_takeoff_roll.get()));
}
return navigatorRoll;
}
/*
* Returns the yaw setpoint to use.
*
* In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
* runway. When it has enough ground clearance we start navigation towards WP.
*/
float RunwayTakeoff::getYaw(float navigatorYaw)
{
if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
return _init_yaw;
} else {
return navigatorYaw;
}
}
/*
* Returns the throttle setpoint to use.
*
* Ramps up in the beginning, until it lifts off the runway it is set to
* parameter value, then it returns the TECS throttle.
*/
float RunwayTakeoff::getThrottle(float tecsThrottle)
{
switch (_state) {
case RunwayTakeoffState::THROTTLE_RAMP: {
float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time *
_takeoff_throttle.get();
return throttle < _takeoff_throttle.get() ?
throttle :
_takeoff_throttle.get();
}
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
return _takeoff_throttle.get();
default:
return tecsThrottle;
}
}
bool RunwayTakeoff::resetIntegrators()
{
// reset integrators if we're still on runway
return _state < RunwayTakeoffState::TAKEOFF;
}
/*
* Returns the minimum pitch for TECS to use.
*
* In climbout we either want what was set on the waypoint (sp_min) but at least
* the climbtout minimum pitch (parameter).
* Otherwise use the minimum that is enforced generally (parameter).
*/
float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
{
if (_state < RunwayTakeoffState::FLY) {
return math::max(sp_min, climbout_min);
}
else {
return min;
}
}
/*
* Returns the maximum pitch for TECS to use.
*
* Limited by parameter (if set) until climbout is done.
*/
float RunwayTakeoff::getMaxPitch(float max)
{
// use max pitch from parameter if set (> 0.1)
if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) {
return _max_takeoff_pitch.get();
}
else {
return max;
}
}
/*
* Returns the "previous" (start) WP for navigation.
*/
math::Vector<2> RunwayTakeoff::getStartWP()
{
return _start_wp;
}
void RunwayTakeoff::reset()
{
_initialized = false;
_state = RunwayTakeoffState::THROTTLE_RAMP;
}
}
src/lib/runway_takeoff/RunwayTakeoff.h
+121
View File
@@ -0,0 +1,121 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file RunwayTakeoff.h
* Runway takeoff handling for fixed-wing UAVs with steerable wheels.
*
* @author Roman Bapst <roman@px4.io>
* @author Andreas Antener <andreas@uaventure.com>
*/
#ifndef RUNWAYTAKEOFF_H
#define RUNWAYTAKEOFF_H
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <mavlink/mavlink_log.h>
#include <mathlib/mathlib.h>
namespace runwaytakeoff
{
enum RunwayTakeoffState {
THROTTLE_RAMP = 0, /**< ramping up throttle */
CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
FLY = 4 /**< fly towards takeoff waypoint */
};
class __EXPORT RunwayTakeoff : public control::SuperBlock
{
public:
RunwayTakeoff();
~RunwayTakeoff();
void init(float yaw, double current_lat, double current_lon);
void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd);
RunwayTakeoffState getState() { return _state; };
bool isInitialized() { return _initialized; };
bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); };
float getInitYaw() { return _init_yaw; };
bool controlYaw();
bool climbout() { return _climbout; };
float getPitch(float tecsPitch);
float getRoll(float navigatorRoll);
float getYaw(float navigatorYaw);
float getThrottle(float tecsThrottle);
bool resetIntegrators();
float getMinPitch(float sp_min, float climbout_min, float min);
float getMaxPitch(float max);
math::Vector<2> getStartWP();
void reset();
protected:
private:
/** state variables **/
RunwayTakeoffState _state;
bool _initialized;
hrt_abstime _initialized_time;
float _init_yaw;
bool _climbout;
unsigned _throttle_ramp_time;
math::Vector<2> _start_wp;
/** parameters **/
control::BlockParamInt _runway_takeoff_enabled;
control::BlockParamInt _heading_mode;
control::BlockParamFloat _nav_alt;
control::BlockParamFloat _takeoff_throttle;
control::BlockParamFloat _runway_pitch_sp;
control::BlockParamFloat _max_takeoff_pitch;
control::BlockParamFloat _max_takeoff_roll;
control::BlockParamFloat _min_airspeed_scaling;
control::BlockParamFloat _airspeed_min;
control::BlockParamFloat _climbout_diff;
};
}
#endif // RUNWAYTAKEOFF_H

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