forked from Archive/PX4-Autopilot
Merged beta into mavlink rework branch
This commit is contained in:
parent
ac77fe9c27
commit
9c355d280e
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -1,74 +1,29 @@
|
|||
#!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_YAWPOS_P 0.5
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_ATT_I 0.0
|
||||
param set MC_YAW_P 0.5
|
||||
param set MC_YAW_I 0.15
|
||||
param set MC_ATTRATE_P 0.17
|
||||
param set MC_ATTRATE_I 0.0
|
||||
param set MC_ATTRATE_D 0.006
|
||||
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
|
||||
|
|
|
@ -1,73 +1,49 @@
|
|||
#!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_YAWPOS_P 0.5
|
||||
param set MC_YAWRATE_D 0.0
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_ATT_I 0.0
|
||||
param set MC_YAW_P 0.5
|
||||
param set MC_YAW_I 0.15
|
||||
param set MC_ATTRATE_P 0.13
|
||||
param set MC_ATTRATE_I 0.0
|
||||
param set MC_ATTRATE_D 0.004
|
||||
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 1.0
|
||||
param set MPC_THR_MAX 1.0
|
||||
param set MPC_THR_MIN 0.1
|
||||
param set MPC_XY_P 1.0
|
||||
param set MPC_XY_VEL_D 0.01
|
||||
param set MPC_XY_VEL_I 0.02
|
||||
param set MPC_XY_VEL_MAX 5
|
||||
param set MPC_XY_VEL_P 0.1
|
||||
param set MPC_Z_P 1.0
|
||||
param set MPC_Z_VEL_D 0.0
|
||||
param set MPC_Z_VEL_I 0.02
|
||||
param set MPC_Z_VEL_MAX 3
|
||||
param set MPC_Z_VEL_P 0.1
|
||||
|
||||
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
|
||||
|
|
|
@ -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"
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#!nsh
|
||||
#
|
||||
# HIL Quadcopter X
|
||||
#
|
||||
# Maintainers: Anton Babushkin <anton.babushkin@me.com>
|
||||
#
|
||||
|
||||
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_YAW_P 2.0
|
||||
param set MC_YAW_I 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_YAWRATE_P 0.3
|
||||
param set MC_YAWRATE_I 0.2
|
||||
param set MC_YAWRATE_D 0.005
|
||||
|
||||
param set MPC_TILT_MAX 1.0
|
||||
param set MPC_THR_MAX 1.0
|
||||
param set MPC_THR_MIN 0.1
|
||||
param set MPC_XY_P 1.0
|
||||
param set MPC_XY_VEL_D 0.01
|
||||
param set MPC_XY_VEL_I 0.02
|
||||
param set MPC_XY_VEL_MAX 5
|
||||
param set MPC_XY_VEL_P 0.1
|
||||
param set MPC_Z_P 1.0
|
||||
param set MPC_Z_VEL_D 0.0
|
||||
param set MPC_Z_VEL_I 0.02
|
||||
param set MPC_Z_VEL_MAX 3
|
||||
param set MPC_Z_VEL_P 0.1
|
||||
fi
|
||||
|
||||
set HIL yes
|
||||
|
||||
set VEHICLE_TYPE mc
|
||||
set MIXER FMU_quad_x
|
|
@ -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
|
||||
|
|
|
@ -1,105 +1,10 @@
|
|||
#!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
|
||||
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_+.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"
|
||||
sh /etc/init.d/1001_rc_quad_x.hil
|
||||
|
||||
set MIXER FMU_quad_+
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -67,7 +67,7 @@ flow_position_estimator start
|
|||
#
|
||||
# Fire up the multi rotor attitude controller
|
||||
#
|
||||
multirotor_att_control start
|
||||
mc_att_control_vector start
|
||||
|
||||
#
|
||||
# Fire up the flow position controller
|
||||
|
|
|
@ -1,69 +1,47 @@
|
|||
#!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_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
|
||||
param set MC_ATT_I 0.0
|
||||
param set MC_YAW_P 2.8
|
||||
param set MC_YAW_I 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_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.05
|
||||
param set MC_YAWRATE_D 0.0
|
||||
|
||||
param set MPC_TILT_MAX 1.0
|
||||
param set MPC_THR_MAX 1.0
|
||||
param set MPC_THR_MIN 0.1
|
||||
param set MPC_XY_P 1.0
|
||||
param set MPC_XY_VEL_D 0.01
|
||||
param set MPC_XY_VEL_I 0.02
|
||||
param set MPC_XY_VEL_MAX 5
|
||||
param set MPC_XY_VEL_P 0.1
|
||||
param set MPC_Z_P 1.0
|
||||
param set MPC_Z_VEL_D 0.0
|
||||
param set MPC_Z_VEL_I 0.02
|
||||
param set MPC_Z_VEL_MAX 3
|
||||
param set MPC_Z_VEL_P 0.1
|
||||
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
|
||||
|
|
|
@ -1,74 +1,35 @@
|
|||
#!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_YAWPOS_P 2.0
|
||||
param set MC_YAWRATE_D 0.005
|
||||
param set MC_YAWRATE_I 0.2
|
||||
param set MC_ATT_I 0.0
|
||||
param set MC_YAW_P 2.0
|
||||
param set MC_YAW_I 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_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
|
||||
|
|
|
@ -0,0 +1,31 @@
|
|||
#!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_YAW_P 0.6
|
||||
param set MC_YAW_I 0
|
||||
param set MC_ATTRATE_P 0.14
|
||||
param set MC_ATTRATE_I 0
|
||||
param set MC_ATTRATE_D 0.006
|
||||
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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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"
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -1,14 +1,16 @@
|
|||
#!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
|
||||
echo "Start sdlog2 at 50Hz"
|
||||
sdlog2 start -r 50 -a -b 16 -t
|
||||
else
|
||||
sdlog2 start -r 200 -a -b 16
|
||||
echo "Start sdlog2 at 200Hz"
|
||||
sdlog2 start -r 200 -a -b 16 -t
|
||||
fi
|
||||
fi
|
||||
|
|
|
@ -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
|
||||
#
|
||||
mc_att_control start
|
||||
|
||||
#
|
||||
# Start position control
|
||||
#
|
||||
mc_pos_control start
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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"
|
|
@ -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
|
||||
|
|
|
@ -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,392 +73,439 @@ then
|
|||
uorb start
|
||||
|
||||
#
|
||||
# Load microSD params
|
||||
# Load parameters
|
||||
#
|
||||
#if ramtron start
|
||||
#then
|
||||
# param select /ramtron/params
|
||||
# if [ -f /ramtron/params ]
|
||||
# then
|
||||
# param load /ramtron/params
|
||||
# fi
|
||||
#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
|
||||
#fi
|
||||
set PARAM_FILE /fs/microsd/params
|
||||
if mtd start
|
||||
then
|
||||
set PARAM_FILE /fs/mtd_params
|
||||
fi
|
||||
|
||||
param select $PARAM_FILE
|
||||
if param load
|
||||
then
|
||||
echo "[init] Parameters loaded: $PARAM_FILE"
|
||||
else
|
||||
echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
|
||||
fi
|
||||
|
||||
#
|
||||
# Start system state indicator
|
||||
#
|
||||
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
|
||||
|
||||
#
|
||||
# Start the datamanager
|
||||
#
|
||||
dataman start
|
||||
|
||||
#
|
||||
# Start the Navigator
|
||||
#
|
||||
navigator 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 ]
|
||||
|
|
|
@ -2,6 +2,7 @@
|
|||
#
|
||||
# PX4FMU startup script for test hackery.
|
||||
#
|
||||
uorb start
|
||||
|
||||
if sercon
|
||||
then
|
||||
|
@ -9,4 +10,68 @@ then
|
|||
|
||||
# Try to get an USB console
|
||||
nshterm /dev/ttyACM0 &
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Try to mount the microSD card.
|
||||
#
|
||||
echo "[init] looking for microSD..."
|
||||
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
||||
then
|
||||
echo "[init] card mounted at /fs/microsd"
|
||||
# Start playing the startup tune
|
||||
tone_alarm start
|
||||
else
|
||||
echo "[init] no microSD card found"
|
||||
# Play SOS
|
||||
tone_alarm error
|
||||
fi
|
||||
|
||||
#
|
||||
# Start a minimal system
|
||||
#
|
||||
|
||||
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"
|
||||
fi
|
||||
|
||||
if px4io checkcrc $io_file
|
||||
then
|
||||
echo "PX4IO CRC OK"
|
||||
else
|
||||
echo "PX4IO CRC failure"
|
||||
tone_alarm MBABGP
|
||||
if px4io forceupdate 14662 $io_file
|
||||
then
|
||||
usleep 500000
|
||||
if px4io start
|
||||
then
|
||||
echo "PX4IO restart OK"
|
||||
tone_alarm MSPAA
|
||||
else
|
||||
echo "PX4IO restart failed"
|
||||
tone_alarm MNGGG
|
||||
sleep 5
|
||||
reboot
|
||||
fi
|
||||
else
|
||||
echo "PX4IO update failed"
|
||||
tone_alarm MNGGG
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# The presence of this file suggests we're running a mount stress test
|
||||
#
|
||||
if [ -f /fs/microsd/mount_test_cmds.txt ]
|
||||
then
|
||||
tests mount
|
||||
fi
|
||||
|
|
|
@ -0,0 +1,201 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table
|
||||
|
||||
convert dot code to png using graphviz:
|
||||
|
||||
dot fsm.dot -Tpng -o fsm.png
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import re
|
||||
|
||||
__author__ = "Julian Oes"
|
||||
|
||||
def get_dot_header():
|
||||
|
||||
return """digraph finite_state_machine {
|
||||
graph [ dpi = 300 ];
|
||||
ratio = 1.5
|
||||
node [shape = circle];"""
|
||||
|
||||
def get_dot_footer():
|
||||
|
||||
return """}\n"""
|
||||
|
||||
def main():
|
||||
|
||||
# parse input arguments
|
||||
parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.')
|
||||
parser.add_argument("-i", "--input-file", default=None, help="choose file to parse")
|
||||
parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file")
|
||||
parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table")
|
||||
args = parser.parse_args()
|
||||
|
||||
# open source file
|
||||
if args.input_file == None:
|
||||
exit('please specify file')
|
||||
f = open(args.input_file,'r')
|
||||
source = f.read()
|
||||
|
||||
# search for state transition table and extract the table itself
|
||||
# first look for StateTable::Tran
|
||||
# then accept anything including newline until {
|
||||
# but don't accept the definition (without ;)
|
||||
# then extract anything inside the brackets until };
|
||||
match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source)
|
||||
|
||||
if not match:
|
||||
exit('no state transition table found')
|
||||
|
||||
table_source = match.group(1)
|
||||
|
||||
# bookkeeping for error checking
|
||||
num_errors_found = 0
|
||||
|
||||
states = []
|
||||
events = []
|
||||
|
||||
# first get all states and events
|
||||
for table_line in table_source.split('\n'):
|
||||
|
||||
match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line)
|
||||
if match:
|
||||
states.append(str(match.group(1)))
|
||||
# go to next line
|
||||
continue
|
||||
|
||||
if len(states) == 1:
|
||||
match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line)
|
||||
if match:
|
||||
events.append(str(match.group(1)))
|
||||
|
||||
print('Found %d states and %d events' % (len(states), len(events)))
|
||||
|
||||
|
||||
# keep track of origin state
|
||||
state = None
|
||||
|
||||
# fill dot code in here
|
||||
dot_code = ''
|
||||
|
||||
# create table len(states)xlen(events)
|
||||
transition_table = [[[] for x in range(len(states))] for y in range(len(events))]
|
||||
|
||||
# now fill the transition table and write the dot code
|
||||
for table_line in table_source.split('\n'):
|
||||
|
||||
# get states
|
||||
# from: /* NAV_STATE_NONE */
|
||||
# extract only "NONE"
|
||||
match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line)
|
||||
if match:
|
||||
state = match.group(1)
|
||||
state_index = states.index(state)
|
||||
# go to next line
|
||||
continue
|
||||
|
||||
# can't advance without proper state
|
||||
if state == None:
|
||||
continue
|
||||
|
||||
# get event and next state
|
||||
# from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}
|
||||
# extract "READY_REQUESTED" and "READY" if there is ACTION
|
||||
match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line)
|
||||
|
||||
# get event and next state
|
||||
# from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
|
||||
# extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION
|
||||
match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line)
|
||||
|
||||
# ignore lines with brackets only
|
||||
if match_action or match_no_action:
|
||||
|
||||
# only write arrows for actions
|
||||
if match_action:
|
||||
event = match_action.group(1)
|
||||
new_state = match_action.group(2)
|
||||
dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n'
|
||||
|
||||
elif match_no_action:
|
||||
event = match_no_action.group(1)
|
||||
new_state = match_no_action.group(2)
|
||||
|
||||
# check for state changes without action
|
||||
if state != new_state:
|
||||
print('Error: no action but state change:')
|
||||
print('State: ' + state + ' changed to: ' + new_state)
|
||||
print(table_line)
|
||||
num_errors_found += 1
|
||||
|
||||
# check for wrong events
|
||||
if event not in events:
|
||||
print('Error: unknown event: ' + event)
|
||||
print(table_line)
|
||||
num_errors_found += 1
|
||||
|
||||
# check for wrong new states
|
||||
if new_state not in states:
|
||||
print('Error: unknown new state: ' + new_state)
|
||||
print(table_line)
|
||||
num_errors_found += 1
|
||||
|
||||
# save new state in transition table
|
||||
event_index = events.index(event)
|
||||
|
||||
# bold for action
|
||||
if match_action:
|
||||
transition_table[event_index][state_index] = '**' + new_state + '**'
|
||||
else:
|
||||
transition_table[event_index][state_index] = new_state
|
||||
|
||||
|
||||
|
||||
# assemble dot code
|
||||
dot_code = get_dot_header() + dot_code + get_dot_footer()
|
||||
|
||||
# write or print dot file
|
||||
if args.dot_file:
|
||||
f = open(args.dot_file,'w')
|
||||
f.write(dot_code)
|
||||
print('Wrote dot file')
|
||||
else:
|
||||
print('##########Dot-start##########')
|
||||
print(dot_code)
|
||||
print('##########Dot-end############')
|
||||
|
||||
|
||||
# assemble doku wiki table
|
||||
table_code = '| ^ '
|
||||
# start with header of all states
|
||||
for state in states:
|
||||
table_code += state + ' ^ '
|
||||
|
||||
table_code += '\n'
|
||||
|
||||
# add events and new states
|
||||
for event, row in zip(events, transition_table):
|
||||
table_code += '^ ' + event + ' | '
|
||||
for new_state in row:
|
||||
table_code += new_state + ' | '
|
||||
table_code += '\n'
|
||||
|
||||
# write or print wiki table
|
||||
if args.table_file:
|
||||
f = open(args.table_file,'w')
|
||||
f.write(table_code)
|
||||
print('Wrote table file')
|
||||
else:
|
||||
print('##########Table-start########')
|
||||
print(table_code)
|
||||
print('##########Table-end##########')
|
||||
|
||||
# report obvous errors
|
||||
if num_errors_found:
|
||||
print('Obvious errors found: %d' % num_errors_found)
|
||||
else:
|
||||
print('No obvious errors found')
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
|
@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output):
|
|||
result = ""
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n"
|
||||
for param in group.GetParams():
|
||||
code = param.GetFieldValue("code")
|
||||
name = param.GetFieldValue("short_desc")
|
||||
if code != name:
|
||||
name = "%s (%s)" % (name, code)
|
||||
result += "=== %s ===\n\n" % name
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
if long_desc is not None:
|
||||
result += "%s\n\n" % long_desc
|
||||
name = name.replace("\n", "")
|
||||
result += "| %s | %s " % (code, name)
|
||||
min_val = param.GetFieldValue("min")
|
||||
if min_val is not None:
|
||||
result += "* Minimal value: %s\n" % min_val
|
||||
result += "| %s " % min_val
|
||||
else:
|
||||
result += "|"
|
||||
max_val = param.GetFieldValue("max")
|
||||
if max_val is not None:
|
||||
result += "* Maximal value: %s\n" % max_val
|
||||
result += "| %s " % max_val
|
||||
else:
|
||||
result += "|"
|
||||
def_val = param.GetFieldValue("default")
|
||||
if def_val is not None:
|
||||
result += "* Default value: %s\n" % def_val
|
||||
result += "\n"
|
||||
result += "| %s " % def_val
|
||||
else:
|
||||
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 += "\n"
|
||||
return result
|
||||
|
|
|
@ -0,0 +1,27 @@
|
|||
import output
|
||||
|
||||
class DokuWikiOutput(output.Output):
|
||||
def Generate(self, groups):
|
||||
result = ""
|
||||
for group in groups:
|
||||
result += "==== %s ====\n\n" % group.GetName()
|
||||
for param in group.GetParams():
|
||||
code = param.GetFieldValue("code")
|
||||
name = param.GetFieldValue("short_desc")
|
||||
if code != name:
|
||||
name = "%s (%s)" % (name, code)
|
||||
result += "=== %s ===\n\n" % name
|
||||
long_desc = param.GetFieldValue("long_desc")
|
||||
if long_desc is not None:
|
||||
result += "%s\n\n" % long_desc
|
||||
min_val = param.GetFieldValue("min")
|
||||
if min_val is not None:
|
||||
result += "* Minimal value: %s\n" % min_val
|
||||
max_val = param.GetFieldValue("max")
|
||||
if max_val is not None:
|
||||
result += "* Maximal value: %s\n" % max_val
|
||||
def_val = param.GetFieldValue("default")
|
||||
if def_val is not None:
|
||||
result += "* Default value: %s\n" % def_val
|
||||
result += "\n"
|
||||
return result
|
|
@ -10,11 +10,13 @@ LIBS=-lm
|
|||
#_DEPS = test.h
|
||||
#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
|
||||
|
||||
_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
|
||||
_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
|
||||
mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
|
||||
OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
|
||||
|
||||
#$(DEPS)
|
||||
$(ODIR)/%.o: %.cpp
|
||||
mkdir -p obj
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
|
||||
|
@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
|
|||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
|
||||
$(CC) -c -o $@ $< $(CFLAGS)
|
||||
|
||||
|
|
|
@ -0,0 +1,16 @@
|
|||
#include <sys/time.h>
|
||||
#include <inttypes.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <stdio.h>
|
||||
|
||||
hrt_abstime hrt_absolute_time() {
|
||||
struct timeval te;
|
||||
gettimeofday(&te, NULL); // get current time
|
||||
hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
|
||||
return us;
|
||||
}
|
||||
|
||||
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) {
|
||||
// not thread safe
|
||||
return hrt_absolute_time() - *then;
|
||||
}
|
|
@ -9,4 +9,6 @@ int main(int argc, char *argv[]) {
|
|||
"../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
|
||||
|
||||
test_mixer(3, args);
|
||||
|
||||
test_conv(1, args);
|
||||
}
|
|
@ -0,0 +1,133 @@
|
|||
/************************************************************************
|
||||
* include/queue.h
|
||||
*
|
||||
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
************************************************************************/
|
||||
|
||||
#ifndef __INCLUDE_QUEUE_H
|
||||
#define __INCLUDE_QUEUE_H
|
||||
|
||||
#ifndef FAR
|
||||
#define FAR
|
||||
#endif
|
||||
|
||||
/************************************************************************
|
||||
* Included Files
|
||||
************************************************************************/
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
/************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************/
|
||||
|
||||
#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
|
||||
#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
|
||||
|
||||
#define sq_next(p) ((p)->flink)
|
||||
#define dq_next(p) ((p)->flink)
|
||||
#define dq_prev(p) ((p)->blink)
|
||||
|
||||
#define sq_empty(q) ((q)->head == NULL)
|
||||
#define dq_empty(q) ((q)->head == NULL)
|
||||
|
||||
#define sq_peek(q) ((q)->head)
|
||||
#define dq_peek(q) ((q)->head)
|
||||
|
||||
/************************************************************************
|
||||
* Global Type Declarations
|
||||
************************************************************************/
|
||||
|
||||
struct sq_entry_s
|
||||
{
|
||||
FAR struct sq_entry_s *flink;
|
||||
};
|
||||
typedef struct sq_entry_s sq_entry_t;
|
||||
|
||||
struct dq_entry_s
|
||||
{
|
||||
FAR struct dq_entry_s *flink;
|
||||
FAR struct dq_entry_s *blink;
|
||||
};
|
||||
typedef struct dq_entry_s dq_entry_t;
|
||||
|
||||
struct sq_queue_s
|
||||
{
|
||||
FAR sq_entry_t *head;
|
||||
FAR sq_entry_t *tail;
|
||||
};
|
||||
typedef struct sq_queue_s sq_queue_t;
|
||||
|
||||
struct dq_queue_s
|
||||
{
|
||||
FAR dq_entry_t *head;
|
||||
FAR dq_entry_t *tail;
|
||||
};
|
||||
typedef struct dq_queue_s dq_queue_t;
|
||||
|
||||
/************************************************************************
|
||||
* Global Function Prototypes
|
||||
************************************************************************/
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define EXTERN extern "C"
|
||||
extern "C" {
|
||||
#else
|
||||
#define EXTERN extern
|
||||
#endif
|
||||
|
||||
EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
|
||||
EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
|
||||
EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
|
||||
sq_queue_t *queue);
|
||||
EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
|
||||
dq_queue_t *queue);
|
||||
EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
|
||||
dq_queue_t *queue);
|
||||
|
||||
EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
|
||||
EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
|
||||
EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
|
||||
EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
|
||||
EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
|
||||
EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
|
||||
|
||||
#undef EXTERN
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __INCLUDE_QUEUE_H_ */
|
||||
|
|
@ -1,148 +0,0 @@
|
|||
#
|
||||
# Makefile for the px4fmu_default configuration
|
||||
#
|
||||
|
||||
#
|
||||
# Use the configuration's ROMFS.
|
||||
#
|
||||
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
|
||||
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
|
||||
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
MODULES += drivers/device
|
||||
MODULES += drivers/stm32
|
||||
MODULES += drivers/stm32/adc
|
||||
MODULES += drivers/stm32/tone_alarm
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/px4io
|
||||
MODULES += drivers/px4fmu
|
||||
MODULES += drivers/boards/px4fmu-v1
|
||||
MODULES += drivers/ardrone_interface
|
||||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/bma180
|
||||
MODULES += drivers/mpu6000
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/rgbled
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
MODULES += modules/sensors
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/eeprom
|
||||
MODULES += systemcmds/ramtron
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/boardinfo
|
||||
MODULES += systemcmds/i2c
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/preflight_check
|
||||
MODULES += systemcmds/pwm
|
||||
MODULES += systemcmds/esc_calib
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/top
|
||||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
|
||||
#
|
||||
# General system control
|
||||
#
|
||||
MODULES += modules/commander
|
||||
MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
MODULES += modules/mavlink_onboard
|
||||
MODULES += modules/gpio_led
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
MODULES += modules/fixedwing_backside
|
||||
|
||||
#
|
||||
# Logging
|
||||
#
|
||||
MODULES += modules/sdlog2
|
||||
|
||||
#
|
||||
# Unit tests
|
||||
#
|
||||
#MODULES += modules/unit_test
|
||||
#MODULES += modules/commander/commander_tests
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
MODULES += modules/systemlib
|
||||
MODULES += modules/systemlib/mixer
|
||||
MODULES += modules/controllib
|
||||
MODULES += modules/uORB
|
||||
MODULES += modules/dataman
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
MODULES += lib/mathlib
|
||||
MODULES += lib/mathlib/math/filter
|
||||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
MODULES += lib/launchdetection
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
#
|
||||
#MODULES += examples/math_demo
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/hello_sky
|
||||
#MODULES += examples/px4_simple_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/daemon
|
||||
#MODULES += examples/px4_daemon_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/debug_values
|
||||
#MODULES += examples/px4_mavlink_debug
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
|
||||
#MODULES += examples/fixedwing_control
|
||||
|
||||
# Hardware test
|
||||
#MODULES += examples/hwtest
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
# In general, these should move to modules over time.
|
||||
#
|
||||
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
|
||||
# to make the table a bit more readable.
|
||||
#
|
||||
define _B
|
||||
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
|
||||
endef
|
||||
|
||||
# command priority stack entrypoint
|
||||
BUILTIN_COMMANDS := \
|
||||
$(call _B, sercon, , 2048, sercon_main ) \
|
||||
$(call _B, serdis, , 2048, serdis_main ) \
|
||||
$(call _B, sysinfo, , 2048, sysinfo_main )
|
|
@ -36,13 +36,13 @@ MODULES += drivers/mkblctrl
|
|||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
MODULES += drivers/frsky_telemetry
|
||||
MODULES += modules/sensors
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/eeprom
|
||||
MODULES += systemcmds/ramtron
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/boardinfo
|
||||
MODULES += systemcmds/i2c
|
||||
|
@ -57,6 +57,7 @@ MODULES += systemcmds/top
|
|||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/hw_ver
|
||||
|
||||
#
|
||||
# General system control
|
||||
|
@ -81,8 +82,8 @@ MODULES += modules/position_estimator_inav
|
|||
#
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/multirotor_att_control
|
||||
MODULES += modules/multirotor_pos_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
#MODULES += examples/flow_position_control
|
||||
#MODULES += examples/flow_speed_control
|
||||
|
||||
|
|
|
@ -36,6 +36,7 @@ MODULES += drivers/roboclaw
|
|||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
MODULES += drivers/frsky_telemetry
|
||||
MODULES += modules/sensors
|
||||
|
||||
# Needs to be burned to the ground and re-written; for now,
|
||||
|
@ -45,7 +46,6 @@ MODULES += modules/sensors
|
|||
#
|
||||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/ramtron
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/boardinfo
|
||||
MODULES += systemcmds/mixer
|
||||
|
@ -59,6 +59,8 @@ MODULES += systemcmds/top
|
|||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/hw_ver
|
||||
|
||||
#
|
||||
# General system control
|
||||
|
@ -83,8 +85,8 @@ MODULES += examples/flow_position_estimator
|
|||
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/multirotor_att_control
|
||||
MODULES += modules/multirotor_pos_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
|
||||
#
|
||||
# Logging
|
||||
|
|
|
@ -6,23 +6,37 @@
|
|||
# Use the configuration's ROMFS.
|
||||
#
|
||||
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test
|
||||
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
|
||||
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
MODULES += drivers/device
|
||||
MODULES += drivers/stm32
|
||||
MODULES += drivers/stm32/adc
|
||||
MODULES += drivers/stm32/tone_alarm
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/boards/px4fmu-v2
|
||||
MODULES += drivers/px4io
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/nshterm
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/hw_ver
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
MODULES += modules/systemlib
|
||||
MODULES += modules/systemlib/mixer
|
||||
MODULES += modules/uORB
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
|
|
|
@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1
|
|||
CONFIG_MMCSD_SPI=y
|
||||
CONFIG_MMCSD_SPICLOCK=24000000
|
||||
# CONFIG_MMCSD_SDIO is not set
|
||||
# CONFIG_MTD is not set
|
||||
CONFIG_MTD=y
|
||||
CONFIG_PIPES=y
|
||||
# CONFIG_PM is not set
|
||||
# CONFIG_POWER is not set
|
||||
|
@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y
|
|||
# CONFIG_USART6_SERIAL_CONSOLE is not set
|
||||
# CONFIG_NO_SERIAL_CONSOLE is not set
|
||||
|
||||
#
|
||||
# MTD Configuration
|
||||
#
|
||||
CONFIG_MTD_PARTITION=y
|
||||
CONFIG_MTD_BYTE_WRITE=y
|
||||
|
||||
#
|
||||
# MTD Device Drivers
|
||||
#
|
||||
# CONFIG_RAMMTD is not set
|
||||
# CONFIG_MTD_AT24XX is not set
|
||||
# CONFIG_MTD_AT45DB is not set
|
||||
# CONFIG_MTD_M25P is not set
|
||||
# CONFIG_MTD_SMART is not set
|
||||
# CONFIG_MTD_RAMTRON is not set
|
||||
# CONFIG_MTD_SST25 is not set
|
||||
# CONFIG_MTD_SST39FV is not set
|
||||
# CONFIG_MTD_W25 is not set
|
||||
|
||||
#
|
||||
# USART1 Configuration
|
||||
#
|
||||
|
@ -566,7 +585,7 @@ CONFIG_CDCACM_NWRREQS=4
|
|||
CONFIG_CDCACM_NRDREQS=4
|
||||
CONFIG_CDCACM_BULKIN_REQLEN=96
|
||||
CONFIG_CDCACM_RXBUFSIZE=512
|
||||
CONFIG_CDCACM_TXBUFSIZE=512
|
||||
CONFIG_CDCACM_TXBUFSIZE=2048
|
||||
CONFIG_CDCACM_VENDORID=0x26ac
|
||||
CONFIG_CDCACM_PRODUCTID=0x0010
|
||||
CONFIG_CDCACM_VENDORSTR="3D Robotics"
|
||||
|
|
|
@ -295,16 +295,16 @@ CONFIG_STM32_USART=y
|
|||
# U[S]ART Configuration
|
||||
#
|
||||
# CONFIG_USART1_RS485 is not set
|
||||
# CONFIG_USART1_RXDMA is not set
|
||||
CONFIG_USART1_RXDMA=y
|
||||
# CONFIG_USART2_RS485 is not set
|
||||
CONFIG_USART2_RXDMA=y
|
||||
# CONFIG_USART3_RS485 is not set
|
||||
CONFIG_USART3_RXDMA=y
|
||||
# CONFIG_UART4_RS485 is not set
|
||||
CONFIG_UART4_RXDMA=y
|
||||
# CONFIG_UART5_RXDMA is not set
|
||||
CONFIG_UART5_RXDMA=y
|
||||
# CONFIG_USART6_RS485 is not set
|
||||
# CONFIG_USART6_RXDMA is not set
|
||||
CONFIG_USART6_RXDMA=y
|
||||
# CONFIG_UART7_RS485 is not set
|
||||
# CONFIG_UART7_RXDMA is not set
|
||||
# CONFIG_UART8_RS485 is not set
|
||||
|
@ -500,8 +500,8 @@ CONFIG_MTD=y
|
|||
#
|
||||
# MTD Configuration
|
||||
#
|
||||
# CONFIG_MTD_PARTITION is not set
|
||||
# CONFIG_MTD_BYTE_WRITE is not set
|
||||
CONFIG_MTD_PARTITION=y
|
||||
CONFIG_MTD_BYTE_WRITE=y
|
||||
|
||||
#
|
||||
# MTD Device Drivers
|
||||
|
@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y
|
|||
#
|
||||
# UART4 Configuration
|
||||
#
|
||||
CONFIG_UART4_RXBUFSIZE=128
|
||||
CONFIG_UART4_TXBUFSIZE=128
|
||||
CONFIG_UART4_RXBUFSIZE=512
|
||||
CONFIG_UART4_TXBUFSIZE=512
|
||||
CONFIG_UART4_BAUD=57600
|
||||
CONFIG_UART4_BITS=8
|
||||
CONFIG_UART4_PARITY=0
|
||||
|
@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0
|
|||
#
|
||||
# USART6 Configuration
|
||||
#
|
||||
CONFIG_USART6_RXBUFSIZE=256
|
||||
CONFIG_USART6_TXBUFSIZE=256
|
||||
CONFIG_USART6_RXBUFSIZE=512
|
||||
CONFIG_USART6_TXBUFSIZE=512
|
||||
CONFIG_USART6_BAUD=57600
|
||||
CONFIG_USART6_BITS=8
|
||||
CONFIG_USART6_PARITY=0
|
||||
|
@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4
|
|||
CONFIG_CDCACM_NRDREQS=4
|
||||
CONFIG_CDCACM_BULKIN_REQLEN=96
|
||||
CONFIG_CDCACM_RXBUFSIZE=512
|
||||
CONFIG_CDCACM_TXBUFSIZE=512
|
||||
CONFIG_CDCACM_TXBUFSIZE=2048
|
||||
CONFIG_CDCACM_VENDORID=0x26ac
|
||||
CONFIG_CDCACM_PRODUCTID=0x0011
|
||||
CONFIG_CDCACM_VENDORSTR="3D Robotics"
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -60,6 +60,7 @@ __BEGIN_DECLS
|
|||
|
||||
/* PX4IO connection configuration */
|
||||
#define PX4IO_SERIAL_DEVICE "/dev/ttyS2"
|
||||
#define UDID_START 0x1FFF7A10
|
||||
|
||||
//#ifdef CONFIG_STM32_SPI2
|
||||
//# error "SPI2 is not supported on this board"
|
||||
|
|
|
@ -52,6 +52,8 @@ __BEGIN_DECLS
|
|||
/* these headers are not C++ safe */
|
||||
#include <stm32.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#define UDID_START 0x1FFF7A10
|
||||
|
||||
/****************************************************************************************************
|
||||
* Definitions
|
||||
|
@ -73,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)
|
||||
|
@ -85,7 +87,7 @@ __BEGIN_DECLS
|
|||
#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0)
|
||||
#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4)
|
||||
#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
|
||||
#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
|
||||
|
||||
/* SPI1 off */
|
||||
#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5)
|
||||
|
@ -96,7 +98,7 @@ __BEGIN_DECLS
|
|||
#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13)
|
||||
#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15)
|
||||
#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7)
|
||||
#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
|
||||
#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2)
|
||||
|
||||
/* SPI chip selects */
|
||||
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
|
|
|
@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void)
|
|||
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
|
||||
up_udelay(20);
|
||||
|
||||
message("[boot] Successfully initialized SPI port 1\n");
|
||||
message("[boot] Initialized SPI port 1 (SENSORS)\n");
|
||||
|
||||
/* Get the SPI port for the FRAM */
|
||||
|
||||
|
@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void)
|
|||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
|
||||
SPI_SETFREQUENCY(spi2, 375000000);
|
||||
/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
|
||||
* and de-assert the known chip selects. */
|
||||
|
||||
// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
|
||||
SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
|
||||
SPI_SETBITS(spi2, 8);
|
||||
SPI_SETMODE(spi2, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi2, SPIDEV_FLASH, false);
|
||||
|
||||
message("[boot] Successfully initialized SPI port 2\n");
|
||||
message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
|
||||
|
||||
#ifdef CONFIG_MMCSD
|
||||
/* First, get an instance of the SDIO interface */
|
||||
|
||||
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
|
||||
if (!sdio) {
|
||||
message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
|
||||
message("[boot] Failed to initialize SDIO slot %d\n",
|
||||
CONFIG_NSH_MMCSDSLOTNO);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void)
|
|||
/* Now bind the SDIO interface to the MMC/SD driver */
|
||||
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
|
||||
if (ret != OK) {
|
||||
message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
|
||||
message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
@ -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 *************************************************************/
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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 a single servo to a specific value */
|
||||
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
||||
|
||||
|
|
|
@ -0,0 +1,289 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* 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 frsky_data.c
|
||||
* @author Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* FrSky telemetry implementation.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "frsky_data.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <arch/math.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
/* FrSky sensor hub data IDs */
|
||||
#define FRSKY_ID_GPS_ALT_BP 0x01
|
||||
#define FRSKY_ID_TEMP1 0x02
|
||||
#define FRSKY_ID_RPM 0x03
|
||||
#define FRSKY_ID_FUEL 0x04
|
||||
#define FRSKY_ID_TEMP2 0x05
|
||||
#define FRSKY_ID_VOLTS 0x06
|
||||
#define FRSKY_ID_GPS_ALT_AP 0x09
|
||||
#define FRSKY_ID_BARO_ALT_BP 0x10
|
||||
#define FRSKY_ID_GPS_SPEED_BP 0x11
|
||||
#define FRSKY_ID_GPS_LONG_BP 0x12
|
||||
#define FRSKY_ID_GPS_LAT_BP 0x13
|
||||
#define FRSKY_ID_GPS_COURS_BP 0x14
|
||||
#define FRSKY_ID_GPS_DAY_MONTH 0x15
|
||||
#define FRSKY_ID_GPS_YEAR 0x16
|
||||
#define FRSKY_ID_GPS_HOUR_MIN 0x17
|
||||
#define FRSKY_ID_GPS_SEC 0x18
|
||||
#define FRSKY_ID_GPS_SPEED_AP 0x19
|
||||
#define FRSKY_ID_GPS_LONG_AP 0x1A
|
||||
#define FRSKY_ID_GPS_LAT_AP 0x1B
|
||||
#define FRSKY_ID_GPS_COURS_AP 0x1C
|
||||
#define FRSKY_ID_BARO_ALT_AP 0x21
|
||||
#define FRSKY_ID_GPS_LONG_EW 0x22
|
||||
#define FRSKY_ID_GPS_LAT_NS 0x23
|
||||
#define FRSKY_ID_ACCEL_X 0x24
|
||||
#define FRSKY_ID_ACCEL_Y 0x25
|
||||
#define FRSKY_ID_ACCEL_Z 0x26
|
||||
#define FRSKY_ID_CURRENT 0x28
|
||||
#define FRSKY_ID_VARIO 0x30
|
||||
#define FRSKY_ID_VFAS 0x39
|
||||
#define FRSKY_ID_VOLTS_BP 0x3A
|
||||
#define FRSKY_ID_VOLTS_AP 0x3B
|
||||
|
||||
#define frac(f) (f - (int)f)
|
||||
|
||||
static int battery_sub = -1;
|
||||
static int sensor_sub = -1;
|
||||
static int global_position_sub = -1;
|
||||
static int vehicle_status_sub = -1;
|
||||
|
||||
/**
|
||||
* Initializes the uORB subscriptions.
|
||||
*/
|
||||
void frsky_init()
|
||||
{
|
||||
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends a 0x5E start/stop byte.
|
||||
*/
|
||||
static void frsky_send_startstop(int uart)
|
||||
{
|
||||
static const uint8_t c = 0x5E;
|
||||
write(uart, &c, sizeof(c));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends one byte, performing byte-stuffing if necessary.
|
||||
*/
|
||||
static void frsky_send_byte(int uart, uint8_t value)
|
||||
{
|
||||
const uint8_t x5E[] = { 0x5D, 0x3E };
|
||||
const uint8_t x5D[] = { 0x5D, 0x3D };
|
||||
|
||||
switch (value) {
|
||||
case 0x5E:
|
||||
write(uart, x5E, sizeof(x5E));
|
||||
break;
|
||||
|
||||
case 0x5D:
|
||||
write(uart, x5D, sizeof(x5D));
|
||||
break;
|
||||
|
||||
default:
|
||||
write(uart, &value, sizeof(value));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends one data id/value pair.
|
||||
*/
|
||||
static void frsky_send_data(int uart, uint8_t id, int16_t data)
|
||||
{
|
||||
/* Cast data to unsigned, because signed shift might behave incorrectly */
|
||||
uint16_t udata = data;
|
||||
|
||||
frsky_send_startstop(uart);
|
||||
|
||||
frsky_send_byte(uart, id);
|
||||
frsky_send_byte(uart, udata); /* LSB */
|
||||
frsky_send_byte(uart, udata >> 8); /* MSB */
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends frame 1 (every 200ms):
|
||||
* acceleration values, barometer altitude, temperature, battery voltage & current
|
||||
*/
|
||||
void frsky_send_frame1(int uart)
|
||||
{
|
||||
/* get a local copy of the current sensor values */
|
||||
struct sensor_combined_s raw;
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
/* get a local copy of the battery data */
|
||||
struct battery_status_s battery;
|
||||
memset(&battery, 0, sizeof(battery));
|
||||
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
|
||||
|
||||
/* send formatted frame */
|
||||
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
|
||||
roundf(raw.accelerometer_m_s2[0] * 1000.0f));
|
||||
frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
|
||||
roundf(raw.accelerometer_m_s2[1] * 1000.0f));
|
||||
frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
|
||||
roundf(raw.accelerometer_m_s2[2] * 1000.0f));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
|
||||
raw.baro_alt_meter);
|
||||
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
|
||||
roundf(frac(raw.baro_alt_meter) * 100.0f));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_TEMP1,
|
||||
roundf(raw.baro_temp_celcius));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_VFAS,
|
||||
roundf(battery.voltage_v * 10.0f));
|
||||
frsky_send_data(uart, FRSKY_ID_CURRENT,
|
||||
(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
|
||||
|
||||
frsky_send_startstop(uart);
|
||||
}
|
||||
|
||||
/**
|
||||
* Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
|
||||
*/
|
||||
static float frsky_format_gps(float dec)
|
||||
{
|
||||
float dms_deg = (int) dec;
|
||||
float dec_deg = dec - dms_deg;
|
||||
float dms_min = (int) (dec_deg * 60);
|
||||
float dec_min = (dec_deg * 60) - dms_min;
|
||||
float dms_sec = dec_min * 60;
|
||||
|
||||
return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends frame 2 (every 1000ms):
|
||||
* GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
|
||||
*/
|
||||
void frsky_send_frame2(int uart)
|
||||
{
|
||||
/* get a local copy of the global position data */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
|
||||
|
||||
/* get a local copy of the vehicle status data */
|
||||
struct vehicle_status_s vehicle_status;
|
||||
memset(&vehicle_status, 0, sizeof(vehicle_status));
|
||||
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
|
||||
|
||||
/* send formatted frame */
|
||||
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
|
||||
char lat_ns = 0, lon_ew = 0;
|
||||
int sec = 0;
|
||||
if (global_pos.valid) {
|
||||
time_t time_gps = global_pos.time_gps_usec / 1000000;
|
||||
struct tm *tm_gps = gmtime(&time_gps);
|
||||
|
||||
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
|
||||
lat = frsky_format_gps(abs(global_pos.lat));
|
||||
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
|
||||
lon = frsky_format_gps(abs(global_pos.lon));
|
||||
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
|
||||
speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
|
||||
* 25.0f / 46.0f;
|
||||
alt = global_pos.alt;
|
||||
sec = tm_gps->tm_sec;
|
||||
}
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_FUEL,
|
||||
roundf(vehicle_status.battery_remaining * 100.0f));
|
||||
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
|
||||
|
||||
frsky_send_startstop(uart);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends frame 3 (every 5000ms):
|
||||
* GPS date & time
|
||||
*/
|
||||
void frsky_send_frame3(int uart)
|
||||
{
|
||||
/* get a local copy of the battery data */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
memset(&global_pos, 0, sizeof(global_pos));
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
|
||||
|
||||
/* send formatted frame */
|
||||
time_t time_gps = global_pos.time_gps_usec / 1000000;
|
||||
struct tm *tm_gps = gmtime(&time_gps);
|
||||
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
|
||||
frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
|
||||
|
||||
frsky_send_startstop(uart);
|
||||
}
|
|
@ -1,6 +1,7 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -32,9 +33,19 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.cpp
|
||||
* @file frsky_data.h
|
||||
* @author Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* FrSky telemetry implementation.
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
#ifndef _FRSKY_DATA_H
|
||||
#define _FRSKY_DATA_H
|
||||
|
||||
#include "Vector.hpp"
|
||||
// Public functions
|
||||
void frsky_init(void);
|
||||
void frsky_send_frame1(int uart);
|
||||
void frsky_send_frame2(int uart);
|
||||
void frsky_send_frame3(int uart);
|
||||
|
||||
#endif /* _FRSKY_TELEMETRY_H */
|
|
@ -0,0 +1,266 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* 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 frsky_telemetry.c
|
||||
* @author Stefan Rado <px4@sradonia.net>
|
||||
*
|
||||
* FrSky telemetry implementation.
|
||||
*
|
||||
* This daemon emulates an FrSky sensor hub by periodically sending data
|
||||
* packets to an attached FrSky receiver.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include "frsky_data.h"
|
||||
|
||||
|
||||
/* thread state */
|
||||
static volatile bool thread_should_exit = false;
|
||||
static volatile bool thread_running = false;
|
||||
static int frsky_task;
|
||||
|
||||
/* functions */
|
||||
static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original);
|
||||
static void usage(void);
|
||||
static int frsky_telemetry_thread_main(int argc, char *argv[]);
|
||||
__EXPORT int frsky_telemetry_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
/**
|
||||
* Opens the UART device and sets all required serial parameters.
|
||||
*/
|
||||
static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
|
||||
{
|
||||
/* Open UART */
|
||||
const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
|
||||
|
||||
if (uart < 0) {
|
||||
err(1, "Error opening port: %s", uart_name);
|
||||
}
|
||||
|
||||
/* Back up the original UART configuration to restore it after exit */
|
||||
int termios_state;
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
struct termios uart_config;
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Disable output post-processing */
|
||||
uart_config.c_oflag &= ~OPOST;
|
||||
|
||||
/* Set baud rate */
|
||||
static const speed_t speed = B9600;
|
||||
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
/**
|
||||
* Print command usage information
|
||||
*/
|
||||
static void usage()
|
||||
{
|
||||
fprintf(stderr,
|
||||
"usage: frsky_telemetry start [-d <devicename>]\n"
|
||||
" frsky_telemetry stop\n"
|
||||
" frsky_telemetry status\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The daemon thread.
|
||||
*/
|
||||
static int frsky_telemetry_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* Default values for arguments */
|
||||
char *device_name = "/dev/ttyS1"; /* USART2 */
|
||||
|
||||
/* Work around some stupidity in task_create's argv handling */
|
||||
argc -= 2;
|
||||
argv += 2;
|
||||
|
||||
int ch;
|
||||
while ((ch = getopt(argc, argv, "d:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'd':
|
||||
device_name = optarg;
|
||||
break;
|
||||
|
||||
default:
|
||||
usage();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Print welcome text */
|
||||
warnx("FrSky telemetry interface starting...");
|
||||
|
||||
/* Open UART */
|
||||
struct termios uart_config_original;
|
||||
const int uart = frsky_open_uart(device_name, &uart_config_original);
|
||||
|
||||
if (uart < 0)
|
||||
err(1, "could not open %s", device_name);
|
||||
|
||||
/* Subscribe to topics */
|
||||
frsky_init();
|
||||
|
||||
thread_running = true;
|
||||
|
||||
/* Main thread loop */
|
||||
unsigned int iteration = 0;
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* Sleep 200 ms */
|
||||
usleep(200000);
|
||||
|
||||
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
|
||||
frsky_send_frame1(uart);
|
||||
|
||||
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
|
||||
if (iteration % 5 == 0)
|
||||
{
|
||||
frsky_send_frame2(uart);
|
||||
}
|
||||
|
||||
/* Send frame 3 (every 5000ms): date, time */
|
||||
if (iteration % 25 == 0)
|
||||
{
|
||||
frsky_send_frame3(uart);
|
||||
|
||||
iteration = 0;
|
||||
}
|
||||
|
||||
iteration++;
|
||||
}
|
||||
|
||||
/* Reset the UART flags to original state */
|
||||
tcsetattr(uart, TCSANOW, &uart_config_original);
|
||||
close(uart);
|
||||
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* The main command function.
|
||||
* Processes command line arguments and starts the daemon.
|
||||
*/
|
||||
int frsky_telemetry_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
warnx("missing command");
|
||||
usage();
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (thread_running)
|
||||
errx(0, "frsky_telemetry already running");
|
||||
|
||||
thread_should_exit = false;
|
||||
frsky_task = task_spawn_cmd("frsky_telemetry",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
frsky_telemetry_thread_main,
|
||||
(const char **)argv);
|
||||
|
||||
while (!thread_running) {
|
||||
usleep(200);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (!thread_running)
|
||||
errx(0, "frsky_telemetry already stopped");
|
||||
|
||||
thread_should_exit = true;
|
||||
|
||||
while (thread_running) {
|
||||
usleep(200000);
|
||||
warnx(".");
|
||||
}
|
||||
|
||||
warnx("terminated.");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
errx(0, "running");
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
|
||||
warnx("unrecognized command");
|
||||
usage();
|
||||
/* not getting here */
|
||||
return 0;
|
||||
}
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2013-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
|
||||
|
@ -32,8 +32,10 @@
|
|||
############################################################################
|
||||
|
||||
#
|
||||
# EEPROM file system driver
|
||||
# FrSky telemetry application.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = eeprom
|
||||
SRCS = 24xxxx_mtd.c eeprom.c
|
||||
MODULE_COMMAND = frsky_telemetry
|
||||
|
||||
SRCS = frsky_data.c \
|
||||
frsky_telemetry.c
|
|
@ -85,7 +85,7 @@ static const int ERROR = -1;
|
|||
class GPS : public device::CDev
|
||||
{
|
||||
public:
|
||||
GPS(const char *uart_path);
|
||||
GPS(const char *uart_path, bool fake_gps);
|
||||
virtual ~GPS();
|
||||
|
||||
virtual int init();
|
||||
|
@ -112,6 +112,7 @@ private:
|
|||
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
|
||||
orb_advert_t _report_pub; ///< uORB pub for gps position
|
||||
float _rate; ///< position update rate
|
||||
bool _fake_gps; ///< fake gps output
|
||||
|
||||
|
||||
/**
|
||||
|
@ -156,7 +157,7 @@ GPS *g_dev;
|
|||
}
|
||||
|
||||
|
||||
GPS::GPS(const char *uart_path) :
|
||||
GPS::GPS(const char *uart_path, bool fake_gps) :
|
||||
CDev("gps", GPS_DEVICE_PATH),
|
||||
_task_should_exit(false),
|
||||
_healthy(false),
|
||||
|
@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) :
|
|||
_mode(GPS_DRIVER_MODE_UBX),
|
||||
_Helper(nullptr),
|
||||
_report_pub(-1),
|
||||
_rate(0.0f)
|
||||
_rate(0.0f),
|
||||
_fake_gps(fake_gps)
|
||||
{
|
||||
/* store port name */
|
||||
strncpy(_port, uart_path, sizeof(_port));
|
||||
|
@ -264,98 +266,133 @@ GPS::task_main()
|
|||
/* loop handling received serial bytes and also configuring in between */
|
||||
while (!_task_should_exit) {
|
||||
|
||||
if (_Helper != nullptr) {
|
||||
delete(_Helper);
|
||||
/* set to zero to ensure parser is not used while not instantiated */
|
||||
_Helper = nullptr;
|
||||
}
|
||||
if (_fake_gps) {
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX(_serial_fd, &_report);
|
||||
break;
|
||||
_report.timestamp_position = hrt_absolute_time();
|
||||
_report.lat = (int32_t)47.378301e7f;
|
||||
_report.lon = (int32_t)8.538777e7f;
|
||||
_report.alt = (int32_t)400e3f;
|
||||
_report.timestamp_variance = hrt_absolute_time();
|
||||
_report.s_variance_m_s = 10.0f;
|
||||
_report.p_variance_m = 10.0f;
|
||||
_report.c_variance_rad = 0.1f;
|
||||
_report.fix_type = 3;
|
||||
_report.eph_m = 10.0f;
|
||||
_report.epv_m = 10.0f;
|
||||
_report.timestamp_velocity = hrt_absolute_time();
|
||||
_report.vel_n_m_s = 0.0f;
|
||||
_report.vel_e_m_s = 0.0f;
|
||||
_report.vel_d_m_s = 0.0f;
|
||||
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
|
||||
_report.cog_rad = 0.0f;
|
||||
_report.vel_ned_valid = true;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
break;
|
||||
//no time and satellite information simulated
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (_report_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
|
||||
unlock();
|
||||
|
||||
if (_Helper->configure(_baudrate) == 0) {
|
||||
unlock();
|
||||
|
||||
// GPS is obviously detected successfully, reset statistics
|
||||
_Helper->reset_update_rates();
|
||||
|
||||
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
|
||||
// lock();
|
||||
/* opportunistic publishing - else invalid data would end up on the bus */
|
||||
if (_report_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
|
||||
} else {
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
}
|
||||
|
||||
last_rate_count++;
|
||||
|
||||
/* measure update rate every 5 seconds */
|
||||
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
|
||||
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
|
||||
last_rate_measurement = hrt_absolute_time();
|
||||
last_rate_count = 0;
|
||||
_Helper->store_update_rates();
|
||||
_Helper->reset_update_rates();
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
char *mode_str = "unknown";
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
mode_str = "UBX";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
mode_str = "MTK";
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("module found: %s", mode_str);
|
||||
_healthy = true;
|
||||
}
|
||||
} else {
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
}
|
||||
|
||||
if (_healthy) {
|
||||
warnx("module lost");
|
||||
_healthy = false;
|
||||
_rate = 0.0f;
|
||||
usleep(2e5);
|
||||
|
||||
} else {
|
||||
|
||||
if (_Helper != nullptr) {
|
||||
delete(_Helper);
|
||||
/* set to zero to ensure parser is not used while not instantiated */
|
||||
_Helper = nullptr;
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX(_serial_fd, &_report);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (_Helper->configure(_baudrate) == 0) {
|
||||
unlock();
|
||||
|
||||
// GPS is obviously detected successfully, reset statistics
|
||||
_Helper->reset_update_rates();
|
||||
|
||||
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
|
||||
// lock();
|
||||
/* opportunistic publishing - else invalid data would end up on the bus */
|
||||
if (_report_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
|
||||
} else {
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
}
|
||||
|
||||
last_rate_count++;
|
||||
|
||||
/* measure update rate every 5 seconds */
|
||||
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
|
||||
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
|
||||
last_rate_measurement = hrt_absolute_time();
|
||||
last_rate_count = 0;
|
||||
_Helper->store_update_rates();
|
||||
_Helper->reset_update_rates();
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
char *mode_str = "unknown";
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
mode_str = "UBX";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
mode_str = "MTK";
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("module found: %s", mode_str);
|
||||
_healthy = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_healthy) {
|
||||
warnx("module lost");
|
||||
_healthy = false;
|
||||
_rate = 0.0f;
|
||||
}
|
||||
|
||||
lock();
|
||||
}
|
||||
|
||||
lock();
|
||||
}
|
||||
|
||||
lock();
|
||||
/* select next mode */
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
|
||||
/* select next mode */
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -417,7 +454,7 @@ namespace gps
|
|||
|
||||
GPS *g_dev;
|
||||
|
||||
void start(const char *uart_path);
|
||||
void start(const char *uart_path, bool fake_gps);
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
|
@ -427,7 +464,7 @@ void info();
|
|||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start(const char *uart_path)
|
||||
start(const char *uart_path, bool fake_gps)
|
||||
{
|
||||
int fd;
|
||||
|
||||
|
@ -435,7 +472,7 @@ start(const char *uart_path)
|
|||
errx(1, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new GPS(uart_path);
|
||||
g_dev = new GPS(uart_path, fake_gps);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
goto fail;
|
||||
|
@ -527,6 +564,7 @@ gps_main(int argc, char *argv[])
|
|||
|
||||
/* set to default */
|
||||
char *device_name = GPS_DEFAULT_UART_PORT;
|
||||
bool fake_gps = false;
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
|
@ -542,7 +580,13 @@ gps_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
gps::start(device_name);
|
||||
/* Detect fake gps option */
|
||||
for (int i = 2; i < argc; i++) {
|
||||
if (!strcmp(argv[i], "-f"))
|
||||
fake_gps = true;
|
||||
}
|
||||
|
||||
gps::start(device_name, fake_gps);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
|
@ -567,5 +611,5 @@ gps_main(int argc, char *argv[])
|
|||
gps::info();
|
||||
|
||||
out:
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
@ -193,9 +193,10 @@ HIL::~HIL()
|
|||
} while (_task != -1);
|
||||
}
|
||||
|
||||
/* clean up the alternate device node */
|
||||
if (_primary_pwm_device)
|
||||
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||
// XXX already claimed with CDEV
|
||||
// /* clean up the alternate device node */
|
||||
// if (_primary_pwm_device)
|
||||
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||
|
||||
g_hil = nullptr;
|
||||
}
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
|
||||
|
|
|
@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[])
|
|||
thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd(daemon_name,
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1024,
|
||||
hott_sensors_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
|
|
|
@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[])
|
|||
thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd(daemon_name,
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
hott_telemetry_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
|
|
|
@ -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,31 +177,26 @@ 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 = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset);
|
||||
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;
|
||||
if (diff_press_pa < 0.0f)
|
||||
diff_press_pa = 0.0f;
|
||||
|
||||
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;
|
||||
}
|
||||
|
@ -390,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))
|
||||
|
|
|
@ -124,6 +124,8 @@ protected:
|
|||
int32_t _TEMP;
|
||||
int64_t _OFF;
|
||||
int64_t _SENS;
|
||||
float _P;
|
||||
float _T;
|
||||
|
||||
/* altitude conversion calibration */
|
||||
unsigned _msl_pressure; /* in kPa */
|
||||
|
@ -623,6 +625,8 @@ MS5611::collect()
|
|||
|
||||
/* pressure calculation, result in Pa */
|
||||
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
|
||||
_P = P * 0.01f;
|
||||
_T = _TEMP * 0.01f;
|
||||
|
||||
/* generate a new report */
|
||||
report.temperature = _TEMP / 100.0f;
|
||||
|
@ -695,6 +699,8 @@ MS5611::print_info()
|
|||
printf("TEMP: %d\n", _TEMP);
|
||||
printf("SENS: %lld\n", _SENS);
|
||||
printf("OFF: %lld\n", _OFF);
|
||||
printf("P: %.3f\n", _P);
|
||||
printf("T: %.3f\n", _T);
|
||||
printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f));
|
||||
|
||||
printf("factory_setup %u\n", _prom.factory_setup);
|
||||
|
|
|
@ -65,6 +65,7 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/pwm_limit/pwm_limit.h>
|
||||
#include <systemlib/board_serial.h>
|
||||
#include <drivers/drv_mixer.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
|
@ -224,10 +225,10 @@ PX4FMU::PX4FMU() :
|
|||
_armed(false),
|
||||
_pwm_on(false),
|
||||
_mixers(nullptr),
|
||||
_failsafe_pwm( {0}),
|
||||
_disarmed_pwm( {0}),
|
||||
_num_failsafe_set(0),
|
||||
_num_disarmed_set(0)
|
||||
_failsafe_pwm({0}),
|
||||
_disarmed_pwm({0}),
|
||||
_num_failsafe_set(0),
|
||||
_num_disarmed_set(0)
|
||||
{
|
||||
for (unsigned i = 0; i < _max_actuators; i++) {
|
||||
_min_pwm[i] = PWM_DEFAULT_MIN;
|
||||
|
@ -575,7 +576,7 @@ PX4FMU::task_main()
|
|||
if (i >= outputs.noutputs ||
|
||||
!isfinite(outputs.output[i]) ||
|
||||
outputs.output[i] < -1.0f ||
|
||||
outputs.output[i] > 1.0f) {
|
||||
outputs.output[i] > 1.0f) {
|
||||
/*
|
||||
* Value is NaN, INF or out of band - set to the minimum value.
|
||||
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||
|
@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
break;
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_SET(3):
|
||||
case PWM_SERVO_SET(2):
|
||||
if (_mode < MODE_4PWM) {
|
||||
|
@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
break;
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_SET(1):
|
||||
case PWM_SERVO_SET(0):
|
||||
if (arg <= 2100) {
|
||||
|
@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
break;
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_GET(3):
|
||||
case PWM_SERVO_GET(2):
|
||||
if (_mode < MODE_4PWM) {
|
||||
|
@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
break;
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_GET(1):
|
||||
case PWM_SERVO_GET(0):
|
||||
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
|
||||
|
@ -1005,6 +1006,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;
|
||||
|
@ -1107,10 +1142,12 @@ PX4FMU::sensor_reset(int ms)
|
|||
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
|
||||
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
|
||||
stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
|
||||
stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
|
||||
|
||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
|
||||
|
||||
stm32_configgpio(GPIO_SPI1_SCK_OFF);
|
||||
stm32_configgpio(GPIO_SPI1_MISO_OFF);
|
||||
|
@ -1123,10 +1160,12 @@ PX4FMU::sensor_reset(int ms)
|
|||
stm32_configgpio(GPIO_GYRO_DRDY_OFF);
|
||||
stm32_configgpio(GPIO_MAG_DRDY_OFF);
|
||||
stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
|
||||
stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
|
||||
|
||||
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
|
||||
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
|
||||
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
|
||||
stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
|
||||
|
||||
/* set the sensor rail off */
|
||||
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
|
||||
|
@ -1159,6 +1198,13 @@ PX4FMU::sensor_reset(int ms)
|
|||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
|
||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
|
||||
|
||||
// // XXX bring up the EXTI pins again
|
||||
// stm32_configgpio(GPIO_GYRO_DRDY);
|
||||
// stm32_configgpio(GPIO_MAG_DRDY);
|
||||
// stm32_configgpio(GPIO_ACCEL_DRDY);
|
||||
// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
|
||||
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
@ -1431,7 +1477,6 @@ void
|
|||
sensor_reset(int ms)
|
||||
{
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
|
||||
|
||||
|
@ -1591,6 +1636,15 @@ fmu_main(int argc, char *argv[])
|
|||
errx(0, "FMU driver stopped");
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "id")) {
|
||||
char id[12];
|
||||
(void)get_board_serial(id);
|
||||
|
||||
errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
|
||||
(unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5],
|
||||
(unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]);
|
||||
}
|
||||
|
||||
|
||||
if (fmu_start() != OK)
|
||||
errx(1, "failed to start the FMU driver");
|
||||
|
@ -1647,6 +1701,7 @@ fmu_main(int argc, char *argv[])
|
|||
sensor_reset(0);
|
||||
warnx("resettet default time");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
|
|
@ -3,5 +3,4 @@
|
|||
#
|
||||
|
||||
MODULE_COMMAND = fmu
|
||||
SRCS = fmu.cpp \
|
||||
../../modules/systemlib/pwm_limit/pwm_limit.c
|
||||
SRCS = fmu.cpp
|
||||
|
|
|
@ -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
|
||||
|
@ -35,7 +35,7 @@
|
|||
* @file px4io.cpp
|
||||
* Driver for the PX4IO board.
|
||||
*
|
||||
* PX4IO is connected via I2C.
|
||||
* PX4IO is connected via I2C or DMA enabled high-speed UART.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
@ -270,7 +270,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
|
||||
|
||||
|
@ -450,7 +451,7 @@ private:
|
|||
namespace
|
||||
{
|
||||
|
||||
PX4IO *g_dev;
|
||||
PX4IO *g_dev = nullptr;
|
||||
|
||||
}
|
||||
|
||||
|
@ -504,7 +505,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()
|
||||
|
@ -578,6 +580,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.");
|
||||
|
@ -772,8 +780,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);
|
||||
|
||||
/*
|
||||
|
@ -807,8 +813,6 @@ PX4IO::task_main()
|
|||
fds[0].fd = _t_actuator_controls_0;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
log("ready");
|
||||
|
||||
/* lock against the ioctl handler */
|
||||
lock();
|
||||
|
||||
|
@ -1300,6 +1304,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
|
||||
|
@ -1331,19 +1336,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);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1450,6 +1454,13 @@ PX4IO::io_publish_raw_rc()
|
|||
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
|
||||
}
|
||||
|
||||
/* set RSSI */
|
||||
|
||||
if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
|
||||
// XXX the correct scaling needs to be validated here
|
||||
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
|
||||
}
|
||||
|
||||
/* lazily advertise on first publication */
|
||||
if (_to_input_rc == 0) {
|
||||
_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
|
||||
|
@ -1664,7 +1675,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) {
|
||||
|
@ -1688,7 +1710,21 @@ 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;
|
||||
|
||||
retries--;
|
||||
|
||||
|
@ -1798,7 +1834,7 @@ PX4IO::print_status()
|
|||
|
||||
printf("\n");
|
||||
|
||||
if (raw_inputs > 0) {
|
||||
if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
|
||||
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
|
||||
printf("RC data (PPM frame len) %u us\n", frame_len);
|
||||
|
||||
|
@ -2355,8 +2391,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;
|
||||
|
@ -2419,6 +2457,69 @@ detect(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
checkcrc(int argc, char *argv[])
|
||||
{
|
||||
bool keep_running = false;
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
/* allocate the interface */
|
||||
device::Device *interface = get_interface();
|
||||
|
||||
/* create the driver - it will set g_dev */
|
||||
(void)new PX4IO(interface);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "driver alloc failed");
|
||||
} else {
|
||||
/* its already running, don't kill the driver */
|
||||
keep_running = true;
|
||||
}
|
||||
|
||||
/*
|
||||
check IO CRC against CRC of a file
|
||||
*/
|
||||
if (argc < 2) {
|
||||
printf("usage: px4io checkcrc filename\n");
|
||||
exit(1);
|
||||
}
|
||||
int fd = open(argv[1], O_RDONLY);
|
||||
if (fd == -1) {
|
||||
printf("open of %s failed - %d\n", argv[1], errno);
|
||||
exit(1);
|
||||
}
|
||||
const uint32_t app_size_max = 0xf000;
|
||||
uint32_t fw_crc = 0;
|
||||
uint32_t nbytes = 0;
|
||||
while (true) {
|
||||
uint8_t buf[16];
|
||||
int n = read(fd, buf, sizeof(buf));
|
||||
if (n <= 0) break;
|
||||
fw_crc = crc32part(buf, n, fw_crc);
|
||||
nbytes += n;
|
||||
}
|
||||
close(fd);
|
||||
while (nbytes < app_size_max) {
|
||||
uint8_t b = 0xff;
|
||||
fw_crc = crc32part(&b, 1, fw_crc);
|
||||
nbytes++;
|
||||
}
|
||||
|
||||
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
|
||||
|
||||
if (!keep_running) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
if (ret != OK) {
|
||||
printf("check CRC failed - %d\n", ret);
|
||||
exit(1);
|
||||
}
|
||||
printf("CRCs match\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
bind(int argc, char *argv[])
|
||||
{
|
||||
|
@ -2569,17 +2670,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");
|
||||
|
@ -2613,12 +2714,16 @@ px4io_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "detect"))
|
||||
detect(argc - 1, argv + 1);
|
||||
|
||||
if (!strcmp(argv[1], "checkcrc"))
|
||||
checkcrc(argc - 1, argv + 1);
|
||||
|
||||
if (!strcmp(argv[1], "update")) {
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
printf("[px4io] loaded, detaching first\n");
|
||||
/* stop the driver */
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
PX4IO_Uploader *up;
|
||||
|
@ -2691,18 +2796,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];
|
||||
|
@ -2760,6 +2877,7 @@ px4io_main(int argc, char *argv[])
|
|||
|
||||
/* stop the driver */
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
@ -2798,49 +2916,6 @@ px4io_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "checkcrc")) {
|
||||
/*
|
||||
check IO CRC against CRC of a file
|
||||
*/
|
||||
if (argc <= 2) {
|
||||
printf("usage: px4io checkcrc filename\n");
|
||||
exit(1);
|
||||
}
|
||||
if (g_dev == nullptr) {
|
||||
printf("px4io is not started\n");
|
||||
exit(1);
|
||||
}
|
||||
int fd = open(argv[2], O_RDONLY);
|
||||
if (fd == -1) {
|
||||
printf("open of %s failed - %d\n", argv[2], errno);
|
||||
exit(1);
|
||||
}
|
||||
const uint32_t app_size_max = 0xf000;
|
||||
uint32_t fw_crc = 0;
|
||||
uint32_t nbytes = 0;
|
||||
while (true) {
|
||||
uint8_t buf[16];
|
||||
int n = read(fd, buf, sizeof(buf));
|
||||
if (n <= 0) break;
|
||||
fw_crc = crc32part(buf, n, fw_crc);
|
||||
nbytes += n;
|
||||
}
|
||||
close(fd);
|
||||
while (nbytes < app_size_max) {
|
||||
uint8_t b = 0xff;
|
||||
fw_crc = crc32part(&b, 1, fw_crc);
|
||||
nbytes++;
|
||||
}
|
||||
|
||||
int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc);
|
||||
if (ret != OK) {
|
||||
printf("check CRC failed - %d\n", ret);
|
||||
exit(1);
|
||||
}
|
||||
printf("CRCs match\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "rx_dsm") ||
|
||||
!strcmp(argv[1], "rx_dsm_10bit") ||
|
||||
!strcmp(argv[1], "rx_dsm_11bit") ||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
|
|||
void
|
||||
rgbled_usage()
|
||||
{
|
||||
warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
|
||||
warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'");
|
||||
warnx("options:");
|
||||
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
|
||||
warnx(" -a addr (0x%x)", ADDR);
|
||||
|
@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[])
|
|||
if (g_rgbled == nullptr) {
|
||||
warnx("not started");
|
||||
rgbled_usage();
|
||||
exit(0);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "test")) {
|
||||
|
@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "off")) {
|
||||
if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
|
||||
fd = open(RGBLED_DEVICE_PATH, 0);
|
||||
|
||||
if (fd == -1) {
|
||||
|
@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[])
|
|||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
delete g_rgbled;
|
||||
g_rgbled = nullptr;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "rgb")) {
|
||||
if (argc < 5) {
|
||||
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
|
||||
|
|
|
@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
|
|||
* Calculate heading error of current position to desired position
|
||||
*/
|
||||
|
||||
/*
|
||||
* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
|
||||
* so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
|
||||
*/
|
||||
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
|
||||
float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
|
||||
|
||||
/* calculate heading error */
|
||||
float yaw_err = att->yaw - bearing;
|
||||
|
|
|
@ -41,22 +41,11 @@
|
|||
#include "rotation.h"
|
||||
|
||||
__EXPORT void
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
|
||||
{
|
||||
/* first set to zero */
|
||||
rot_matrix->Matrix::zero(3, 3);
|
||||
|
||||
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
|
||||
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
|
||||
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
|
||||
|
||||
math::EulerAngles euler(roll, pitch, yaw);
|
||||
|
||||
math::Dcm R(euler);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
(*rot_matrix)(i, j) = R(i, j);
|
||||
}
|
||||
}
|
||||
rot_matrix->from_euler(roll, pitch, yaw);
|
||||
}
|
||||
|
|
|
@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = {
|
|||
* Get the rotation matrix
|
||||
*/
|
||||
__EXPORT void
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
|
||||
|
||||
#endif /* ROTATION_H_ */
|
||||
|
|
|
@ -174,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
|
|||
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
|
||||
|
||||
/* Apply PI rate controller and store non-limited output */
|
||||
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
|
||||
// warnx("roll: _last_output %.4f", (double)_last_output);
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
|
|
|
@ -141,7 +141,7 @@ float ECL_RollController::control_bodyrate(float pitch,
|
|||
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
|
||||
|
||||
/* Apply PI rate controller and store non-limited output */
|
||||
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
|
||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||
}
|
||||
|
|
|
@ -158,7 +158,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
|
|||
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
|
||||
|
||||
/* Apply PI rate controller and store non-limited output */
|
||||
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
|
||||
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
|
||||
|
||||
|
||||
|
|
|
@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
|
|||
return _crosstrack_error;
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
|
||||
const math::Vector2f &ground_speed_vector)
|
||||
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
|
||||
const math::Vector<2> &ground_speed_vector)
|
||||
{
|
||||
|
||||
/* this follows the logic presented in [1] */
|
||||
|
@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
float ltrack_vel;
|
||||
|
||||
/* get the direction between the last (visited) and next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
|
||||
|
||||
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
|
@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
_L1_distance = _L1_ratio * ground_speed;
|
||||
|
||||
/* calculate vector from A to B */
|
||||
math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
|
||||
math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
|
||||
|
||||
/*
|
||||
* check if waypoints are on top of each other. If yes,
|
||||
|
@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
vector_AB.normalize();
|
||||
|
||||
/* calculate the vector from waypoint A to the aircraft */
|
||||
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
|
||||
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
|
||||
|
||||
/* calculate crosstrack error (output only) */
|
||||
_crosstrack_error = vector_AB % vector_A_to_airplane;
|
||||
|
@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
float alongTrackDist = vector_A_to_airplane * vector_AB;
|
||||
|
||||
/* estimate airplane position WRT to B */
|
||||
math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
|
||||
math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
|
||||
|
||||
/* calculate angle of airplane position vector relative to line) */
|
||||
|
||||
|
@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
/* calculate eta to fly to waypoint A */
|
||||
|
||||
/* unit vector from waypoint A to current position */
|
||||
math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
|
||||
math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
|
||||
/* velocity across / orthogonal to line */
|
||||
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
|
||||
/* velocity along line */
|
||||
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
|
||||
eta = atan2f(xtrack_vel, ltrack_vel);
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
|
||||
|
||||
/*
|
||||
* If the AB vector and the vector from B to airplane point in the same
|
||||
|
@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
|
||||
eta = atan2f(xtrack_vel, ltrack_vel);
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
|
||||
_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
|
||||
|
||||
} else {
|
||||
|
||||
|
@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
float eta1 = asinf(sine_eta1);
|
||||
eta = eta1 + eta2;
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
|
||||
_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
|
||||
|
||||
}
|
||||
|
||||
|
@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
|
|||
_bearing_error = eta;
|
||||
}
|
||||
|
||||
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
|
||||
const math::Vector2f &ground_speed_vector)
|
||||
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
|
||||
const math::Vector<2> &ground_speed_vector)
|
||||
{
|
||||
/* the complete guidance logic in this section was proposed by [2] */
|
||||
|
||||
|
@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
|
|||
float K_velocity = 2.0f * _L1_damping * omega;
|
||||
|
||||
/* update bearing to next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
|
||||
|
||||
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
|
||||
|
@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
|
|||
_L1_distance = _L1_ratio * ground_speed;
|
||||
|
||||
/* calculate the vector from waypoint A to current position */
|
||||
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
|
||||
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
|
||||
|
||||
/* store the normalized vector from waypoint A to current position */
|
||||
math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
|
||||
math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
|
||||
|
||||
/* calculate eta angle towards the loiter center */
|
||||
|
||||
|
@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
|
|||
/* angle between requested and current velocity vector */
|
||||
_bearing_error = eta;
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
|
||||
|
||||
} else {
|
||||
_lateral_accel = lateral_accel_sp_circle;
|
||||
_circle_mode = true;
|
||||
_bearing_error = 0.0f;
|
||||
/* bearing from current position to L1 point */
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
|
||||
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
|
||||
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
|
||||
{
|
||||
/* the complete guidance logic in this section was proposed by [2] */
|
||||
|
||||
|
@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
|
|||
}
|
||||
|
||||
|
||||
math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
|
||||
math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
|
||||
{
|
||||
/* this is an approximation for small angles, proposed by [2] */
|
||||
|
||||
math::Vector2f out;
|
||||
|
||||
out.setX(math::radians((target.getX() - origin.getX())));
|
||||
out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
|
||||
math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
|
||||
|
||||
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
|
||||
}
|
||||
|
|
|
@ -160,8 +160,8 @@ public:
|
|||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
|
||||
const math::Vector2f &ground_speed);
|
||||
void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
|
||||
const math::Vector<2> &ground_speed);
|
||||
|
||||
|
||||
/**
|
||||
|
@ -172,8 +172,8 @@ public:
|
|||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
|
||||
const math::Vector2f &ground_speed_vector);
|
||||
void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
|
||||
const math::Vector<2> &ground_speed_vector);
|
||||
|
||||
|
||||
/**
|
||||
|
@ -185,7 +185,7 @@ public:
|
|||
*
|
||||
* @return sets _lateral_accel setpoint
|
||||
*/
|
||||
void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
|
||||
void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
|
||||
|
||||
|
||||
/**
|
||||
|
@ -260,7 +260,7 @@ private:
|
|||
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
|
||||
* @return The vector in meters pointing from the reference position to the coordinates
|
||||
*/
|
||||
math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
|
||||
math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
@ -30,7 +30,7 @@ using namespace math;
|
|||
*
|
||||
*/
|
||||
|
||||
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
|
||||
void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
|
||||
{
|
||||
// Implement third order complementary filter for height and height rate
|
||||
// estimted height rate = _integ2_state
|
||||
|
@ -282,7 +282,7 @@ void TECS::_update_energies(void)
|
|||
_SKEdot = _integ5_state * _vel_dot;
|
||||
}
|
||||
|
||||
void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
|
||||
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
|
||||
{
|
||||
// Calculate total energy values
|
||||
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
|
||||
|
@ -505,7 +505,7 @@ void TECS::_update_STE_rate_lim(void)
|
|||
_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
|
||||
}
|
||||
|
||||
void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
|
||||
void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
|
||||
float throttle_min, float throttle_max, float throttle_cruise,
|
||||
float pitch_limit_min, float pitch_limit_max)
|
||||
{
|
||||
|
|
|
@ -71,10 +71,10 @@ public:
|
|||
// Update of the estimated height and height rate internal state
|
||||
// Update of the inertial speed rate internal state
|
||||
// Should be called at 50Hz or greater
|
||||
void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
|
||||
void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
|
||||
|
||||
// Update the control loop calculations
|
||||
void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
|
||||
void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
|
||||
float throttle_min, float throttle_max, float throttle_cruise,
|
||||
float pitch_limit_min, float pitch_limit_max);
|
||||
// demanded throttle in percentage
|
||||
|
@ -348,7 +348,7 @@ private:
|
|||
void _update_energies(void);
|
||||
|
||||
// Update Demanded Throttle
|
||||
void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
|
||||
void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
|
||||
|
||||
// Detect Bad Descent
|
||||
void _detect_bad_descent(void);
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue