dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merged master into lockdown_disable

This commit is contained in:
Lorenz Meier 2014-01-30 10:00:01 +01:00
parent 62076c0e81 3b31a6b1b9
commit ff753b9e24
98 changed files with 2764 additions and 2745 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Documentation/Doxyfile
View File

@ -599,7 +599,7 @@ RECURSIVE = YES
# excluded from the INPUT source files. This way you can easily exclude a
# subdirectory from a directory tree whose root is specified with the INPUT tag.
EXCLUDE = ../src/modules/mathlib/CMSIS \
EXCLUDE = ../src/lib/mathlib/CMSIS \
../src/modules/attitude_estimator_ekf/codegen
# The EXCLUDE_SYMLINKS tag can be used select whether or not files or

58
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
View File

@ -1,14 +1,13 @@
#!nsh
#
# USB HIL start
# HILStar / X-Plane
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
#
echo "[HIL] HILStar starting.."
echo "HIL Rascal 110 starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
@ -40,48 +39,7 @@ then
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 1
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT

85
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
View File

@ -1,74 +1,31 @@
#!nsh
echo "[init] Team Blacksheep Discovery Quad"
#
# Load default params for this platform
# Team Blacksheep Discovery Quadcopter
#
if param compare SYS_AUTOCONFIG 1
# Maintainers: Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.006
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.17
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
#
# Default parameters for this platform
#
param set MC_ATT_P 5.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.15
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.17
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.006
param set MC_YAWPOS_P 0.5
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.2
param save
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set VEHICLE_TYPE mc
set MIXER FMU_quad_w
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load the mixer for a quad with wide arms
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1100
pwm max -c 1234 -p 1900
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
set PWM_OUTPUTS 1234
set PWM_RATE 400

104
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -1,73 +1,53 @@
#!nsh
echo "[init] 3DR Iris Quad"
#
# Load default params for this platform
# 3DR Iris Quadcopter
#
if param compare SYS_AUTOCONFIG 1
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.13
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
#
# Default parameters for this platform
#
param set MC_ATT_P 9.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.15
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.13
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 0.5
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWPOS_I 0.15
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
param set BAT_V_SCALING 0.00989
param set BAT_C_SCALING 0.0124
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set VEHICLE_TYPE mc
set MIXER FMU_quad_w
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.0098
set EXIT_ON_END yes
fi
#
# Load the mixer for a quad with wide arms
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1050
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1000
set PWM_MAX 2000

105
ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
View File

@ -1,105 +0,0 @@
#!nsh
#
# USB HIL start
#
echo "[HIL] HILS quadrotor starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.0
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.05
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 3.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.1
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.05
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.5
param set MPC_THR_MIN 0.1
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start
echo "[HIL] setup done, running"

46
ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil Normal file
View File

@ -0,0 +1,46 @@
#!nsh
#
# HIL Quadcopter X
#
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
fi
set HIL yes
set VEHICLE_TYPE mc
set MIXER FMU_quad_x

56
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
View File

@ -1,14 +1,13 @@
#!nsh
#
# USB HIL start
# HIL Rascal 110 (Flightgear)
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
#
echo "[HIL] HILStar starting in state-HIL mode.."
echo "HIL Rascal 110 starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
@ -32,48 +31,15 @@ then
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 1
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT

135
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
View File

@ -1,105 +1,46 @@
#!nsh
#
# USB HIL start
# HIL Quadcopter +
#
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
#
echo "[HIL] HILS quadrotor + starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.0
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.05
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 3.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.1
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.05
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.5
param set MPC_THR_MIN 0.1
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
#
# Default parameters for this platform
#
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start
echo "[HIL] setup done, running"
set HIL yes
set VEHICLE_TYPE mc
set MIXER FMU_quad_+

69
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
View File

@ -1,14 +1,13 @@
#!nsh
#
# USB HIL start
# HIL Rascal 110 (Flightgear)
#
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>
#
echo "[HIL] HILStar starting.."
echo "HIL Rascal 110 starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
@ -40,59 +39,7 @@ then
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 1
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
set MODE autostart
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
else
echo "Using /etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
fi
fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"
set HIL yes
set VEHICLE_TYPE fw
set MIXER FMU_AERT

37
ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm Normal file
View File

@ -0,0 +1,37 @@
#!nsh
#
# Generic 10” Octo coaxial geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_octo_cox
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

61
ROMFS/px4fmu_common/init.d/2100_mpx_easystar
View File

@ -1,13 +1,15 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on EasyStar"
#
# Load default params for this platform
# MPX EasyStar Plane
#
if param compare SYS_AUTOCONFIG 1
# Maintainers: ???
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
#
# Default parameters for this platform
#
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
@ -31,50 +33,7 @@ then
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_RET.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
else
echo "Using /etc/mixers/FMU_RET.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
set VEHICLE_TYPE fw
set MIXER FMU_RET

48
ROMFS/px4fmu_common/init.d/2101_hk_bixler
View File

@ -1,11 +1,11 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler"
echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
@ -35,46 +35,6 @@ then
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
set VEHICLE_TYPE fw
set MIXER FMU_AERT

48
ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
View File

@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
@ -35,48 +35,6 @@ then
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
pwm disarmed -c 3 -p 1056
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
set VEHICLE_TYPE fw
set MIXER FMU_AERT

84
ROMFS/px4fmu_common/init.d/3030_io_camflyer
View File

@ -2,57 +2,39 @@
echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
set VEHICLE_TYPE fw
set MIXER FMU_Q

85
ROMFS/px4fmu_common/init.d/3031_io_phantom
View File

@ -1,85 +0,0 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MIN 11.4
param set FW_AIRSPD_TRIM 14
param set FW_AIRSPD_MAX 22
param set FW_L1_PERIOD 15
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 15
param set FW_R_P 80
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.8
param set FW_THR_LND_MAX 0
param set FW_THR_MAX 1
param set FW_THR_MIN 0.5
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 2.0
param set FW_Y_ROLLFF 1.0
param set RC_SCALE_ROLL 0.6
param set RC_SCALE_PITCH 0.6
param set TRIM_PITCH 0.1
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing

44
ROMFS/px4fmu_common/init.d/3031_phantom Normal file
View File

@ -0,0 +1,44 @@
#!nsh
#
# Phantom FPV Flying Wing
#
# Maintainers: Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 11.4
param set FW_AIRSPD_TRIM 14
param set FW_AIRSPD_MAX 22
param set FW_L1_PERIOD 15
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 45
param set FW_P_LIM_MIN -45
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 15
param set FW_R_P 80
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.8
param set FW_THR_LND_MAX 0
param set FW_THR_MAX 1
param set FW_THR_MIN 0.5
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 2.0
param set FW_Y_ROLLFF 1.0
param set RC_SCALE_ROLL 0.6
param set RC_SCALE_PITCH 0.6
param set TRIM_PITCH 0.1
fi
set VEHICLE_TYPE fw
set MIXER FMU_Q

89
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

@ -1,58 +1,43 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5"
#
# Load default params for this platform
# Skywalker X5 Flying Wing
#
if param compare SYS_AUTOCONFIG 1
# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing
set VEHICLE_TYPE fw
set MIXER FMU_X5

84
ROMFS/px4fmu_common/init.d/3033_io_wingwing
View File

@ -1,84 +0,0 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing

43
ROMFS/px4fmu_common/init.d/3033_wingwing Normal file
View File

@ -0,0 +1,43 @@
#!nsh
#
# Wing Wing (aka Z-84) Flying Wing
#
# Maintainers: Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 9
param set FW_AIRSPD_MAX 14
param set FW_L1_PERIOD 10
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 20
param set FW_P_LIM_MAX 30
param set FW_P_LIM_MIN -20
param set FW_P_P 30
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 2
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 60
param set FW_R_RMAX 60
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 0.7
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 2.0
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
set VEHICLE_TYPE fw
set MIXER FMU_Q

43
ROMFS/px4fmu_common/init.d/3034_fx79 Normal file
View File

@ -0,0 +1,43 @@
#!nsh
#
# FX-79 Buffalo Flying Wing
#
# Maintainers: Simon Wilks <sjwilks@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_TRIM 12
param set FW_AIRSPD_MIN 15
param set FW_L1_PERIOD 12
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 80
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.75
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 1.1
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
fi
set VEHICLE_TYPE fw
set MIXER FMU_FX79

84
ROMFS/px4fmu_common/init.d/3034_io_fx79
View File

@ -1,84 +0,0 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_AIRSPD_MAX 20
param set FW_AIRSPD_TRIM 12
param set FW_AIRSPD_MIN 15
param set FW_L1_PERIOD 12
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 80
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.75
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_T_TIME_CONST 9
param set FW_Y_ROLLFF 1.1
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix
else
echo "Using /etc/mixers/FMU_FX79.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix
fi
#
# Start common fixedwing apps
#
sh /etc/init.d/rc.fixedwing

76
ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

@ -1,31 +1,31 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on DJI F330"
#
# Load default params for this platform
# DJI Flame Wheel F330 Quadcopter
#
if param compare SYS_AUTOCONFIG 1
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.8
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.05
param set MC_YAWRATE_D 0.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_THR_MAX 0.8
param set MPC_THR_MIN 0.2
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
@ -38,32 +38,14 @@ then
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
sh /etc/init.d/rc.mc_interface
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

91
ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

@ -1,74 +1,37 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on DJI F450"
#
# Load default params for this platform
# DJI Flame Wheel F450 Quadcopter
#
if param compare SYS_AUTOCONFIG 1
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
#
# Set PWM output frequency
#
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals (for DJI ESCs)
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common multirotor apps
#
sh /etc/init.d/rc.multirotor
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

33
ROMFS/px4fmu_common/init.d/4012_hk_x550 Normal file
View File

@ -0,0 +1,33 @@
#!nsh
#
# HobbyKing X550 Quadcopter
#
# Maintainers: Todd Stellanova <tstellanova@gmail.com>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_YAWPOS_P 0.6
param set MC_YAWPOS_I 0
param set MC_YAWPOS_D 0
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0
param set MC_YAWRATE_D 0
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400

51
ROMFS/px4fmu_common/init.d/40_io_segway
View File

@ -1,51 +0,0 @@
#!nsh
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 10 = ground rover
#
param set MAV_TYPE 10
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start and configure PX4IO interface
#
sh /etc/init.d/rc.io
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator (depends on orb)
#
attitude_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
roboclaw start /dev/ttyS2 128 1200
segway start

37
ROMFS/px4fmu_common/init.d/5001_quad_+_pwm Normal file
View File

@ -0,0 +1,37 @@
#!nsh
#
# Generic 10” Quad + geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_quad_+
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

37
ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm Normal file
View File

@ -0,0 +1,37 @@
#!nsh
#
# Generic 10” Hexa X geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_hexa_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

76
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
View File

@ -1,76 +0,0 @@
#!nsh
echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set MC_ATTRATE_P 0.14
param set MC_ATTRATE_I 0
param set MC_ATTRATE_D 0.006
param set MC_ATT_P 5.5
param set MC_ATT_I 0
param set MC_ATT_D 0
param set MC_YAWPOS_D 0
param set MC_YAWPOS_I 0
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0
param set MC_YAWRATE_I 0
param set MC_YAWRATE_P 0.08
param set RC_SCALE_PITCH 1
param set RC_SCALE_ROLL 1
param set RC_SCALE_YAW 3
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1100
pwm max -c 1234 -p 1900
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor

37
ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm Normal file
View File

@ -0,0 +1,37 @@
#!nsh
#
# Generic 10” Hexa + geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_hexa_+
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

37
ROMFS/px4fmu_common/init.d/8001_octo_x_pwm Normal file
View File

@ -0,0 +1,37 @@
#!nsh
#
# Generic 10” Octo X geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_octo_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

51
ROMFS/px4fmu_common/init.d/800_sdlogger
View File

@ -1,51 +0,0 @@
#!nsh
echo "[init] PX4FMU v1, v2 init to log only
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param save
fi
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
sh /etc/init.d/rc.sensors
gps start
attitude_estimator_ekf start
position_estimator_inav start
if [ -d /fs/microsd ]
then
if [ $BOARD == fmuv1 ]
then
sdlog2 start -r 50 -e -b 16
else
sdlog2 start -r 200 -e -b 16
fi
fi

37
ROMFS/px4fmu_common/init.d/9001_octo_+_pwm Normal file
View File

@ -0,0 +1,37 @@
#!nsh
#
# Generic 10” Octo + geometry
#
# Maintainers: Lorenz Meier <lm@inf.ethz.ch>
#
if [ $DO_AUTOCONFIG == yes ]
then
#
# Default parameters for this platform
#
param set MC_ATT_P 7.0
param set MC_ATT_I 0.0
param set MC_ATT_D 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_D 0.004
param set MC_YAWPOS_P 2.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.005
# TODO add default MPC parameters
fi
set VEHICLE_TYPE mc
set MIXER FMU_octo_+
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
set PWM_MAX 1900

9
ROMFS/px4fmu_common/init.d/cmp_test
View File

@ -1,9 +0,0 @@
#!nsh
cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded
if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded
then
echo "CMP returned true"
else
echo "CMP returned false"
fi

195
ROMFS/px4fmu_common/init.d/rc.autostart Normal file
View File

@ -0,0 +1,195 @@
#
# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
# AUTOSTART PARTITION:
# 0 .. 999 Reserved (historical)
# 1000 .. 1999 Simulation setups
# 2000 .. 2999 Standard planes
# 3000 .. 3999 Flying wing
# 4000 .. 4999 Quad X
# 5000 .. 5999 Quad +
# 6000 .. 6999 Hexa X
# 7000 .. 7999 Hexa +
# 8000 .. 8999 Octo X
# 9000 .. 9999 Octo +
# 10000 .. 10999 Wide arm / H frame
# 11000 .. 11999 Hexa Cox
# 12000 .. 12999 Octo Cox
#
# Simulation setups
#
if param compare SYS_AUTOSTART 1000
then
#sh /etc/init.d/1000_rc_fw_easystar.hil
fi
if param compare SYS_AUTOSTART 1001
then
sh /etc/init.d/1001_rc_quad_x.hil
fi
if param compare SYS_AUTOSTART 1002
then
sh /etc/init.d/1002_rc_fw_state.hil
fi
if param compare SYS_AUTOSTART 1003
then
sh /etc/init.d/1003_rc_quad_+.hil
fi
if param compare SYS_AUTOSTART 1004
then
sh /etc/init.d/1004_rc_fw_Rascal110.hil
fi
#
# Standard plane
#
if param compare SYS_AUTOSTART 2100 100
then
sh /etc/init.d/2100_mpx_easystar
set MODE custom
fi
if param compare SYS_AUTOSTART 2101 101
then
sh /etc/init.d/2101_hk_bixler
set MODE custom
fi
if param compare SYS_AUTOSTART 2102 102
then
sh /etc/init.d/2102_3dr_skywalker
set MODE custom
fi
#
# Flying wing
#
if param compare SYS_AUTOSTART 3030 30
then
sh /etc/init.d/3030_io_camflyer
fi
if param compare SYS_AUTOSTART 3031 31
then
sh /etc/init.d/3031_phantom
fi
if param compare SYS_AUTOSTART 3032 32
then
sh /etc/init.d/3032_skywalker_x5
fi
if param compare SYS_AUTOSTART 3033 33
then
sh /etc/init.d/3033_wingwing
fi
if param compare SYS_AUTOSTART 3034 34
then
sh /etc/init.d/3034_fx79
fi
#
# Quad X
#
if param compare SYS_AUTOSTART 4008 8
then
#sh /etc/init.d/4008_ardrone
fi
if param compare SYS_AUTOSTART 4009 9
then
#sh /etc/init.d/4009_ardrone_flow
fi
if param compare SYS_AUTOSTART 4010 10
then
sh /etc/init.d/4010_dji_f330
fi
if param compare SYS_AUTOSTART 4011 11
then
sh /etc/init.d/4011_dji_f450
fi
if param compare SYS_AUTOSTART 4012 12
then
sh /etc/init.d/4012_hk_x550
fi
#
# Quad +
#
if param compare SYS_AUTOSTART 5001
then
sh /etc/init.d/5001_quad_+_pwm
fi
#
# Hexa X
#
if param compare SYS_AUTOSTART 6001
then
sh /etc/init.d/6001_hexa_x_pwm
fi
#
# Hexa +
#
if param compare SYS_AUTOSTART 7001
then
sh /etc/init.d/7001_hexa_+_pwm
fi
#
# Octo X
#
if param compare SYS_AUTOSTART 8001
then
sh /etc/init.d/8001_octo_x_pwm
fi
#
# Octo +
#
if param compare SYS_AUTOSTART 9001
then
sh /etc/init.d/9001_octo_+_pwm
fi
#
# Wide arm / H frame
#
if param compare SYS_AUTOSTART 10015 15
then
sh /etc/init.d/10015_tbs_discovery
fi
if param compare SYS_AUTOSTART 10016 16
then
sh /etc/init.d/10016_3dr_iris
fi
#
# Octo Coaxial
#
if param compare SYS_AUTOSTART 12001
then
sh /etc/init.d/12001_octo_cox_pwm
fi

113
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
View File

@ -1,113 +0,0 @@
#!nsh
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.002
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.09
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 6.8
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
#
# Start the Mikrokopter ESC driver
#
if [ $MKBLCTRL_MODE == yes ]
then
if [ $MKBLCTRL_FRAME == x ]
then
echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing"
mkblctrl -mkmode x
else
echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing"
mkblctrl -mkmode +
fi
else
if [ $MKBLCTRL_FRAME == x ]
then
echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame"
else
echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame"
fi
mkblctrl
fi
usleep 10000
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer
#
if [ $MKBLCTRL_FRAME == x ]
then
mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix
else
mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix
fi
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

120
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
View File

@ -1,120 +0,0 @@
#!nsh
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.002
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.09
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 6.8
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
echo "RC script for PX4FMU + PX4IO + PPM-ESCs running"
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
usleep 10000
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
sh /etc/init.d/rc.io
else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
if [ $ESC_MAKER = afro ]
then
# Set PWM values for Afro ESCs
pwm disarmed -c 1234 -p 1050
pwm min -c 1234 -p 1080
pwm max -c 1234 -p 1860
else
# Set PWM values for typical ESCs
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 980
pwm max -c 1234 -p 1800
fi
#
# Load mixer
#
if [ $FRAME_GEOMETRY == x ]
then
echo "Frame geometry X"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
else
if [ $FRAME_GEOMETRY == w ]
then
echo "Frame geometry W"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
else
echo "Frame geometry +"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi
fi
#
# Set PWM output frequency
#
pwm rate -r 400 -c 1234
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi
echo "Script end"

34
ROMFS/px4fmu_common/init.d/rc.fixedwing
View File

@ -1,34 +0,0 @@
#!nsh
#
# Standard everything needed for fixedwing except mixer, actuator output and mavlink
#
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
#
# Start attitude controller
#
fw_att_control start
#
# Start the position controller
#
fw_pos_control_l1 start

19
ROMFS/px4fmu_common/init.d/rc.fw_apps Normal file
View File

@ -0,0 +1,19 @@
#!nsh
#
# Standard apps for fixed wing
#
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
#
# Start attitude controller
#
fw_att_control start
#
# Start the position controller
#
fw_pos_control_l1 start

94
ROMFS/px4fmu_common/init.d/rc.hexa
View File

@ -1,94 +0,0 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Hex"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
# 13 = hexarotor
#
param set MAV_TYPE 13
set EXIT_ON_END no
#
# Start and configure PX4IO interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# This is not possible on a hexa
tone_alarm error
fi
#
# Load mixer
#
mixer load /dev/pwm_output $MIXER
#
# Set PWM output frequency to 400 Hz
#
pwm rate -a -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 123456 -p 900
pwm min -c 123456 -p 1100
pwm max -c 123456 -p 1900
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

72
ROMFS/px4fmu_common/init.d/rc.interface Normal file
View File

@ -0,0 +1,72 @@
#!nsh
#
# Script to configure control interface
#
if [ $MIXER != none ]
then
#
# Load mixer
#
set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
#Use the mixer file from the SD-card if it exists
if [ -f $MIXERSD ]
then
set MIXER_FILE $MIXERSD
else
set MIXER_FILE /etc/mixers/$MIXER.mix
fi
if [ $OUTPUT_MODE == mkblctrl ]
then
set OUTPUT_DEV /dev/mkblctrl
else
set OUTPUT_DEV /dev/pwm_output
fi
if mixer load $OUTPUT_DEV $MIXER_FILE
then
echo "[init] Mixer loaded: $MIXER_FILE"
else
echo "[init] Error loading mixer: $MIXER_FILE"
tone_alarm $TUNE_OUT_ERROR
fi
else
echo "[init] Mixer not defined"
tone_alarm $TUNE_OUT_ERROR
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
then
if [ $PWM_OUTPUTS != none ]
then
#
# Set PWM output frequency
#
if [ $PWM_RATE != none ]
then
echo "[init] Set PWM rate: $PWM_RATE"
pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
fi
#
# Set disarmed, min and max PWM values
#
if [ $PWM_DISARMED != none ]
then
echo "[init] Set PWM disarmed: $PWM_DISARMED"
pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
fi
if [ $PWM_MIN != none ]
then
echo "[init] Set PWM min: $PWM_MIN"
pwm min -c $PWM_OUTPUTS -p $PWM_MIN
fi
if [ $PWM_MAX != none ]
then
echo "[init] Set PWM max: $PWM_MAX"
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
fi
fi
fi

36
ROMFS/px4fmu_common/init.d/rc.io
View File

@ -1,23 +1,21 @@
#
# Start PX4IO interface (depends on orb, commander)
# Init PX4IO interface
#
if px4io start
then
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# Disable px4io topic limiting
#
if [ $BOARD == fmuv1 ]
then
px4io limit 200
else
px4io limit 400
fi
else
# SOS
tone_alarm error
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
#
px4io recovery
#
# Adjust PX4IO update rate limit
#
set PX4IO_LIMIT 400
if hw_ver compare PX4FMU_V1
then
set PX4IO_LIMIT 200
fi
echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
px4io limit $PX4IO_LIMIT

