forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into fw_autoland
This commit is contained in:
commit
20728e83f5
|
@ -0,0 +1,110 @@
|
|||
#!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
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# 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"
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
#!nsh
|
||||
#
|
||||
# USB HIL start
|
||||
#
|
||||
|
||||
echo "[HIL] HILStar starting.."
|
||||
|
||||
#
|
||||
# Load default params for this platform
|
||||
#
|
||||
if param compare SYS_AUTOCONFIG 1
|
||||
then
|
||||
# Set all params here, then disable autoconfig
|
||||
|
||||
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 100
|
||||
param set FW_R_RMAX 100
|
||||
param set FW_THR_CRUISE 0.65
|
||||
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_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
|
||||
|
||||
#
|
||||
# Start the commander (depends on orb, mavlink)
|
||||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# 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"
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
#!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 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
|
||||
px4io idle 900 900 900 900 900 900
|
||||
px4io min 1200 1200 1200 1200 1200 1200
|
||||
px4io max 1900 1900 1900 1900 1900 1900
|
||||
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 $MIXER
|
||||
|
||||
#
|
||||
# Set PWM output frequency to 400 Hz
|
||||
#
|
||||
pwm -u 400 -m 0xff
|
||||
|
||||
#
|
||||
# Start common for all multirotors apps
|
||||
#
|
||||
sh /etc/init.d/rc.multirotor
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
|
@ -0,0 +1,86 @@
|
|||
#!nsh
|
||||
|
||||
echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5"
|
||||
|
||||
#
|
||||
# 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 1 = fixed wing
|
||||
#
|
||||
param set MAV_TYPE 1
|
||||
|
||||
set EXIT_ON_END no
|
||||
|
||||
#
|
||||
# Start and configure PX4IO or FMU interface
|
||||
#
|
||||
if px4io detect
|
||||
then
|
||||
# Start MAVLink (depends on orb)
|
||||
mavlink start
|
||||
|
||||
commander 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
|
||||
|
||||
commander start
|
||||
|
||||
fmu mode_pwm
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
fi
|
||||
|
||||
#
|
||||
# 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
|
||||
|
||||
#
|
||||
# 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
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
if [ $EXIT_ON_END == yes ]
|
||||
then
|
||||
exit
|
||||
fi
|
|
@ -52,5 +52,5 @@ attitude_estimator_ekf start
|
|||
#
|
||||
# Load mixer and start controllers (depends on px4io)
|
||||
#
|
||||
md25 start 3 0x58
|
||||
roboclaw start /dev/ttyS2 128 1200
|
||||
segway start
|
||||
|
|
|
@ -58,7 +58,7 @@ usleep 10000
|
|||
if px4io detect
|
||||
then
|
||||
# Start MAVLink (depends on orb)
|
||||
mavlink start -d /dev/ttyS1 -b 115200
|
||||
mavlink start -d /dev/ttyS1 -b 57600
|
||||
usleep 5000
|
||||
|
||||
sh /etc/init.d/rc.io
|
||||
|
@ -78,7 +78,7 @@ then
|
|||
else
|
||||
fmu mode_pwm
|
||||
# Start MAVLink (on UART1 / ttyS0)
|
||||
mavlink start -d /dev/ttyS1 -b 115200
|
||||
mavlink start -d /dev/ttyS1 -b 57600
|
||||
usleep 5000
|
||||
param set BAT_V_SCALING 0.004593
|
||||
set EXIT_ON_END yes
|
||||
|
|
|
@ -108,24 +108,40 @@ then
|
|||
|
||||
if param compare SYS_AUTOSTART 1000
|
||||
then
|
||||
sh /etc/init.d/1000_rc_fw.hil
|
||||
sh /etc/init.d/1000_rc_fw_easystar.hil
|
||||
set MODE custom
|
||||
else
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1001
|
||||
then
|
||||
sh /etc/init.d/1001_rc_quad.hil
|
||||
set MODE custom
|
||||
else
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1002
|
||||
then
|
||||
sh /etc/init.d/1002_rc_fw_state.hil
|
||||
set MODE custom
|
||||
else
|
||||
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
|
||||
fi
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# Upgrade PX4IO firmware
|
||||
|
@ -178,6 +194,20 @@ then
|
|||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 12
|
||||
then
|
||||
set MIXER /etc/mixers/FMU_hex_x.mix
|
||||
sh /etc/init.d/12-13_hex
|
||||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 13
|
||||
then
|
||||
set MIXER /etc/mixers/FMU_hex_+.mix
|
||||
sh /etc/init.d/12-13_hex
|
||||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 15
|
||||
then
|
||||
sh /etc/init.d/15_tbs_discovery
|
||||
|
@ -255,6 +285,18 @@ then
|
|||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 32
|
||||
then
|
||||
sh /etc/init.d/32_skywalker_x5
|
||||
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 100
|
||||
then
|
||||
sh /etc/init.d/100_mpx_easystar
|
||||
|
|
|
@ -62,3 +62,23 @@ range. Inputs below zero are treated as zero.
|
|||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -58,3 +58,23 @@ range. Inputs below zero are treated as zero.
|
|||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -50,3 +50,21 @@ M: 1
|
|||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -51,3 +51,23 @@ range. Inputs below zero are treated as zero.
|
|||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -48,3 +48,22 @@ M: 1
|
|||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
||||
|
|
|
@ -48,3 +48,23 @@ M: 1
|
|||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
||||
|
|
|
@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co
|
|||
are mixed 100%.
|
||||
|
||||
R: 6+ 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last two channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration. All c
|
|||
are mixed 100%.
|
||||
|
||||
R: 6x 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last two channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con
|
|||
are mixed 100%.
|
||||
|
||||
R: 4+ 10000 10000 10000 0
|
||||
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration. All co
|
|||
are mixed 100%.
|
||||
|
||||
R: 4v 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU
|
|||
This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%.
|
||||
|
||||
R: 4w 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration. All co
|
|||
are mixed 100%.
|
||||
|
||||
R: 4x 10000 10000 10000 0
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
|
|
@ -0,0 +1,2 @@
|
|||
parameters.wiki
|
||||
parameters.xml
|
|
@ -0,0 +1,9 @@
|
|||
h1. PX4 Parameters Processor
|
||||
|
||||
It's designed to scan PX4 source codes, find declarations of tunable parameters,
|
||||
and generate the list in various formats.
|
||||
|
||||
Currently supported formats are:
|
||||
|
||||
* XML for the parametric UI generator
|
||||
* Human-readable description in DokuWiki format
|
|
@ -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
|
|
@ -0,0 +1,5 @@
|
|||
class Output(object):
|
||||
def Save(self, groups, fn):
|
||||
data = self.Generate(groups)
|
||||
with open(fn, 'w') as f:
|
||||
f.write(data)
|
|
@ -0,0 +1,220 @@
|
|||
import sys
|
||||
import re
|
||||
|
||||
class ParameterGroup(object):
|
||||
"""
|
||||
Single parameter group
|
||||
"""
|
||||
def __init__(self, name):
|
||||
self.name = name
|
||||
self.params = []
|
||||
|
||||
def AddParameter(self, param):
|
||||
"""
|
||||
Add parameter to the group
|
||||
"""
|
||||
self.params.append(param)
|
||||
|
||||
def GetName(self):
|
||||
"""
|
||||
Get parameter group name
|
||||
"""
|
||||
return self.name
|
||||
|
||||
def GetParams(self):
|
||||
"""
|
||||
Returns the parsed list of parameters. Every parameter is a Parameter
|
||||
object. Note that returned object is not a copy. Modifications affect
|
||||
state of the parser.
|
||||
"""
|
||||
return sorted(self.params,
|
||||
cmp=lambda x, y: cmp(x.GetFieldValue("code"),
|
||||
y.GetFieldValue("code")))
|
||||
|
||||
class Parameter(object):
|
||||
"""
|
||||
Single parameter
|
||||
"""
|
||||
|
||||
# Define sorting order of the fields
|
||||
priority = {
|
||||
"code": 10,
|
||||
"type": 9,
|
||||
"short_desc": 8,
|
||||
"long_desc": 7,
|
||||
"default": 6,
|
||||
"min": 5,
|
||||
"max": 4,
|
||||
# all others == 0 (sorted alphabetically)
|
||||
}
|
||||
|
||||
def __init__(self):
|
||||
self.fields = {}
|
||||
|
||||
def SetField(self, code, value):
|
||||
"""
|
||||
Set named field value
|
||||
"""
|
||||
self.fields[code] = value
|
||||
|
||||
def GetFieldCodes(self):
|
||||
"""
|
||||
Return list of existing field codes in convenient order
|
||||
"""
|
||||
return sorted(self.fields.keys(),
|
||||
cmp=lambda x, y: cmp(self.priority.get(y, 0),
|
||||
self.priority.get(x, 0)) or cmp(x, y))
|
||||
|
||||
def GetFieldValue(self, code):
|
||||
"""
|
||||
Return value of the given field code or None if not found.
|
||||
"""
|
||||
return self.fields.get(code)
|
||||
|
||||
class Parser(object):
|
||||
"""
|
||||
Parses provided data and stores all found parameters internally.
|
||||
"""
|
||||
|
||||
re_split_lines = re.compile(r'[\r\n]+')
|
||||
re_comment_start = re.compile(r'^\/\*\*')
|
||||
re_comment_content = re.compile(r'^\*\s*(.*)')
|
||||
re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
|
||||
re_comment_end = re.compile(r'(.*?)\s*\*\/')
|
||||
re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
|
||||
re_cut_type_specifier = re.compile(r'[a-z]+$')
|
||||
re_is_a_number = re.compile(r'^-?[0-9\.]')
|
||||
re_remove_dots = re.compile(r'\.+$')
|
||||
|
||||
valid_tags = set(["min", "max", "group"])
|
||||
|
||||
# Order of parameter groups
|
||||
priority = {
|
||||
# All other groups = 0 (sort alphabetically)
|
||||
"Miscellaneous": -10
|
||||
}
|
||||
|
||||
def __init__(self):
|
||||
self.param_groups = {}
|
||||
|
||||
def GetSupportedExtensions(self):
|
||||
"""
|
||||
Returns list of supported file extensions that can be parsed by this
|
||||
parser.
|
||||
"""
|
||||
return ["cpp", "c"]
|
||||
|
||||
def Parse(self, contents):
|
||||
"""
|
||||
Incrementally parse program contents and append all found parameters
|
||||
to the list.
|
||||
"""
|
||||
# This code is essentially a comment-parsing grammar. "state"
|
||||
# represents parser state. It contains human-readable state
|
||||
# names.
|
||||
state = None
|
||||
for line in self.re_split_lines.split(contents):
|
||||
line = line.strip()
|
||||
# Ignore empty lines
|
||||
if line == "":
|
||||
continue
|
||||
if self.re_comment_start.match(line):
|
||||
state = "wait-short"
|
||||
short_desc = None
|
||||
long_desc = None
|
||||
tags = {}
|
||||
elif state is not None and state != "comment-processed":
|
||||
m = self.re_comment_end.search(line)
|
||||
if m:
|
||||
line = m.group(1)
|
||||
last_comment_line = True
|
||||
else:
|
||||
last_comment_line = False
|
||||
m = self.re_comment_content.match(line)
|
||||
if m:
|
||||
comment_content = m.group(1)
|
||||
if comment_content == "":
|
||||
# When short comment ends with empty comment line,
|
||||
# start waiting for the next part - long comment.
|
||||
if state == "wait-short-end":
|
||||
state = "wait-long"
|
||||
else:
|
||||
m = self.re_comment_tag.match(comment_content)
|
||||
if m:
|
||||
tag, desc = m.group(1, 2)
|
||||
tags[tag] = desc
|
||||
current_tag = tag
|
||||
state = "wait-tag-end"
|
||||
elif state == "wait-short":
|
||||
# Store first line of the short description
|
||||
short_desc = comment_content
|
||||
state = "wait-short-end"
|
||||
elif state == "wait-short-end":
|
||||
# Append comment line to the short description
|
||||
short_desc += "\n" + comment_content
|
||||
elif state == "wait-long":
|
||||
# Store first line of the long description
|
||||
long_desc = comment_content
|
||||
state = "wait-long-end"
|
||||
elif state == "wait-long-end":
|
||||
# Append comment line to the long description
|
||||
long_desc += "\n" + comment_content
|
||||
elif state == "wait-tag-end":
|
||||
# Append comment line to the tag text
|
||||
tags[current_tag] += "\n" + comment_content
|
||||
else:
|
||||
raise AssertionError(
|
||||
"Invalid parser state: %s" % state)
|
||||
elif not last_comment_line:
|
||||
# Invalid comment line (inside comment, but not starting with
|
||||
# "*" or "*/". Reset parsed content.
|
||||
state = None
|
||||
if last_comment_line:
|
||||
state = "comment-processed"
|
||||
else:
|
||||
# Non-empty line outside the comment
|
||||
m = self.re_parameter_definition.match(line)
|
||||
if m:
|
||||
tp, code, defval = m.group(1, 2, 3)
|
||||
# Remove trailing type specifier from numbers: 0.1f => 0.1
|
||||
if self.re_is_a_number.match(defval):
|
||||
defval = self.re_cut_type_specifier.sub('', defval)
|
||||
param = Parameter()
|
||||
param.SetField("code", code)
|
||||
param.SetField("short_desc", code)
|
||||
param.SetField("type", tp)
|
||||
param.SetField("default", defval)
|
||||
# If comment was found before the parameter declaration,
|
||||
# inject its data into the newly created parameter.
|
||||
group = "Miscellaneous"
|
||||
if state == "comment-processed":
|
||||
if short_desc is not None:
|
||||
param.SetField("short_desc",
|
||||
self.re_remove_dots.sub('', short_desc))
|
||||
if long_desc is not None:
|
||||
param.SetField("long_desc", long_desc)
|
||||
for tag in tags:
|
||||
if tag == "group":
|
||||
group = tags[tag]
|
||||
elif tag not in self.valid_tags:
|
||||
sys.stderr.write("Skipping invalid"
|
||||
"documentation tag: '%s'\n" % tag)
|
||||
else:
|
||||
param.SetField(tag, tags[tag])
|
||||
# Store the parameter
|
||||
if group not in self.param_groups:
|
||||
self.param_groups[group] = ParameterGroup(group)
|
||||
self.param_groups[group].AddParameter(param)
|
||||
# Reset parsed comment.
|
||||
state = None
|
||||
|
||||
def GetParamGroups(self):
|
||||
"""
|
||||
Returns the parsed list of parameters. Every parameter is a Parameter
|
||||
object. Note that returned object is not a copy. Modifications affect
|
||||
state of the parser.
|
||||
"""
|
||||
return sorted(self.param_groups.values(),
|
||||
cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0),
|
||||
self.priority.get(x.GetName(), 0)) or cmp(x.GetName(),
|
||||
y.GetName()))
|
|
@ -0,0 +1,61 @@
|
|||
#!/usr/bin/env python
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# PX4 paramers processor (main executable file)
|
||||
#
|
||||
# It scans src/ subdirectory of the project, collects all parameters
|
||||
# definitions, and outputs list of parameters in XML and DokuWiki formats.
|
||||
#
|
||||
|
||||
import scanner
|
||||
import parser
|
||||
import xmlout
|
||||
import dokuwikiout
|
||||
|
||||
# Initialize parser
|
||||
prs = parser.Parser()
|
||||
|
||||
# Scan directories, and parse the files
|
||||
sc = scanner.Scanner()
|
||||
sc.ScanDir("../../src", prs)
|
||||
output = prs.GetParamGroups()
|
||||
|
||||
# Output into XML
|
||||
out = xmlout.XMLOutput()
|
||||
out.Save(output, "parameters.xml")
|
||||
|
||||
# Output into DokuWiki
|
||||
out = dokuwikiout.DokuWikiOutput()
|
||||
out.Save(output, "parameters.wiki")
|
|
@ -0,0 +1,34 @@
|
|||
import os
|
||||
import re
|
||||
|
||||
class Scanner(object):
|
||||
"""
|
||||
Traverses directory tree, reads all source files, and passes their contents
|
||||
to the Parser.
|
||||
"""
|
||||
|
||||
re_file_extension = re.compile(r'\.([^\.]+)$')
|
||||
|
||||
def ScanDir(self, srcdir, parser):
|
||||
"""
|
||||
Scans provided path and passes all found contents to the parser using
|
||||
parser.Parse method.
|
||||
"""
|
||||
extensions = set(parser.GetSupportedExtensions())
|
||||
for dirname, dirnames, filenames in os.walk(srcdir):
|
||||
for filename in filenames:
|
||||
m = self.re_file_extension.search(filename)
|
||||
if m:
|
||||
ext = m.group(1)
|
||||
if ext in extensions:
|
||||
path = os.path.join(dirname, filename)
|
||||
self.ScanFile(path, parser)
|
||||
|
||||
def ScanFile(self, path, parser):
|
||||
"""
|
||||
Scans provided file and passes its contents to the parser using
|
||||
parser.Parse method.
|
||||
"""
|
||||
with open(path, 'r') as f:
|
||||
contents = f.read()
|
||||
parser.Parse(contents)
|
|
@ -0,0 +1,22 @@
|
|||
import output
|
||||
from xml.dom.minidom import getDOMImplementation
|
||||
|
||||
class XMLOutput(output.Output):
|
||||
def Generate(self, groups):
|
||||
impl = getDOMImplementation()
|
||||
xml_document = impl.createDocument(None, "parameters", None)
|
||||
xml_parameters = xml_document.documentElement
|
||||
for group in groups:
|
||||
xml_group = xml_document.createElement("group")
|
||||
xml_group.setAttribute("name", group.GetName())
|
||||
xml_parameters.appendChild(xml_group)
|
||||
for param in group.GetParams():
|
||||
xml_param = xml_document.createElement("parameter")
|
||||
xml_group.appendChild(xml_param)
|
||||
for code in param.GetFieldCodes():
|
||||
value = param.GetFieldValue(code)
|
||||
xml_field = xml_document.createElement(code)
|
||||
xml_param.appendChild(xml_field)
|
||||
xml_value = xml_document.createTextNode(value)
|
||||
xml_field.appendChild(xml_value)
|
||||
return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8")
|
|
@ -25,8 +25,12 @@ import struct, sys
|
|||
|
||||
if sys.hexversion >= 0x030000F0:
|
||||
runningPython3 = True
|
||||
def _parseCString(cstr):
|
||||
return str(cstr, 'ascii').split('\0')[0]
|
||||
else:
|
||||
runningPython3 = False
|
||||
def _parseCString(cstr):
|
||||
return str(cstr).split('\0')[0]
|
||||
|
||||
class SDLog2Parser:
|
||||
BLOCK_SIZE = 8192
|
||||
|
@ -202,14 +206,9 @@ class SDLog2Parser:
|
|||
msg_type = data[0]
|
||||
if msg_type != self.MSG_TYPE_FORMAT:
|
||||
msg_length = data[1]
|
||||
if runningPython3:
|
||||
msg_name = str(data[2], 'ascii').strip("\0")
|
||||
msg_format = str(data[3], 'ascii').strip("\0")
|
||||
msg_labels = str(data[4], 'ascii').strip("\0").split(",")
|
||||
else:
|
||||
msg_name = str(data[2]).strip("\0")
|
||||
msg_format = str(data[3]).strip("\0")
|
||||
msg_labels = str(data[4]).strip("\0").split(",")
|
||||
msg_name = _parseCString(data[2])
|
||||
msg_format = _parseCString(data[3])
|
||||
msg_labels = _parseCString(data[4]).split(",")
|
||||
# Convert msg_format to struct.unpack format string
|
||||
msg_struct = ""
|
||||
msg_mults = []
|
||||
|
@ -243,7 +242,7 @@ class SDLog2Parser:
|
|||
data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))
|
||||
for i in range(len(data)):
|
||||
if type(data[i]) is str:
|
||||
data[i] = data[i].strip("\0")
|
||||
data[i] = _parseCString(data[i])
|
||||
m = msg_mults[i]
|
||||
if m != None:
|
||||
data[i] = data[i] * m
|
||||
|
|
|
@ -33,7 +33,7 @@ MODULES += drivers/hott/hott_sensors
|
|||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/rgbled
|
||||
MODULES += drivers/mkblctrl
|
||||
MODULES += drivers/md25
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
|
@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator
|
|||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
#MODULES += modules/segway # XXX needs state machine update
|
||||
MODULES += modules/segway
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/multirotor_att_control
|
||||
|
@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter
|
|||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
|
|
|
@ -31,6 +31,7 @@ MODULES += drivers/hil
|
|||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
|
@ -77,7 +78,7 @@ MODULES += examples/flow_position_estimator
|
|||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
#MODULES += modules/segway # XXX needs state machine update
|
||||
MODULES += modules/segway
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/multirotor_att_control
|
||||
|
@ -111,6 +112,7 @@ MODULES += lib/mathlib/math/filter
|
|||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
|
|
|
@ -9,6 +9,10 @@
|
|||
#endif
|
||||
#include <math.h>
|
||||
|
||||
#ifndef M_PI_2
|
||||
#define M_PI_2 ((float)asin(1))
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file mavlink_conversions.h
|
||||
*
|
||||
|
@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo
|
|||
float phi, theta, psi;
|
||||
theta = asin(-dcm[2][0]);
|
||||
|
||||
if (fabs(theta - M_PI_2) < 1.0e-3f) {
|
||||
if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
|
||||
phi = 0.0f;
|
||||
psi = (atan2(dcm[1][2] - dcm[0][1],
|
||||
psi = (atan2f(dcm[1][2] - dcm[0][1],
|
||||
dcm[0][2] + dcm[1][1]) + phi);
|
||||
|
||||
} else if (fabs(theta + M_PI_2) < 1.0e-3f) {
|
||||
} else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
|
||||
phi = 0.0f;
|
||||
psi = atan2f(dcm[1][2] - dcm[0][1],
|
||||
dcm[0][2] + dcm[1][1] - phi);
|
||||
|
|
|
@ -66,7 +66,7 @@ struct accel_report {
|
|||
int16_t temperature_raw;
|
||||
};
|
||||
|
||||
/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
|
||||
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
|
||||
struct accel_scale {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
|
|
|
@ -132,11 +132,8 @@ ETSAirspeed::measure()
|
|||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
@ -138,11 +138,8 @@ MEASAirspeed::measure()
|
|||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
@ -172,7 +172,20 @@ RGBLED::probe()
|
|||
bool on, powersave;
|
||||
uint8_t r, g, b;
|
||||
|
||||
ret = get(on, powersave, r, g, b);
|
||||
/**
|
||||
this may look strange, but is needed. There is a serial
|
||||
EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
|
||||
a bunch of I2C addresses, including the 0x55 used by this
|
||||
LED device. So we need to do enough operations to be sure
|
||||
we are talking to the right device. These 3 operations seem
|
||||
to be enough, as the 3rd one consistently fails if no
|
||||
RGBLED is on the bus.
|
||||
*/
|
||||
if ((ret=get(on, powersave, r, g, b)) != OK ||
|
||||
(ret=send_led_enable(false) != OK) ||
|
||||
(ret=send_led_enable(false) != OK)) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
@ -561,7 +574,7 @@ rgbled_main(int argc, char *argv[])
|
|||
int ch;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
|
||||
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'a':
|
||||
rgbledadr = strtol(optarg, NULL, 0);
|
||||
|
@ -577,7 +590,12 @@ rgbled_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
const char *verb = argv[1];
|
||||
if (optind >= argc) {
|
||||
rgbled_usage();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
int fd;
|
||||
int ret;
|
||||
|
@ -598,6 +616,9 @@ rgbled_main(int argc, char *argv[])
|
|||
|
||||
if (g_rgbled == nullptr) {
|
||||
// fall back to default bus
|
||||
if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
|
||||
errx(1, "init failed");
|
||||
}
|
||||
i2cdevice = PX4_I2C_BUS_LED;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -0,0 +1,328 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file RoboClaw.cpp
|
||||
*
|
||||
* RoboClaw Motor Driver
|
||||
*
|
||||
* references:
|
||||
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
|
||||
*
|
||||
*/
|
||||
|
||||
#include "RoboClaw.hpp"
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
uint8_t RoboClaw::checksum_mask = 0x7f;
|
||||
|
||||
RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
|
||||
uint16_t pulsesPerRev):
|
||||
_address(address),
|
||||
_pulsesPerRev(pulsesPerRev),
|
||||
_uart(0),
|
||||
_controlPoll(),
|
||||
_actuators(NULL, ORB_ID(actuator_controls_0), 20),
|
||||
_motor1Position(0),
|
||||
_motor1Speed(0),
|
||||
_motor1Overflow(0),
|
||||
_motor2Position(0),
|
||||
_motor2Speed(0),
|
||||
_motor2Overflow(0)
|
||||
{
|
||||
// setup control polling
|
||||
_controlPoll.fd = _actuators.getHandle();
|
||||
_controlPoll.events = POLLIN;
|
||||
|
||||
// start serial port
|
||||
_uart = open(deviceName, O_RDWR | O_NOCTTY);
|
||||
if (_uart < 0) err(1, "could not open %s", deviceName);
|
||||
int ret = 0;
|
||||
struct termios uart_config;
|
||||
ret = tcgetattr(_uart, &uart_config);
|
||||
if (ret < 0) err (1, "failed to get attr");
|
||||
uart_config.c_oflag &= ~ONLCR; // no CR for every LF
|
||||
ret = cfsetispeed(&uart_config, B38400);
|
||||
if (ret < 0) err (1, "failed to set input speed");
|
||||
ret = cfsetospeed(&uart_config, B38400);
|
||||
if (ret < 0) err (1, "failed to set output speed");
|
||||
ret = tcsetattr(_uart, TCSANOW, &uart_config);
|
||||
if (ret < 0) err (1, "failed to set attr");
|
||||
|
||||
// setup default settings, reset encoders
|
||||
resetEncoders();
|
||||
}
|
||||
|
||||
RoboClaw::~RoboClaw()
|
||||
{
|
||||
setMotorDutyCycle(MOTOR_1, 0.0);
|
||||
setMotorDutyCycle(MOTOR_2, 0.0);
|
||||
close(_uart);
|
||||
}
|
||||
|
||||
int RoboClaw::readEncoder(e_motor motor)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
if (motor == MOTOR_1) {
|
||||
_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
|
||||
} else if (motor == MOTOR_2) {
|
||||
_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
|
||||
}
|
||||
uint8_t rbuf[50];
|
||||
usleep(5000);
|
||||
int nread = read(_uart, rbuf, 50);
|
||||
if (nread < 6) {
|
||||
printf("failed to read\n");
|
||||
return -1;
|
||||
}
|
||||
//printf("received: \n");
|
||||
//for (int i=0;i<nread;i++) {
|
||||
//printf("%d\t", rbuf[i]);
|
||||
//}
|
||||
//printf("\n");
|
||||
uint32_t count = 0;
|
||||
uint8_t * countBytes = (uint8_t *)(&count);
|
||||
countBytes[3] = rbuf[0];
|
||||
countBytes[2] = rbuf[1];
|
||||
countBytes[1] = rbuf[2];
|
||||
countBytes[0] = rbuf[3];
|
||||
uint8_t status = rbuf[4];
|
||||
uint8_t checksum = rbuf[5];
|
||||
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
|
||||
checksum_mask;
|
||||
// check if checksum is valid
|
||||
if (checksum != checksum_computed) {
|
||||
printf("checksum failed: expected %d got %d\n",
|
||||
checksum, checksum_computed);
|
||||
return -1;
|
||||
}
|
||||
int overFlow = 0;
|
||||
|
||||
if (status & STATUS_REVERSE) {
|
||||
//printf("roboclaw: reverse\n");
|
||||
}
|
||||
|
||||
if (status & STATUS_UNDERFLOW) {
|
||||
//printf("roboclaw: underflow\n");
|
||||
overFlow = -1;
|
||||
} else if (status & STATUS_OVERFLOW) {
|
||||
//printf("roboclaw: overflow\n");
|
||||
overFlow = +1;
|
||||
}
|
||||
|
||||
static int64_t overflowAmount = 0x100000000LL;
|
||||
if (motor == MOTOR_1) {
|
||||
_motor1Overflow += overFlow;
|
||||
_motor1Position = float(int64_t(count) +
|
||||
_motor1Overflow*overflowAmount)/_pulsesPerRev;
|
||||
} else if (motor == MOTOR_2) {
|
||||
_motor2Overflow += overFlow;
|
||||
_motor2Position = float(int64_t(count) +
|
||||
_motor2Overflow*overflowAmount)/_pulsesPerRev;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void RoboClaw::printStatus(char *string, size_t n)
|
||||
{
|
||||
snprintf(string, n,
|
||||
"pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
|
||||
double(getMotorPosition(MOTOR_1)),
|
||||
double(getMotorSpeed(MOTOR_1)),
|
||||
double(getMotorPosition(MOTOR_2)),
|
||||
double(getMotorSpeed(MOTOR_2)));
|
||||
}
|
||||
|
||||
float RoboClaw::getMotorPosition(e_motor motor)
|
||||
{
|
||||
if (motor == MOTOR_1) {
|
||||
return _motor1Position;
|
||||
} else if (motor == MOTOR_2) {
|
||||
return _motor2Position;
|
||||
}
|
||||
}
|
||||
|
||||
float RoboClaw::getMotorSpeed(e_motor motor)
|
||||
{
|
||||
if (motor == MOTOR_1) {
|
||||
return _motor1Speed;
|
||||
} else if (motor == MOTOR_2) {
|
||||
return _motor2Speed;
|
||||
}
|
||||
}
|
||||
|
||||
int RoboClaw::setMotorSpeed(e_motor motor, float value)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
// bound
|
||||
if (value > 1) value = 1;
|
||||
if (value < -1) value = -1;
|
||||
uint8_t speed = fabs(value)*127;
|
||||
// send command
|
||||
if (motor == MOTOR_1) {
|
||||
if (value > 0) {
|
||||
return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
|
||||
} else {
|
||||
return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
|
||||
}
|
||||
} else if (motor == MOTOR_2) {
|
||||
if (value > 0) {
|
||||
return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
|
||||
} else {
|
||||
return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
// bound
|
||||
if (value > 1) value = 1;
|
||||
if (value < -1) value = -1;
|
||||
int16_t duty = 1500*value;
|
||||
// send command
|
||||
if (motor == MOTOR_1) {
|
||||
return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
|
||||
(uint8_t *)(&duty), 2, sum);
|
||||
} else if (motor == MOTOR_2) {
|
||||
return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
|
||||
(uint8_t *)(&duty), 2, sum);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int RoboClaw::resetEncoders()
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
return _sendCommand(CMD_RESET_ENCODERS,
|
||||
nullptr, 0, sum);
|
||||
}
|
||||
|
||||
int RoboClaw::update()
|
||||
{
|
||||
// wait for an actuator publication,
|
||||
// check for exit condition every second
|
||||
// note "::poll" is required to distinguish global
|
||||
// poll from member function for driver
|
||||
if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
|
||||
|
||||
// if new data, send to motors
|
||||
if (_actuators.updated()) {
|
||||
_actuators.update();
|
||||
setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
|
||||
setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
//printf("sum\n");
|
||||
for (size_t i=0;i<n;i++) {
|
||||
sum += buf[i];
|
||||
//printf("%d\t", buf[i]);
|
||||
}
|
||||
//printf("total sum %d\n", sum);
|
||||
return sum;
|
||||
}
|
||||
|
||||
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
|
||||
size_t n_data, uint16_t & prev_sum)
|
||||
{
|
||||
tcflush(_uart, TCIOFLUSH); // flush buffers
|
||||
uint8_t buf[n_data + 3];
|
||||
buf[0] = _address;
|
||||
buf[1] = cmd;
|
||||
for (size_t i=0;i<n_data;i++) {
|
||||
buf[i+2] = data[n_data - i - 1]; // MSB
|
||||
}
|
||||
uint16_t sum = _sumBytes(buf, n_data + 2);
|
||||
prev_sum += sum;
|
||||
buf[n_data + 2] = sum & checksum_mask;
|
||||
//printf("\nmessage:\n");
|
||||
//for (size_t i=0;i<n_data+3;i++) {
|
||||
//printf("%d\t", buf[i]);
|
||||
//}
|
||||
//printf("\n");
|
||||
return write(_uart, buf, n_data + 3);
|
||||
}
|
||||
|
||||
int roboclawTest(const char *deviceName, uint8_t address,
|
||||
uint16_t pulsesPerRev)
|
||||
{
|
||||
printf("roboclaw test: starting\n");
|
||||
|
||||
// setup
|
||||
RoboClaw roboclaw(deviceName, address, pulsesPerRev);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
|
||||
char buf[200];
|
||||
for (int i=0; i<10; i++) {
|
||||
usleep(100000);
|
||||
roboclaw.readEncoder(RoboClaw::MOTOR_1);
|
||||
roboclaw.readEncoder(RoboClaw::MOTOR_2);
|
||||
roboclaw.printStatus(buf,200);
|
||||
printf("%s", buf);
|
||||
}
|
||||
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
|
||||
for (int i=0; i<10; i++) {
|
||||
usleep(100000);
|
||||
roboclaw.readEncoder(RoboClaw::MOTOR_1);
|
||||
roboclaw.readEncoder(RoboClaw::MOTOR_2);
|
||||
roboclaw.printStatus(buf,200);
|
||||
printf("%s", buf);
|
||||
}
|
||||
|
||||
printf("Test complete\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
|
|
@ -0,0 +1,192 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file RoboClas.hpp
|
||||
*
|
||||
* RoboClaw Motor Driver
|
||||
*
|
||||
* references:
|
||||
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
/**
|
||||
* This is a driver for the RoboClaw motor controller
|
||||
*/
|
||||
class RoboClaw
|
||||
{
|
||||
public:
|
||||
|
||||
/** control channels */
|
||||
enum e_channel {
|
||||
CH_VOLTAGE_LEFT = 0,
|
||||
CH_VOLTAGE_RIGHT
|
||||
};
|
||||
|
||||
/** motors */
|
||||
enum e_motor {
|
||||
MOTOR_1 = 0,
|
||||
MOTOR_2
|
||||
};
|
||||
|
||||
/**
|
||||
* constructor
|
||||
* @param deviceName the name of the
|
||||
* serial port e.g. "/dev/ttyS2"
|
||||
* @param address the adddress of the motor
|
||||
* (selectable on roboclaw)
|
||||
* @param pulsesPerRev # of encoder
|
||||
* pulses per revolution of wheel
|
||||
*/
|
||||
RoboClaw(const char *deviceName, uint16_t address,
|
||||
uint16_t pulsesPerRev);
|
||||
|
||||
/**
|
||||
* deconstructor
|
||||
*/
|
||||
virtual ~RoboClaw();
|
||||
|
||||
/**
|
||||
* @return position of a motor, rev
|
||||
*/
|
||||
float getMotorPosition(e_motor motor);
|
||||
|
||||
/**
|
||||
* @return speed of a motor, rev/sec
|
||||
*/
|
||||
float getMotorSpeed(e_motor motor);
|
||||
|
||||
/**
|
||||
* set the speed of a motor, rev/sec
|
||||
*/
|
||||
int setMotorSpeed(e_motor motor, float value);
|
||||
|
||||
/**
|
||||
* set the duty cycle of a motor, rev/sec
|
||||
*/
|
||||
int setMotorDutyCycle(e_motor motor, float value);
|
||||
|
||||
/**
|
||||
* reset the encoders
|
||||
* @return status
|
||||
*/
|
||||
int resetEncoders();
|
||||
|
||||
/**
|
||||
* main update loop that updates RoboClaw motor
|
||||
* dutycycle based on actuator publication
|
||||
*/
|
||||
int update();
|
||||
|
||||
/**
|
||||
* read data from serial
|
||||
*/
|
||||
int readEncoder(e_motor motor);
|
||||
|
||||
/**
|
||||
* print status
|
||||
*/
|
||||
void printStatus(char *string, size_t n);
|
||||
|
||||
private:
|
||||
|
||||
// Quadrature status flags
|
||||
enum e_quadrature_status_flags {
|
||||
STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/
|
||||
STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/
|
||||
STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/
|
||||
};
|
||||
|
||||
// commands
|
||||
// We just list the commands we want from the manual here.
|
||||
enum e_command {
|
||||
|
||||
// simple
|
||||
CMD_DRIVE_FWD_1 = 0,
|
||||
CMD_DRIVE_REV_1 = 1,
|
||||
CMD_DRIVE_FWD_2 = 4,
|
||||
CMD_DRIVE_REV_2 = 5,
|
||||
|
||||
// encoder commands
|
||||
CMD_READ_ENCODER_1 = 16,
|
||||
CMD_READ_ENCODER_2 = 17,
|
||||
CMD_RESET_ENCODERS = 20,
|
||||
|
||||
// advanced motor control
|
||||
CMD_READ_SPEED_HIRES_1 = 30,
|
||||
CMD_READ_SPEED_HIRES_2 = 31,
|
||||
CMD_SIGNED_DUTYCYCLE_1 = 32,
|
||||
CMD_SIGNED_DUTYCYCLE_2 = 33,
|
||||
};
|
||||
|
||||
static uint8_t checksum_mask;
|
||||
|
||||
uint16_t _address;
|
||||
uint16_t _pulsesPerRev;
|
||||
|
||||
int _uart;
|
||||
|
||||
/** poll structure for control packets */
|
||||
struct pollfd _controlPoll;
|
||||
|
||||
/** actuator controls subscription */
|
||||
control::UOrbSubscription<actuator_controls_s> _actuators;
|
||||
|
||||
// private data
|
||||
float _motor1Position;
|
||||
float _motor1Speed;
|
||||
int16_t _motor1Overflow;
|
||||
|
||||
float _motor2Position;
|
||||
float _motor2Speed;
|
||||
int16_t _motor2Overflow;
|
||||
|
||||
// private methods
|
||||
uint16_t _sumBytes(uint8_t * buf, size_t n);
|
||||
int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum);
|
||||
};
|
||||
|
||||
// unit testing
|
||||
int roboclawTest(const char *deviceName, uint8_t address,
|
||||
uint16_t pulsesPerRev);
|
||||
|
||||
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
|
|
@ -0,0 +1,41 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# RoboClaw Motor Controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = roboclaw
|
||||
|
||||
SRCS = roboclaw_main.cpp \
|
||||
RoboClaw.cpp
|
|
@ -0,0 +1,195 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @ file roboclaw_main.cpp
|
||||
*
|
||||
* RoboClaw Motor Driver
|
||||
*
|
||||
* references:
|
||||
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include "RoboClaw.hpp"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int roboclaw_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage();
|
||||
|
||||
static void usage()
|
||||
{
|
||||
fprintf(stderr, "usage: roboclaw "
|
||||
"{start|stop|status|test}\n\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int roboclaw_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1)
|
||||
usage();
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("roboclaw already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd("roboclaw",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
2048,
|
||||
roboclaw_thread_main,
|
||||
(const char **)argv);
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "test")) {
|
||||
|
||||
const char * deviceName = "/dev/ttyS2";
|
||||
uint8_t address = 128;
|
||||
uint16_t pulsesPerRev = 1200;
|
||||
|
||||
if (argc == 2) {
|
||||
printf("testing with default settings\n");
|
||||
} else if (argc != 4) {
|
||||
printf("usage: roboclaw test device address pulses_per_rev\n");
|
||||
exit(-1);
|
||||
} else {
|
||||
deviceName = argv[2];
|
||||
address = strtoul(argv[3], nullptr, 0);
|
||||
pulsesPerRev = strtoul(argv[4], nullptr, 0);
|
||||
}
|
||||
|
||||
printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
|
||||
deviceName, address, pulsesPerRev);
|
||||
|
||||
roboclawTest(deviceName, address, pulsesPerRev);
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "status")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("\troboclaw app is running\n");
|
||||
|
||||
} else {
|
||||
printf("\troboclaw app not started\n");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage();
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int roboclaw_thread_main(int argc, char *argv[])
|
||||
{
|
||||
printf("[roboclaw] starting\n");
|
||||
|
||||
// skip parent process args
|
||||
argc -=2;
|
||||
argv +=2;
|
||||
|
||||
if (argc < 3) {
|
||||
printf("usage: roboclaw start device address\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
const char *deviceName = argv[1];
|
||||
uint8_t address = strtoul(argv[2], nullptr, 0);
|
||||
uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
|
||||
|
||||
printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
|
||||
deviceName, address, pulsesPerRev);
|
||||
|
||||
// start
|
||||
RoboClaw roboclaw(deviceName, address, pulsesPerRev);
|
||||
|
||||
thread_running = true;
|
||||
|
||||
// loop
|
||||
while (!thread_should_exit) {
|
||||
roboclaw.update();
|
||||
}
|
||||
|
||||
// exit
|
||||
printf("[roboclaw] exiting.\n");
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
|
|
@ -0,0 +1,38 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Conversion library
|
||||
#
|
||||
|
||||
SRCS = rotation.cpp
|
|
@ -0,0 +1,62 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rotation.cpp
|
||||
*
|
||||
* Vector rotation library
|
||||
*/
|
||||
|
||||
#include "math.h"
|
||||
#include "rotation.h"
|
||||
|
||||
__EXPORT void
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix *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);
|
||||
}
|
||||
}
|
||||
}
|
|
@ -0,0 +1,121 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rotation.h
|
||||
*
|
||||
* Vector rotation library
|
||||
*/
|
||||
|
||||
#ifndef ROTATION_H_
|
||||
#define ROTATION_H_
|
||||
|
||||
#include <unistd.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
/**
|
||||
* Enum for board and external compass rotations.
|
||||
* This enum maps from board attitude to airframe attitude.
|
||||
*/
|
||||
enum Rotation {
|
||||
ROTATION_NONE = 0,
|
||||
ROTATION_YAW_45 = 1,
|
||||
ROTATION_YAW_90 = 2,
|
||||
ROTATION_YAW_135 = 3,
|
||||
ROTATION_YAW_180 = 4,
|
||||
ROTATION_YAW_225 = 5,
|
||||
ROTATION_YAW_270 = 6,
|
||||
ROTATION_YAW_315 = 7,
|
||||
ROTATION_ROLL_180 = 8,
|
||||
ROTATION_ROLL_180_YAW_45 = 9,
|
||||
ROTATION_ROLL_180_YAW_90 = 10,
|
||||
ROTATION_ROLL_180_YAW_135 = 11,
|
||||
ROTATION_PITCH_180 = 12,
|
||||
ROTATION_ROLL_180_YAW_225 = 13,
|
||||
ROTATION_ROLL_180_YAW_270 = 14,
|
||||
ROTATION_ROLL_180_YAW_315 = 15,
|
||||
ROTATION_ROLL_90 = 16,
|
||||
ROTATION_ROLL_90_YAW_45 = 17,
|
||||
ROTATION_ROLL_90_YAW_90 = 18,
|
||||
ROTATION_ROLL_90_YAW_135 = 19,
|
||||
ROTATION_ROLL_270 = 20,
|
||||
ROTATION_ROLL_270_YAW_45 = 21,
|
||||
ROTATION_ROLL_270_YAW_90 = 22,
|
||||
ROTATION_ROLL_270_YAW_135 = 23,
|
||||
ROTATION_PITCH_90 = 24,
|
||||
ROTATION_PITCH_270 = 25,
|
||||
ROTATION_MAX
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
uint16_t roll;
|
||||
uint16_t pitch;
|
||||
uint16_t yaw;
|
||||
} rot_lookup_t;
|
||||
|
||||
const rot_lookup_t rot_lookup[] = {
|
||||
{ 0, 0, 0 },
|
||||
{ 0, 0, 45 },
|
||||
{ 0, 0, 90 },
|
||||
{ 0, 0, 135 },
|
||||
{ 0, 0, 180 },
|
||||
{ 0, 0, 225 },
|
||||
{ 0, 0, 270 },
|
||||
{ 0, 0, 315 },
|
||||
{180, 0, 0 },
|
||||
{180, 0, 45 },
|
||||
{180, 0, 90 },
|
||||
{180, 0, 135 },
|
||||
{ 0, 180, 0 },
|
||||
{180, 0, 225 },
|
||||
{180, 0, 270 },
|
||||
{180, 0, 315 },
|
||||
{ 90, 0, 0 },
|
||||
{ 90, 0, 45 },
|
||||
{ 90, 0, 90 },
|
||||
{ 90, 0, 135 },
|
||||
{270, 0, 0 },
|
||||
{270, 0, 45 },
|
||||
{270, 0, 90 },
|
||||
{270, 0, 135 },
|
||||
{ 0, 90, 0 },
|
||||
{ 0, 270, 0 }
|
||||
};
|
||||
|
||||
/**
|
||||
* Get the rotation matrix
|
||||
*/
|
||||
__EXPORT void
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
|
||||
|
||||
#endif /* ROTATION_H_ */
|
|
@ -43,7 +43,7 @@
|
|||
float ECL_L1_Pos_Controller::nav_roll()
|
||||
{
|
||||
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
|
||||
ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
|
||||
ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
@ -222,6 +222,15 @@ public:
|
|||
_K_L1 = 4.0f * _L1_damping * _L1_damping;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Set the maximum roll angle output in radians
|
||||
*
|
||||
*/
|
||||
void set_l1_roll_limit(float roll_lim_rad) {
|
||||
_roll_lim_rad = roll_lim_rad;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
|
||||
|
@ -238,6 +247,8 @@ private:
|
|||
float _K_L1; ///< L1 control gain for _L1_damping
|
||||
float _heading_omega; ///< Normalized frequency
|
||||
|
||||
float _roll_lim_rad; ///<maximum roll angle
|
||||
|
||||
/**
|
||||
* Convert a 2D vector from WGS84 to planar coordinates.
|
||||
*
|
||||
|
|
|
@ -100,10 +100,29 @@
|
|||
* accel_T = A^-1 * g
|
||||
* g = 9.80665
|
||||
*
|
||||
* ===== Rotation =====
|
||||
*
|
||||
* Calibrating using model:
|
||||
* accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
|
||||
*
|
||||
* Actual correction:
|
||||
* accel_corr = rot * accel_T * (accel_raw - accel_offs)
|
||||
*
|
||||
* Known: accel_T_r, accel_offs_r, rot
|
||||
* Unknown: accel_T, accel_offs
|
||||
*
|
||||
* Solution:
|
||||
* accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
|
||||
* rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
|
||||
* rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
|
||||
* => accel_T = rot^-1 * accel_T_r * rot
|
||||
* => accel_offs = rot^-1 * accel_offs_r
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include "accelerometer_calibration.h"
|
||||
#include "calibration_messages.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
@ -112,11 +131,13 @@
|
|||
#include <fcntl.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <string.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <geo/geo.h>
|
||||
#include <conversion/rotation.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
@ -127,93 +148,122 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
|
||||
static const char *sensor_name = "accel";
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
|
||||
int mat_invert3(float src[3][3], float dst[3][3]);
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
|
||||
|
||||
int do_accel_calibration(int mavlink_fd) {
|
||||
/* announce change */
|
||||
mavlink_log_info(mavlink_fd, "accel calibration started");
|
||||
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
|
||||
int do_accel_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
|
||||
/* measure and calculate offsets & scales */
|
||||
float accel_offs[3];
|
||||
float accel_scale[3];
|
||||
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
|
||||
|
||||
if (res == OK) {
|
||||
/* measurements complete successfully, set parameters */
|
||||
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|
||||
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|
||||
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|
||||
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|
||||
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|
||||
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
|
||||
}
|
||||
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
struct accel_scale ascale = {
|
||||
accel_offs[0],
|
||||
accel_scale[0],
|
||||
accel_offs[1],
|
||||
accel_scale[1],
|
||||
accel_offs[2],
|
||||
accel_scale[2],
|
||||
struct accel_scale accel_scale = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
|
||||
warn("WARNING: failed to set scale / offsets for accel");
|
||||
int res = OK;
|
||||
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
float accel_offs[3];
|
||||
float accel_T[3][3];
|
||||
|
||||
if (res == OK) {
|
||||
/* measure and calculate offsets & scales */
|
||||
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* measurements completed successfully, rotate calibration values */
|
||||
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
|
||||
int32_t board_rotation_int;
|
||||
param_get(board_rotation_h, &(board_rotation_int));
|
||||
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
|
||||
math::Matrix board_rotation(3, 3);
|
||||
get_rot_matrix(board_rotation_id, &board_rotation);
|
||||
math::Matrix board_rotation_t = board_rotation.transpose();
|
||||
math::Vector3 accel_offs_vec;
|
||||
accel_offs_vec.set(&accel_offs[0]);
|
||||
math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
|
||||
math::Matrix accel_T_mat(3, 3);
|
||||
accel_T_mat.set(&accel_T[0][0]);
|
||||
math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
|
||||
|
||||
accel_scale.x_offset = accel_offs_rotated(0);
|
||||
accel_scale.x_scale = accel_T_rotated(0, 0);
|
||||
accel_scale.y_offset = accel_offs_rotated(1);
|
||||
accel_scale.y_scale = accel_T_rotated(1, 1);
|
||||
accel_scale.z_offset = accel_offs_rotated(2);
|
||||
accel_scale.z_scale = accel_T_rotated(2, 2);
|
||||
|
||||
/* set parameters */
|
||||
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
|
||||
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
|
||||
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
|
||||
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
|
||||
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
|
||||
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* apply new scaling and offsets */
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
res = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "accel calibration done");
|
||||
return OK;
|
||||
if (res == OK) {
|
||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
/* measurements error */
|
||||
mavlink_log_info(mavlink_fd, "accel calibration aborted");
|
||||
return ERROR;
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
/* exit accel calibration mode */
|
||||
return res;
|
||||
}
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
|
||||
{
|
||||
const int samples_num = 2500;
|
||||
float accel_ref[6][3];
|
||||
bool data_collected[6] = { false, false, false, false, false, false };
|
||||
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
|
||||
|
||||
/* reset existing calibration */
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
struct accel_scale ascale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
|
||||
close(fd);
|
||||
|
||||
if (OK != ioctl_res) {
|
||||
warn("ERROR: failed to set scale / offsets for accel");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
||||
unsigned done_count = 0;
|
||||
int res = OK;
|
||||
|
||||
while (true) {
|
||||
bool done = true;
|
||||
|
@ -221,11 +271,20 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
done_count = 0;
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (!data_collected[i]) {
|
||||
if (data_collected[i]) {
|
||||
done_count++;
|
||||
|
||||
} else {
|
||||
done = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (old_done_count != done_count)
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
|
||||
|
||||
if (done)
|
||||
break;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
|
||||
(!data_collected[0]) ? "x+ " : "",
|
||||
(!data_collected[1]) ? "x- " : "",
|
||||
|
@ -234,20 +293,15 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
(!data_collected[4]) ? "z+ " : "",
|
||||
(!data_collected[5]) ? "z- " : "");
|
||||
|
||||
if (old_done_count != done_count)
|
||||
mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
|
||||
|
||||
if (done)
|
||||
break;
|
||||
|
||||
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
|
||||
|
||||
if (orient < 0) {
|
||||
close(sensor_combined_sub);
|
||||
return ERROR;
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
if (data_collected[orient]) {
|
||||
mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
|
||||
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
@ -261,24 +315,19 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
data_collected[orient] = true;
|
||||
tune_neutral();
|
||||
}
|
||||
|
||||
close(sensor_combined_sub);
|
||||
|
||||
/* calculate offsets and rotation+scale matrix */
|
||||
float accel_T[3][3];
|
||||
int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
|
||||
if (res != 0) {
|
||||
if (res == OK) {
|
||||
/* calculate offsets and transform matrix */
|
||||
res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* convert accel transform matrix to scales,
|
||||
* rotation part of transform matrix is not used by now
|
||||
*/
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_scale[i] = accel_T[i][i];
|
||||
}
|
||||
|
||||
return OK;
|
||||
return res;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -287,14 +336,15 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
* @return 0..5 according to orientation when vehicle is still and ready for measurements,
|
||||
* ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
|
||||
*/
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined)
|
||||
{
|
||||
struct sensor_combined_s sensor;
|
||||
/* exponential moving average of accel */
|
||||
float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
|
||||
/* max-hold dispersion of accel */
|
||||
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
|
||||
/* EMA time constant in seconds*/
|
||||
float ema_len = 0.2f;
|
||||
float ema_len = 0.5f;
|
||||
/* set "still" threshold to 0.25 m/s^2 */
|
||||
float still_thr2 = pow(0.25f, 2);
|
||||
/* set accel error threshold to 5m/s^2 */
|
||||
|
@ -318,30 +368,38 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
|
|||
while (true) {
|
||||
/* wait blocking for new data */
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
|
||||
t = hrt_absolute_time();
|
||||
float dt = (t - t_prev) / 1000000.0f;
|
||||
t_prev = t;
|
||||
float w = dt / ema_len;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
|
||||
float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
|
||||
float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
|
||||
accel_ema[i] += d * w;
|
||||
d = d * d;
|
||||
accel_disp[i] = accel_disp[i] * (1.0f - w);
|
||||
|
||||
if (d > still_thr2 * 8.0f)
|
||||
d = still_thr2 * 8.0f;
|
||||
|
||||
if (d > accel_disp[i])
|
||||
accel_disp[i] = d;
|
||||
}
|
||||
|
||||
/* still detector with hysteresis */
|
||||
if ( accel_disp[0] < still_thr2 &&
|
||||
if (accel_disp[0] < still_thr2 &&
|
||||
accel_disp[1] < still_thr2 &&
|
||||
accel_disp[2] < still_thr2 ) {
|
||||
accel_disp[2] < still_thr2) {
|
||||
/* is still now */
|
||||
if (t_still == 0) {
|
||||
/* first time */
|
||||
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
|
||||
t_still = t;
|
||||
t_timeout = t + timeout;
|
||||
|
||||
} else {
|
||||
/* still since t_still */
|
||||
if (t > t_still + still_time) {
|
||||
|
@ -349,62 +407,71 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
|
|||
break;
|
||||
}
|
||||
}
|
||||
} else if ( accel_disp[0] > still_thr2 * 2.0f ||
|
||||
accel_disp[1] > still_thr2 * 2.0f ||
|
||||
accel_disp[2] > still_thr2 * 2.0f) {
|
||||
|
||||
} else if (accel_disp[0] > still_thr2 * 4.0f ||
|
||||
accel_disp[1] > still_thr2 * 4.0f ||
|
||||
accel_disp[2] > still_thr2 * 4.0f) {
|
||||
/* not still, reset still start time */
|
||||
if (t_still != 0) {
|
||||
mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
|
||||
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
|
||||
t_still = 0;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (t > t_timeout) {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
|
||||
return -1;
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 0; // [ g, 0, 0 ]
|
||||
if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
|
||||
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 1; // [ -g, 0, 0 ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 2; // [ 0, g, 0 ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 3; // [ 0, -g, 0 ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
|
||||
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
|
||||
return 4; // [ 0, 0, g ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
|
||||
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
|
||||
return 5; // [ 0, 0, -g ]
|
||||
|
||||
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
|
||||
|
||||
return -2; // Can't detect orientation
|
||||
return ERROR; // Can't detect orientation
|
||||
}
|
||||
|
||||
/*
|
||||
* Read specified number of accelerometer samples, calculate average and dispersion.
|
||||
*/
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num)
|
||||
{
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sensor_combined_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
@ -415,12 +482,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
|
|||
|
||||
while (count < samples_num) {
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret == 1) {
|
||||
struct sensor_combined_s sensor;
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
accel_sum[i] += sensor.accelerometer_m_s2[i];
|
||||
|
||||
count++;
|
||||
|
||||
} else {
|
||||
errcount++;
|
||||
continue;
|
||||
|
@ -437,10 +508,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
|
|||
return OK;
|
||||
}
|
||||
|
||||
int mat_invert3(float src[3][3], float dst[3][3]) {
|
||||
int mat_invert3(float src[3][3], float dst[3][3])
|
||||
{
|
||||
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
|
||||
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
|
||||
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
|
||||
|
||||
if (det == 0.0f)
|
||||
return ERROR; // Singular matrix
|
||||
|
||||
|
@ -457,7 +530,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
|
|||
return OK;
|
||||
}
|
||||
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g)
|
||||
{
|
||||
/* calculate offsets */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
|
||||
|
@ -466,6 +540,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
|
|||
/* fill matrix A for linear equations system*/
|
||||
float mat_A[3][3];
|
||||
memset(mat_A, 0, sizeof(mat_A));
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
float a = accel_ref[i * 2][j] - accel_offs[j];
|
||||
|
@ -475,6 +550,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
|
|||
|
||||
/* calculate inverse matrix for A */
|
||||
float mat_A_inv[3][3];
|
||||
|
||||
if (mat_invert3(mat_A, mat_A_inv) != OK)
|
||||
return ERROR;
|
||||
|
||||
|
|
|
@ -0,0 +1,57 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 calibration_messages.h
|
||||
*
|
||||
* Common calibration messages.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef CALIBRATION_MESSAGES_H_
|
||||
#define CALIBRATION_MESSAGES_H_
|
||||
|
||||
#define CAL_STARTED_MSG "%s calibration: started"
|
||||
#define CAL_DONE_MSG "%s calibration: done"
|
||||
#define CAL_FAILED_MSG "%s calibration: failed"
|
||||
#define CAL_PROGRESS_MSG "%s calibration: progress <%u>"
|
||||
|
||||
#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor"
|
||||
#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration"
|
||||
#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration"
|
||||
#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters"
|
||||
#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters"
|
||||
|
||||
#endif /* CALIBRATION_MESSAGES_H_ */
|
|
@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
|
||||
/* reset the arming mode to disarmed */
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
|
||||
|
||||
if (arming_res != TRANSITION_DENIED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
|
||||
}
|
||||
|
@ -481,9 +483,9 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
break;
|
||||
}
|
||||
|
||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM:
|
||||
{
|
||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
||||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
|
||||
if (safety->safety_switch_available && !safety->safety_off) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
|
@ -496,6 +498,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
|
||||
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
|
@ -687,7 +690,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
bool updated = false;
|
||||
|
||||
bool rc_calibration_ok = (OK == rc_calibration_check());
|
||||
bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
|
||||
/* Subscribe to safety topic */
|
||||
int safety_sub = orb_subscribe(ORB_ID(safety));
|
||||
|
@ -802,7 +805,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_changed = true;
|
||||
|
||||
/* re-check RC calibration */
|
||||
rc_calibration_ok = (OK == rc_calibration_check());
|
||||
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
|
||||
/* navigation parameters */
|
||||
param_get(_param_takeoff_alt, &takeoff_alt);
|
||||
|
@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
last_idle_time = system_load.tasks[0].total_runtime;
|
||||
|
||||
/* check if board is connected via USB */
|
||||
struct stat statbuf;
|
||||
//struct stat statbuf;
|
||||
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
|
||||
}
|
||||
|
||||
|
@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
if (armed.armed) {
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
|
||||
|
||||
} else {
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
|
||||
}
|
||||
|
@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
counter++;
|
||||
|
||||
int blink_state = blink_msg_state();
|
||||
|
||||
if (blink_state > 0) {
|
||||
/* blinking LED message, don't touch LEDs */
|
||||
if (blink_state == 2) {
|
||||
/* blinking LED message completed, restore normal state */
|
||||
control_status_leds(&status, &armed, true);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* normal state */
|
||||
control_status_leds(&status, &armed, status_changed);
|
||||
|
@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
ret = pthread_join(commander_low_prio_thread, NULL);
|
||||
|
||||
if (ret) {
|
||||
warn("join failed", ret);
|
||||
warn("join failed: %d", ret);
|
||||
}
|
||||
|
||||
rgbled_set_mode(RGBLED_MODE_OFF);
|
||||
|
@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
|
|||
/* driving rgbled */
|
||||
if (changed) {
|
||||
bool set_normal_color = false;
|
||||
|
||||
/* set mode */
|
||||
if (status->arming_state == ARMING_STATE_ARMED) {
|
||||
rgbled_set_mode(RGBLED_MODE_ON);
|
||||
|
@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
|
|||
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
|
||||
rgbled_set_color(RGBLED_COLOR_AMBER);
|
||||
}
|
||||
|
||||
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
|
||||
|
||||
} else {
|
||||
|
@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg)
|
|||
fds[0].events = POLLIN;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* wait for up to 100ms for data */
|
||||
/* wait for up to 200ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
|
||||
|
||||
/* timed out - periodic check for _task_should_exit, etc. */
|
||||
/* timed out - periodic check for thread_should_exit, etc. */
|
||||
if (pret == 0)
|
||||
continue;
|
||||
|
||||
|
@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
|
||||
} else if ((int)(cmd.param4) == 1) {
|
||||
/* RC calibration */
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
calib_ret = do_rc_calibration(mavlink_fd);
|
||||
|
||||
} else if ((int)(cmd.param5) == 1) {
|
||||
|
@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg)
|
|||
/* send acknowledge command */
|
||||
// XXX TODO
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
close(cmd_sub);
|
||||
|
|
|
@ -33,10 +33,12 @@
|
|||
|
||||
/**
|
||||
* @file gyro_calibration.cpp
|
||||
*
|
||||
* Gyroscope calibration routine
|
||||
*/
|
||||
|
||||
#include "gyro_calibration.h"
|
||||
#include "calibration_messages.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
@ -56,21 +58,14 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
static const char *sensor_name = "gyro";
|
||||
|
||||
int do_gyro_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
|
||||
const unsigned calibration_count = 5000;
|
||||
|
||||
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
unsigned calibration_counter = 0;
|
||||
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* set offsets to zero */
|
||||
int fd = open(GYRO_DEVICE_PATH, 0);
|
||||
struct gyro_scale gscale_null = {
|
||||
struct gyro_scale gyro_scale = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
|
@ -79,98 +74,101 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
|
||||
warn("WARNING: failed to set scale / offsets for gyro");
|
||||
int res = OK;
|
||||
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
int fd = open(GYRO_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* determine gyro mean values */
|
||||
const unsigned calibration_count = 5000;
|
||||
unsigned calibration_counter = 0;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
/* subscribe to gyro sensor topic */
|
||||
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_sensor_combined;
|
||||
fds[0].fd = sub_sensor_gyro;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
gyro_offset[0] += raw.gyro_rad_s[0];
|
||||
gyro_offset[1] += raw.gyro_rad_s[1];
|
||||
gyro_offset[2] += raw.gyro_rad_s[2];
|
||||
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
|
||||
gyro_scale.x_offset += gyro_report.x;
|
||||
gyro_scale.y_offset += gyro_report.y;
|
||||
gyro_scale.z_offset += gyro_report.z;
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_count / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
|
||||
close(sub_sensor_combined);
|
||||
return ERROR;
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
gyro_offset[0] = gyro_offset[0] / calibration_count;
|
||||
gyro_offset[1] = gyro_offset[1] / calibration_count;
|
||||
gyro_offset[2] = gyro_offset[2] / calibration_count;
|
||||
close(sub_sensor_gyro);
|
||||
|
||||
|
||||
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
|
||||
|
||||
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|
||||
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|
||||
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
|
||||
gyro_scale.x_offset /= calibration_count;
|
||||
gyro_scale.y_offset /= calibration_count;
|
||||
gyro_scale.z_offset /= calibration_count;
|
||||
}
|
||||
|
||||
/* set offsets to actual value */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
struct gyro_scale gscale = {
|
||||
gyro_offset[0],
|
||||
1.0f,
|
||||
gyro_offset[1],
|
||||
1.0f,
|
||||
gyro_offset[2],
|
||||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
|
||||
warn("WARNING: failed to set scale / offsets for gyro");
|
||||
|
||||
close(fd);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warnx("WARNING: auto-save of params to storage failed");
|
||||
mavlink_log_critical(mavlink_fd, "gyro store failed");
|
||||
close(sub_sensor_combined);
|
||||
return ERROR;
|
||||
if (res == OK) {
|
||||
/* check offsets */
|
||||
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
tune_neutral();
|
||||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
|
||||
close(sub_sensor_combined);
|
||||
return ERROR;
|
||||
if (res == OK) {
|
||||
/* set offset parameters to new values */
|
||||
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|
||||
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|
||||
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "offset calibration done.");
|
||||
|
||||
#if 0
|
||||
/*** --- SCALING --- ***/
|
||||
/* beep on offset calibration end */
|
||||
mavlink_log_info(mavlink_fd, "gyro offset calibration done");
|
||||
tune_neutral();
|
||||
|
||||
/* scale calibration */
|
||||
/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
|
||||
|
||||
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
|
||||
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
|
||||
|
||||
/* apply new offsets */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
|
||||
warn("WARNING: failed to apply new offsets for gyro");
|
||||
|
||||
close(fd);
|
||||
|
||||
|
||||
unsigned rotations_count = 30;
|
||||
float gyro_integral = 0.0f;
|
||||
float baseline_integral = 0.0f;
|
||||
|
@ -178,9 +176,11 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
// XXX change to mag topic
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
|
||||
float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
|
||||
if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
|
||||
if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
|
||||
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
|
||||
|
||||
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
|
||||
|
||||
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
|
||||
|
||||
|
||||
uint64_t last_time = hrt_absolute_time();
|
||||
|
@ -218,14 +218,17 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
// calculate error between estimate and measurement
|
||||
// apply declination correction for true heading as well.
|
||||
//float mag = -atan2f(magNav(1),magNav(0));
|
||||
float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
|
||||
if (mag > M_PI_F) mag -= 2*M_PI_F;
|
||||
if (mag < -M_PI_F) mag += 2*M_PI_F;
|
||||
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
|
||||
|
||||
if (mag > M_PI_F) mag -= 2 * M_PI_F;
|
||||
|
||||
if (mag < -M_PI_F) mag += 2 * M_PI_F;
|
||||
|
||||
float diff = mag - mag_last;
|
||||
|
||||
if (diff > M_PI_F) diff -= 2*M_PI_F;
|
||||
if (diff < -M_PI_F) diff += 2*M_PI_F;
|
||||
if (diff > M_PI_F) diff -= 2 * M_PI_F;
|
||||
|
||||
if (diff < -M_PI_F) diff += 2 * M_PI_F;
|
||||
|
||||
baseline_integral += diff;
|
||||
mag_last = mag;
|
||||
|
@ -246,52 +249,57 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
|
||||
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
|
||||
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
|
||||
#else
|
||||
float gyro_scales[] = { 1.0f, 1.0f, 1.0f };
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
|
||||
|
||||
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
|
||||
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
|
||||
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
|
||||
}
|
||||
|
||||
/* set offsets to actual value */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
struct gyro_scale gscale = {
|
||||
gyro_offset[0],
|
||||
gyro_scales[0],
|
||||
gyro_offset[1],
|
||||
gyro_scales[1],
|
||||
gyro_offset[2],
|
||||
gyro_scales[2],
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
|
||||
warn("WARNING: failed to set scale / offsets for gyro");
|
||||
|
||||
close(fd);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration done");
|
||||
|
||||
/* third beep by cal end routine */
|
||||
close(sub_sensor_combined);
|
||||
return OK;
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
|
||||
close(sub_sensor_combined);
|
||||
if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
|
||||
mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
|
||||
close(sub_sensor_gyro);
|
||||
mavlink_log_critical(mavlink_fd, "gyro calibration failed");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* beep on calibration end */
|
||||
mavlink_log_info(mavlink_fd, "gyro scale calibration done");
|
||||
tune_neutral();
|
||||
|
||||
#endif
|
||||
|
||||
if (res == OK) {
|
||||
/* set scale parameters to new values */
|
||||
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|
||||
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|
||||
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* apply new scaling and offsets */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
res = param_save_default();
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
|
|
@ -33,12 +33,14 @@
|
|||
|
||||
/**
|
||||
* @file mag_calibration.cpp
|
||||
*
|
||||
* Magnetometer calibration routine
|
||||
*/
|
||||
|
||||
#include "mag_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
#include "calibration_routines.h"
|
||||
#include "calibration_messages.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
@ -59,26 +61,20 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
static const char *sensor_name = "mag";
|
||||
|
||||
int do_mag_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
|
||||
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||
struct mag_report mag;
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
|
||||
/* 45 seconds */
|
||||
uint64_t calibration_interval = 45 * 1000 * 1000;
|
||||
|
||||
/* maximum 2000 values */
|
||||
/* maximum 500 values */
|
||||
const unsigned int calibration_maxcount = 500;
|
||||
unsigned int calibration_counter = 0;
|
||||
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
/* erase old calibration */
|
||||
struct mag_scale mscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
|
@ -88,45 +84,58 @@ int do_mag_calibration(int mavlink_fd)
|
|||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
|
||||
int res = OK;
|
||||
|
||||
/* erase old calibration */
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* calibrate range */
|
||||
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
|
||||
warnx("failed to calibrate scale");
|
||||
res = ioctl(fd, MAGIOCCALIBRATE, fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
|
||||
float *x;
|
||||
float *y;
|
||||
float *z;
|
||||
|
||||
/* calibrate offsets */
|
||||
if (res == OK) {
|
||||
/* allocate memory */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
|
||||
|
||||
// uint64_t calibration_start = hrt_absolute_time();
|
||||
|
||||
uint64_t axis_deadline = hrt_absolute_time();
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
|
||||
const char axislabels[3] = { 'X', 'Y', 'Z'};
|
||||
int axis_index = -1;
|
||||
|
||||
float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
warnx("mag cal failed: out of memory");
|
||||
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
|
||||
warnx("x:%p y:%p z:%p\n", x, y, z);
|
||||
return ERROR;
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
|
||||
if (res == OK) {
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||
struct mag_report mag;
|
||||
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
/* calibrate offsets */
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
|
||||
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
|
||||
|
@ -135,22 +144,6 @@ int do_mag_calibration(int mavlink_fd)
|
|||
fds[0].fd = sub_mag;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
/* user guidance */
|
||||
if (hrt_absolute_time() >= axis_deadline &&
|
||||
axis_index < 3) {
|
||||
|
||||
axis_index++;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
|
||||
tune_neutral();
|
||||
|
||||
axis_deadline += calibration_interval / 3;
|
||||
}
|
||||
|
||||
if (!(axis_index < 3)) {
|
||||
break;
|
||||
}
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
|
@ -161,24 +154,22 @@ int do_mag_calibration(int mavlink_fd)
|
|||
z[calibration_counter] = mag.z;
|
||||
|
||||
calibration_counter++;
|
||||
if (calibration_counter % (calibration_maxcount / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
|
||||
|
||||
if (calibration_counter % (calibration_maxcount / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
|
||||
close(sub_mag);
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
return ERROR;
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
close(sub_mag);
|
||||
}
|
||||
|
||||
float sphere_x;
|
||||
|
@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd)
|
|||
float sphere_z;
|
||||
float sphere_radius;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
|
||||
if (res == OK) {
|
||||
/* sphere fit */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
|
||||
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
|
||||
|
||||
if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (x != NULL)
|
||||
free(x);
|
||||
|
||||
if (y != NULL)
|
||||
free(y);
|
||||
|
||||
if (z != NULL)
|
||||
free(z);
|
||||
|
||||
if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
|
||||
if (res == OK) {
|
||||
/* apply calibration and set parameters */
|
||||
struct mag_scale mscale;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to get scale / offsets for mag");
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
mscale.x_offset = sphere_x;
|
||||
mscale.y_offset = sphere_y;
|
||||
mscale.z_offset = sphere_z;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
/* announce and set new offset */
|
||||
if (res == OK) {
|
||||
/* set parameters */
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||
warnx("Setting X mag offset failed!\n");
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
|
||||
res = ERROR;
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
|
||||
warnx("Setting Y mag offset failed!\n");
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
|
||||
warnx("Setting Z mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
|
||||
warnx("Setting X mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
|
||||
warnx("Setting Y mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
|
||||
warnx("Setting Z mag scale failed!\n");
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
|
||||
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
res = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
|
||||
close(sub_mag);
|
||||
return ERROR;
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
|
||||
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
|
||||
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
|
||||
|
||||
char buf[52];
|
||||
sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
|
||||
mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
|
||||
(double)mscale.y_offset, (double)mscale.z_offset);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
|
||||
mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
|
||||
(double)mscale.y_scale, (double)mscale.z_scale);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
|
||||
|
||||
close(sub_mag);
|
||||
return OK;
|
||||
/* third beep by cal end routine */
|
||||
if (res == OK) {
|
||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
|
||||
close(sub_mag);
|
||||
return ERROR;
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -47,4 +47,3 @@ SRCS = commander.cpp \
|
|||
baro_calibration.cpp \
|
||||
rc_calibration.cpp \
|
||||
airspeed_calibration.cpp
|
||||
|
||||
|
|
|
@ -683,6 +683,10 @@ FixedwingAttitudeControl::task_main()
|
|||
_actuators.control[4] = _manual.flaps;
|
||||
}
|
||||
|
||||
_actuators.control[5] = _manual.aux1;
|
||||
_actuators.control[6] = _manual.aux2;
|
||||
_actuators.control[7] = _manual.aux3;
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
|
||||
|
|
|
@ -192,6 +192,7 @@ private:
|
|||
|
||||
float pitch_limit_min;
|
||||
float pitch_limit_max;
|
||||
float roll_limit;
|
||||
float throttle_min;
|
||||
float throttle_max;
|
||||
float throttle_cruise;
|
||||
|
@ -225,6 +226,7 @@ private:
|
|||
|
||||
param_t pitch_limit_min;
|
||||
param_t pitch_limit_max;
|
||||
param_t roll_limit;
|
||||
param_t throttle_min;
|
||||
param_t throttle_max;
|
||||
param_t throttle_cruise;
|
||||
|
@ -343,6 +345,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
|
||||
_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
|
||||
_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
|
||||
_parameter_handles.roll_limit = param_find("FW_R_LIM");
|
||||
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
|
||||
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
|
||||
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
|
||||
|
@ -405,6 +408,7 @@ FixedwingPositionControl::parameters_update()
|
|||
|
||||
param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
|
||||
param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
|
||||
param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
|
||||
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
|
||||
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
|
||||
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
|
||||
|
@ -426,6 +430,7 @@ FixedwingPositionControl::parameters_update()
|
|||
|
||||
_l1_control.set_l1_damping(_parameters.l1_damping);
|
||||
_l1_control.set_l1_period(_parameters.l1_period);
|
||||
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
|
||||
|
||||
_tecs.set_time_const(_parameters.time_const);
|
||||
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
|
||||
|
|
|
@ -67,6 +67,9 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
|
|||
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
|
||||
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
||||
|
||||
|
||||
|
|
|
@ -422,13 +422,13 @@ handle_message(mavlink_message_t *msg)
|
|||
hil_sensors.accelerometer_counter = hil_counter;
|
||||
|
||||
/* differential pressure */
|
||||
hil_sensors.differential_pressure_pa = imu.diff_pressure;
|
||||
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
|
||||
hil_sensors.differential_pressure_counter = hil_counter;
|
||||
|
||||
/* airspeed from differential pressure, ambient pressure and temp */
|
||||
struct airspeed_s airspeed;
|
||||
|
||||
float ias = calc_indicated_airspeed(imu.diff_pressure);
|
||||
float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
|
||||
// XXX need to fix this
|
||||
float tas = ias;
|
||||
|
||||
|
|
|
@ -368,6 +368,12 @@ mc_thread_main(int argc, char *argv[])
|
|||
actuators.control[3] = 0.0f;
|
||||
}
|
||||
|
||||
/* fill in manual control values */
|
||||
actuators.control[4] = manual.flaps;
|
||||
actuators.control[5] = manual.aux1;
|
||||
actuators.control[6] = manual.aux2;
|
||||
actuators.control[7] = manual.aux3;
|
||||
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
|
|
|
@ -58,6 +58,7 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <unistd.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
@ -84,12 +85,14 @@
|
|||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "logbuffer.h"
|
||||
#include "sdlog2_format.h"
|
||||
#include "sdlog2_messages.h"
|
||||
#include "sdlog2_version.h"
|
||||
|
||||
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
|
||||
log_msgs_written++; \
|
||||
|
@ -180,8 +183,17 @@ static void sdlog2_stop_log(void);
|
|||
/**
|
||||
* Write a header to log file: list of message formats.
|
||||
*/
|
||||
static void write_formats(int fd);
|
||||
static int write_formats(int fd);
|
||||
|
||||
/**
|
||||
* Write version message to log file.
|
||||
*/
|
||||
static int write_version(int fd);
|
||||
|
||||
/**
|
||||
* Write parameters to log file.
|
||||
*/
|
||||
static int write_parameters(int fd);
|
||||
|
||||
static bool file_exist(const char *filename);
|
||||
|
||||
|
@ -354,10 +366,13 @@ static void *logwriter_thread(void *arg)
|
|||
|
||||
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
|
||||
|
||||
int log_file = open_logfile();
|
||||
int log_fd = open_logfile();
|
||||
|
||||
/* write log messages formats */
|
||||
write_formats(log_file);
|
||||
/* write log messages formats, version and parameters */
|
||||
log_bytes_written += write_formats(log_fd);
|
||||
log_bytes_written += write_version(log_fd);
|
||||
log_bytes_written += write_parameters(log_fd);
|
||||
fsync(log_fd);
|
||||
|
||||
int poll_count = 0;
|
||||
|
||||
|
@ -396,7 +411,7 @@ static void *logwriter_thread(void *arg)
|
|||
n = available;
|
||||
}
|
||||
|
||||
n = write(log_file, read_ptr, n);
|
||||
n = write(log_fd, read_ptr, n);
|
||||
|
||||
should_wait = (n == available) && !is_part;
|
||||
|
||||
|
@ -411,21 +426,23 @@ static void *logwriter_thread(void *arg)
|
|||
|
||||
} else {
|
||||
n = 0;
|
||||
|
||||
/* exit only with empty buffer */
|
||||
if (main_thread_should_exit || logwriter_should_exit) {
|
||||
break;
|
||||
}
|
||||
|
||||
should_wait = true;
|
||||
}
|
||||
|
||||
if (++poll_count == 10) {
|
||||
fsync(log_file);
|
||||
fsync(log_fd);
|
||||
poll_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
fsync(log_file);
|
||||
close(log_file);
|
||||
fsync(log_fd);
|
||||
close(log_fd);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
@ -479,34 +496,92 @@ void sdlog2_stop_log()
|
|||
|
||||
/* wait for write thread to return */
|
||||
int ret;
|
||||
|
||||
if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
|
||||
warnx("error joining logwriter thread: %i", ret);
|
||||
}
|
||||
|
||||
logwriter_pthread = 0;
|
||||
|
||||
sdlog2_status();
|
||||
}
|
||||
|
||||
|
||||
void write_formats(int fd)
|
||||
int write_formats(int fd)
|
||||
{
|
||||
/* construct message format packet */
|
||||
struct {
|
||||
LOG_PACKET_HEADER;
|
||||
struct log_format_s body;
|
||||
} log_format_packet = {
|
||||
} log_msg_format = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
|
||||
};
|
||||
|
||||
/* fill message format packet for each format and write to log */
|
||||
int i;
|
||||
int written = 0;
|
||||
|
||||
for (i = 0; i < log_formats_num; i++) {
|
||||
log_format_packet.body = log_formats[i];
|
||||
log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet));
|
||||
/* fill message format packet for each format and write it */
|
||||
for (int i = 0; i < log_formats_num; i++) {
|
||||
log_msg_format.body = log_formats[i];
|
||||
written += write(fd, &log_msg_format, sizeof(log_msg_format));
|
||||
}
|
||||
|
||||
fsync(fd);
|
||||
return written;
|
||||
}
|
||||
|
||||
int write_version(int fd)
|
||||
{
|
||||
/* construct version message */
|
||||
struct {
|
||||
LOG_PACKET_HEADER;
|
||||
struct log_VER_s body;
|
||||
} log_msg_VER = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
|
||||
};
|
||||
|
||||
/* fill version message and write it */
|
||||
strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
|
||||
strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
|
||||
return write(fd, &log_msg_VER, sizeof(log_msg_VER));
|
||||
}
|
||||
|
||||
int write_parameters(int fd)
|
||||
{
|
||||
/* construct parameter message */
|
||||
struct {
|
||||
LOG_PACKET_HEADER;
|
||||
struct log_PARM_s body;
|
||||
} log_msg_PARM = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
|
||||
};
|
||||
|
||||
int written = 0;
|
||||
param_t params_cnt = param_count();
|
||||
|
||||
for (param_t param = 0; param < params_cnt; param++) {
|
||||
/* fill parameter message and write it */
|
||||
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
|
||||
float value = NAN;
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_INT32: {
|
||||
int32_t i;
|
||||
param_get(param, &i);
|
||||
value = i; // cast integer to float
|
||||
break;
|
||||
}
|
||||
|
||||
case PARAM_TYPE_FLOAT:
|
||||
param_get(param, &value);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
log_msg_PARM.body.value = value;
|
||||
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
|
||||
}
|
||||
|
||||
return written;
|
||||
}
|
||||
|
||||
int sdlog2_thread_main(int argc, char *argv[])
|
||||
|
@ -584,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
const char *converter_in = "/etc/logging/conv.zip";
|
||||
char* converter_out = malloc(120);
|
||||
char *converter_out = malloc(120);
|
||||
sprintf(converter_out, "%s/conv.zip", folder_path);
|
||||
|
||||
if (file_copy(converter_in, converter_out)) {
|
||||
errx(1, "unable to copy conversion scripts, exiting.");
|
||||
}
|
||||
|
||||
free(converter_out);
|
||||
|
||||
/* only print logging path, important to find log file later */
|
||||
|
@ -603,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
struct vehicle_status_s buf_status;
|
||||
|
||||
memset(&buf_status, 0, sizeof(buf_status));
|
||||
|
||||
/* warning! using union here to save memory, elements should be used separately! */
|
||||
|
@ -628,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct esc_status_s esc;
|
||||
struct vehicle_global_velocity_setpoint_s global_vel_sp;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
|
||||
struct {
|
||||
|
@ -886,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
continue;
|
||||
}
|
||||
|
||||
ifds = 1; // Begin from fds[1] again
|
||||
ifds = 1; // begin from fds[1] again
|
||||
|
||||
pthread_mutex_lock(&logbuffer_mutex);
|
||||
|
||||
|
@ -1145,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
/* --- ESCs --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
|
||||
for (uint8_t i=0; i<buf.esc.esc_count; i++)
|
||||
{
|
||||
|
||||
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
|
||||
log_msg.msg_type = LOG_ESC_MSG;
|
||||
log_msg.body.log_ESC.counter = buf.esc.counter;
|
||||
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
|
||||
|
@ -1294,6 +1372,7 @@ void handle_status(struct vehicle_status_s *status)
|
|||
{
|
||||
// TODO use flag from actuator_armed here?
|
||||
bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
|
||||
|
||||
if (armed != flag_system_armed) {
|
||||
flag_system_armed = armed;
|
||||
|
||||
|
|
|
@ -48,12 +48,6 @@
|
|||
/* define message formats */
|
||||
|
||||
#pragma pack(push, 1)
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
#define LOG_TIME_MSG 1
|
||||
struct log_TIME_s {
|
||||
uint64_t t;
|
||||
};
|
||||
|
||||
/* --- ATT - ATTITUDE --- */
|
||||
#define LOG_ATT_MSG 2
|
||||
struct log_ATT_s {
|
||||
|
@ -253,30 +247,31 @@ struct log_GVSP_s {
|
|||
float vz;
|
||||
};
|
||||
|
||||
/* --- FWRV - FIRMWARE REVISION --- */
|
||||
#define LOG_FWRV_MSG 20
|
||||
struct log_FWRV_s {
|
||||
char fw_revision[64];
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
#define LOG_TIME_MSG 129
|
||||
struct log_TIME_s {
|
||||
uint64_t t;
|
||||
};
|
||||
|
||||
/* --- VER - VERSION --- */
|
||||
#define LOG_VER_MSG 130
|
||||
struct log_VER_s {
|
||||
char arch[16];
|
||||
char fw_git[64];
|
||||
};
|
||||
|
||||
/* --- PARM - PARAMETER --- */
|
||||
#define LOG_PARM_MSG 131
|
||||
struct log_PARM_s {
|
||||
char name[16];
|
||||
float value;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
|
||||
/*
|
||||
GIT_VERSION is defined at build time via a Makefile call to the
|
||||
git command line. We create a fake log message format for
|
||||
the firmware revision "FWRV" that is written to every log
|
||||
header. This makes it easier to determine which version
|
||||
of the firmware was running when a log was created.
|
||||
*/
|
||||
#define FREEZE_STR(s) #s
|
||||
#define STRINGIFY(s) FREEZE_STR(s)
|
||||
#define FW_VERSION_STR STRINGIFY(GIT_VERSION)
|
||||
|
||||
/* construct list of all message formats */
|
||||
|
||||
static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(TIME, "Q", "StartTime"),
|
||||
/* business-level messages, ID < 0x80 */
|
||||
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
|
||||
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
|
||||
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
|
@ -295,7 +290,11 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(FWRV,"Z",FW_VERSION_STR),
|
||||
/* system-level messages, ID >= 0x80 */
|
||||
// FMT: don't write format of format message, it's useless
|
||||
LOG_FORMAT(TIME, "Q", "StartTime"),
|
||||
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
|
||||
LOG_FORMAT(PARM, "Nf", "Name,Value"),
|
||||
};
|
||||
|
||||
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
|
||||
|
|
|
@ -0,0 +1,62 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 sdlog2_version.h
|
||||
*
|
||||
* Tools for system version detection.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef SDLOG2_VERSION_H_
|
||||
#define SDLOG2_VERSION_H_
|
||||
|
||||
/*
|
||||
GIT_VERSION is defined at build time via a Makefile call to the
|
||||
git command line.
|
||||
*/
|
||||
#define FREEZE_STR(s) #s
|
||||
#define STRINGIFY(s) FREEZE_STR(s)
|
||||
#define FW_GIT STRINGIFY(GIT_VERSION)
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
#define HW_ARCH "PX4FMU_V1"
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
#define HW_ARCH "PX4FMU_V2"
|
||||
#endif
|
||||
|
||||
#endif /* SDLOG2_VERSION_H_ */
|
|
@ -26,7 +26,7 @@ void BlockSegwayController::update() {
|
|||
_actuators.control[i] = 0.0f;
|
||||
|
||||
// only update guidance in auto mode
|
||||
if (_status.state_machine == SYSTEM_STATE_AUTO) {
|
||||
if (_status.main_state == MAIN_STATE_AUTO) {
|
||||
// update guidance
|
||||
}
|
||||
|
||||
|
@ -34,17 +34,18 @@ void BlockSegwayController::update() {
|
|||
float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
|
||||
|
||||
// handle autopilot modes
|
||||
if (_status.state_machine == SYSTEM_STATE_AUTO ||
|
||||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||
if (_status.main_state == MAIN_STATE_AUTO ||
|
||||
_status.main_state == MAIN_STATE_SEATBELT ||
|
||||
_status.main_state == MAIN_STATE_EASY) {
|
||||
_actuators.control[0] = spdCmd;
|
||||
_actuators.control[1] = spdCmd;
|
||||
|
||||
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
|
||||
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
} else if (_status.main_state == MAIN_STATE_MANUAL) {
|
||||
if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
|
||||
_actuators.control[CH_LEFT] = _manual.throttle;
|
||||
_actuators.control[CH_RIGHT] = _manual.pitch;
|
||||
|
||||
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
|
||||
_actuators.control[0] = spdCmd;
|
||||
_actuators.control[1] = spdCmd;
|
||||
}
|
||||
|
|
|
@ -44,8 +44,34 @@
|
|||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Gyro X offset FIXME
|
||||
*
|
||||
* This is an X-axis offset for the gyro.
|
||||
* Adjust it according to the calibration data.
|
||||
*
|
||||
* @min -10.0
|
||||
* @max 10.0
|
||||
* @group Gyro Config
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
|
||||
|
||||
/**
|
||||
* Gyro Y offset FIXME with dot.
|
||||
*
|
||||
* @min -10.0
|
||||
* @max 10.0
|
||||
* @group Gyro Config
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
|
||||
|
||||
/**
|
||||
* Gyro Z offset FIXME
|
||||
*
|
||||
* @min -5.0
|
||||
* @max 5.0
|
||||
* @group Gyro Config
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
|
||||
|
@ -193,8 +219,9 @@ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
|
|||
|
||||
PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
|
||||
|
||||
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */
|
||||
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */
|
||||
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */
|
||||
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
|
||||
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
|
||||
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
|
||||
|
|
|
@ -67,6 +67,7 @@
|
|||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <conversion/rotation.h>
|
||||
|
||||
#include <systemlib/airspeed.h>
|
||||
|
||||
|
@ -74,6 +75,7 @@
|
|||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
@ -135,75 +137,6 @@
|
|||
|
||||
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
|
||||
|
||||
/**
|
||||
* Enum for board and external compass rotations.
|
||||
* This enum maps from board attitude to airframe attitude.
|
||||
*/
|
||||
enum Rotation {
|
||||
ROTATION_NONE = 0,
|
||||
ROTATION_YAW_45 = 1,
|
||||
ROTATION_YAW_90 = 2,
|
||||
ROTATION_YAW_135 = 3,
|
||||
ROTATION_YAW_180 = 4,
|
||||
ROTATION_YAW_225 = 5,
|
||||
ROTATION_YAW_270 = 6,
|
||||
ROTATION_YAW_315 = 7,
|
||||
ROTATION_ROLL_180 = 8,
|
||||
ROTATION_ROLL_180_YAW_45 = 9,
|
||||
ROTATION_ROLL_180_YAW_90 = 10,
|
||||
ROTATION_ROLL_180_YAW_135 = 11,
|
||||
ROTATION_PITCH_180 = 12,
|
||||
ROTATION_ROLL_180_YAW_225 = 13,
|
||||
ROTATION_ROLL_180_YAW_270 = 14,
|
||||
ROTATION_ROLL_180_YAW_315 = 15,
|
||||
ROTATION_ROLL_90 = 16,
|
||||
ROTATION_ROLL_90_YAW_45 = 17,
|
||||
ROTATION_ROLL_90_YAW_90 = 18,
|
||||
ROTATION_ROLL_90_YAW_135 = 19,
|
||||
ROTATION_ROLL_270 = 20,
|
||||
ROTATION_ROLL_270_YAW_45 = 21,
|
||||
ROTATION_ROLL_270_YAW_90 = 22,
|
||||
ROTATION_ROLL_270_YAW_135 = 23,
|
||||
ROTATION_PITCH_90 = 24,
|
||||
ROTATION_PITCH_270 = 25,
|
||||
ROTATION_MAX
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
uint16_t roll;
|
||||
uint16_t pitch;
|
||||
uint16_t yaw;
|
||||
} rot_lookup_t;
|
||||
|
||||
const rot_lookup_t rot_lookup[] = {
|
||||
{ 0, 0, 0 },
|
||||
{ 0, 0, 45 },
|
||||
{ 0, 0, 90 },
|
||||
{ 0, 0, 135 },
|
||||
{ 0, 0, 180 },
|
||||
{ 0, 0, 225 },
|
||||
{ 0, 0, 270 },
|
||||
{ 0, 0, 315 },
|
||||
{180, 0, 0 },
|
||||
{180, 0, 45 },
|
||||
{180, 0, 90 },
|
||||
{180, 0, 135 },
|
||||
{ 0, 180, 0 },
|
||||
{180, 0, 225 },
|
||||
{180, 0, 270 },
|
||||
{180, 0, 315 },
|
||||
{ 90, 0, 0 },
|
||||
{ 90, 0, 45 },
|
||||
{ 90, 0, 90 },
|
||||
{ 90, 0, 135 },
|
||||
{270, 0, 0 },
|
||||
{270, 0, 45 },
|
||||
{270, 0, 90 },
|
||||
{270, 0, 135 },
|
||||
{ 0, 90, 0 },
|
||||
{ 0, 270, 0 }
|
||||
};
|
||||
|
||||
/**
|
||||
* Sensor app start / stop handling function
|
||||
*
|
||||
|
@ -264,6 +197,7 @@ private:
|
|||
|
||||
orb_advert_t _sensor_pub; /**< combined sensor data topic */
|
||||
orb_advert_t _manual_control_pub; /**< manual control signal topic */
|
||||
orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */
|
||||
orb_advert_t _rc_pub; /**< raw r/c control topic */
|
||||
orb_advert_t _battery_pub; /**< battery status */
|
||||
orb_advert_t _airspeed_pub; /**< airspeed */
|
||||
|
@ -384,11 +318,6 @@ private:
|
|||
*/
|
||||
int parameters_update();
|
||||
|
||||
/**
|
||||
* Get the rotation matrices
|
||||
*/
|
||||
void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
|
||||
|
||||
/**
|
||||
* Do accel-related initialisation.
|
||||
*/
|
||||
|
@ -519,6 +448,7 @@ Sensors::Sensors() :
|
|||
/* publications */
|
||||
_sensor_pub(-1),
|
||||
_manual_control_pub(-1),
|
||||
_actuator_group_3_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_battery_pub(-1),
|
||||
_airspeed_pub(-1),
|
||||
|
@ -803,24 +733,6 @@ Sensors::parameters_update()
|
|||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *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);
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::accel_init()
|
||||
{
|
||||
|
@ -1318,6 +1230,7 @@ Sensors::rc_poll()
|
|||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
struct actuator_controls_s actuator_group_3;
|
||||
|
||||
/* initialize to default values */
|
||||
manual_control.roll = NAN;
|
||||
|
@ -1485,6 +1398,16 @@ Sensors::rc_poll()
|
|||
manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
|
||||
}
|
||||
|
||||
/* copy from mapped manual control to control group 3 */
|
||||
actuator_group_3.control[0] = manual_control.roll;
|
||||
actuator_group_3.control[1] = manual_control.pitch;
|
||||
actuator_group_3.control[2] = manual_control.yaw;
|
||||
actuator_group_3.control[3] = manual_control.throttle;
|
||||
actuator_group_3.control[4] = manual_control.flaps;
|
||||
actuator_group_3.control[5] = manual_control.aux1;
|
||||
actuator_group_3.control[6] = manual_control.aux2;
|
||||
actuator_group_3.control[7] = manual_control.aux3;
|
||||
|
||||
/* check if ready for publishing */
|
||||
if (_rc_pub > 0) {
|
||||
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
|
||||
|
@ -1501,6 +1424,14 @@ Sensors::rc_poll()
|
|||
} else {
|
||||
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
|
||||
}
|
||||
|
||||
/* check if ready for publishing */
|
||||
if (_actuator_group_3_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
|
||||
|
||||
} else {
|
||||
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -508,64 +508,63 @@ param_get_default_file(void)
|
|||
int
|
||||
param_save_default(void)
|
||||
{
|
||||
int result;
|
||||
unsigned retries = 0;
|
||||
|
||||
/* delete the file in case it exists */
|
||||
struct stat buffer;
|
||||
if (stat(param_get_default_file(), &buffer) == 0) {
|
||||
|
||||
do {
|
||||
result = unlink(param_get_default_file());
|
||||
if (result != 0) {
|
||||
retries++;
|
||||
usleep(1000 * retries);
|
||||
}
|
||||
} while (result != OK && retries < 10);
|
||||
|
||||
if (result != OK)
|
||||
warnx("unlinking file %s failed.", param_get_default_file());
|
||||
}
|
||||
|
||||
/* create the file */
|
||||
int res;
|
||||
int fd;
|
||||
|
||||
do {
|
||||
/* do another attempt in case the unlink call is not synced yet */
|
||||
fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
|
||||
if (fd < 0) {
|
||||
retries++;
|
||||
usleep(1000 * retries);
|
||||
}
|
||||
const char *filename = param_get_default_file();
|
||||
const char *filename_tmp = malloc(strlen(filename) + 5);
|
||||
sprintf(filename_tmp, "%s.tmp", filename);
|
||||
|
||||
} while (fd < 0 && retries < 10);
|
||||
/* delete temp file if exist */
|
||||
res = unlink(filename_tmp);
|
||||
|
||||
if (res != OK && errno == ENOENT)
|
||||
res = OK;
|
||||
|
||||
if (res != OK)
|
||||
warn("failed to delete temp file: %s", filename_tmp);
|
||||
|
||||
if (res == OK) {
|
||||
/* write parameters to temp file */
|
||||
fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
|
||||
|
||||
if (fd < 0) {
|
||||
|
||||
warn("opening '%s' for writing failed", param_get_default_file());
|
||||
return fd;
|
||||
warn("failed to open temp file: %s", filename_tmp);
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
do {
|
||||
result = param_export(fd, false);
|
||||
if (res == OK) {
|
||||
res = param_export(fd, false);
|
||||
|
||||
if (result != OK) {
|
||||
retries++;
|
||||
usleep(1000 * retries);
|
||||
if (res != OK)
|
||||
warnx("failed to write parameters to file: %s", filename_tmp);
|
||||
}
|
||||
|
||||
} while (result != 0 && retries < 10);
|
||||
|
||||
|
||||
close(fd);
|
||||
|
||||
if (result != OK) {
|
||||
warn("error exporting parameters to '%s'", param_get_default_file());
|
||||
(void)unlink(param_get_default_file());
|
||||
return result;
|
||||
}
|
||||
|
||||
return 0;
|
||||
if (res == OK) {
|
||||
/* delete parameters file */
|
||||
res = unlink(filename);
|
||||
|
||||
if (res != OK && errno == ENOENT)
|
||||
res = OK;
|
||||
|
||||
if (res != OK)
|
||||
warn("failed to delete parameters file: %s", filename);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* rename temp file to parameters */
|
||||
res = rename(filename_tmp, filename);
|
||||
|
||||
if (res != OK)
|
||||
warn("failed to rename %s to %s", filename_tmp, filename);
|
||||
}
|
||||
|
||||
free(filename_tmp);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -47,14 +47,12 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
|
||||
int rc_calibration_check(void) {
|
||||
int rc_calibration_check(int mavlink_fd) {
|
||||
|
||||
char nbuf[20];
|
||||
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
|
||||
_parameter_handles_rev, _parameter_handles_dz;
|
||||
|
||||
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
float param_min, param_max, param_trim, param_rev, param_dz;
|
||||
|
||||
/* first check channel mappings */
|
||||
|
|
|
@ -47,6 +47,6 @@
|
|||
* @return 0 / OK if RC calibration is ok, index + 1 of the first
|
||||
* channel that failed else (so 1 == first channel failed)
|
||||
*/
|
||||
__EXPORT int rc_calibration_check(void);
|
||||
__EXPORT int rc_calibration_check(int mavlink_fd);
|
||||
|
||||
__END_DECLS
|
||||
|
|
|
@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[])
|
|||
|
||||
/* ---- RC CALIBRATION ---- */
|
||||
|
||||
bool rc_ok = (OK == rc_calibration_check());
|
||||
bool rc_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
|
||||
/* warn */
|
||||
if (!rc_ok)
|
||||
|
|
Loading…
Reference in New Issue