8
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -1,14 +1,14 @@
#!nsh
#
# Initialise logging services.
# Initialize logging services.
#
if [ -d /fs/microsd ]
then
if [ $BOARD == fmuv1 ]
if hw_ver compare PX4FMU_V1
then
sdlog2 start -r 50 -a -b 16
sdlog2 start -r 50 -a -b 16 -t
else
sdlog2 start -r 200 -a -b 16
sdlog2 start -r 200 -a -b 16 -t
fi
fi

24
ROMFS/px4fmu_common/init.d/rc.mc_apps Normal file
View File

@ -0,0 +1,24 @@
#!nsh
#
# Standard apps for multirotors
#
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start

49
ROMFS/px4fmu_common/init.d/rc.mc_interface
View File

@ -1,49 +0,0 @@
#!nsh
#
# Script to set PWM min / max limits and mixer
#
#
# Load mixer
#
if [ $FRAME_GEOMETRY == x ]
then
echo "Frame geometry X"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
else
if [ $FRAME_GEOMETRY == w ]
then
echo "Frame geometry W"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
else
echo "Frame geometry +"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi
fi
if [ $FRAME_COUNT == 4 ]
then
set OUTPUTS 1234
param set MAV_TYPE 2
else
if [ $FRAME_COUNT == 6 ]
then
set OUTPUTS 123456
param set MAV_TYPE 13
else
set OUTPUTS 12345678
fi
fi
#
# Set PWM output frequency
#
pwm rate -c $OUTPUTS -r $PWM_RATE
#
# Set disarmed, min and max PWM signals (for DJI ESCs)
#
pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
pwm min -c $OUTPUTS -p $PWM_MIN
pwm max -c $OUTPUTS -p $PWM_MAX

39
ROMFS/px4fmu_common/init.d/rc.multirotor
View File

@ -1,39 +0,0 @@
#!nsh
#
# Standard everything needed for multirotors except mixer, actuator output and mavlink
#
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator
#
attitude_estimator_ekf start
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start

94
ROMFS/px4fmu_common/init.d/rc.octo
View File

@ -1,94 +0,0 @@
#!nsh
echo "[init] Octorotor startup"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
# 14 = octorotor
#
param set MAV_TYPE 14
set EXIT_ON_END no
#
# Start and configure PX4IO interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
else
# This is not possible on an octo
tone_alarm error
fi
#
# Load mixer
#
mixer load /dev/pwm_output $MIXER
#
# Set PWM output frequency to 400 Hz
#
pwm rate -a -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 12345678 -p 900
pwm min -c 12345678 -p 1100
pwm max -c 12345678 -p 1900
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi

23
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -10,41 +10,42 @@
ms5611 start
adc start
# mag might be external
# Mag might be external
if hmc5883 start
then
echo "using HMC5883"
echo "[init] Using HMC5883"
fi
if mpu6000 start
then
echo "using MPU6000"
echo "[init] Using MPU6000"
fi
if l3gd20 start
then
echo "using L3GD20(H)"
echo "[init] Using L3GD20(H)"
fi
if lsm303d start
if hw_ver compare PX4FMU_V2
then
set BOARD fmuv2
else
set BOARD fmuv1
if lsm303d start
then
echo "[init] Using LSM303D"
fi
fi
# Start airspeed sensors
if meas_airspeed start
then
echo "using MEAS airspeed sensor"
echo "[init] Using MEAS airspeed sensor"
else
if ets_airspeed start
then
echo "using ETS airspeed sensor (bus 3)"
echo "[init] Using ETS airspeed sensor (bus 3)"
else
if ets_airspeed start -b 1
then
echo "Using ETS airspeed sensor (bus 1)"
echo "[init] Using ETS airspeed sensor (bus 1)"
fi
fi
fi

13
ROMFS/px4fmu_common/init.d/rc.standalone
View File

@ -1,13 +0,0 @@
#!nsh
#
# Flight startup script for PX4FMU standalone configuration.
#
echo "[init] doing standalone PX4FMU startup..."
#
# Start the ORB
#
uorb start
echo "[init] startup done"

33
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -36,39 +36,6 @@ then
echo "Commander started"
fi
# Start px4io if present
if px4io start
then
echo "PX4IO driver started"
else
if fmu mode_serial
then
echo "FMU driver started"
fi
fi
# Start sensors
sh /etc/init.d/rc.sensors
# Start one of the estimators
if attitude_estimator_ekf status
then
echo "multicopter att filter running"
else
if att_pos_estimator_ekf status
then
echo "fixedwing att filter running"
else
attitude_estimator_ekf start
fi
fi
# Start GPS
if gps start
then
echo "GPS started"
fi
echo "MAVLink started, exiting shell.."
# Exit shell to make it available to MAVLink

768
ROMFS/px4fmu_common/init.d/rcS
View File

@ -8,39 +8,40 @@
#
set MODE autostart
set logfile /fs/microsd/bootlog.txt
set RC_FILE /fs/microsd/etc/rc.txt
set CONFIG_FILE /fs/microsd/etc/config.txt
set EXTRAS_FILE /fs/microsd/etc/extras.txt
set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
#
# Try to mount the microSD card.
#
echo "[init] looking for microSD..."
echo "[init] Looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
set LOG_FILE /fs/microsd/bootlog.txt
echo "[init] microSD card mounted at /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
echo "[init] no microSD card found"
set LOG_FILE /dev/null
echo "[init] No microSD card found"
# Play SOS
tone_alarm error
fi
#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
# To prevent automatic startup in the current flight mode,
# the script should set MODE to some other value.
#
if [ -f /fs/microsd/etc/rc ]
if [ -f $RC_FILE ]
then
echo "[init] reading /fs/microsd/etc/rc"
sh /fs/microsd/etc/rc
fi
# Also consider rc.txt files
if [ -f /fs/microsd/etc/rc.txt ]
then
echo "[init] reading /fs/microsd/etc/rc.txt"
sh /fs/microsd/etc/rc.txt
echo "[init] Executing init script: $RC_FILE"
sh $RC_FILE
set MODE custom
else
echo "[init] Init script not found: $RC_FILE"
fi
# if this is an APM build then there will be a rc.APM script
@ -52,20 +53,19 @@ then
echo "[init] USB interface connected"
fi
echo "Running rc.APM"
echo "[init] Running rc.APM"
# if APM startup is successful then nsh will exit
sh /etc/init.d/rc.APM
fi
if [ $MODE == autostart ]
then
echo "[init] AUTOSTART mode"
#
# Start terminal
# Start CDC/ACM serial driver
#
if sercon
then
echo "USB connected"
fi
sercon
#
# Start the ORB (first app to start)
@ -73,27 +73,20 @@ then
uorb start
#
# Load microSD params
# Load parameters
#
set PARAM_FILE /fs/microsd/params
if mtd start
then
param select /fs/mtd_params
if param load /fs/mtd_params
then
else
echo "FAILED LOADING PARAMS"
fi
set PARAM_FILE /fs/mtd_params
fi
param select $PARAM_FILE
if param load
then
echo "[init] Parameters loaded: $PARAM_FILE"
else
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
if param load /fs/microsd/params
then
echo "Parameters loaded"
else
echo "Parameter file corrupt - ignoring"
fi
fi
echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
fi
#
@ -101,355 +94,418 @@ then
#
if rgbled start
then
echo "Using external RGB Led"
echo "[init] Using external RGB Led"
else
if blinkm start
then
echo "[init] Using blinkm"
blinkm systemstate
fi
fi
#
# Set default values
#
set HIL no
set VEHICLE_TYPE none
set MIXER none
set USE_IO yes
set OUTPUT_MODE none
set PWM_OUTPUTS none
set PWM_RATE none
set PWM_DISARMED none
set PWM_MIN none
set PWM_MAX none
set MKBLCTRL_MODE none
set FMU_MODE pwm
set MAV_TYPE none
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
#
if param compare SYS_AUTOCONFIG 1
then
set DO_AUTOCONFIG yes
else
set DO_AUTOCONFIG no
fi
#
# Set parameters and env variables for selected AUTOSTART
#
if param compare SYS_AUTOSTART 0
then
echo "[init] Don't try to find autostart script"
else
sh /etc/init.d/rc.autostart
fi
#
# Override parameters from user configuration file
#
if [ -f $CONFIG_FILE ]
then
echo "[init] Reading config: $CONFIG_FILE"
sh $CONFIG_FILE
else
echo "[init] Config file not found: $CONFIG_FILE"
fi
#
# If autoconfig parameter was set, reset it and save parameters
#
if [ $DO_AUTOCONFIG == yes ]
then
param set SYS_AUTOCONFIG 0
param save
fi
set IO_PRESENT no
if [ $USE_IO == yes ]
then
#
# Check if PX4IO present and update firmware if needed
#
if [ -f /etc/extras/px4io-v2_default.bin ]
then
set IO_FILE /etc/extras/px4io-v2_default.bin
else
set IO_FILE /etc/extras/px4io-v1_default.bin
fi
if px4io checkcrc $IO_FILE
then
echo "[init] PX4IO CRC OK"
echo "PX4IO CRC OK" >> $LOG_FILE
set IO_PRESENT yes
else
echo "[init] Trying to update"
echo "PX4IO Trying to update" >> $LOG_FILE
tone_alarm MLL32CP8MB
if px4io forceupdate 14662 $IO_FILE
then
usleep 500000
if px4io checkcrc $IO_FILE
then
echo "[init] PX4IO CRC OK, update successful"
echo "PX4IO CRC OK after updating" >> $LOG_FILE
tone_alarm MLL8CDE
set IO_PRESENT yes
else
echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
fi
else
echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
fi
fi
if [ $IO_PRESENT == no ]
then
echo "[init] ERROR: PX4IO not found"
tone_alarm $TUNE_OUT_ERROR
fi
fi
#
# Set default output if not set
#
if [ $OUTPUT_MODE == none ]
then
if [ $USE_IO == yes ]
then
set OUTPUT_MODE io
else
set OUTPUT_MODE fmu
fi
fi
if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ]
then
# Need IO for output but it not present, disable output
set OUTPUT_MODE none
echo "[init] ERROR: PX4IO not found, disabling output"
# Avoid using ttyS0 for MAVLink on FMUv1
if hw_ver compare PX4FMU_V1
then
set FMU_MODE serial
fi
fi
if [ $HIL == yes ]
then
set OUTPUT_MODE hil
if hw_ver compare PX4FMU_V1
then
set FMU_MODE serial
fi
else
# Try to get an USB console if not in HIL mode
nshterm /dev/ttyACM0 &
fi
#
# Start the Commander (needs to be this early for in-air-restarts)
#
commander start
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/1000_rc_fw_easystar.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1001
then
sh /etc/init.d/1001_rc_quad.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1002
then
sh /etc/init.d/1002_rc_fw_state.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1003
then
sh /etc/init.d/1003_rc_quad_+.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1004
then
sh /etc/init.d/1004_rc_fw_Rascal110.hil
set MODE custom
fi
if [ $MODE != custom ]
then
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
#
# Upgrade PX4IO firmware
# Start primary output
#
if [ -f /etc/extras/px4io-v2_default.bin ]
then
set io_file /etc/extras/px4io-v2_default.bin
else
set io_file /etc/extras/px4io-v1_default.bin
fi
if px4io start
then
echo "PX4IO OK"
echo "PX4IO OK" >> $logfile
fi
set TTYS1_BUSY no
if px4io checkcrc $io_file
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ]
then
echo "PX4IO CRC OK"
echo "PX4IO CRC OK" >> $logfile
else
echo "PX4IO CRC failure"
echo "PX4IO CRC failure" >> $logfile
tone_alarm MBABGP
if px4io forceupdate 14662 $io_file
if [ $OUTPUT_MODE == io ]
then
usleep 500000
echo "[init] Use PX4IO PWM as primary output"
if px4io start
then
echo "PX4IO restart OK"
echo "PX4IO restart OK" >> $logfile
tone_alarm MSPAA
echo "[init] PX4IO started"
sh /etc/init.d/rc.io
else
echo "PX4IO restart failed"
echo "PX4IO restart failed" >> $logfile
tone_alarm MNGGG
sleep 10
reboot
echo "[init] ERROR: PX4IO start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
if [ $OUTPUT_MODE == fmu ]
then
echo "[init] Use FMU PWM as primary output"
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
else
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
fi
if hw_ver compare PX4FMU_V1
then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
then
set TTYS1_BUSY yes
fi
if [ $FMU_MODE == pwm_gpio ]
then
set TTYS1_BUSY yes
fi
fi
fi
if [ $OUTPUT_MODE == mkblctrl ]
then
echo "[init] Use MKBLCTRL as primary output"
set MKBLCTRL_ARG ""
if [ $MKBLCTRL_MODE == x ]
then
set MKBLCTRL_ARG "-mkmode x"
fi
if [ $MKBLCTRL_MODE == + ]
then
set MKBLCTRL_ARG "-mkmode +"
fi
if mkblctrl $MKBLCTRL_ARG
then
echo "[init] MKBLCTRL started"
else
echo "[init] ERROR: MKBLCTRL start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
if [ $OUTPUT_MODE == hil ]
then
echo "[init] Use HIL as primary output"
if hil mode_pwm
then
echo "[init] HIL output started"
else
echo "[init] ERROR: HIL output start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
#
# Start IO or FMU for RC PPM input if needed
#
if [ $IO_PRESENT == yes ]
then
if [ $OUTPUT_MODE != io ]
then
if px4io start
then
echo "[init] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] ERROR: PX4IO start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
else
echo "PX4IO update failed"
echo "PX4IO update failed" >> $logfile
tone_alarm MNGGG
if [ $OUTPUT_MODE != fmu ]
then
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
else
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
fi
if hw_ver compare PX4FMU_V1
then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
then
set TTYS1_BUSY yes
fi
if [ $FMU_MODE == pwm_gpio ]
then
set TTYS1_BUSY yes
fi
fi
fi
fi
fi
#
# MAVLink
#
set EXIT_ON_END no
#
# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
# AUTOSTART PARTITION:
# 0 .. 999 Reserved (historical)
# 1000 .. 1999 Simulation setups
# 2000 .. 2999 Standard planes
# 3000 .. 3999 Flying wing
# 4000 .. 4999 Quad X
# 5000 .. 5999 Quad +
# 6000 .. 6999 Hexa X
# 7000 .. 7999 Hexa +
# 8000 .. 8999 Octo X
# 9000 .. 9999 Octo +
# 10000 .. 10999 Wide arm / H frame
# 11000 .. 11999 Hexa Cox
# 12000 .. 12999 Octo Cox
if param compare SYS_AUTOSTART 4008 8
if [ $HIL == yes ]
then
sh /etc/init.d/4008_ardrone
set MODE custom
fi
if param compare SYS_AUTOSTART 4009 9
then
sh /etc/init.d/4009_ardrone_flow
set MODE custom
fi
if param compare SYS_AUTOSTART 4010 10
then
set FRAME_GEOMETRY x
set FRAME_COUNT 4
set PWM_MIN 1200
set PWM_MAX 1900
set PWM_DISARMED 900
sh /etc/init.d/4010_dji_f330
set MODE custom
fi
if param compare SYS_AUTOSTART 4011 11
then
sh /etc/init.d/4011_dji_f450
set MODE custom
fi
if param compare SYS_AUTOSTART 4012
then
sh /etc/init.d/666_fmu_q_x550
set MODE custom
fi
if param compare SYS_AUTOSTART 6012 12
then
set MIXER /etc/mixers/FMU_hex_x.mix
sh /etc/init.d/rc.hexa
set MODE custom
fi
if param compare SYS_AUTOSTART 7013 13
then
set MIXER /etc/mixers/FMU_hex_+.mix
sh /etc/init.d/rc.hexa
set MODE custom
fi
if param compare SYS_AUTOSTART 8001
then
set MIXER /etc/mixers/FMU_octo_x.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 9001
then
set MIXER /etc/mixers/FMU_octo_+.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 12001
then
set MIXER /etc/mixers/FMU_octo_cox.mix
sh /etc/init.d/rc.octo
set MODE custom
fi
if param compare SYS_AUTOSTART 10015 15
then
sh /etc/init.d/10015_tbs_discovery
set MODE custom
fi
if param compare SYS_AUTOSTART 10016 16
then
sh /etc/init.d/10016_3dr_iris
set MODE custom
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame
if param compare SYS_AUTOSTART 4017 17
then
set MKBLCTRL_MODE no
set MKBLCTRL_FRAME x
sh /etc/init.d/rc.custom_dji_f330_mkblctrl
set MODE custom
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame
if param compare SYS_AUTOSTART 5018 18
then
set MKBLCTRL_MODE no
set MKBLCTRL_FRAME +
sh /etc/init.d/rc.custom_dji_f330_mkblctrl
set MODE custom
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing
if param compare SYS_AUTOSTART 4019 19
then
set MKBLCTRL_MODE yes
set MKBLCTRL_FRAME x
sh /etc/init.d/rc.custom_dji_f330_mkblctrl
set MODE custom
fi
# PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing
if param compare SYS_AUTOSTART 5020 20
then
set MKBLCTRL_MODE yes
set MKBLCTRL_FRAME +
sh /etc/init.d/rc.custom_dji_f330_mkblctrl
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 4021 21
then
set FRAME_GEOMETRY x
set ESC_MAKER afro
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 10022 22
then
set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
if param compare SYS_AUTOSTART 3030 30
then
sh /etc/init.d/3030_io_camflyer
set MODE custom
fi
if param compare SYS_AUTOSTART 3031 31
then
sh /etc/init.d/3031_io_phantom
set MODE custom
fi
if param compare SYS_AUTOSTART 3032 32
then
sh /etc/init.d/3032_skywalker_x5
set MODE custom
fi
if param compare SYS_AUTOSTART 3033 33
then
sh /etc/init.d/3033_io_wingwing
set MODE custom
fi
if param compare SYS_AUTOSTART 3034 34
then
sh /etc/init.d/3034_io_fx79
set MODE custom
fi
if param compare SYS_AUTOSTART 40
then
sh /etc/init.d/40_io_segway
set MODE custom
fi
if param compare SYS_AUTOSTART 2100 100
then
sh /etc/init.d/2100_mpx_easystar
set MODE custom
fi
if param compare SYS_AUTOSTART 2101 101
then
sh /etc/init.d/2101_hk_bixler
set MODE custom
fi
if param compare SYS_AUTOSTART 2102 102
then
sh /etc/init.d/2102_3dr_skywalker
set MODE custom
fi
if param compare SYS_AUTOSTART 800
then
sh /etc/init.d/800_sdlogger
set MODE custom
fi
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]
then
sh /fs/microsd/etc/rc.local
fi
# If none of the autostart scripts triggered, get a minimal setup
if [ $MODE == autostart ]
then
# Telemetry port is on both FMU boards ttyS1
# but the AR.Drone motors can be get 'flashed'
# if starting MAVLink on them - so do not
# start it as default (default link: USB)
# Start commander
commander start
# Start px4io if present
if px4io detect
sleep 1
mavlink start -b 230400 -d /dev/ttyACM0
usleep 5000
else
if [ $TTYS1_BUSY == yes ]
then
px4io start
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
mavlink start -d /dev/ttyS0
usleep 5000
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
if fmu mode_serial
# Start MAVLink on default port: ttyS1
mavlink start
usleep 5000
fi
fi
#
# Sensors, Logging, GPS
#
echo "[init] Start sensors"
sh /etc/init.d/rc.sensors
if [ $HIL == no ]
then
echo "[init] Start logging"
sh /etc/init.d/rc.logging
echo "[init] Start GPS"
gps start
fi
#
# Fixed wing setup
#
if [ $VEHICLE_TYPE == fw ]
then
echo "[init] Vehicle type: FIXED WING"
if [ $MIXER == none ]
then
# Set default mixer for fixed wing if not defined
set MIXER FMU_AERT
fi
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 1 (fixed wing) if not defined
set MAV_TYPE 1
fi
param set MAV_TYPE $MAV_TYPE
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
# Start standard fixedwing apps
sh /etc/init.d/rc.fw_apps
fi
#
# Multicopters setup
#
if [ $VEHICLE_TYPE == mc ]
then
echo "[init] Vehicle type: MULTICOPTER"
if [ $MIXER == none ]
then
# Set default mixer for multicopter if not defined
set MIXER quad_x
fi
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 2 (quadcopter) if not defined
set MAV_TYPE 2
# Use mixer to detect vehicle type
if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ]
then
echo "FMU driver (no PWM) started"
param set MAV_TYPE 13
fi
if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
then
param set MAV_TYPE 14
fi
if [ $MIXER == FMU_octo_cox ]
then
param set MAV_TYPE 14
fi
fi
# Start sensors
sh /etc/init.d/rc.sensors
# Start one of the estimators
attitude_estimator_ekf start
# Start GPS
gps start
param set MAV_TYPE $MAV_TYPE
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
# Start standard multicopter apps
sh /etc/init.d/rc.mc_apps
fi
#
# Generic setup (autostart ID not found)
#
if [ $VEHICLE_TYPE == none ]
then
echo "[init] Vehicle type: GENERIC"
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
fi
# Start any custom addons
if [ -f $EXTRAS_FILE ]
then
echo "[init] Starting addons script: $EXTRAS_FILE"
sh $EXTRAS_FILE
else
echo "[init] Addons script not found: $EXTRAS_FILE"
fi
if [ $EXIT_ON_END == yes ]

3
Tools/px4params/.gitignore vendored
View File

@ -1,2 +1,3 @@
parameters.wiki
parameters.xml
parameters.xml
cookies.txt

43
Tools/px4params/dokuwikiout.py
View File

@ -1,10 +1,24 @@
import output
from xml.sax.saxutils import escape
class DokuWikiOutput(output.Output):
def Generate(self, groups):
result = ""
pre_text = """<?xml version='1.0'?>
<methodCall>
<methodName>wiki.putPage</methodName>
<params>
<param>
<value>
<string>:firmware:parameters</string>
</value>
</param>
<param>
<value>
<string>"""
result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values."
for group in groups:
result += "==== %s ====\n\n" % group.GetName()
result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
for param in group.GetParams():
code = param.GetFieldValue("code")
@ -13,25 +27,36 @@ class DokuWikiOutput(output.Output):
result += "| %s | %s " % (code, name)
min_val = param.GetFieldValue("min")
if min_val is not None:
result += "| %s " % min_val
result += " | %s " % min_val
else:
result += "|"
result += " | "
max_val = param.GetFieldValue("max")
if max_val is not None:
result += "| %s " % max_val
result += " | %s " % max_val
else:
result += "|"
result += " | "
def_val = param.GetFieldValue("default")
if def_val is not None:
result += "| %s " % def_val
else:
result += "|"
result += " | "
long_desc = param.GetFieldValue("long_desc")
if long_desc is not None:
long_desc = long_desc.replace("\n", "")
result += "| %s " % long_desc
else:
result += "|"
result += "|\n"
result += " | "
result += " |\n"
result += "\n"
return result
post_text = """</string>
</value>
</param>
<param>
<value>
<name>sum</name>
<string>Updated parameters automagically from code.</string>
</value>
</param>
</params>
</methodCall>"""
return pre_text + escape(result) + post_text

5
Tools/px4params/xmlrpc.sh Normal file
View File

@ -0,0 +1,5 @@
python px_process_params.py
rm cookies.txt
curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login
curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php"

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -57,6 +57,7 @@ MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/hw_ver
#
# General system control

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -60,6 +60,7 @@ MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/hw_ver
#
# General system control

1
makefiles/config_px4fmu-v2_test.mk
View File

@ -23,6 +23,7 @@ MODULES += systemcmds/reboot
MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/hw_ver
#
# Library modules

14
nuttx-configs/px4fmu-v1/include/board.h
View File

@ -246,14 +246,14 @@
*
* There are sensors on SPI1, and SPI3 is connected to the microSD slot.
*/
#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2
#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1
#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2
#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2
#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz)
#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz)
#define GPIO_SPI3_NSS (GPIO_SPI3_NSS_2|GPIO_SPEED_50MHz)
/* SPI DMA configuration for SPI3 (microSD) */
#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1

12
nuttx-configs/px4fmu-v2/include/board.h
View File

@ -260,13 +260,13 @@
*
* There are sensors on SPI1, and SPI2 is connected to the FRAM.
*/
#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1
#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1
#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2
#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
/************************************************************************************
* Public Data

2
src/drivers/airspeed/airspeed.cpp
View File

@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));

2
src/drivers/boards/px4fmu-v2/board_config.h
View File

@ -75,7 +75,7 @@ __BEGIN_DECLS
/* PX4FMU GPIOs ***********************************************************************************/
/* LEDs */
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
/* External interrupts */
#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)

6
src/drivers/boards/px4io-v1/board_config.h
View File

@ -58,11 +58,11 @@
/* PX4IO GPIOs **********************************************************************/
/* LEDs */
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* Safety switch button *************************************************************/

8
src/drivers/boards/px4io-v2/board_config.h
View File

@ -74,9 +74,9 @@
/* LEDS **********************************************************************/
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
/* Safety switch button *******************************************************/
@ -114,7 +114,7 @@
/* XXX these should be UART pins */
#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
/*
* High-resolution timer

2
src/drivers/boards/px4io-v2/px4iov2_init.c
View File

@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ADC_VSERVO);
stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
stm32_configgpio(GPIO_SBUS_OUTPUT);
/* sbus output enable is active low - disable it by default */

4
src/drivers/drv_pwm_output.h
View File

@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm);
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
/** set the number of servos in (unsigned)arg - allows change of
* split between servos and GPIO */
#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20)
/** set the lockdown override flag to enable outputs in HIL */
#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21)

54
src/drivers/drv_rc_input.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -62,6 +62,11 @@
*/
#define RC_INPUT_MAX_CHANNELS 18
/**
* Maximum RSSI value
*/
#define RC_INPUT_RSSI_MAX 255
/**
* Input signal type, value is a control position from zero to 100
* percent.
@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE {
* on the board involved.
*/
struct rc_input_values {
/** decoding time */
uint64_t timestamp;
/** publication time */
uint64_t timestamp_publication;
/** last valid reception time */
uint64_t timestamp_last_signal;
/** number of channels actually being seen */
uint32_t channel_count;
@ -92,6 +100,40 @@ struct rc_input_values {
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
int32_t rssi;
/**
* explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
* Only the true state is reliable, as there are some (PPM) receivers on the market going
* into failsafe without telling us explicitly.
* */
bool rc_failsafe;
/**
* RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
* True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
* Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
* */
bool rc_lost;
/**
* Number of lost RC frames.
* Note: intended purpose: observe the radio link quality if RSSI is not available
* This value must not be used to trigger any failsafe-alike funtionality.
* */
uint16_t rc_lost_frame_count;
/**
* Number of total RC frames.
* Note: intended purpose: observe the radio link quality if RSSI is not available
* This value must not be used to trigger any failsafe-alike funtionality.
* */
uint16_t rc_total_frame_count;
/**
* Length of a single PPM frame.
* Zero for non-PPM systems
*/
uint16_t rc_ppm_frame_length;
/** Input source */
enum RC_INPUT_SOURCE input_source;
@ -107,8 +149,12 @@ ORB_DECLARE(input_rc);
#define _RC_INPUT_BASE 0x2b00
/** Fetch R/C input values into (rc_input_values *)arg */
#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0)
/** Enable RSSI input via ADC */
#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1)
/** Enable RSSI input via PWM signal */
#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2)
#endif /* _DRV_RC_INPUT_H */

58
src/drivers/drv_sbus.h Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (c) 2014 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 drv_sbus.h
*
* Futaba S.BUS / S.BUS 2 compatible interface.
*/
#ifndef _DRV_SBUS_H
#define _DRV_SBUS_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_orb_dev.h"
/**
* Path for the default S.BUS device
*/
#define SBUS_DEVICE_PATH "/dev/sbus"
#define _SBUS_BASE 0x2c00
/** Enable S.BUS version 1 / 2 output (0 to disable) */
#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0)
#endif /* _DRV_SBUS_H */

2
src/drivers/gps/gps.cpp
View File

@ -209,7 +209,7 @@ GPS::init()
goto out;
/* start the GPS driver worker task */
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);

160
src/drivers/hmc5883/hmc5883.cpp
View File

@ -849,42 +849,24 @@ HMC5883::collect()
/* scale values for output */
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
#ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) {
/* to align the sensor axes with the board, x and y need to be flipped */
new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch x and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD
}
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
#endif
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
struct mag_report report;
ssize_t sz;
int ret = 1;
uint8_t good_count = 0;
// XXX do something smarter here
int fd = (int)enable;
@ -932,32 +915,17 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
1.0f,
};
float avg_excited[3] = {0.0f, 0.0f, 0.0f};
unsigned i;
float sum_excited[3] = {0.0f, 0.0f, 0.0f};
/* expected axis scaling. The datasheet says that 766 will
* be places in the X and Y axes and 713 in the Z
* axis. Experiments show that in fact 766 is placed in X,
* and 713 in Y and Z. This is relative to a base of 660
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
warnx("starting mag scale calibration");
/* do a simple demand read */
sz = read(filp, (char *)&report, sizeof(report));
if (sz != sizeof(report)) {
warn("immediate read failed");
ret = 1;
goto out;
}
warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
/* don't do this for now, it can lead to a crash in start() respectively work_queue() */
// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
// warn("failed to set queue depth");
// ret = 1;
// goto out;
// }
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out;
}
/* Set to 2.5 Gauss */
if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
ret = 1;
goto out;
@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out;
}
/* read the sensor 10x and report each value */
for (i = 0; i < 500; i++) {
// discard 10 samples to let the sensor settle
for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
}
} else {
avg_excited[0] += report.x;
avg_excited[1] += report.y;
avg_excited[2] += report.z;
/* read the sensor up to 50x, stopping when we have 10 good values */
for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
cal[2] > 0.7f && cal[2] < 1.35f) {
good_count++;
sum_excited[0] += cal[0];
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
avg_excited[0] /= i;
avg_excited[1] /= i;
avg_excited[2] /= i;
if (good_count < 5) {
warn("failed calibration");
ret = -EIO;
goto out;
}
warnx("done. Performed %u reads", i);
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
#if 0
warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3];
/* calculate axis scaling */
scaling[0] = fabsf(1.16f / avg_excited[0]);
/* second axis inverted */
scaling[1] = fabsf(1.16f / -avg_excited[1]);
scaling[2] = fabsf(1.08f / avg_excited[2]);
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable)
conf_reg &= ~0x03;
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
ret = write_reg(ADDR_CONF_A, conf_reg);
if (OK != ret)
@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable)
uint8_t conf_reg_ret;
read_reg(ADDR_CONF_A, conf_reg_ret);
//print_info();
return !(conf_reg == conf_reg_ret);
}
@ -1211,6 +1221,10 @@ HMC5883::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)1.0/_range_scale, (double)_range_ga);
_reports->print_info("report queue");
}

22
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -77,7 +77,6 @@
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
@ -178,24 +177,17 @@ MEASAirspeed::collect()
return ret;
}
//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
// XXX leaving this in until new calculation method has been cross-checked
//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
//diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
dp_raw = 0x3FFF & dp_raw; //mask the used bits
/* mask the used bits */
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
// XXX we may want to smooth out the readings to remove noise.
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value
// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
/* calculate differential pressure. As its centered around 8000
* and can go positive or negative, enforce absolute value
*/
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
@ -204,7 +196,7 @@ MEASAirspeed::collect()
struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed).
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
@ -392,7 +384,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))

47
src/drivers/px4fmu/fmu.cpp
View File

@ -626,7 +626,7 @@ PX4FMU::task_main()
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
@ -638,7 +638,15 @@ PX4FMU::task_main()
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
rc_in.timestamp_publication = ppm_last_valid_decode;
rc_in.timestamp_last_signal = ppm_last_valid_decode;
rc_in.rc_ppm_frame_length = ppm_frame_length;
rc_in.rssi = RC_INPUT_RSSI_MAX;
rc_in.rc_failsafe = false;
rc_in.rc_lost = false;
rc_in.rc_lost_frame_count = 0;
rc_in.rc_total_frame_count = 0;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
@ -1006,6 +1014,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_COUNT: {
/* change the number of outputs that are enabled for
* PWM. This is used to change the split between GPIO
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
* FMUv1
*/
switch (arg) {
case 0:
set_mode(MODE_NONE);
break;
case 2:
set_mode(MODE_2PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
case 6:
set_mode(MODE_6PWM);
break;
#endif
default:
ret = -EINVAL;
break;
}
break;
}
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
@ -1443,7 +1485,6 @@ void
sensor_reset(int ms)
{
int fd;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);

255
src/drivers/px4io/px4io.cpp
View File

@ -61,6 +61,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@ -238,6 +239,7 @@ private:
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
uint64_t _rc_last_valid; ///< last valid timestamp
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
@ -270,7 +272,8 @@ private:
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
actuator_outputs_s _outputs; ///< mixed outputs
servorail_status_s _servorail_status; ///< servorail status
bool _primary_pwm_device; ///< true if we are the default PWM output
bool _lockdown_override; ///< allow to override the safety lockdown
@ -444,14 +447,14 @@ private:
* @param vservo vservo register
* @param vrssi vrssi register
*/
void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
void io_handle_vservo(uint16_t vservo, uint16_t vrssi);
};
namespace
{
PX4IO *g_dev;
PX4IO *g_dev = nullptr;
}
@ -467,6 +470,7 @@ PX4IO::PX4IO(device::Device *interface) :
_update_interval(0),
_rc_handling_disabled(false),
_rc_chan_count(0),
_rc_last_valid(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
@ -506,7 +510,8 @@ PX4IO::PX4IO(device::Device *interface) :
/* open MAVLink text channel */
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
_debug_enabled = true;
_debug_enabled = false;
_servorail_status.rssi_v = 0;
}
PX4IO::~PX4IO()
@ -580,6 +585,12 @@ PX4IO::init()
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol == _io_reg_get_error) {
log("failed to communicate with IO");
mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
return -1;
}
if (protocol != PX4IO_PROTOCOL_VERSION) {
log("protocol/firmware mismatch");
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
@ -750,7 +761,7 @@ PX4IO::init()
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
@ -774,8 +785,6 @@ PX4IO::task_main()
hrt_abstime poll_last = 0;
hrt_abstime orb_check_last = 0;
log("starting");
_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/*
@ -809,8 +818,6 @@ PX4IO::task_main()
fds[0].fd = _t_actuator_controls_0;
fds[0].events = POLLIN;
log("ready");
/* lock against the ioctl handler */
lock();
@ -1308,6 +1315,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
/* voltage is scaled to mV */
battery_status.voltage_v = vbatt / 1000.0f;
battery_status.voltage_filtered_v = vbatt / 1000.0f;
/*
ibatt contains the raw ADC count, as 12 bit ADC
@ -1339,19 +1347,18 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
void
PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
{
servorail_status_s servorail_status;
servorail_status.timestamp = hrt_absolute_time();
_servorail_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
servorail_status.voltage_v = vservo * 0.001f;
servorail_status.rssi_v = vrssi * 0.001f;
_servorail_status.voltage_v = vservo * 0.001f;
_servorail_status.rssi_v = vrssi * 0.001f;
/* lazily publish the servorail voltages */
if (_to_servorail > 0) {
orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
} else {
_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
_to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status);
}
}
@ -1361,7 +1368,10 @@ PX4IO::io_get_status()
uint16_t regs[6];
int ret;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
/* get
* STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT,
* STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI
* in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
if (ret != OK)
@ -1398,7 +1408,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*
* This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
input_rc.timestamp = hrt_absolute_time();
input_rc.timestamp_publication = hrt_absolute_time();
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
if (ret != OK)
@ -1408,13 +1419,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
* Get the channel count any any extra channels. This is no more expensive than reading the
* channel count once.
*/
channel_count = regs[0];
channel_count = regs[PX4IO_P_RAW_RC_COUNT];
if (channel_count != _rc_chan_count)
perf_count(_perf_chan_count);
_rc_chan_count = channel_count;
input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
/* rc_lost has to be set before the call to this function */
if (!input_rc.rc_lost && !input_rc.rc_failsafe)
_rc_last_valid = input_rc.timestamp_publication;
input_rc.timestamp_last_signal = _rc_last_valid;
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@ -1431,13 +1454,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
int
PX4IO::io_publish_raw_rc()
{
/* if no raw RC, just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
return OK;
/* fetch values from IO */
rc_input_values rc_val;
rc_val.timestamp = hrt_absolute_time();
/* set the RC status flag ORDER MATTERS! */
rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
int ret = io_get_raw_rc_input(rc_val);
@ -1456,6 +1478,11 @@ PX4IO::io_publish_raw_rc()
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
/* we do not know the RC input, only publish if RC OK flag is set */
/* if no raw RC, just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
return OK;
}
/* lazily advertise on first publication */
@ -1672,7 +1699,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
total_len++;
}
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
int ret;
for (int i = 0; i < 30; i++) {
/* failed, but give it a 2nd shot */
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
if (ret) {
usleep(333);
} else {
break;
}
}
/* print mixer chunk */
if (debuglevel > 5 || ret) {
@ -1696,7 +1734,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
msg->text[0] = '\n';
msg->text[1] = '\0';
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
int ret;
for (int i = 0; i < 30; i++) {
/* failed, but give it a 2nd shot */
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
if (ret) {
usleep(333);
} else {
break;
}
}
if (ret)
return ret;
@ -1742,15 +1791,14 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
uint16_t io_status_flags = flags;
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
@ -1809,8 +1857,17 @@ PX4IO::print_status()
printf("\n");
if (raw_inputs > 0) {
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags,
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
(((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "")
);
if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
@ -1836,7 +1893,13 @@ PX4IO::print_status()
printf("\n");
/* setup and state */
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
printf("features 0x%04x%s%s%s%s\n", features,
((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s\n",
arming,
@ -2267,6 +2330,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
case RC_INPUT_ENABLE_RSSI_ANALOG:
if (arg) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0);
}
break;
case RC_INPUT_ENABLE_RSSI_PWM:
if (arg) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0);
}
break;
case SBUS_SET_PROTO_VERSION:
if (arg == 1) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
} else if (arg == 2) {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
} else {
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
}
break;
default:
/* not a recognized value */
ret = -ENOTTY;
@ -2375,8 +2470,10 @@ start(int argc, char *argv[])
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr)
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
@ -2638,7 +2735,7 @@ monitor(void)
/* clear screen */
printf("\033[2J");
unsigned cancels = 3;
unsigned cancels = 2;
for (;;) {
pollfd fds[1];
@ -2652,17 +2749,17 @@ monitor(void)
read(0, &c, 1);
if (cancels-- == 0) {
printf("\033[H"); /* move cursor home and clear screen */
printf("\033[2J\033[H"); /* move cursor home and clear screen */
exit(0);
}
}
if (g_dev != nullptr) {
printf("\033[H"); /* move cursor home and clear screen */
printf("\033[2J\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_debug();
printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
} else {
errx(1, "driver not loaded, exiting");
@ -2764,6 +2861,7 @@ px4io_main(int argc, char *argv[])
printf("[px4io] loaded, detaching first\n");
/* stop the driver */
delete g_dev;
g_dev = nullptr;
}
PX4IO_Uploader *up;
@ -2836,18 +2934,30 @@ px4io_main(int argc, char *argv[])
}
if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade");
} else {
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
}
// tear down the px4io instance
delete g_dev;
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
}
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
}
// tear down the px4io instance
delete g_dev;
g_dev = nullptr;
// upload the specified firmware
const char *fn[2];
fn[0] = argv[3];
@ -2905,6 +3015,7 @@ px4io_main(int argc, char *argv[])
/* stop the driver */
delete g_dev;
g_dev = nullptr;
exit(0);
}
@ -2962,6 +3073,60 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "lockdown"))
lockdown(argc, argv);
if (!strcmp(argv[1], "sbus1_out")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
if (ret != 0) {
errx(ret, "S.BUS v1 failed");
}
exit(0);
}
if (!strcmp(argv[1], "sbus2_out")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
if (ret != 0) {
errx(ret, "S.BUS v2 failed");
}
exit(0);
}
if (!strcmp(argv[1], "rssi_analog")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1);
if (ret != 0) {
errx(ret, "RSSI analog failed");
}
exit(0);
}
if (!strcmp(argv[1], "rssi_pwm")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
*/
int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1);
if (ret != 0) {
errx(ret, "RSSI PWM failed");
}
exit(0);
}
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
"'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
"'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
}

17
src/drivers/px4io/px4io_uploader.cpp
View File

@ -51,6 +51,7 @@
#include <poll.h>
#include <termios.h>
#include <sys/stat.h>
#include <nuttx/arch.h>
#include <crc32.h>
@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[])
cfsetspeed(&t, 115200);
tcsetattr(_io_fd, TCSANOW, &t);
/* look for the bootloader */
ret = sync();
/* look for the bootloader for 150 ms */
for (int i = 0; i < 15; i++) {
ret = sync();
if (ret == OK) {
break;
} else {
usleep(10000);
}
}
if (ret != OK) {
/* this is immediately fatal */
@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[])
close(_fw_fd);
close(_io_fd);
_io_fd = -1;
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
return ret;
}

2
src/drivers/px4io/uploader.h
View File

@ -91,7 +91,7 @@ private:
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);
int get_sync(unsigned timeout = 1000);
int get_sync(unsigned timeout = 40);
int sync();
int get_info(int param, uint32_t &val);
int erase();

8
src/modules/sdlog2/sdlog2_version.h → src/lib/version/version.h
View File

@ -33,15 +33,15 @@
****************************************************************************/
/**
* @file sdlog2_version.h
* @file version.h
*
* Tools for system version detection.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_VERSION_H_
#define SDLOG2_VERSION_H_
#ifndef VERSION_H_
#define VERSION_H_
/*
GIT_VERSION is defined at build time via a Makefile call to the
@ -59,4 +59,4 @@
#define HW_ARCH "PX4FMU_V2"
#endif
#endif /* SDLOG2_VERSION_H_ */
#endif /* VERSION_H_ */

9
src/mainpage.dox Normal file
View File

@ -0,0 +1,9 @@
/**
\mainpage PX4 Autopilot Flight Control Stack and Middleware
This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows.
http://pixhawk.org
*/

4
src/modules/commander/commander.cpp
View File

@ -250,7 +250,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
3000,
2088,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@ -685,7 +685,7 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
pthread_attr_setstacksize(&commander_low_prio_attr, 1728);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);

2
src/modules/mavlink/mavlink.c
View File

@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[])
mavlink_task = task_spawn_cmd("mavlink",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
1200,
mavlink_thread_main,
(const char **)argv);

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -871,7 +871,7 @@ receive_start(int uart)
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_attr_setstacksize(&receiveloop_attr, 1816);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);

6
src/modules/mavlink/orb_listener.c
View File

@ -352,10 +352,10 @@ l_input_rc(const struct listener *l)
const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
rc_raw.timestamp / 1000,
rc_raw.timestamp_publication / 1000,
i,
(rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
(rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
@ -838,7 +838,7 @@ uorb_receive_start(void)
pthread_attr_init(&uorb_attr);
/* Set stack size, needs less than 2k */
pthread_attr_setstacksize(&uorb_attr, 2048);
pthread_attr_setstacksize(&uorb_attr, 1648);
pthread_t thread;
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);

2
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[])
deamon_task = task_spawn_cmd("multirotor_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
2408,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568,
position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL);
exit(0);

53
src/modules/px4iofirmware/adc.c
View File

@ -83,6 +83,14 @@ adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* put the ADC into power-down mode */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
/* bring the ADC out of power-down mode */
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
@ -96,41 +104,25 @@ adc_init(void)
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
/*
* Configure sampling time.
*
* For electrical protection reasons, we want to be able to have
* 10K in series with ADC inputs that leave the board. At 12MHz this
* means we need 28.5 cycles of sampling time (per table 43 in the
* datasheet).
*/
rSMPR1 = 0b00000000011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rSQR3 = 0; /* will be updated with the channel at conversion time */
return 0;
}
@ -141,11 +133,12 @@ adc_init(void)
uint16_t
adc_measure(unsigned channel)
{
perf_begin(adc_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
rSR = 0;
(void)rDR;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
@ -158,7 +151,6 @@ adc_measure(unsigned channel)
/* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
perf_end(adc_perf);
return 0xffff;
}
@ -166,6 +158,7 @@ adc_measure(unsigned channel)
/* read the result and clear EOC */
uint16_t result = rDR;
rSR = 0;
perf_end(adc_perf);
return result;

222
src/modules/px4iofirmware/controls.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
/* no channels */
r_raw_rc_count = 0;
system_state.rc_channels_timestamp_received = 0;
system_state.rc_channels_timestamp_valid = 0;
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
@ -97,26 +102,64 @@ controls_tick() {
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
#ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 */
rssi = mV / 13;
}
}
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
rssi = 255;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
rssi = 255;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
rssi = 100;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
rssi = 0;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
perf_end(c_gather_sbus);
/*
@ -125,13 +168,12 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
/* XXX sample RSSI properly here */
rssi = 255;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
@ -139,6 +181,9 @@ controls_tick() {
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
/*
* In some cases we may have received a frame, but input has still
* been lost.
@ -150,97 +195,100 @@ controls_tick() {
*/
if (dsm_updated || sbus_updated || ppm_updated) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp = hrt_absolute_time();
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/* do not command anything in failsafe, kick in the RC loss counter */
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
uint16_t raw = r_raw_rc_values[i];
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
int16_t scaled;
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
uint16_t raw = r_raw_rc_values[i];
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
int16_t scaled;
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/*
* If we got an update with zero channels, treat it as
* a loss of input.
*
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if (assigned_channels == 0 || rssi == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag */
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
}
/*
@ -253,7 +301,7 @@ controls_tick() {
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
@ -261,24 +309,32 @@ controls_tick() {
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
/*
* Handle losing RC input
*/
if (rc_input_lost) {
/* this kicks in if the receiver is gone or the system went to failsafe */
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* Mark the arrays as empty */
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
r_rc_valid = 0;
}
/*

30
src/modules/px4iofirmware/mixer.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -71,6 +71,7 @@ extern "C" {
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
/* selected control values and count for mixing */
enum mixer_source {
@ -95,6 +96,7 @@ static void mixer_set_failsafe();
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
@ -199,13 +201,17 @@ mixer_tick(void)
}
} else if (source != MIX_NONE) {
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
in_mixer = false;
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@ -308,12 +314,17 @@ mixer_callback(uintptr_t handle,
static char mixer_text[256]; /* large enough for one mixer */
static unsigned mixer_text_length = 0;
void
int
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
return;
return 1;
}
/* abort if we're in the mixer */
if (in_mixer) {
return 1;
}
px4io_mixdata *msg = (px4io_mixdata *)buffer;
@ -321,7 +332,7 @@ mixer_handle_text(const void *buffer, size_t length)
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
return 0;
unsigned text_length = length - sizeof(px4io_mixdata);
@ -339,13 +350,16 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* disable mixing during the update */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
return 0;
}
/* append mixer text and nul-terminate */
/* append mixer text and nul-terminate, guard against overflow */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
@ -380,6 +394,8 @@ mixer_handle_text(const void *buffer, size_t length)
break;
}
return 0;
}
static void

19
src/modules/px4iofirmware/protocol.h
View File

@ -111,7 +111,6 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@ -128,8 +127,6 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@ -140,7 +137,17 @@
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RC_INPUT 5
@ -157,6 +164,10 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */

61
src/modules/px4iofirmware/px4io.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
@ -205,6 +210,9 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
@ -223,24 +231,41 @@ user_start(int argc, char *argv[])
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
while (true) {
phase = !phase;
usleep(300000);
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
#endif
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
@ -270,11 +295,12 @@ user_start(int argc, char *argv[])
check_reboot();
#if 0
/* check for debug activity */
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz */
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
@ -287,7 +313,6 @@ user_start(int argc, char *argv[])
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
#endif
}
}

14
src/modules/px4iofirmware/px4io.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -96,8 +96,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
@ -115,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
*/
struct sys_state_s {
volatile uint64_t rc_channels_timestamp;
volatile uint64_t rc_channels_timestamp_received;
volatile uint64_t rc_channels_timestamp_valid;
/**
* Last FMU receive time, in microseconds since system boot
@ -160,6 +162,7 @@ extern pwm_limit_t pwm_limit;
# define PX4IO_RELAY_CHANNELS 0
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
@ -177,12 +180,13 @@ extern pwm_limit_t pwm_limit;
* Mixer
*/
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
extern int mixer_handle_text(const void *buffer, size_t length);
/**
* Safety switch/LED.
*/
extern void safety_init(void);
extern void failsafe_led_init(void);
/**
* FMU communications
@ -213,7 +217,7 @@ extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;

63
src/modules/px4iofirmware/registers.c
View File

@ -90,8 +90,6 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0,
[PX4IO_P_STATUS_NRSSI] = 0,
[PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
uint16_t r_page_raw_rc_input[] =
{
[PX4IO_P_RAW_RC_COUNT] = 0,
[PX4IO_P_RAW_RC_FLAGS] = 0,
[PX4IO_P_RAW_RC_NRSSI] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_FRAME_COUNT] = 0,
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
@ -144,7 +148,12 @@ uint16_t r_page_scratch[32];
*/
volatile uint16_t r_page_setup[] =
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
/* default to RSSI ADC functionality */
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
#else
[PX4IO_P_SETUP_FEATURES] = 0,
#endif
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
@ -162,7 +171,14 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
#else
#define PX4IO_P_SETUP_FEATURES_VALID 0
#endif
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
@ -383,7 +399,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return mixer_handle_text(values, num_values * sizeof(*values));
}
break;
default:
@ -436,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_FEATURES:
value &= PX4IO_P_SETUP_FEATURES_VALID;
r_setup_features = value;
/* no implemented feature selection at this point */
/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
/* switch S.Bus output pin as needed */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
/* disable the conflicting options */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
}
#endif
/* disable the conflicting options with ADC RSSI */
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* apply changes */
r_setup_features = value;
break;
@ -505,8 +550,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
// don't allow reboot while armed
break;
}
@ -536,8 +580,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* do not allow a RC config change while outputs armed
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}

9
src/modules/px4iofirmware/safety.c
View File

@ -45,7 +45,6 @@
#include "px4io.h"
static struct hrt_call arming_call;
static struct hrt_call heartbeat_call;
static struct hrt_call failsafe_call;
/*
@ -84,7 +83,11 @@ safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
}
void
failsafe_led_init(void)
{
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@ -165,8 +168,8 @@ failsafe_blink(void *arg)
/* indicate that a serious initialisation error occured */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
LED_AMBER(true);
return;
}
return;
}
static bool failsafe = false;

48
src/modules/px4iofirmware/sbus.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@ -87,7 +87,7 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
@ -118,7 +118,7 @@ sbus_init(const char *device)
}
bool
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
* decode it.
*/
partial_frame_count = 0;
return sbus_decode(now, values, num_values, rssi, max_channels);
return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
}
/*
@ -215,14 +215,36 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
if ((frame[0] != 0x0f)) {
sbus_frame_drops++;
return false;
}
switch (frame[24]) {
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
@ -267,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
*rssi = 0;
return false;
*sbus_failsafe = true;
*sbus_frame_drop = true;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
/* set a special warning flag
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;
}
*rssi = 255;
return true;
}

282
src/modules/sdlog2/sdlog2.c
View File

@ -85,13 +85,13 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
#include "logbuffer.h"
#include "sdlog2_format.h"
#include "sdlog2_messages.h"
#include "sdlog2_version.h"
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
@ -108,13 +108,13 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */
static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
static const int MAX_WRITE_CHUNK = 512;
static const int MIN_BYTES_TO_WRITE = 512;
static const char *mountpoint = "/fs/microsd";
static const char *log_root = "/fs/microsd/log";
static int mavlink_fd = -1;
struct logbuffer_s lb;
@ -122,14 +122,17 @@ struct logbuffer_s lb;
static pthread_mutex_t logbuffer_mutex;
static pthread_cond_t logbuffer_cond;
static char folder_path[64];
static char log_dir[32];
/* statistics counters */
static unsigned long log_bytes_written = 0;
static uint64_t start_time = 0;
static unsigned long log_bytes_written = 0;
static unsigned long log_msgs_written = 0;
static unsigned long log_msgs_skipped = 0;
/* GPS time, used for log files naming */
static uint64_t gps_time = 0;
/* current state of logging */
static bool logging_enabled = false;
/* enable logging on start (-e option) */
@ -138,11 +141,14 @@ static bool log_on_start = false;
static bool log_when_armed = false;
/* delay = 1 / rate (rate defined by -r option) */
static useconds_t sleep_delay = 0;
/* use date/time for naming directories and files (-t option) */
static bool log_name_timestamp = false;
/* helper flag to track system state changes */
static bool flag_system_armed = false;
static pthread_t logwriter_pthread = 0;
static pthread_attr_t logwriter_attr;
/**
* Log buffer writing thread. Open and close file here.
@ -203,14 +209,14 @@ static void handle_command(struct vehicle_command_s *cmd);
static void handle_status(struct vehicle_status_s *cmd);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
* Create dir for current logging session. Store dir name in 'log_dir'.
*/
static int create_logfolder(void);
static int create_log_dir(void);
/**
* Select first free log file name and open it.
*/
static int open_logfile(void);
static int open_log_file(void);
static void
sdlog2_usage(const char *reason)
@ -218,11 +224,12 @@ sdlog2_usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n"
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n");
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
}
/**
@ -280,82 +287,112 @@ int sdlog2_main(int argc, char *argv[])
exit(1);
}
int create_logfolder()
int create_log_dir()
{
/* make folder on sdcard */
uint16_t folder_number = 1; // start with folder sess001
/* create dir on sdcard if needed */
uint16_t dir_number = 1; // start with dir sess001
int mkdir_ret;
/* look for the next folder that does not exist */
while (folder_number <= MAX_NO_LOGFOLDER) {
/* set up folder path: e.g. /fs/microsd/sess001 */
sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number);
mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
/* the result is -1 if the folder exists */
if (log_name_timestamp && gps_time != 0) {
/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
/* folder does not exist, success */
break;
if (mkdir_ret == OK) {
warnx("log dir created: %s", log_dir);
} else if (mkdir_ret == -1) {
/* folder exists already */
folder_number++;
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
} else {
/* look for the next dir that does not exist */
while (dir_number <= MAX_NO_LOGFOLDER) {
/* format log dir: e.g. /fs/microsd/sess001 */
sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
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) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
/* dir exists already */
dir_number++;
continue;
}
} else {
warn("failed creating new folder");
if (dir_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
return -1;
}
}
if (folder_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER);
return -1;
}
/* print logging path, important to find log file later */
warnx("log dir: %s", log_dir);
mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
return 0;
}
int open_logfile()
int open_log_file()
{
/* make folder on sdcard */
uint16_t file_number = 1; // start with file log001
/* string to hold the path to the log */
char path_buf[64] = "";
char log_file_name[16] = "";
char log_file_path[48] = "";
int fd = 0;
if (log_name_timestamp && gps_time != 0) {
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* set up file path: e.g. /fs/microsd/sess001/log001.bin */
sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number);
} else {
uint16_t file_number = 1; // start with file log001
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
if (!file_exist(log_file_path)) {
break;
}
if (file_exist(path_buf)) {
file_number++;
continue;
}
fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC);
if (fd == 0) {
warn("opening %s failed", path_buf);
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 */
warnx("all %d possible files exist already", MAX_NO_LOGFILE);
return -1;
}
warnx("logging to: %s.", path_buf);
mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf);
return fd;
}
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 */
warnx("all %d possible files exist already.", MAX_NO_LOGFILE);
return -1;
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
if (fd < 0) {
warn("failed opening log: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
} else {
warnx("log file: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
}
return 0;
return fd;
}
static void *logwriter_thread(void *arg)
@ -363,9 +400,12 @@ static void *logwriter_thread(void *arg)
/* set name */
prctl(PR_SET_NAME, "sdlog2_writer", 0);
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
int log_fd = open_log_file();
int log_fd = open_logfile();
if (log_fd < 0)
return;
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
/* write log messages formats, version and parameters */
log_bytes_written += write_formats(log_fd);
@ -443,14 +483,20 @@ static void *logwriter_thread(void *arg)
fsync(log_fd);
close(log_fd);
return OK;
return;
}
void sdlog2_start_log()
{
warnx("start logging.");
warnx("start logging");
mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
/* create log dir if needed */
if (create_log_dir() != 0) {
mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
errx(1, "error creating log dir");
}
/* initialize statistics counter */
log_bytes_written = 0;
start_time = hrt_absolute_time();
@ -458,30 +504,28 @@ void sdlog2_start_log()
log_msgs_skipped = 0;
/* initialize log buffer emptying thread */
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
pthread_attr_init(&logwriter_attr);
struct sched_param param;
/* low priority, as this is expensive disk I/O */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
(void)pthread_attr_setschedparam(&logwriter_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_attr_setstacksize(&logwriter_attr, 2048);
logwriter_should_exit = false;
/* start log buffer emptying thread */
if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) {
if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
errx(1, "error creating logwriter thread");
}
logging_enabled = true;
// XXX we have to destroy the attr at some point
}
void sdlog2_stop_log()
{
warnx("stop logging.");
warnx("stop logging");
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
logging_enabled = false;
@ -501,6 +545,7 @@ void sdlog2_stop_log()
}
logwriter_pthread = 0;
pthread_attr_destroy(&logwriter_attr);
sdlog2_status();
}
@ -569,8 +614,8 @@ int write_parameters(int fd)
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
param_get(param, &value);
break;
default:
break;
@ -588,18 +633,25 @@ int sdlog2_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("failed to open MAVLink log stream, start mavlink app first.");
warnx("failed to open MAVLink log stream, start mavlink app first");
}
/* log buffer size */
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
logging_enabled = false;
log_on_start = false;
log_when_armed = false;
log_name_timestamp = false;
flag_system_armed = false;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
@ -632,49 +684,52 @@ int sdlog2_thread_main(int argc, char *argv[])
log_when_armed = true;
break;
case 't':
log_name_timestamp = true;
break;
case '?':
if (optopt == 'c') {
warnx("Option -%c requires an argument.", optopt);
warnx("option -%c requires an argument", optopt);
} else if (isprint(optopt)) {
warnx("Unknown option `-%c'.", optopt);
warnx("unknown option `-%c'", optopt);
} else {
warnx("Unknown option character `\\x%x'.", optopt);
warnx("unknown option character `\\x%x'", optopt);
}
default:
sdlog2_usage("unrecognized flag");
errx(1, "exiting.");
errx(1, "exiting");
}
}
if (!file_exist(mountpoint)) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
if (create_logfolder()) {
errx(1, "unable to create logging folder, exiting.");
gps_time = 0;
/* create log root dir */
int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
err("failed creating log root dir: %s", log_root);
}
/* copy conversion scripts */
const char *converter_in = "/etc/logging/conv.zip";
char *converter_out = malloc(120);
sprintf(converter_out, "%s/conv.zip", folder_path);
char *converter_out = malloc(64);
snprintf(converter_out, 64, "%s/conv.zip", log_root);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
if (file_copy(converter_in, converter_out) != OK) {
warn("unable to copy conversion scripts");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes.", log_buffer_size);
warnx("log buffer size: %i bytes", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting.");
errx(1, "can't allocate log buffer, exiting");
}
struct vehicle_status_s buf_status;
@ -895,7 +950,7 @@ int sdlog2_thread_main(int argc, char *argv[])
* differs from the number of messages in the above list.
*/
if (fdsc_count > fdsc) {
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__);
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__);
fdsc_count = fdsc;
}
@ -919,19 +974,27 @@ int sdlog2_thread_main(int argc, char *argv[])
uint16_t differential_pressure_counter = 0;
/* enable logging on start if needed */
if (log_on_start)
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) {
gps_time = buf.gps_pos.time_gps_usec;
}
}
sdlog2_start_log();
}
while (!main_thread_should_exit) {
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
/* poll all topics if logging enabled or only management (first 2) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout);
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
/* handle the poll result */
if (poll_ret < 0) {
warnx("ERROR: poll error, stop logging.");
warnx("ERROR: poll error, stop logging");
main_thread_should_exit = true;
} else if (poll_ret > 0) {
@ -960,6 +1023,17 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
if (log_name_timestamp) {
gps_time = buf.gps_pos.time_gps_usec;
}
handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
@ -988,7 +1062,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@ -1279,7 +1353,7 @@ int sdlog2_thread_main(int argc, char *argv[])
free(lb.data);
warnx("exiting.");
warnx("exiting");
thread_running = false;
@ -1292,8 +1366,8 @@ void sdlog2_status()
float mebibytes = kibibytes / 1024.0f;
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);
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);
}
/**
@ -1312,7 +1386,7 @@ int file_copy(const char *file_old, const char *file_new)
int ret = 0;
if (source == NULL) {
warnx("failed opening input file to copy.");
warnx("failed opening input file to copy");
return 1;
}
@ -1320,7 +1394,7 @@ int file_copy(const char *file_old, const char *file_new)
if (target == NULL) {
fclose(source);
warnx("failed to open output file to copy.");
warnx("failed to open output file to copy");
return 1;
}
@ -1331,7 +1405,7 @@ int file_copy(const char *file_old, const char *file_new)
ret = fwrite(buf, 1, nread, target);
if (ret <= 0) {
warnx("error writing file.");
warnx("error writing file");
ret = 1;
break;
}

63
src/modules/sensors/sensor_params.c
View File

@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
/**
* Roll control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading roll inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
/**
* Pitch control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading pitch inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
/**
* Throttle control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading throttle inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
/**
* Yaw control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading yaw inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
/**
* Mode switch channel mapping.
*
* This is the main flight mode selector.
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for deciding about the main mode.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);

18
src/modules/sensors/sensors.cpp
View File

@ -797,7 +797,6 @@ Sensors::accel_init()
#endif
warnx("using system accel");
close(fd);
}
}
@ -837,7 +836,6 @@ Sensors::gyro_init()
#endif
warnx("using system gyro");
close(fd);
}
}
@ -1278,6 +1276,9 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
if (rc_input.rc_lost)
return;
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
@ -1322,7 +1323,7 @@ Sensors::rc_poll()
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
_rc_last_valid = rc_input.timestamp;
_rc_last_valid = rc_input.timestamp_last_signal;
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
@ -1371,9 +1372,9 @@ Sensors::rc_poll()
}
_rc.chan_count = rc_input.channel_count;
_rc.timestamp = rc_input.timestamp;
_rc.timestamp = rc_input.timestamp_last_signal;
manual_control.timestamp = rc_input.timestamp;
manual_control.timestamp = rc_input.timestamp_last_signal;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
@ -1507,9 +1508,6 @@ void
Sensors::task_main()
{
/* inform about start */
warnx("Initializing..");
/* start individual sensors */
accel_init();
gyro_init();
@ -1548,8 +1546,8 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = 0.0f;
_battery_status.voltage_filtered_v = 0.0f;
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;

73
src/systemcmds/hw_ver/hw_ver.c Normal file
View File

@ -0,0 +1,73 @@
/****************************************************************************
*
* Copyright (c) 2014 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 hw_ver.c
*
* Show and test hardware version.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <version/version.h>
__EXPORT int hw_ver_main(int argc, char *argv[]);
int
hw_ver_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "show")) {
printf(HW_ARCH "\n");
exit(0);
}
if (!strcmp(argv[1], "compare")) {
if (argc >= 3) {
int ret = strcmp(HW_ARCH, argv[2]) != 0;
if (ret == 0) {
printf("hw_ver match: %s\n", HW_ARCH);
}
exit(ret);
} else {
errx(1, "not enough arguments, try 'compare PX4FMU_1'");
}
}
}
errx(1, "expected a command, try 'show' or 'compare'");
}

43
src/systemcmds/hw_ver/module.mk Normal file
View File

@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2014 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.
#
############################################################################
#
# Show and test hardware version
#
MODULE_COMMAND = hw_ver
SRCS = hw_ver.c
MODULE_STACKSIZE = 1024
MAXOPTIMIZATION = -Os

35
src/systemcmds/tests/test_mtd.c
View File

@ -90,6 +90,16 @@ int check_user_abort(int fd) {
return 1;
}
void print_fail()
{
printf("<[T]: MTD: FAIL>\n");
}
void print_success()
{
printf("<[T]: MTD: OK>\n");
}
int
test_mtd(int argc, char *argv[])
{
@ -99,6 +109,7 @@ test_mtd(int argc, char *argv[])
struct stat buffer;
if (stat(PARAM_FILE_NAME, &buffer)) {
warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
print_fail();
return 1;
}
@ -123,9 +134,17 @@ test_mtd(int argc, char *argv[])
uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
hrt_abstime start, end;
int fd = open(PARAM_FILE_NAME, O_WRONLY);
int fd = open(PARAM_FILE_NAME, O_RDONLY);
int rret = read(fd, read_buf, chunk_sizes[c]);
close(fd);
warnx("testing unaligned writes - please wait..");
fd = open(PARAM_FILE_NAME, O_WRONLY);
printf("printing 2 percent of the first chunk:\n");
for (int i = 0; i < sizeof(read_buf) / 50; i++) {
printf("%02X", read_buf[i]);
}
printf("\n");
iterations = file_size / chunk_sizes[c];
@ -133,9 +152,9 @@ test_mtd(int argc, char *argv[])
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
if (wret != chunk_sizes[c]) {
if (wret != (int)chunk_sizes[c]) {
warn("WRITE ERROR!");
print_fail();
return 1;
}
@ -156,6 +175,7 @@ test_mtd(int argc, char *argv[])
if (rret != chunk_sizes[c]) {
warnx("READ ERROR!");
print_fail();
return 1;
}
@ -165,6 +185,7 @@ test_mtd(int argc, char *argv[])
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d", j);
print_fail();
compare_ok = false;
break;
}
@ -172,6 +193,7 @@ test_mtd(int argc, char *argv[])
if (!compare_ok) {
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
print_fail();
return 1;
}
@ -183,7 +205,6 @@ test_mtd(int argc, char *argv[])
close(fd);
printf("RESULT: OK! No readback errors.\n\n");
}
/* fill the file with 0xFF to make it look new again */
@ -193,14 +214,16 @@ test_mtd(int argc, char *argv[])
for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
int ret = write(fd, ffbuf, sizeof(ffbuf));
if (ret) {
if (ret != sizeof(ffbuf)) {
warnx("ERROR! Could not fill file with 0xFF");
close(fd);
print_fail();
return 1;
}
}
(void)close(fd);
print_success();
return 0;
}

2
src/systemcmds/tests/test_rc.c
View File

@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[])
return ERROR;
}
if (hrt_absolute_time() - rc_input.timestamp > 100000) {
if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
warnx("TIMEOUT, less than 10 Hz updates");
(void)close(_rc_sub);
return ERROR;

2
src/systemcmds/tests/tests.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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