diff options
220 files changed, 10630 insertions, 9642 deletions
diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 241702811..b674fbc48 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -599,7 +599,7 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../src/modules/mathlib/CMSIS \ +EXCLUDE = ../src/lib/mathlib/CMSIS \ ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 40a13b5d1..ebe8a1a1e 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler <thomasgubler@gmail.com> # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 81d4b5d57..56c74a3b5 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,29 @@ #!nsh - -echo "[init] Team Blacksheep Discovery Quad" - # -# Load default params for this platform +# Team Blacksheep Discovery Quadcopter # -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 - - param save -fi - +# Maintainers: Simon Wilks <sjwilks@gmail.com> # -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect +if [ $DO_AUTOCONFIG == yes ] then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set MC_ATT_P 5.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 +set VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79..a3bcb63eb 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,49 @@ #!nsh - -echo "[init] 3DR Iris Quad" - # -# Load default params for this platform +# 3DR Iris Quadcopter # -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 - - param save -fi - +# Maintainers: Anton Babushkin <anton.babushkin@me.com> # -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect +if [ $DO_AUTOCONFIG == yes ] then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.0098 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set MC_ATT_P 9.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 + + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 fi -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 +set VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil deleted file mode 100644 index 9b664d63e..000000000 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ /dev/null @@ -1,105 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] HILS quadrotor starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil new file mode 100644 index 000000000..2d497374a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -0,0 +1,42 @@ +#!nsh +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin <anton.babushkin@me.com> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 +fi + +set HIL yes + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6..46da24d35 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler <thomasgubler@gmail.com> # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad34..e95844891 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,10 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + # - -echo "[HIL] HILS quadrotor + starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control +# Maintainers: Anton Babushkin <anton.babushkin@me.com> # -multirotor_pos_control start -echo "[HIL] setup done, running" +sh /etc/init.d/1001_rc_quad_x.hil +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 344d78422..46da24d35 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler <thomasgubler@gmail.com> # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,59 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -set MODE autostart -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - - -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 000000000..5f3cec4e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_cox + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c90..0e5bf60d6 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,15 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" - # -# Load default params for this platform +# MPX EasyStar Plane +# +# Maintainers: ??? # -if param compare SYS_AUTOCONFIG 1 + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +33,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba07..1ed923b19 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index a6d2ace96..1ed923b19 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 65f01c974..cbcc6189b 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom deleted file mode 100644 index 0cf6ee39a..000000000 --- a/ROMFS/px4fmu_common/init.d/3031_io_phantom +++ /dev/null @@ -1,85 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom new file mode 100644 index 000000000..4ebbe9c61 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -0,0 +1,44 @@ +#!nsh +# +# Phantom FPV Flying Wing +# +# Maintainers: Simon Wilks <sjwilks@gmail.com> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 41e041654..143310af9 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,58 +1,43 @@ #!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 - +# Skywalker X5 Flying Wing # -# Start and configure PX4IO or FMU interface +# Maintainers: Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch> # -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +if [ $DO_AUTOCONFIG == yes ] 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 + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing deleted file mode 100644 index 82ff425e6..000000000 --- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing new file mode 100644 index 000000000..e53763278 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -0,0 +1,43 @@ +#!nsh +# +# Wing Wing (aka Z-84) Flying Wing +# +# Maintainers: Simon Wilks <sjwilks@gmail.com> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 new file mode 100644 index 000000000..8d179d1fd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -0,0 +1,43 @@ +#!nsh +# +# FX-79 Buffalo Flying Wing +# +# Maintainers: Simon Wilks <sjwilks@gmail.com> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 deleted file mode 100644 index 759c17bb4..000000000 --- a/ROMFS/px4fmu_common/init.d/3034_io_fx79 +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow index 2886bcb75..e2cb8833d 100644 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow @@ -67,7 +67,7 @@ flow_position_estimator start # # Fire up the multi rotor attitude controller # -multirotor_att_control start +mc_att_control_vector start # # Fire up the flow position controller diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e2..e0cf92d97 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,69 +1,47 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# -# 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 - +# DJI Flame Wheel F330 Quadcopter # -# Start and configure PX4IO or FMU interface +# Maintainers: Anton Babushkin <anton.babushkin@me.com> # -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi -sh /etc/init.d/rc.mc_interface +set VEHICLE_TYPE mc +set MIXER FMU_quad_x -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191..ced69783d 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,74 +1,35 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - -# -# 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 save -fi - +# DJI Flame Wheel F450 Quadcopter # -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> # -param set MAV_TYPE 2 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect +if [ $DO_AUTOCONFIG == yes ] then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 +set VEHICLE_TYPE mc +set MIXER FMU_quad_x -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 000000000..e1423e008 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,31 @@ +#!nsh +# +# HobbyKing X550 Quadcopter +# +# Maintainers: Todd Stellanova <tstellanova@gmail.com> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_YAW_P 0.6 + param set MC_YAW_I 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b440..000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 000000000..2e5f6ca4f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 000000000..ddec8f36e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb..000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 000000000..106e0fb54 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 000000000..f0eea339b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 9b90cbdd0..000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param save -fi - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if [ $BOARD == fmuv1 ] - then - sdlog2 start -r 50 -e -b 16 - else - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 000000000..992a7aeba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier <lm@inf.ethz.ch> +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85b..000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 000000000..34da2dfef --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,195 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +# AUTOSTART PARTITION: +# 0 .. 999 Reserved (historical) +# 1000 .. 1999 Simulation setups +# 2000 .. 2999 Standard planes +# 3000 .. 3999 Flying wing +# 4000 .. 4999 Quad X +# 5000 .. 5999 Quad + +# 6000 .. 6999 Hexa X +# 7000 .. 7999 Hexa + +# 8000 .. 8999 Octo X +# 9000 .. 9999 Octo + +# 10000 .. 10999 Wide arm / H frame +# 11000 .. 11999 Hexa Cox +# 12000 .. 12999 Octo Cox + +# +# Simulation setups +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad_x.hil +fi + +if param compare SYS_AUTOSTART 1002 +then + sh /etc/init.d/1002_rc_fw_state.hil +fi + +if param compare SYS_AUTOSTART 1003 +then + sh /etc/init.d/1003_rc_quad_+.hil +fi + +if param compare SYS_AUTOSTART 1004 +then + sh /etc/init.d/1004_rc_fw_Rascal110.hil +fi + +# +# Standard plane +# + +if param compare SYS_AUTOSTART 2100 100 +then + sh /etc/init.d/2100_mpx_easystar + set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + sh /etc/init.d/2101_hk_bixler + set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + sh /etc/init.d/2102_3dr_skywalker + set MODE custom +fi + +# +# Flying wing +# + +if param compare SYS_AUTOSTART 3030 30 +then + sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + sh /etc/init.d/3031_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + sh /etc/init.d/3033_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + sh /etc/init.d/3034_fx79 +fi + +# +# Quad X +# + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +fi + +if param compare SYS_AUTOSTART 4010 10 +then + sh /etc/init.d/4010_dji_f330 +fi + +if param compare SYS_AUTOSTART 4011 11 +then + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 12 +then + sh /etc/init.d/4012_hk_x550 +fi + +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 15 +then + sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 16 +then + sh /etc/init.d/10016_3dr_iris +fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e52..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f02851006..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 000000000..d354fb06f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e4..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 000000000..d25f01dde --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined" + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + echo "[init] Set PWM disarmed: $PWM_DISARMED" + pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + echo "[init] Set PWM min: $PWM_MIN" + pwm min -c $PWM_OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + echo "[init] Set PWM max: $PWM_MAX" + pwm max -c $PWM_OUTPUTS -p $PWM_MAX + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index aaf91b316..c9d964f8e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,21 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -if px4io start -then - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - # - # Disable px4io topic limiting - # - if [ $BOARD == fmuv1 ] - then - px4io limit 200 - else - px4io limit 400 - fi -else - # SOS - tone_alarm error +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust PX4IO update rate limit +# +set PX4IO_LIMIT 400 +if hw_ver compare PX4FMU_V1 +then + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055..dcf5bbced 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -1,14 +1,16 @@ #!nsh # -# Initialise logging services. +# Initialize logging services. # if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then - sdlog2 start -r 50 -a -b 16 + echo "Start sdlog2 at 50Hz" + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + echo "Start sdlog2 at 200Hz" + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps new file mode 100644 index 000000000..96fe32c8a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -0,0 +1,24 @@ +#!nsh +# +# Standard apps for multirotors +# + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +mc_att_control start + +# +# Start position control +# +mc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6bb2e84ec..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,49 +0,0 @@ -#!nsh -# -# Script to set PWM min / max limits and mixer -# - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -if [ $FRAME_COUNT == 4 ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then - set OUTPUTS 123456 - param set MAV_TYPE 13 - else - set OUTPUTS 12345678 - fi -fi - - -# -# Set PWM output frequency -# -pwm rate -c $OUTPUTS -r $PWM_RATE - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor deleted file mode 100644 index bc550ac5a..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ /dev/null @@ -1,39 +0,0 @@ -#!nsh -# -# Standard everything needed for multirotors except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 070a4e7e3..badbf92c3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,41 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - set BOARD fmuv2 -else - set BOARD fmuv1 + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47e..0cd8a0e04 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# Start px4io if present -if px4io start -then - echo "PX4IO driver started" -else - if fmu mode_serial - then - echo "FMU driver started" - fi -fi - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status -then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status - then - echo "fixedwing att filter running" - else - attitude_estimator_ekf start - fi -fi - -# Start GPS -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 90f2e2b17..6f4e1f3b5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,39 +8,40 @@ # set MODE autostart -set logfile /fs/microsd/bootlog.txt +set RC_FILE /fs/microsd/etc/rc.txt +set CONFIG_FILE /fs/microsd/etc/config.txt +set EXTRAS_FILE /fs/microsd/etc/extras.txt + +set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4 # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[init] Looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + set LOG_FILE /fs/microsd/bootlog.txt + echo "[init] microSD card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + set LOG_FILE /dev/null + echo "[init] No microSD card found" # Play SOS tone_alarm error fi # # Look for an init script on the microSD card. +# Disable autostart if the script found. # -# To prevent automatic startup in the current flight mode, -# the script should set MODE to some other value. -# -if [ -f /fs/microsd/etc/rc ] -then - echo "[init] reading /fs/microsd/etc/rc" - sh /fs/microsd/etc/rc -fi -# Also consider rc.txt files -if [ -f /fs/microsd/etc/rc.txt ] +if [ -f $RC_FILE ] then - echo "[init] reading /fs/microsd/etc/rc.txt" - sh /fs/microsd/etc/rc.txt + echo "[init] Executing init script: $RC_FILE" + sh $RC_FILE + set MODE custom +else + echo "[init] Init script not found: $RC_FILE" fi # if this is an APM build then there will be a rc.APM script @@ -52,20 +53,19 @@ then echo "[init] USB interface connected" fi - echo "Running rc.APM" + echo "[init] Running rc.APM" # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi if [ $MODE == autostart ] then + echo "[init] AUTOSTART mode" + # - # Start terminal + # Start CDC/ACM serial driver # - if sercon - then - echo "USB connected" - fi + sercon # # Start the ORB (first app to start) @@ -73,392 +73,439 @@ then uorb start # - # Load microSD params - # - #if ramtron start - #then - # param select /ramtron/params - # if [ -f /ramtron/params ] - # then - # param load /ramtron/params - # fi - #else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] - then - if param load /fs/microsd/params - then - echo "Parameters loaded" - else - echo "Parameter file corrupt - ignoring" - fi - fi - #fi + # Load parameters + # + set PARAM_FILE /fs/microsd/params + if mtd start + then + set PARAM_FILE /fs/mtd_params + fi + + param select $PARAM_FILE + if param load + then + echo "[init] Parameters loaded: $PARAM_FILE" + else + echo "[init] ERROR: Parameters loading failed: $PARAM_FILE" + fi # # Start system state indicator # if rgbled start then - echo "Using external RGB Led" + echo "[init] Using external RGB Led" else if blinkm start then + echo "[init] Using blinkm" blinkm systemstate fi fi # - # Start the Commander (needs to be this early for in-air-restarts) + # Set default values # - commander start - + set HIL no + set VEHICLE_TYPE none + set MIXER none + set USE_IO yes + set OUTPUT_MODE none + set PWM_OUTPUTS none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + set MKBLCTRL_MODE none + set FMU_MODE pwm + set MAV_TYPE none + # - # Start the datamanager + # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # - dataman start - + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi + # - # Start the Navigator + # Set parameters and env variables for selected AUTOSTART # - navigator start + if param compare SYS_AUTOSTART 0 + then + echo "[init] Don't try to find autostart script" + else + sh /etc/init.d/rc.autostart + fi - if param compare SYS_AUTOSTART 1000 + # + # Override parameters from user configuration file + # + if [ -f $CONFIG_FILE ] then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom + echo "[init] Reading config: $CONFIG_FILE" + sh $CONFIG_FILE + else + echo "[init] Config file not found: $CONFIG_FILE" fi - - if param compare SYS_AUTOSTART 1001 + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom + param set SYS_AUTOCONFIG 0 + param save fi - if param compare SYS_AUTOSTART 1002 + set IO_PRESENT no + + if [ $USE_IO == yes ] then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom + # + # Check if PX4IO present and update firmware if needed + # + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set IO_FILE /etc/extras/px4io-v2_default.bin + else + set IO_FILE /etc/extras/px4io-v1_default.bin + fi + + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK" + echo "PX4IO CRC OK" >> $LOG_FILE + + set IO_PRESENT yes + else + echo "[init] Trying to update" + echo "PX4IO Trying to update" >> $LOG_FILE + + tone_alarm MLL32CP8MB + + if px4io forceupdate 14662 $IO_FILE + then + usleep 500000 + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK, update successful" + echo "PX4IO CRC OK after updating" >> $LOG_FILE + tone_alarm MLL8CDE + + set IO_PRESENT yes + else + echo "[init] ERROR: PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR + fi + else + echo "[init] ERROR: PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR + fi + fi + + if [ $IO_PRESENT == no ] + then + echo "[init] ERROR: PX4IO not found" + tone_alarm $TUNE_OUT_ERROR + fi fi - if param compare SYS_AUTOSTART 1003 + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom + if [ $USE_IO == yes ] + then + set OUTPUT_MODE io + else + set OUTPUT_MODE fmu + fi fi - if param compare SYS_AUTOSTART 1004 + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi fi - - if [ $MODE != custom ] + + if [ $HIL == yes ] then - # Try to get an USB console + set OUTPUT_MODE hil + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + else + # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & fi - + # - # Upgrade PX4IO firmware + # Start the Commander (needs to be this early for in-air-restarts) # - - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set io_file /etc/extras/px4io-v2_default.bin - else - set io_file /etc/extras/px4io-v1_default.bin - fi - - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi + commander start + + # + # Start primary output + # + set TTYS1_BUSY no - if px4io checkcrc $io_file + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output + if [ $OUTPUT_MODE != none ] then - echo "PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile - else - echo "PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile - tone_alarm MBABGP - if px4io forceupdate 14662 $io_file + if [ $OUTPUT_MODE == io ] then - usleep 500000 + echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile - tone_alarm MSPAA + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + if [ $OUTPUT_MODE == fmu ] + then + echo "[init] Use FMU PWM as primary output" + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" else - echo "PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG + then + echo "[init] MKBLCTRL started" + else + echo "[init] ERROR: MKBLCTRL start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] ERROR: HIL output start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Start IO or FMU for RC PPM input if needed + # + if [ $IO_PRESENT == yes ] + then + if [ $OUTPUT_MODE != io ] + then + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi fi else - echo "PX4IO update failed" - echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi fi fi - - set EXIT_ON_END no # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) - # - # AUTOSTART PARTITION: - # 0 .. 999 Reserved (historical) - # 1000 .. 1999 Simulation setups - # 2000 .. 2999 Standard planes - # 3000 .. 3999 Flying wing - # 4000 .. 4999 Quad X - # 5000 .. 5999 Quad + - # 6000 .. 6999 Hexa X - # 7000 .. 7999 Hexa + - # 8000 .. 8999 Octo X - # 9000 .. 9999 Octo + - # 10000 .. 10999 Wide arm / H frame - # 11000 .. 11999 Hexa Cox - # 12000 .. 12999 Octo Cox - - if param compare SYS_AUTOSTART 4008 8 - then - sh /etc/init.d/4008_ardrone - set MODE custom - fi + # MAVLink + # + set EXIT_ON_END no - if param compare SYS_AUTOSTART 4009 9 + if [ $HIL == yes ] then - sh /etc/init.d/4009_ardrone_flow - set MODE custom + sleep 1 + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $TTYS1_BUSY == yes ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi fi - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors - if param compare SYS_AUTOSTART 12001 + if [ $HIL == no ] then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom + echo "[init] Start logging" + sh /etc/init.d/rc.logging + + echo "[init] Start GPS" + gps start fi - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom + echo "[init] Vehicle type: FIXED WING" + + if [ $MIXER == none ] + then + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.fw_apps fi - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi + echo "[init] Vehicle type: MULTICOPTER" - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi + if [ $MIXER == none ] + then + # Set default mixer for multicopter if not defined + set MIXER quad_x + fi - if param compare SYS_AUTOSTART 3031 31 - then - sh /etc/init.d/3031_io_phantom - set MODE custom - fi - - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + then + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 + fi + fi - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps fi - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - - if param compare SYS_AUTOSTART 2100 100 + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi + echo "[init] Vehicle type: GENERIC" - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom + # Load mixer and configure outputs + sh /etc/init.d/rc.interface fi - if param compare SYS_AUTOSTART 2102 102 - then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom - fi - - if param compare SYS_AUTOSTART 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing - if [ -f /fs/microsd/etc/rc.local ] - then - sh /fs/microsd/etc/rc.local - fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] + # Start any custom addons + if [ -f $EXTRAS_FILE ] then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect - then - px4io start - else - if fmu mode_serial - then - echo "FMU driver (no PWM) started" - fi - fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - + echo "[init] Starting addons script: $EXTRAS_FILE" + sh $EXTRAS_FILE + else + echo "[init] Addons script not found: $EXTRAS_FILE" fi if [ $EXIT_ON_END == yes ] diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6aa1d3d46..56482d140 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,6 +2,7 @@ # # PX4FMU startup script for test hackery. # +uorb start if sercon then @@ -9,4 +10,68 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & -fi
\ No newline at end of file +fi + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +# +# Start a minimal system +# + +if [ -f /etc/extras/px4io-v2_default.bin ] +then + set io_file /etc/extras/px4io-v2_default.bin +else + set io_file /etc/extras/px4io-v1_default.bin +fi + +if px4io start +then + echo "PX4IO OK" +fi + +if px4io checkcrc $io_file +then + echo "PX4IO CRC OK" +else + echo "PX4IO CRC failure" + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file + then + usleep 500000 + if px4io start + then + echo "PX4IO restart OK" + tone_alarm MSPAA + else + echo "PX4IO restart failed" + tone_alarm MNGGG + sleep 5 + reboot + fi + else + echo "PX4IO update failed" + tone_alarm MNGGG + fi +fi + +# +# The presence of this file suggests we're running a mount stress test +# +if [ -f /fs/microsd/mount_test_cmds.txt ] +then + tests mount +fi diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py new file mode 100755 index 000000000..c678ef0f4 --- /dev/null +++ b/Tools/fsm_visualisation.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 + +"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table + +convert dot code to png using graphviz: + +dot fsm.dot -Tpng -o fsm.png +""" + +import argparse +import re + +__author__ = "Julian Oes" + +def get_dot_header(): + + return """digraph finite_state_machine { + graph [ dpi = 300 ]; + ratio = 1.5 + node [shape = circle];""" + +def get_dot_footer(): + + return """}\n""" + +def main(): + + # parse input arguments + parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.') + parser.add_argument("-i", "--input-file", default=None, help="choose file to parse") + parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table") + args = parser.parse_args() + + # open source file + if args.input_file == None: + exit('please specify file') + f = open(args.input_file,'r') + source = f.read() + + # search for state transition table and extract the table itself + # first look for StateTable::Tran + # then accept anything including newline until { + # but don't accept the definition (without ;) + # then extract anything inside the brackets until }; + match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source) + + if not match: + exit('no state transition table found') + + table_source = match.group(1) + + # bookkeeping for error checking + num_errors_found = 0 + + states = [] + events = [] + + # first get all states and events + for table_line in table_source.split('\n'): + + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) + if match: + states.append(str(match.group(1))) + # go to next line + continue + + if len(states) == 1: + match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line) + if match: + events.append(str(match.group(1))) + + print('Found %d states and %d events' % (len(states), len(events))) + + + # keep track of origin state + state = None + + # fill dot code in here + dot_code = '' + + # create table len(states)xlen(events) + transition_table = [[[] for x in range(len(states))] for y in range(len(events))] + + # now fill the transition table and write the dot code + for table_line in table_source.split('\n'): + + # get states + # from: /* NAV_STATE_NONE */ + # extract only "NONE" + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) + if match: + state = match.group(1) + state_index = states.index(state) + # go to next line + continue + + # can't advance without proper state + if state == None: + continue + + # get event and next state + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY} + # extract "READY_REQUESTED" and "READY" if there is ACTION + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line) + + # get event and next state + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + # extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line) + + # ignore lines with brackets only + if match_action or match_no_action: + + # only write arrows for actions + if match_action: + event = match_action.group(1) + new_state = match_action.group(2) + dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n' + + elif match_no_action: + event = match_no_action.group(1) + new_state = match_no_action.group(2) + + # check for state changes without action + if state != new_state: + print('Error: no action but state change:') + print('State: ' + state + ' changed to: ' + new_state) + print(table_line) + num_errors_found += 1 + + # check for wrong events + if event not in events: + print('Error: unknown event: ' + event) + print(table_line) + num_errors_found += 1 + + # check for wrong new states + if new_state not in states: + print('Error: unknown new state: ' + new_state) + print(table_line) + num_errors_found += 1 + + # save new state in transition table + event_index = events.index(event) + + # bold for action + if match_action: + transition_table[event_index][state_index] = '**' + new_state + '**' + else: + transition_table[event_index][state_index] = new_state + + + + # assemble dot code + dot_code = get_dot_header() + dot_code + get_dot_footer() + + # write or print dot file + if args.dot_file: + f = open(args.dot_file,'w') + f.write(dot_code) + print('Wrote dot file') + else: + print('##########Dot-start##########') + print(dot_code) + print('##########Dot-end############') + + + # assemble doku wiki table + table_code = '| ^ ' + # start with header of all states + for state in states: + table_code += state + ' ^ ' + + table_code += '\n' + + # add events and new states + for event, row in zip(events, transition_table): + table_code += '^ ' + event + ' | ' + for new_state in row: + table_code += new_state + ' | ' + table_code += '\n' + + # write or print wiki table + if args.table_file: + f = open(args.table_file,'w') + f.write(table_code) + print('Wrote table file') + else: + print('##########Table-start########') + print(table_code) + print('##########Table-end##########') + + # report obvous errors + if num_errors_found: + print('Obvious errors found: %d' % num_errors_found) + else: + print('No obvious errors found') + +if __name__ == '__main__': + main() diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 33f76b415..4d40a6201 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output): result = "" for group in groups: result += "==== %s ====\n\n" % group.GetName() + result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") name = param.GetFieldValue("short_desc") - if code != name: - name = "%s (%s)" % (name, code) - result += "=== %s ===\n\n" % name - long_desc = param.GetFieldValue("long_desc") - if long_desc is not None: - result += "%s\n\n" % long_desc + name = name.replace("\n", "") + result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") if min_val is not None: - result += "* Minimal value: %s\n" % min_val + result += "| %s " % min_val + else: + result += "|" max_val = param.GetFieldValue("max") if max_val is not None: - result += "* Maximal value: %s\n" % max_val + result += "| %s " % max_val + else: + result += "|" def_val = param.GetFieldValue("default") if def_val is not None: - result += "* Default value: %s\n" % def_val - result += "\n" + result += "| %s " % def_val + else: + result += "|" + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + long_desc = long_desc.replace("\n", "") + result += "| %s " % long_desc + else: + result += "|" + result += "|\n" + result += "\n" return result diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py new file mode 100644 index 000000000..33f76b415 --- /dev/null +++ b/Tools/px4params/dokuwikiout_listings.py @@ -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 diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 97410ff47..7ab1454f0 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,11 +10,13 @@ LIBS=-lm #_DEPS = test.h #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \ + mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) #$(DEPS) $(ODIR)/%.o: %.cpp + mkdir -p obj $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp @@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(CC) -c -o $@ $< $(CFLAGS) +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c + $(CC) -c -o $@ $< $(CFLAGS) + $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c $(CC) -c -o $@ $< $(CFLAGS) diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp new file mode 100644 index 000000000..01b5958b7 --- /dev/null +++ b/Tools/tests-host/hrt.cpp @@ -0,0 +1,16 @@ +#include <sys/time.h> +#include <inttypes.h> +#include <drivers/drv_hrt.h> +#include <stdio.h> + +hrt_abstime hrt_absolute_time() { + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; +} + +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { + // not thread safe + return hrt_absolute_time() - *then; +} diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 042322aad..e311617f9 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -9,4 +9,6 @@ int main(int argc, char *argv[]) { "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; test_mixer(3, args); + + test_conv(1, args); }
\ No newline at end of file diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h new file mode 100644 index 000000000..0fdb170db --- /dev/null +++ b/Tools/tests-host/queue.h @@ -0,0 +1,133 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +#ifndef FAR +#define FAR +#endif + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include <sys/types.h> + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index 6d2a9f7bd..000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,148 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/blinkm -MODULES += drivers/rgbled -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/att_pos_estimator_ekf - -# -# Vehicle Control -# -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB -MODULES += modules/dataman - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion -MODULES += lib/launchdetection - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index fba50aaf8..51be7e1a1 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -36,13 +36,13 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # # System commands # -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron +MODULES += systemcmds/mtd MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c @@ -57,6 +57,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control @@ -81,8 +82,8 @@ MODULES += modules/position_estimator_inav # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control #MODULES += examples/flow_position_control #MODULES += examples/flow_speed_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index f0a9c0c06..ab05d4e3d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -36,6 +36,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # Needs to be burned to the ground and re-written; for now, @@ -45,7 +46,6 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/ramtron MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer @@ -59,6 +59,8 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/hw_ver # # General system control @@ -83,8 +85,8 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control # # Logging diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0f60e88b5..8623c0584 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -6,24 +6,38 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules # MODULES += drivers/device MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/px4io MODULES += systemcmds/perf MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/hw_ver # # Library modules # MODULES += modules/systemlib +MODULES += modules/systemlib/mixer MODULES += modules/uORB # +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS + +# # Transitional support - add commands from the NuttX export archive. # # In general, these should move to modules over time. diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e..1dc96b3c3 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1 CONFIG_MMCSD_SPI=y CONFIG_MMCSD_SPICLOCK=24000000 # CONFIG_MMCSD_SDIO is not set -# CONFIG_MTD is not set +CONFIG_MTD=y CONFIG_PIPES=y # CONFIG_PM is not set # CONFIG_POWER is not set @@ -483,6 +483,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_NO_SERIAL_CONSOLE is not set # +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +# CONFIG_MTD_RAMTRON is not set +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set + +# # USART1 Configuration # CONFIG_USART1_RXBUFSIZE=512 @@ -566,7 +585,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb363..2a734c27e 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -295,16 +295,16 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set CONFIG_USART3_RXDMA=y # CONFIG_UART4_RS485 is not set CONFIG_UART4_RXDMA=y -# CONFIG_UART5_RXDMA is not set +CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set @@ -500,8 +500,8 @@ CONFIG_MTD=y # # MTD Configuration # -# CONFIG_MTD_PARTITION is not set -# CONFIG_MTD_BYTE_WRITE is not set +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y # # MTD Device Drivers @@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936..f73a3ef01 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 6f7166284..02c26b5c0 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -60,6 +60,7 @@ __BEGIN_DECLS /* PX4IO connection configuration */ #define PX4IO_SERIAL_DEVICE "/dev/ttyS2" +#define UDID_START 0x1FFF7A10 //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index a19ed9d24..7cfca7656 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -52,6 +52,8 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include <stm32.h> #include <arch/board/board.h> + +#define UDID_START 0x1FFF7A10 /**************************************************************************************************** * Definitions @@ -73,7 +75,7 @@ __BEGIN_DECLS /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) @@ -85,7 +87,7 @@ __BEGIN_DECLS #define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) #define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) #define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) -#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI1 off */ #define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) @@ -96,7 +98,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 269ec238e..71414d62c 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\n"); + message("[boot] Initialized SPI port 1 (SENSORS)\n"); /* Get the SPI port for the FRAM */ @@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 375000000); + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - message("[boot] Successfully initialized SPI port 2\n"); + message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } @@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { - message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index c3f39addf..1be4877ba 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -58,11 +58,11 @@ /* PX4IO GPIOs **********************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8da555211..ef9bb5cad 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -74,9 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf5..9f8c0eeb2 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37..88da94b1e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 000000000..e201ecbb3 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado <px4@sradonia.net> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_data.c + * @author Stefan Rado <px4@sradonia.net> + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <arch/math.h> +#include <geo/geo.h> + +#include <uORB/topics/battery_status.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_status.h> + +/* FrSky sensor hub data IDs */ +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +#define frac(f) (f - (int)f) + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; +static int vehicle_status_sub = -1; + +/** + * Initializes the uORB subscriptions. + */ +void frsky_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * Sends a 0x5E start/stop byte. + */ +static void frsky_send_startstop(int uart) +{ + static const uint8_t c = 0x5E; + write(uart, &c, sizeof(c)); +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void frsky_send_byte(int uart, uint8_t value) +{ + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; + + switch (value) { + case 0x5E: + write(uart, x5E, sizeof(x5E)); + break; + + case 0x5D: + write(uart, x5D, sizeof(x5D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +static void frsky_send_data(int uart, uint8_t id, int16_t data) +{ + /* Cast data to unsigned, because signed shift might behave incorrectly */ + uint16_t udata = data; + + frsky_send_startstop(uart); + + frsky_send_byte(uart, id); + frsky_send_byte(uart, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, barometer altitude, temperature, battery voltage & current + */ +void frsky_send_frame1(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + frsky_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter) * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); + + frsky_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + frsky_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); + + frsky_send_startstop(uart); +} + +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +static float frsky_format_gps(float dec) +{ + float dms_deg = (int) dec; + float dec_deg = dec - dms_deg; + float dms_min = (int) (dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + +/** + * Sends frame 2 (every 1000ms): + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level + */ +void frsky_send_frame2(int uart) +{ + /* get a local copy of the global position data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send formatted frame */ + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; + char lat_ns = 0, lon_ew = 0; + int sec = 0; + if (global_pos.valid) { + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + lat = frsky_format_gps(abs(global_pos.lat)); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = frsky_format_gps(abs(global_pos.lon)); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) + * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; + } + + frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); + + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); + + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * GPS date & time + */ +void frsky_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); + + frsky_send_startstop(uart); +} diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/drivers/frsky_telemetry/frsky_data.h index 7ea6496bb..a7d9eee53 100644 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado <px4@sradonia.net> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +33,19 @@ ****************************************************************************/ /** - * @file Vector.cpp + * @file frsky_data.h + * @author Stefan Rado <px4@sradonia.net> + * + * FrSky telemetry implementation. * - * math vector */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); -#include "Vector.hpp" +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c new file mode 100644 index 000000000..7b08ca69e --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado <px4@sradonia.net> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_telemetry.c + * @author Stefan Rado <px4@sradonia.net> + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <string.h> +#include <sys/types.h> +#include <fcntl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> +#include <termios.h> + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + static const speed_t speed = B9600; + + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + return uart; +} + +/** + * Print command usage information + */ +static void usage() +{ + fprintf(stderr, + "usage: frsky_telemetry start [-d <devicename>]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* Print welcome text */ + warnx("FrSky telemetry interface starting..."); + + /* Open UART */ + struct termios uart_config_original; + const int uart = frsky_open_uart(device_name, &uart_config_original); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) + { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) + { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int frsky_telemetry_main(int argc, char *argv[]) +{ + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "frsky_telemetry already running"); + + thread_should_exit = false; + frsky_task = task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + frsky_telemetry_thread_main, + (const char **)argv); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "frsky_telemetry already stopped"); + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} diff --git a/src/systemcmds/eeprom/module.mk b/src/drivers/frsky_telemetry/module.mk index 07f3945be..1632c74f7 100644 --- a/src/systemcmds/eeprom/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,8 +32,10 @@ ############################################################################ # -# EEPROM file system driver +# FrSky telemetry application. # -MODULE_COMMAND = eeprom -SRCS = 24xxxx_mtd.c eeprom.c +MODULE_COMMAND = frsky_telemetry + +SRCS = frsky_data.c \ + frsky_telemetry.c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ec..6b72d560f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,98 +266,133 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } + if (_fake_gps) { + + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + usleep(2e5); - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + } else { - default: - break; - } + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } - unlock(); + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } - if (_Helper->configure(_baudrate) == 0) { unlock(); - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + if (_Helper->configure(_baudrate) == 0) { + unlock(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - last_rate_count++; + last_rate_count++; - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } - if (!_healthy) { - char *mode_str = "unknown"; + if (!_healthy) { + char *mode_str = "unknown"; - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; - default: - break; + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; } + } - warnx("module found: %s", mode_str); - _healthy = true; + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; } - } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + lock(); } lock(); - } - - lock(); - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; - default: - break; + default: + break; + } } } @@ -417,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -427,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -435,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -527,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -542,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -567,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87..0a047f38f 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -193,9 +193,10 @@ HIL::~HIL() } while (_task != -1); } - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // XXX already claimed with CDEV + // /* clean up the alternate device node */ + // if (_primary_pwm_device) + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); g_hil = nullptr; } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66..9b9c11af2 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,31 +915,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; - warnx("starting mag scale calibration"); + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } + warnx("starting mag scale calibration"); /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* Set to 2.5 Gauss. We ask for 3 to get the right part of + * the chained if statement above. */ + if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("failed to set 2.5 Ga range"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; } //warnx("periodic read %u", i); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#if 0 + warnx("measurement avg: %.6f %.6f %.6f", + (double)sum_excited[0]/good_count, + (double)sum_excited[1]/good_count, + (double)sum_excited[2]/good_count); +#endif float scaling[3]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index e322c6349..a3d3a3933 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 042d9f816..d293f9954 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d6720..9251cff7b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include <systemlib/err.h> #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> -#include <mathlib/mathlib.h> #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> @@ -178,31 +177,26 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } @@ -390,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a..6326cf7fc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b878d29bc..c067d363b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -65,6 +65,7 @@ #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> #include <systemlib/pwm_limit/pwm_limit.h> +#include <systemlib/board_serial.h> #include <drivers/drv_mixer.h> #include <drivers/drv_rc_input.h> @@ -224,10 +225,10 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), - _failsafe_pwm( {0}), - _disarmed_pwm( {0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _failsafe_pwm({0}), + _disarmed_pwm({0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -575,7 +576,7 @@ PX4FMU::task_main() if (i >= outputs.noutputs || !isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + outputs.output[i] > 1.0f) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(3): case PWM_SERVO_SET(2): if (_mode < MODE_4PWM) { @@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): if (arg <= 2100) { @@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(3): case PWM_SERVO_GET(2): if (_mode < MODE_4PWM) { @@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); @@ -1005,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1107,10 +1142,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_OFF); stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF); @@ -1123,10 +1160,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); /* set the sensor rail off */ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); @@ -1159,6 +1198,13 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1431,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -1591,6 +1636,15 @@ fmu_main(int argc, char *argv[]) errx(0, "FMU driver stopped"); } + if (!strcmp(verb, "id")) { + char id[12]; + (void)get_board_serial(id); + + errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", + (unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5], + (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]); + } + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); @@ -1647,6 +1701,7 @@ fmu_main(int argc, char *argv[]) sensor_reset(0); warnx("resettet default time"); } + exit(0); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index d918abd57..05bc7a5b3 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,5 +3,4 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp \ - ../../modules/systemlib/pwm_limit/pwm_limit.c +SRCS = fmu.cpp diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cbdd5acc4..df847a64d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via I2C. + * PX4IO is connected via I2C or DMA enabled high-speed UART. */ #include <nuttx/config.h> @@ -270,7 +270,8 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; ///<mixed outputs + actuator_outputs_s _outputs; ///< mixed outputs + servorail_status_s _servorail_status; ///< servorail status bool _primary_pwm_device; ///< true if we are the default PWM output @@ -450,7 +451,7 @@ private: namespace { -PX4IO *g_dev; +PX4IO *g_dev = nullptr; } @@ -504,7 +505,8 @@ PX4IO::PX4IO(device::Device *interface) : /* open MAVLink text channel */ _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = true; + _debug_enabled = false; + _servorail_status.rssi_v = 0; } PX4IO::~PX4IO() @@ -578,6 +580,12 @@ PX4IO::init() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol == _io_reg_get_error) { + log("failed to communicate with IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort."); + return -1; + } + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); @@ -772,8 +780,6 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - log("starting"); - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* @@ -807,8 +813,6 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; - log("ready"); - /* lock against the ioctl handler */ lock(); @@ -1300,6 +1304,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) /* voltage is scaled to mV */ battery_status.voltage_v = vbatt / 1000.0f; + battery_status.voltage_filtered_v = vbatt / 1000.0f; /* ibatt contains the raw ADC count, as 12 bit ADC @@ -1331,19 +1336,18 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) void PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) { - servorail_status_s servorail_status; - servorail_status.timestamp = hrt_absolute_time(); + _servorail_status.timestamp = hrt_absolute_time(); /* voltage is scaled to mV */ - servorail_status.voltage_v = vservo * 0.001f; - servorail_status.rssi_v = vrssi * 0.001f; + _servorail_status.voltage_v = vservo * 0.001f; + _servorail_status.rssi_v = vrssi * 0.001f; /* lazily publish the servorail voltages */ if (_to_servorail > 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1450,6 +1454,13 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } + /* set RSSI */ + + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } + /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); @@ -1664,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1688,7 +1710,21 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } + + if (ret) + return ret; retries--; @@ -1798,7 +1834,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2355,8 +2391,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2420,6 +2458,69 @@ detect(int argc, char *argv[]) } void +checkcrc(int argc, char *argv[]) +{ + bool keep_running = false; + + if (g_dev == nullptr) { + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + } else { + /* its already running, don't kill the driver */ + keep_running = true; + } + + /* + check IO CRC against CRC of a file + */ + if (argc < 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + int fd = open(argv[1], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[1], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + + if (!keep_running) { + delete g_dev; + g_dev = nullptr; + } + + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); +} + +void bind(int argc, char *argv[]) { int pulses; @@ -2569,17 +2670,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); @@ -2613,12 +2714,16 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "detect")) detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "checkcrc")) + checkcrc(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2691,18 +2796,30 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); } + } - // tear down the px4io instance - delete g_dev; + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); } + // tear down the px4io instance + delete g_dev; + g_dev = nullptr; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; @@ -2760,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } @@ -2798,49 +2916,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "checkcrc")) { - /* - check IO CRC against CRC of a file - */ - if (argc <= 2) { - printf("usage: px4io checkcrc filename\n"); - exit(1); - } - if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - int fd = open(argv[2], O_RDONLY); - if (fd == -1) { - printf("open of %s failed - %d\n", argv[2], errno); - exit(1); - } - const uint32_t app_size_max = 0xf000; - uint32_t fw_crc = 0; - uint32_t nbytes = 0; - while (true) { - uint8_t buf[16]; - int n = read(fd, buf, sizeof(buf)); - if (n <= 0) break; - fw_crc = crc32part(buf, n, fw_crc); - nbytes += n; - } - close(fd); - while (nbytes < app_size_max) { - uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); - nbytes++; - } - - int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); - if (ret != OK) { - printf("check CRC failed - %d\n", ret); - exit(1); - } - printf("CRCs match\n"); - exit(0); - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee..dd8abbac5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include <poll.h> #include <termios.h> #include <sys/stat.h> +#include <nuttx/arch.h> #include <crc32.h> @@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ @@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e2..55f63eef9 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 727c86e02..4f58891ed 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { warnx("not started"); rgbled_usage(); - exit(0); + exit(1); } if (!strcmp(verb, "test")) { @@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { @@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[]) exit(ret); } + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } + if (!strcmp(verb, "rgb")) { if (argc < 5) { errx(1, "Usage: rgbled rgb <red> <green> <blue>"); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e0007..067d77364 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b078562c2..614877b18 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,22 +41,11 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) { - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - (*rot_matrix)(i, j) = R(i, j); - } - } + rot_matrix->from_euler(roll, pitch, yaw); } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fc..0c56494c5 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b66d1dba5..9584924cc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -174,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9e7d35f68..2e86c72dc 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -141,7 +141,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5e2200727..255776765 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -158,7 +158,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 11def2371..3b68a0a4e 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); /* * If the AB vector and the vector from B to airplane point in the same @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } } -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const { /* this is an approximation for small angles, proposed by [2] */ - math::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH); } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 7a3c42a92..5c0804a39 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5a56dce65..0f28bccad 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -30,7 +30,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -282,7 +282,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -505,7 +505,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4fc009da9..d1ebacda1 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -348,7 +348,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081..000000000 --- a/src/lib/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Dcm.cpp - * - * math direction cosine matrix - */ - -#include <mathlib/math/test/test.hpp> - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3a..000000000 --- a/src/lib/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb..000000000 --- a/src/lib/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3..000000000 --- a/src/lib/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include <math.h> - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..ea0cf4ca1 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * Pavel Kirienko <pavel.kirienko@gmail.com> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +35,401 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix.hpp * - * matrix code + * Matrix class */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include <nuttx/config.h> - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include <stdio.h> +#include "../CMSIS/Include/arm_math.h" namespace math { -class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +template <unsigned int M, unsigned int N> +class __EXPORT Matrix; + +// MxN matrix with float elements +template <unsigned int M, unsigned int N> +class __EXPORT MatrixBase +{ +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + arm_mat = {M, N, &data[0][0]}; + } + + /** + * copyt ctor + */ + MatrixBase(const MatrixBase<M, N> &m) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, m.data, sizeof(data)); + } + + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + + MatrixBase(const float d[M][N]) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float *d) { + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[M][N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access by index + */ + float &operator()(const unsigned int row, const unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + float operator()(const unsigned int row, const unsigned int col) const { + return data[row][col]; + } + + /** + * get rows number + */ + unsigned int get_rows() const { + return M; + } + + /** + * get columns number + */ + unsigned int get_cols() const { + return N; + } + + /** + * test for equality + */ + bool operator ==(const Matrix<M, N> &m) const { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; + + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Matrix<M, N> &m) const { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; + + return false; + } + + /** + * set to value + */ + const Matrix<M, N> &operator =(const Matrix<M, N> &m) { + memcpy(data, m.data, sizeof(data)); + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * negation + */ + Matrix<M, N> operator -(void) const { + Matrix<M, N> res; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; + + return res; + } + + /** + * addition + */ + Matrix<M, N> operator +(const Matrix<M, N> &m) const { + Matrix<M, N> res; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; + + return res; + } + + Matrix<M, N> &operator +=(const Matrix<M, N> &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * subtraction + */ + Matrix<M, N> operator -(const Matrix<M, N> &m) const { + Matrix<M, N> res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; + + return res; + } + + Matrix<M, N> &operator -=(const Matrix<M, N> &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * uniform scaling + */ + Matrix<M, N> operator *(const float num) const { + Matrix<M, N> res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; + + return res; + } + + Matrix<M, N> &operator *=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + + return *static_cast<Matrix<M, N>*>(this); + } + + Matrix<M, N> operator /(const float num) const { + Matrix<M, N> res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + + return res; + } + + Matrix<M, N> &operator /=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + + return *static_cast<Matrix<M, N>*>(this); + } + + /** + * multiplication by another matrix + */ + template <unsigned int P> + Matrix<M, P> operator *(const Matrix<N, P> &m) const { + Matrix<M, P> res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * transpose the matrix + */ + Matrix<N, M> transposed(void) const { + Matrix<N, M> res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * invert the matrix + */ + Matrix<M, N> inversed(void) const { + Matrix<M, N> res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * set zero matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * set identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + unsigned int n = (M < N) ? M : N; + + for (unsigned int i = 0; i < n; i++) + data[i][i] = 1; + } + + void print(void) { + for (unsigned int i = 0; i < M; i++) { + printf("[ "); + + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + + printf(" ]\n"); + } + } +}; + +template <unsigned int M, unsigned int N> +class __EXPORT Matrix : public MatrixBase<M, N> +{ +public: + using MatrixBase<M, N>::operator *; + + Matrix() : MatrixBase<M, N>() {} + + Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {} + + Matrix(const float *d) : MatrixBase<M, N>(d) {} + + Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {} + + /** + * set to value + */ + const Matrix<M, N> &operator =(const Matrix<M, N> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + Vector<M> operator *(const Vector<N> &v) const { + Vector<M> res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; + } +}; + +template <> +class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> +{ +public: + using MatrixBase<3, 3>::operator *; + + Matrix() : MatrixBase<3, 3>() {} + + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} + + Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {} + + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + Vector<3> operator *(const Vector<3> &v) const { + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + /** + * get euler angles from rotation matrix + */ + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } + + return euler; + } +}; + +} + +#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca6..000000000 --- a/src/lib/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d33..21d05c7ef 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * Pavel Kirienko <pavel.kirienko@gmail.com> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,82 +37,129 @@ /** * @file Quaternion.hpp * - * math quaternion lib + * Quaternion class */ -#pragma once +#ifndef QUATERNION_HPP +#define QUATERNION_HPP +#include <math.h> +#include "../CMSIS/Include/arm_math.h" #include "Vector.hpp" #include "Matrix.hpp" namespace math { -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector +class __EXPORT Quaternion : public Vector<4> { public: - /** - * default ctor + * trivial ctor */ - Quaternion(); + Quaternion() : Vector<4>() {} /** - * ctor from floats + * copy ctor */ - Quaternion(float a, float b, float c, float d); + Quaternion(const Quaternion &q) : Vector<4>(q) {} /** - * ctor from data + * casting from vector */ - Quaternion(const float *data); + Quaternion(const Vector<4> &v) : Vector<4>(v) {} /** - * ctor from Vector + * setting ctor */ - Quaternion(const Vector &v); + Quaternion(const float d[4]) : Vector<4>(d) {} /** - * ctor from EulerAngles + * setting ctor */ - Quaternion(const EulerAngles &euler); + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {} + + using Vector<4>::operator *; /** - * ctor from Dcm + * multiplication */ - Quaternion(const Dcm &dcm); + const Quaternion operator *(const Quaternion &q) const { + return Quaternion( + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + } /** - * deep copy ctor + * derivative */ - Quaternion(const Quaternion &right); + const Quaternion derivative(const Vector<3> &w) { + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4, 4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; + } /** - * dtor + * imaginary part of quaternion */ - virtual ~Quaternion(); + Vector<3> imag(void) { + return Vector<3>(&data[1]); + } /** - * derivative + * set quaternion to rotation defined by euler angles */ - Vector derivative(const Vector &w); + void from_euler(float roll, float pitch, float yaw) { + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; + } + + void from_dcm(const Matrix<3, 3> &m) { + // avoiding singularities by not using division equations + data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]); + data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]); + data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]); + data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]); + } /** - * accessors + * create rotation matrix for the quaternion */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } + Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> R; + float aSq = data[0] * data[0]; + float bSq = data[1] * data[1]; + float cSq = data[2] * data[2]; + float dSq = data[3] * data[3]; + R.data[0][0] = aSq + bSq - cSq - dSq; + R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); + R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); + R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); + R.data[1][1] = aSq - bSq + cSq - dSq; + R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); + R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); + R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); + R.data[2][2] = aSq - bSq - cSq + dSq; + return R; + } }; -int __EXPORT quaternionTest(); -} // math +} +#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396..000000000 --- a/src/lib/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d5..c7323c215 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * Pavel Kirienko <pavel.kirienko@gmail.com> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +35,466 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Vector class */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include <nuttx/config.h> - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include <stdio.h> +#include <math.h> +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template <unsigned int N> +class __EXPORT Vector; + +template <unsigned int N> +class __EXPORT VectorBase +{ +public: + /** + * vector data + */ + float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + VectorBase() { + arm_col = {N, 1, &data[0]}; + } + + /** + * copy ctor + */ + VectorBase(const VectorBase<N> &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } + + /** + * setting ctor + */ + VectorBase(const float d[N]) { + arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access to elements by index + */ + float &operator()(const unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + float operator()(const unsigned int i) const { + return data[i]; + } + + /** + * get vector size + */ + unsigned int get_size() const { + return N; + } + + /** + * test for equality + */ + bool operator ==(const Vector<N> &v) const { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return false; + + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector<N> &v) const { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return true; + + return false; + } + + /** + * set to value + */ + const Vector<N> &operator =(const Vector<N> &v) { + memcpy(data, v.data, sizeof(data)); + return *static_cast<const Vector<N>*>(this); + } + + /** + * negation + */ + const Vector<N> operator -(void) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = -data[i]; + + return res; + } + + /** + * addition + */ + const Vector<N> operator +(const Vector<N> &v) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] + v.data[i]; + + return res; + } + + /** + * subtraction + */ + const Vector<N> operator -(const Vector<N> &v) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] - v.data[i]; + + return res; + } + + /** + * uniform scaling + */ + const Vector<N> operator *(const float num) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + + return res; + } + + /** + * uniform scaling + */ + const Vector<N> operator /(const float num) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + + return res; + } + + /** + * addition + */ + const Vector<N> &operator +=(const Vector<N> &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v.data[i]; + + return *static_cast<const Vector<N>*>(this); + } + + /** + * subtraction + */ + const Vector<N> &operator -=(const Vector<N> &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v.data[i]; + + return *static_cast<const Vector<N>*>(this); + } + + /** + * uniform scaling + */ + const Vector<N> &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + + return *static_cast<const Vector<N>*>(this); + } + + /** + * uniform scaling + */ + const Vector<N> &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + + return *static_cast<const Vector<N>*>(this); + } + + /** + * dot product + */ + float operator *(const Vector<N> &v) const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * v.data[i]; + + return res; + } + + /** + * element by element multiplication + */ + const Vector<N> emult(const Vector<N> &v) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * v.data[i]; + + return res; + } + + /** + * element by element division + */ + const Vector<N> edivide(const Vector<N> &v) const { + Vector<N> res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / v.data[i]; + + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return res; + } + + /** + * gets the length of this vector + */ + float length() const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return sqrtf(res); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector<N> normalized() const { + return *this / length(); + } + + /** + * set zero vector + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + void print(void) { + printf("[ "); + + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + + printf("]\n"); + } +}; + +template <unsigned int N> +class __EXPORT Vector : public VectorBase<N> +{ +public: + Vector() : VectorBase<N>() {} + + Vector(const Vector<N> &v) : VectorBase<N>(v) {} + + Vector(const float d[N]) : VectorBase<N>(d) {} + + /** + * set to value + */ + const Vector<N> &operator =(const Vector<N> &v) { + memcpy(this->data, v.data, sizeof(this->data)); + return *this; + } +}; + +template <> +class __EXPORT Vector<2> : public VectorBase<2> +{ +public: + Vector() : VectorBase<2>() {} + + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<2> &v) : VectorBase<2>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const float d[2]) : VectorBase<2>() { + data[0] = d[0]; + data[1] = d[1]; + } + + Vector(const float x, const float y) : VectorBase<2>() { + data[0] = x; + data[1] = y; + } + + /** + * set data + */ + void set(const float d[2]) { + data[0] = d[0]; + data[1] = d[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float operator %(const Vector<2> &v) const { + return data[0] * v.data[1] - data[1] * v.data[0]; + } +}; + +template <> +class __EXPORT Vector<3> : public VectorBase<3> +{ +public: + Vector() : VectorBase<3>() {} + + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<3> &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; + } + + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + Vector(const float x, const float y, const float z) : VectorBase<3>() { + data[0] = x; + data[1] = y; + data[2] = z; + } + + /** + * set data + */ + void set(const float d[3]) { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; + + return *this; + } + + Vector<3> operator %(const Vector<3> &v) const { + return Vector<3>( + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); + } +}; + +template <> +class __EXPORT Vector<4> : public VectorBase<4> +{ +public: + Vector() : VectorBase() {} + + Vector(const Vector<4> &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; + } + + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; + } + + Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { + data[0] = x0; + data[1] = x1; + data[2] = x2; + data[3] = x3; + } + + /** + * set data + */ + void set(const float d[4]) { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; + } + + /** + * set to value + */ + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; + + return *this; + } +}; + +} + +#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817..000000000 --- a/src/lib/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * 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 Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600e..000000000 --- a/src/lib/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02d..000000000 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.h - * - * matrix code - */ - -#pragma once - - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exponent; - float num = (*this)(i, j); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc15..000000000 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.h - * - * math vector - */ - -#pragma once - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exponent; - float num = (*this)(i); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447..000000000 --- a/src/lib/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.h - * - * matrix code - */ - -#pragma once - - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d..000000000 --- a/src/lib/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.h - * - * math vector - */ - -#pragma once - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc..9e03855c5 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,13 +41,9 @@ #pragma once -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" +#include "math/Vector.hpp" #include "math/Matrix.hpp" #include "math/Quaternion.hpp" -#include "math/Vector.hpp" -#include "math/Vector3.hpp" -#include "math/Vector2f.hpp" #include "math/Limits.hpp" #endif @@ -56,4 +52,4 @@ #include "CMSIS/Include/arm_math.h" -#endif
\ No newline at end of file +#endif diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 72bc7db8a..191e2da73 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,13 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ math/Limits.cpp # @@ -49,13 +42,3 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/lib/version/version.h index c6a9ba638..af733aaf0 100644 --- a/src/modules/sdlog2/sdlog2_version.h +++ b/src/lib/version/version.h @@ -33,15 +33,15 @@ ****************************************************************************/ /** - * @file sdlog2_version.h + * @file version.h * * Tools for system version detection. * * @author Anton Babushkin <anton.babushkin@me.com> */ -#ifndef SDLOG2_VERSION_H_ -#define SDLOG2_VERSION_H_ +#ifndef VERSION_H_ +#define VERSION_H_ /* GIT_VERSION is defined at build time via a Makefile call to the @@ -59,4 +59,4 @@ #define HW_ARCH "PX4FMU_V2" #endif -#endif /* SDLOG2_VERSION_H_ */ +#endif /* VERSION_H_ */ diff --git a/src/mainpage.dox b/src/mainpage.dox new file mode 100644 index 000000000..7ca410341 --- /dev/null +++ b/src/mainpage.dox @@ -0,0 +1,9 @@ +/** +\mainpage PX4 Autopilot Flight Control Stack and Middleware + +This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows. + +http://pixhawk.org + + +*/
\ No newline at end of file diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7..83145ac72 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), - // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), - // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), - // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), - // attitude representations - C_nb(), - q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz @@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -228,8 +222,8 @@ void KalmanNav::update() if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude\n"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -259,8 +253,8 @@ void KalmanNav::update() // lat/lon and not have init map_projection_init(lat0, lon0); _positionInitialized = true; - warnx("initialized EKF state with GPS\n"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(vN), double(vE), double(vD), lat, lon, double(alt)); } @@ -321,13 +315,12 @@ void KalmanNav::updatePublications() _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; - _pos.lat = getLatDegE7(); - _pos.lon = getLonDegE7(); + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication @@ -404,28 +397,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb = q.to_dcm(); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +542,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +570,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +586,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +605,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,17 +622,17 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { // abort correction and return - warnx("numerical failure in att correction\n"); + warnx("numerical failure in att correction"); // reset P matrix to P0 P = P0; return ret_error; @@ -669,7 +659,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +668,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +678,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,17 +688,17 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { // abort correction and return - warnx("numerical failure in gps correction\n"); + warnx("numerical failure in gps correction"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; @@ -735,7 +725,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a6..46ee4b6c8 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */ diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 372b2d162..3d20d4d2d 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 4096, + 8192, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4..66ec20b95 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -58,9 +58,13 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> +#include <lib/mathlib/mathlib.h> + #include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -214,6 +218,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -221,12 +229,32 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + uint64_t last_vel_t = 0; + + /* current velocity */ + math::Vector<3> vel; + vel.zero(); + /* previous velocity */ + math::Vector<3> vel_prev; + vel_prev.zero(); + /* actual acceleration (by GPS velocity) in body frame */ + math::Vector<3> acc; + acc.zero(); + /* rotation matrix */ + math::Matrix<3, 3> R; + R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); + /* subscribe to GPS */ + int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* subscribe to GPS */ + int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -265,6 +293,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* rotation matrix for magnetic declination */ + math::Matrix<3, 3> R_decl; + R_decl.identity(); + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -299,6 +331,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -307,6 +342,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + bool gps_updated; + orb_check(sub_gps, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + } + + bool global_pos_updated; + orb_check(sub_global_pos, &global_pos_updated); + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); + } + if (!initialized) { // XXX disabling init for now initialized = true; @@ -352,9 +399,50 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + hrt_abstime vel_t = 0; + bool vel_valid = false; + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + vel_valid = true; + if (gps_updated) { + vel_t = gps.timestamp_velocity; + vel(0) = gps.vel_n_m_s; + vel(1) = gps.vel_e_m_s; + vel(2) = gps.vel_d_m_s; + } + + } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + vel_valid = true; + if (global_pos_updated) { + vel_t = global_pos.timestamp; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; + } + } + + if (vel_valid) { + /* velocity is valid */ + if (vel_t != 0) { + /* velocity updated */ + if (last_vel_t != 0 && vel_t != last_vel_t) { + float vel_dt = (vel_t - last_vel_t) / 1000000.0f; + /* calculate acceleration in body frame */ + acc = R.transposed() * ((vel - vel_prev) / vel_dt); + } + last_vel_t = vel_t; + vel_prev = vel; + } + + } else { + /* velocity is valid, reset acceleration */ + acc.zero(); + vel_prev.zero(); + last_vel_t = 0; + } + + z_k[3] = raw.accelerometer_m_s2[0] - acc(0); + z_k[4] = raw.accelerometer_m_s2[1] - acc(1); + z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { @@ -425,7 +513,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) + if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; @@ -433,10 +521,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -445,12 +532,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - //att.yawspeed =z_k[2] ; + att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); + att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); + /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + /* magnetic declination */ + + math::Matrix<3, 3> R_body = (&Rot_matrix[0]); + R = R_decl * R_body; + /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3cfddf28e..44f47b47c 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -65,6 +65,11 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); +/* magnetic declination, in degrees */ +PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); + +PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -83,6 +88,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->pitch_off = param_find("ATT_PITCH_OFF3"); h->yaw_off = param_find("ATT_YAW_OFF3"); + h->mag_decl = param_find("ATT_MAG_DECL"); + + h->acc_comp = param_find("ATT_ACC_COMP"); + return OK; } @@ -103,5 +112,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->pitch_off, &(p->pitch_off)); param_get(h->yaw_off, &(p->yaw_off)); + param_get(h->mag_decl, &(p->mag_decl)); + + param_get(h->acc_comp, &(p->acc_comp)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 09817d58e..74a141609 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -47,12 +47,16 @@ struct attitude_estimator_ekf_params { float roll_off; float pitch_off; float yaw_off; + float mag_decl; + int acc_comp; }; struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; + param_t mag_decl; + param_t acc_comp; }; /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a..36b75dd58 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) 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); + math::Matrix<3,3> board_rotation; 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; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> 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); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e5318121..f579fb52a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -106,14 +106,9 @@ static const int ERROR = -1; extern struct system_load_s system_load; -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define MAVLINK_OPEN_INTERVAL 50000 @@ -204,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void print_reject_mode(const char *msg); @@ -376,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -410,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -452,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) ret = true; @@ -491,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -513,11 +512,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -566,7 +565,6 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -580,6 +578,27 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -609,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -666,8 +686,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ unsigned counter = 0; - unsigned low_voltage_counter = 0; - unsigned critical_voltage_counter = 0; unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; @@ -745,7 +763,6 @@ int commander_thread_main(int argc, char *argv[]) int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; /* Subscribe to subsystem info topic */ int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); @@ -889,13 +906,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - // warnx("bat v: %2.2f", battery.voltage_v); - - /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { - status.battery_voltage = battery.voltage_v; + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + status.battery_voltage = battery.voltage_filtered_v; + status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); } } @@ -948,46 +964,29 @@ int commander_thread_main(int argc, char *argv[]) //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - // XXX remove later - //warnx("bat remaining: %2.2f", status.battery_remaining); - /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { - //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; - status_changed = true; - battery_tune_played = false; - } - - low_voltage_counter++; + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status_changed = true; + battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; - - if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + battery_tune_played = false; - } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } + if (armed.armed) { + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); - status_changed = true; + } else { + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } - critical_voltage_counter++; - - } else { - - low_voltage_counter = 0; - critical_voltage_counter = 0; + status_changed = true; } /* End battery voltage check */ @@ -1029,19 +1028,18 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed && global_position.valid) { - /* copy position data to uORB home message, store it locally as well */ + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { @@ -1052,129 +1050,163 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); - status_changed = true; + /* start RC input check */ + if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + status_changed = true; } + } - status.rc_signal_lost = false; - - transition_result_t res; // store all transitions results here + status.rc_signal_lost = false; - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + transition_result_t res; // store all transitions results here - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * do it only for rotary wings */ - if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - } else { - stick_off_counter++; - } + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); + } else { + stick_off_counter = 0; + } - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } - - stick_on_counter = 0; + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - stick_on_counter++; + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else { stick_on_counter = 0; + + } else { + stick_on_counter++; } - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + } else { + stick_on_counter = 0; + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } + + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + } - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* fill status according to mode switches */ + check_mode_switches(&sp_man, &status); - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); + /* evaluate the main state machine according to mode switches */ + res = set_main_state_rc(&status); + + if (res == TRANSITION_CHANGED) { + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1190,13 +1222,24 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + /* print new state */ + if (arming_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1375,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { + switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state @@ -1449,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status) print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages print_reject_mode("SEATBELT"); // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 565b4b66a..21a1c4c2c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -44,6 +44,7 @@ #include <stdint.h> #include <stdbool.h> #include <fcntl.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } -float battery_remaining_estimate_voltage(float voltage) +float battery_remaining_estimate_voltage(float voltage, float discharged) { float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; + static param_t bat_v_empty_h; + static param_t bat_v_full_h; + static param_t bat_n_cells_h; + static param_t bat_capacity_h; + static float bat_v_empty = 3.2f; + static float bat_v_full = 4.0f; + static int bat_n_cells = 3; + static float bat_capacity = -1.0f; static bool initialized = false; static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); + bat_v_empty_h = param_find("BAT_V_EMPTY"); + bat_v_full_h = param_find("BAT_V_FULL"); + bat_n_cells_h = param_find("BAT_N_CELLS"); + bat_capacity_h = param_find("BAT_CAPACITY"); initialized = true; } - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); + param_get(bat_v_empty_h, &bat_v_empty); + param_get(bat_v_full_h, &bat_v_full); + param_get(bat_n_cells_h, &bat_n_cells); + param_get(bat_capacity_h, &bat_capacity); } counter++; - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + /* remaining charge estimate based on voltage */ + float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + + if (bat_capacity > 0.0f) { + /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ + ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { + /* else use voltage */ + ret = remaining_voltage; + } /* limit to sane values */ ret = (ret < 0.0f) ? 0.0f : ret; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e9514446c..d0393f45a 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode); void rgbled_set_pattern(rgbled_pattern_t *pattern); /** - * Provides a coarse estimate of remaining battery power. + * Estimate remaining battery charge. * - * The estimate is very basic and based on decharging voltage curves. + * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), + * else use simple estimate based on voltage. * * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage); +float battery_remaining_estimate_voltage(float voltage, float discharged); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 691d3efcb..d3155f7bf 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,6 +48,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..c7256583a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,11 +63,11 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -219,55 +220,54 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } + } - break; + break; - case MAIN_STATE_EASY: + case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - case MAIN_STATE_AUTO: + break; - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO: - break; + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; } - if (ret == TRANSITION_CHANGED) { - current_state->main_state = new_main_state; + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,63 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { ret = TRANSITION_NOT_CHANGED; + } - } else { + } else { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ + ret = TRANSITION_CHANGED; + break; - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: + case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; + } + + break; - default: - break; + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; } - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + break; + + case FAILSAFE_STATE_TERMINATION: + /* always allowed */ + ret = TRANSITION_CHANGED; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } + } - return ret; + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f3..f04879ff9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e213ac17f..e8fecef0d 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 8cc0d77d4..7c80c4b2b 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd); + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<mission_item_triplet_s> _missionCmd; + UOrbSubscription<position_setpoint_triplet_s> _missionCmd; UOrbSubscription<manual_control_setpoint_s> _manual; UOrbSubscription<vehicle_status_s> _status; UOrbSubscription<parameter_update_s> _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d..f7c0b6148 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index b4dbc36b0..e1c85c261 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - mission_item_triplet_s _lastMissionCmd; + position_setpoint_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e..888dd0942 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c94180d68..17b1028f9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { @@ -637,7 +637,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 04caf0bbc..a62b53221 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,7 +68,7 @@ #include <uORB/uORB.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> @@ -126,7 +126,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _mission_item_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -145,7 +145,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -175,7 +175,6 @@ private: /* takeoff/launch states */ bool launch_detected; bool usePreTakeoffThrust; - bool launch_detection_message_sent; /* Landingslope object */ Landingslope landingslope; @@ -193,7 +192,7 @@ private: uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid - math::Dcm _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -332,11 +331,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, - const struct mission_item_triplet_s &_mission_item_triplet); + bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -368,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _mission_item_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -397,8 +396,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false), - launch_detection_message_sent(false) + usePreTakeoffThrust(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -408,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - mission_item_triplet_s _mission_item_triplet = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; sensor_combined_s _sensor_combined = {0}; @@ -655,11 +653,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool mission_item_triplet_updated; - orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (mission_item_triplet_updated) { - orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); _setpoint_valid = true; } } @@ -702,25 +700,25 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid) { /* rotate ground speed vector with current attitude */ - math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; - if (mission_item_triplet.previous_valid) { - distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { - distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); @@ -752,12 +750,12 @@ void FixedwingPositionControl::navigation_capabilities_publish() } bool -FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, - const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); + calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -765,11 +763,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); - math::Vector3 accel_earth = _R_nb * accel_body; + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -787,70 +785,68 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ - math::Vector2f prev_wp; + math::Vector<2> prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp.setX(mission_item_triplet.previous.lat); - prev_wp.setY(mission_item_triplet.previous.lon); + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp.setX(mission_item_triplet.current.lat); - prev_wp.setY(mission_item_triplet.current.lon); + prev_wp(0) = pos_sp_triplet.current.lat; + prev_wp(1) = pos_sp_triplet.current.lon; } - if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (mission_item_triplet.previous_valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()); + if (pos_sp_triplet.previous.valid) { + target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; } @@ -881,23 +877,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // /* do not go down too early */ // if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; // } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -916,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = mission_item_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; } @@ -979,16 +975,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { -// warnx("Launch detection enabled"); - if(!launch_detection_message_sent) { + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { +// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - launch_detection_message_sent = true; + last_sent = hrt_absolute_time(); } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { @@ -1012,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -1023,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1037,15 +1034,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset land state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1054,10 +1051,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } /* reset takeoff/launch state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; - launch_detection_message_sent = false; } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1178,7 +1174,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1266,14 +1262,14 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _mission_item_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1287,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9f46b5170..7954d75c2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,21 +48,75 @@ * */ +/** + * L1 period + * + * This is the L1 distance and defines the tracking + * point ahead of the aircraft its following. + * A value of 25 meters works for most aircraft. Shorten + * slowly during tuning until response is sharp without oscillation. + * + * @min 1.0 + * @max 100.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); - +/** + * L1 damping + * + * Damping factor for L1 control. + * + * @min 0.6 + * @max 0.9 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); - +/** + * Default Loiter Radius + * + * This radius is used when no other loiter radius is set. + * + * @min 10.0 + * @max 100.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); - +/** + * Cruise throttle + * + * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); - +/** + * Negative pitch limit + * + * The minimum negative pitch the controller will output. + * + * @unit degrees + * @min -60.0 + * @max 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); - +/** + * Positive pitch limit + * + * The maximum positive pitch the controller will output. + * + * @unit degrees + * @min 0.0 + * @max 60.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index e2675dfa7..44bf77bb0 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -59,7 +59,6 @@ #include "mavlink_orb_listener.h" #include "mavlink_main.h" -//#include "util.h" void *uorb_receive_thread(void *arg); @@ -200,10 +199,10 @@ MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -333,13 +332,13 @@ MavlinkOrbListener::l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.0f, + (global_pos.alt - home.alt) * 1000.0f, + global_pos.vel_n * 100.0f, + global_pos.vel_e * 100.0f, + global_pos.vel_d * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } @@ -363,23 +362,18 @@ MavlinkOrbListener::l_local_position(const struct listener *l) void MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { - struct mission_item_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); + struct position_setpoint_triplet_s triplet; + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (!triplet.current_valid) + if (!triplet.current.valid) return; - if (triplet.current.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - if (gcs_link) mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), - coordinate_frame, + MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), + (int32_t)(triplet.current.alt * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } @@ -615,11 +609,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) void MavlinkOrbListener::l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void @@ -757,7 +749,7 @@ MavlinkOrbListener::uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ab4074558..4f763c3c6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -555,6 +555,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_gps.alt = gps.alt; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.timestamp_variance = gps.time_usec; hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -566,6 +567,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -613,9 +615,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); @@ -659,8 +661,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -675,9 +677,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 556b6f8ad..ea57714d2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,9 +53,8 @@ #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 86bfa26f2..bbc9f6e66 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp new file mode 100644 index 000000000..245ac024b --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -0,0 +1,779 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @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 mc_att_control_main.c + * Multicopter attitude controller. + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> +#include <lib/geo/geo.h> + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +#define MIN_TAKEOFF_THROTTLE 0.3f +#define YAW_DEADZONE 0.01f +#define RATES_I_LIMIT 0.5f + +class MulticopterAttitudeControl +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + + struct { + param_t att_p; + param_t yaw_p; + param_t att_rate_p; + param_t att_rate_i; + param_t att_rate_d; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + } _params; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterAttitudeControl *g_control; +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + +/* publications */ + _att_sp_pub(-1), + _rates_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + + _params.p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + + _rates_prev.zero(); + + _params_handles.att_p = param_find("MC_ATT_P"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); + _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + + /* fetch initial parameter values */ + parameters_update(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + param_get(_params_handles.att_p, &v); + _params.p(0) = v; + _params.p(1) = v; + param_get(_params_handles.yaw_p, &v); + _params.p(2) = v; + + param_get(_params_handles.att_rate_p, &v); + _params.rate_p(0) = v; + _params.rate_p(1) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + + param_get(_params_handles.att_rate_i, &v); + _params.rate_i(0) = v; + _params.rate_i(1) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + + param_get(_params_handles.att_rate_d, &v); + _params.rate_d(0) = v; + _params.rate_d(1) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + return OK; +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool control_mode_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_control_mode_sub, &control_mode_updated); + + if (control_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +void +MulticopterAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool arming_updated; + orb_check(_arming_sub, &arming_updated); + + if (arming_updated) { + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } +} + +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +MulticopterAttitudeControl::task_main() +{ + /* inform about start */ + warnx("started"); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit attitude updates to 100Hz */ + orb_set_interval(_att_sub, 10); + + parameters_update(); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + vehicle_setpoint_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + arming_status_poll(); + + /* setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + R_sp.identity(); + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.identity(); + + /* current angular rates */ + math::Vector<3> rates; + rates.zero(); + + /* angular rates integral error */ + math::Vector<3> rates_int; + rates_int.zero(); + + /* identity matrix */ + math::Matrix<3, 3> I; + I.identity(); + + math::Quaternion q; + + bool reset_yaw_sp = true; + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* copy the topic to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large dt's */ + if (dt > 0.02f) + dt = 0.02f; + + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + vehicle_setpoint_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } + + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } + + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + _att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; + } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } + + if (_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + } + + if (publish_att_sp) { + /* publish the attitude setpoint */ + _att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + } + + /* rotation matrix for current state */ + R.set(_att.R); + + /* current body angular rates */ + rates(0) = _att.rollspeed; + rates(1) = _att.pitchspeed; + rates(2) = _att.yawspeed; + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + q.from_dcm(R.transposed() * R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* angular rates setpoint*/ + math::Vector<3> rates_sp = _params.p.emult(e_R); + + /* feed forward yaw setpoint rate */ + rates_sp(2) += yaw_sp_move_rate * yaw_w; + + /* reset integral if disarmed */ + // TODO add LANDED flag here + if (!_arming.armed) { + rates_int.zero(); + } + + /* rate controller */ + math::Vector<3> rates_err = rates_sp - rates; + math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; + _rates_prev = rates; + + /* update integral */ + for (int i = 0; i < 3; i++) { + float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i)) { + if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { + rates_int(i) = rate_i; + } + } + } + + /* publish the attitude rates setpoint */ + _rates_sp.roll = rates_sp(0); + _rates_sp.pitch = rates_sp(1); + _rates_sp.yaw = rates_sp(2); + _rates_sp.thrust = _att_sp.thrust; + _rates_sp.timestamp = hrt_absolute_time(); + + if (_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); + + } else { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } + + /* publish the attitude controls */ + if (_control_mode.flag_control_rates_enabled) { + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + } else { + /* controller disabled, publish zero attitude controls */ + _actuators.control[0] = 0.0f; + _actuators.control[1] = 0.0f; + _actuators.control[2] = 0.0f; + _actuators.control[3] = 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + + perf_end(_loop_perf); + } + + warnx("exit"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new MulticopterAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/modules/mc_att_control/mc_att_control_params.c index 7ea6496bb..a170365ee 100644 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @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 @@ -32,9 +35,19 @@ ****************************************************************************/ /** - * @file Vector.cpp - * - * math vector + * @file mc_att_control_params.c + * Parameters for multicopter attitude controller. */ -#include "Vector.hpp" +#include <systemlib/param/param.h> + +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_att_control/module.mk index bc4b48fb4..64b876f69 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor position control +# Multirotor attitude controller (vector based, no Euler singularities) # -MODULE_COMMAND = multirotor_pos_control +MODULE_COMMAND = mc_att_control -SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c \ - thrust_pid.c +SRCS = mc_att_control_main.cpp \ + mc_att_control_params.c diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp new file mode 100644 index 000000000..d3e39e3a0 --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -0,0 +1,1031 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @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 mc_pos_control_main.cpp + * Multicopter position controller. + * + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> +#include <lib/geo/geo.h> +#include <mavlink/mavlink_log.h> + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f + +/** + * Multicopter position control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + int _local_pos_sub; /**< vehicle local position */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ + + orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + + struct { + param_t thr_min; + param_t thr_max; + param_t z_p; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t z_ff; + param_t xy_p; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; + param_t xy_ff; + param_t tilt_max; + param_t land_speed; + param_t land_tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max; + float land_speed; + float land_tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + math::Vector<3> _pos; + math::Vector<3> _vel; + math::Vector<3> _pos_sp; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + + /** + * Update our local parameter cache. + */ + int parameters_update(bool force); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in subscribed topics. + */ + void poll_subscriptions(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace pos_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterPositionControl *g_control; +} + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + _local_pos_sub(-1), + _pos_sp_triplet_sub(-1), + +/* publications */ + _local_pos_sp_pub(-1), + _att_sp_pub(-1), + _global_vel_sp_pub(-1) +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); + memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _vel.zero(); + _pos_sp.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + + _params_handles.thr_min = param_find("MPC_THR_MIN"); + _params_handles.thr_max = param_find("MPC_THR_MAX"); + _params_handles.z_p = param_find("MPC_Z_P"); + _params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); + _params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); + _params_handles.z_vel_d = param_find("MPC_Z_VEL_D"); + _params_handles.z_vel_max = param_find("MPC_Z_VEL_MAX"); + _params_handles.z_ff = param_find("MPC_Z_FF"); + _params_handles.xy_p = param_find("MPC_XY_P"); + _params_handles.xy_vel_p = param_find("MPC_XY_VEL_P"); + _params_handles.xy_vel_i = param_find("MPC_XY_VEL_I"); + _params_handles.xy_vel_d = param_find("MPC_XY_VEL_D"); + _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); + _params_handles.xy_ff = param_find("MPC_XY_FF"); + _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + + /* fetch initial parameter values */ + parameters_update(true); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + pos_control::g_control = nullptr; +} + +int +MulticopterPositionControl::parameters_update(bool force) +{ + bool updated; + struct parameter_update_s param_upd; + + orb_check(_params_sub, &updated); + + if (updated) + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + + if (updated || force) { + param_get(_params_handles.thr_min, &_params.thr_min); + param_get(_params_handles.thr_max, &_params.thr_max); + param_get(_params_handles.tilt_max, &_params.tilt_max); + param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); + param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); + param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + + float v; + param_get(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + param_get(_params_handles.z_p, &v); + _params.pos_p(2) = v; + param_get(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + param_get(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + param_get(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + param_get(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + param_get(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + param_get(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + param_get(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + param_get(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + param_get(_params_handles.xy_ff, &v); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + param_get(_params_handles.z_ff, &v); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + } + + return OK; +} + +void +MulticopterPositionControl::poll_subscriptions() +{ + bool updated; + + orb_check(_att_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + orb_check(_att_sp_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + + orb_check(_control_mode_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + orb_check(_manual_sub, &updated); + + if (updated) + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + + orb_check(_arming_sub, &updated); + + if (updated) + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); +} + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + pos_control::g_control->task_main(); +} + +void +MulticopterPositionControl::task_main() +{ + warnx("started"); + + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); + + /* + * do subscriptions + */ + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + parameters_update(true); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + poll_subscriptions(); + + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + + hrt_abstime t_prev = 0; + + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + + hrt_abstime ref_timestamp = 0; + int32_t ref_lat = 0.0f; + int32_t ref_lon = 0.0f; + float ref_alt = 0.0f; + + math::Vector<3> sp_move_rate; + sp_move_rate.zero(); + math::Vector<3> thrust_int; + thrust_int.zero(); + math::Matrix<3, 3> R; + R.identity(); + + /* wakeup source */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _local_pos_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + + poll_subscriptions(); + parameters_update(false); + + hrt_abstime t = hrt_absolute_time(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; + + if (_control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode.flag_armed; + + if (_control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_climb_rate_enabled || + _control_mode.flag_control_velocity_enabled) { + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; + + sp_move_rate.zero(); + + if (_local_pos.ref_timestamp != ref_timestamp) { + /* initialize local projection with new reference */ + double lat_home = _local_pos.ref_lat * 1e-7; + double lon_home = _local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + + if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { + /* correct setpoint in manual mode to stay in the same point */ + float ref_change_x = 0.0f; + float ref_change_y = 0.0f; + map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); + _pos_sp(0) += ref_change_x; + _pos_sp(1) += ref_change_y; + _pos_sp(2) += _local_pos.ref_alt - ref_alt; + } + + ref_timestamp = _local_pos.ref_timestamp; + ref_lat = _local_pos.ref_lat; + ref_lon = _local_pos.ref_lon; + ref_alt = _local_pos.ref_alt; + } + + if (_control_mode.flag_control_manual_enabled) { + /* manual control */ + if (_control_mode.flag_control_altitude_enabled) { + /* reset setpoint Z to current altitude if needed */ + if (reset_sp_z) { + reset_sp_z = false; + _pos_sp(2) = _pos(2); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); + } + + /* move altitude setpoint with throttle stick */ + sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset setpoint XY to current position if needed */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + } + + /* move position setpoint with roll/pitch stick */ + sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); + sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + } + + /* limit setpoint move rate */ + float sp_move_norm = sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + sp_move_rate /= sp_move_norm; + } + + /* scale to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); + + /* move position setpoint */ + _pos_sp += sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } + + } else { + /* AUTO */ + if (_pos_sp_triplet.current.valid) { + struct position_setpoint_s current_sp = _pos_sp_triplet.current; + + _pos_sp(2) = -(current_sp.alt - ref_alt); + + map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); + + if (isfinite(current_sp.yaw)) { + _att_sp.yaw_body = current_sp.yaw; + } + + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + + if (reset_sp_z) { + reset_sp_z = false; + _pos_sp(2) = _pos(2); + } + } + } + + /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ + _local_pos_sp.yaw = _att_sp.yaw_body; + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + + if (!_control_mode.flag_control_altitude_enabled) { + reset_sp_z = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode.flag_control_position_enabled) { + reset_sp_xy = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_manual_enabled) { + /* use constant descend rate when landing, ignore altitude setpoint */ + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + /* limit 3D speed only in AUTO mode */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } + } + + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } + + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual.throttle; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + thrust_int(2) = -i; + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + } else { + reset_int_z = true; + } + + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } + + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + } + + _att_sp.thrust = thrust_abs; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } else { + reset_int_z = true; + } + + } else { + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + } + + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_pos_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (pos_control::g_control != nullptr) + errx(1, "already running"); + + pos_control::g_control = new MulticopterPositionControl; + + if (pos_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != pos_control::g_control->start()) { + delete pos_control::g_control; + pos_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (pos_control::g_control == nullptr) + errx(1, "not running"); + + delete pos_control::g_control; + pos_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (pos_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/modules/mc_pos_control/mc_pos_control_params.c index ecd62e81c..9eb56545d 100644 --- a/src/lib/mathlib/math/Vector2f.hpp +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @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 @@ -32,48 +33,26 @@ ****************************************************************************/ /** - * @file Vector2f.hpp - * - * math 3 vector + * @file mc_pos_control_params.c + * Multicopter position controller parameters. */ -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math - +#include <systemlib/param/param.h> + +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/mc_pos_control/module.mk index 7569e1c7e..0b566d7bd 100755..100644 --- a/src/modules/multirotor_att_control/module.mk +++ b/src/modules/mc_pos_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor attitude controller +# Build multicopter position controller # -MODULE_COMMAND = multirotor_att_control +MODULE_COMMAND = mc_pos_control -SRCS = multirotor_att_control_main.c \ - multirotor_attitude_control.c \ - multirotor_rate_control.c +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_params.c diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c deleted file mode 100644 index 111e9197f..000000000 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * 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 multirotor_att_control_main.c - * - * Implementation of multirotor attitude control main loop. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <stdbool.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <debug.h> -#include <getopt.h> -#include <time.h> -#include <math.h> -#include <poll.h> -#include <sys/prctl.h> -#include <drivers/drv_hrt.h> -#include <uORB/uORB.h> -#include <drivers/drv_gyro.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/parameter_update.h> - -#include <systemlib/perf_counter.h> -#include <systemlib/systemlib.h> -#include <systemlib/param/param.h> - -#include "multirotor_attitude_control.h" -#include "multirotor_rate_control.h" - -__EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static bool thread_should_exit; -static int mc_task; -static bool motor_test_mode = false; -static const float min_takeoff_throttle = 0.3f; -static const float yaw_deadzone = 0.01f; - -static int -mc_thread_main(int argc, char *argv[]) -{ - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_status_s status; - memset(&status, 0, sizeof(status)); - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - /* subscribe */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - - warnx("starting"); - - /* store last control mode to detect mode switches */ - bool control_yaw_position = true; - bool reset_yaw_sp = true; - - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - - } else if (ret > 0) { - /* only run controller if attitude changed */ - perf_begin(mc_loop_perf); - - /* attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - - bool updated; - - /* parameters */ - orb_check(parameter_update_sub, &updated); - - if (updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - /* update parameters */ - } - - /* control mode */ - orb_check(vehicle_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); - } - - /* manual control setpoint */ - orb_check(manual_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); - } - - /* attitude setpoint */ - orb_check(vehicle_attitude_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - } - - /* offboard control setpoint */ - orb_check(offboard_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); - } - - /* vehicle status */ - orb_check(vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); - } - - /* sensors */ - orb_check(sensor_combined_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - } - - /* set flag to safe value */ - control_yaw_position = true; - - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* define which input is the dominating control input */ - if (control_mode.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = true; - - } else if (control_mode.flag_control_manual_enabled) { - /* manual input */ - if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - if (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; - - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } - - } else { - /* only move yaw setpoint if manual input is != 0 */ - if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { - /* control yaw rate */ - control_yaw_position = false; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } - } - - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } - - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; - } - - } else { - if (!control_mode.flag_control_attitude_enabled) { - /* no control, try to stay on place */ - if (!control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } - - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; - } - - /* check if we should we reset integrals */ - bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - - /* run attitude controller if needed */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - - /* run rates controller if needed */ - if (control_mode.flag_control_rates_enabled) { - /* get current rate setpoint */ - bool rates_sp_updated = false; - orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); - - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); - } - - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - rates[1] = att.pitchspeed; - rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); - - } else { - /* rates controller disabled, set actuators to zero for safety */ - actuators.control[0] = 0.0f; - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - 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); - - perf_end(mc_loop_perf); - } - } - - warnx("stopping, disarming motors"); - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(vehicle_attitude_sub); - close(vehicle_control_mode_sub); - close(manual_control_setpoint_sub); - close(actuator_pub); - close(att_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - exit(0); -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n"); - fprintf(stderr, " <mode> is 'rates' or 'attitude'\n"); - fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); - exit(1); -} - -int multirotor_att_control_main(int argc, char *argv[]) -{ - int ch; - unsigned int optioncount = 0; - - while ((ch = getopt(argc, argv, "tm:")) != EOF) { - switch (ch) { - case 't': - motor_test_mode = true; - optioncount += 1; - break; - - case ':': - usage("missing parameter"); - break; - - default: - fprintf(stderr, "option: -%c\n", ch); - usage("unrecognized option"); - break; - } - } - - argc -= optioncount; - //argv += optioncount; - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1 + optioncount], "start")) { - - thread_should_exit = false; - mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); - exit(0); - } - - if (!strcmp(argv[1 + optioncount], "stop")) { - thread_should_exit = true; - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c deleted file mode 100644 index 8245aa560..000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ /dev/null @@ -1,254 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Laurens Mackay <mackayl@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> - * Martin Rutschmann <rutmarti@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * 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 multirotor_attitude_control.c - * - * Implementation of attitude controller for multirotors. - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include "multirotor_attitude_control.h" -#include <stdio.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdint.h> -#include <stdbool.h> -#include <float.h> -#include <math.h> -#include <systemlib/pid/pid.h> -#include <systemlib/param/param.h> -#include <drivers/drv_hrt.h> - -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); - -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); - -struct mc_att_control_params { - float yaw_p; - float yaw_i; - float yaw_d; - //float yaw_awu; - //float yaw_lim; - - float att_p; - float att_i; - float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; -}; - -struct mc_att_control_param_handles { - param_t yaw_p; - param_t yaw_i; - param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; - - param_t att_p; - param_t att_i; - param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); - - -static int parameters_init(struct mc_att_control_param_handles *h) -{ - /* PID parameters */ - h->yaw_p = param_find("MC_YAWPOS_P"); - h->yaw_i = param_find("MC_YAWPOS_I"); - h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); - - h->att_p = param_find("MC_ATT_P"); - h->att_i = param_find("MC_ATT_I"); - h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); - - return OK; -} - -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) -{ - param_get(h->yaw_p, &(p->yaw_p)); - param_get(h->yaw_i, &(p->yaw_i)); - param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); - - param_get(h->att_p, &(p->att_p)); - param_get(h->att_i, &(p->att_i)); - param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); - - return OK; -} - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) -{ - static uint64_t last_run = 0; - static uint64_t last_input = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (last_input != att_sp->timestamp) { - last_input = att_sp->timestamp; - } - - static int motor_skip_counter = 0; - - static PID_t pitch_controller; - static PID_t roll_controller; - - static struct mc_att_control_params p; - static struct mc_att_control_param_handles h; - - static bool initialized = false; - - static float yaw_error; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - - initialized = true; - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - - /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_controller); - pid_reset_integral(&roll_controller); - //TODO pid_reset_integral(&yaw_controller); - } - - /* calculate current control outputs */ - - /* control pitch (forward) output */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); - - /* control roll (left/right) output */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); - - if (control_yaw_position) { - /* control yaw rate */ - // TODO use pid lib - - /* positive error: rotate to right, negative error, rotate to left (NED frame) */ - // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); - - yaw_error = att_sp->yaw_body - att->yaw; - - if (yaw_error > M_PI_F) { - yaw_error -= M_TWOPI_F; - - } else if (yaw_error < -M_PI_F) { - yaw_error += M_TWOPI_F; - } - - rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); - } - - rates_sp->thrust = att_sp->thrust; - //need to update the timestamp now that we've touched rates_sp - rates_sp->timestamp = hrt_absolute_time(); - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h deleted file mode 100644 index 431a435f7..000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Laurens Mackay <mackayl@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> - * Martin Rutschmann <rutmarti@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * 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 multirotor_attitude_control.h - * - * Definition of attitude controller for multirotors. - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ -#define MULTIROTOR_ATTITUDE_CONTROL_H_ - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/actuator_controls.h> - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); - -#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c deleted file mode 100644 index 86ac0e4ff..000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> - * Julian Oes <joes@student.ethz.ch> - * - * 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 multirotor_rate_control.c - * - * Implementation of rate controller for multirotors. - * - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> - * @author Julian Oes <joes@student.ethz.ch> - */ - -#include "multirotor_rate_control.h" -#include <stdio.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdint.h> -#include <stdbool.h> -#include <float.h> -#include <math.h> -#include <systemlib/pid/pid.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> -#include <drivers/drv_hrt.h> - -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); - -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); - -struct mc_rate_control_params { - - float yawrate_p; - float yawrate_d; - float yawrate_i; - - float attrate_p; - float attrate_d; - float attrate_i; - - float rate_lim; -}; - -struct mc_rate_control_param_handles { - - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_d; - - param_t attrate_p; - param_t attrate_i; - param_t attrate_d; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_rate_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p); - - -static int parameters_init(struct mc_rate_control_param_handles *h) -{ - /* PID parameters */ - h->yawrate_p = param_find("MC_YAWRATE_P"); - h->yawrate_i = param_find("MC_YAWRATE_I"); - h->yawrate_d = param_find("MC_YAWRATE_D"); - - h->attrate_p = param_find("MC_ATTRATE_P"); - h->attrate_i = param_find("MC_ATTRATE_I"); - h->attrate_d = param_find("MC_ATTRATE_D"); - - return OK; -} - -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p) -{ - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_d, &(p->yawrate_d)); - - param_get(h->attrate_p, &(p->attrate_p)); - param_get(h->attrate_i, &(p->attrate_i)); - param_get(h->attrate_d, &(p->attrate_d)); - - return OK; -} - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral) -{ - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - static uint64_t last_input = 0; - - if (last_input != rate_sp->timestamp) { - last_input = rate_sp->timestamp; - } - - last_run = hrt_absolute_time(); - - static int motor_skip_counter = 0; - - static PID_t pitch_rate_controller; - static PID_t roll_rate_controller; - static PID_t yaw_rate_controller; - - static struct mc_rate_control_params p; - static struct mc_rate_control_param_handles h; - - static bool initialized = false; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - initialized = true; - - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 2500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_rate_controller); - pid_reset_integral(&roll_rate_controller); - pid_reset_integral(&yaw_rate_controller); - } - - /* run pitch, roll and yaw controllers */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_control; - actuators->control[3] = rate_sp->thrust; - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h deleted file mode 100644 index ca7794c59..000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Laurens Mackay <mackayl@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> - * Martin Rutschmann <rutmarti@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * 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 multirotor_attitude_control.h - * - * Definition of rate controller for multirotors. - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef MULTIROTOR_RATE_CONTROL_H_ -#define MULTIROTOR_RATE_CONTROL_H_ - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/actuator_controls.h> - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral); - -#endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c deleted file mode 100644 index 2ca650420..000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ /dev/null @@ -1,553 +0,0 @@ -/**************************************************************************** - * - * 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 multirotor_pos_control.c - * - * Multirotor position controller - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <math.h> -#include <stdbool.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <debug.h> -#include <termios.h> -#include <time.h> -#include <sys/prctl.h> -#include <drivers/drv_hrt.h> -#include <uORB/uORB.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_global_velocity_setpoint.h> -#include <uORB/topics/mission_item_triplet.h> -#include <systemlib/systemlib.h> -#include <systemlib/pid/pid.h> -#include <mavlink/mavlink_log.h> - -#include "multirotor_pos_control_params.h" -#include "thrust_pid.h" - - -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 */ - -__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int multirotor_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static float scale_control(float ctl, float end, float dz); - -static float norm(float x, float y); - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * 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_spawn(). - */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - warnx("start"); - thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("app is running"); - - } else { - warnx("app not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static float scale_control(float ctl, float end, float dz) -{ - if (ctl > dz) { - return (ctl - dz) / (end - dz); - - } else if (ctl < -dz) { - return (ctl + dz) / (end - dz); - - } else { - return 0.0f; - } -} - -static float norm(float x, float y) -{ - return sqrtf(x * x + y * y); -} - -static int multirotor_pos_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); - - /* structures */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct mission_item_triplet_s triplet; - memset(&triplet, 0, sizeof(triplet)); - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(global_vel_sp)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); - - /* publish setpoint */ - orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; - bool reset_int_z = true; - bool reset_int_z_manual = false; - bool reset_int_xy = true; - bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; - bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; - - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - uint64_t local_ref_timestamp = 0; - - PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; - PID_t z_pos_pid; - thrust_pid_t z_vel_pid; - - thread_running = true; - - struct multirotor_position_control_params params; - struct multirotor_position_control_param_handles params_h; - parameters_init(¶ms_h); - parameters_update(¶ms_h, ¶ms); - - - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - } - - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - - while (!thread_should_exit) { - - bool param_updated; - orb_check(param_sub, ¶m_updated); - - if (param_updated) { - /* clear updated flag */ - struct parameter_update_s ps; - orb_copy(ORB_ID(parameter_update), param_sub, &ps); - /* update params */ - parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); - } - - bool updated; - - orb_check(control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } - - orb_check(mission_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); - global_pos_sp_valid = triplet.current_valid; - reset_mission_sp = true; - } - - hrt_abstime t = hrt_absolute_time(); - float dt; - - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - - } else { - dt = 0.0f; - } - - if (control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; - reset_takeoff_sp = true; - reset_int_z = true; - reset_int_xy = true; - } - - was_armed = control_mode.flag_armed; - - t_prev = t; - - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; - float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ - if (control_mode.flag_control_altitude_enabled) { - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); - } - - /* move altitude setpoint with throttle stick */ - float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - - if (z_sp_ctl != 0.0f) { - sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; - local_pos_sp.z += sp_move_rate[2] * dt; - - if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { - local_pos_sp.z = local_pos.z + z_sp_offs_max; - - } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { - local_pos_sp.z = local_pos.z - z_sp_offs_max; - } - } - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - /* move position setpoint with roll/pitch stick */ - float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - - if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { - /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; - sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - local_pos_sp.x += sp_move_rate[0] * dt; - local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction - * fail safe, should not happen in normal operation */ - float pos_vec_x = local_pos_sp.x - local_pos.x; - float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; - - if (pos_vec_norm > 1.0f) { - local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; - local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; - } - } - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !control_mode.flag_control_position_enabled; - reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; - reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; - } - /* AUTO not implemented */ - - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - - /* run position & altitude controllers, calculate velocity setpoint */ - if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; - - } else { - reset_man_sp_z = true; - global_vel_sp.vz = 0.0f; - } - - if (control_mode.flag_control_position_enabled) { - /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; - - /* limit horizontal speed */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - - } else { - reset_man_sp_xy = true; - global_vel_sp.vx = 0.0f; - global_vel_sp.vy = 0.0f; - } - - /* publish new velocity setpoint */ - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subscribe to velocity setpoint if altitude/position control disabled - - if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ - float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - - if (reset_int_z_manual) { - i = manual.throttle; - - if (i < params.thr_min) { - i = params.thr_min; - - } else if (i > params.thr_max) { - i = params.thr_max; - } - } - - thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); - att_sp.thrust = -thrust_sp[2]; - - } else { - /* reset thrust integral when altitude control enabled */ - reset_int_z = true; - } - - if (control_mode.flag_control_velocity_enabled) { - /* calculate thrust set point in NED frame */ - if (reset_int_xy) { - reset_int_xy = false; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); - } - - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); - - /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ - /* limit horizontal part of thrust */ - float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - /* assuming that vertical component of thrust is g, - * horizontal component = g * tan(alpha) */ - float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - - if (tilt > params.tilt_max) { - tilt = params.tilt_max; - } - - /* convert direction to body frame */ - thrust_xy_dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * tilt; - att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); - - } else { - reset_int_xy = true; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - } else { - /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_int_z = true; - reset_int_xy = true; - reset_mission_sp = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - - /* run at approximately 50 Hz */ - usleep(20000); - } - - warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); - - thread_running = false; - - fflush(stdout); - return 0; -} - diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c deleted file mode 100644 index b7041e4d5..000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * 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 multirotor_pos_control_params.c - * - * Parameters for multirotor_pos_control - */ - -#include "multirotor_pos_control_params.h" - -/* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); -PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); -PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); - -int parameters_init(struct multirotor_position_control_param_handles *h) -{ - h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); - h->thr_min = param_find("MPC_THR_MIN"); - h->thr_max = param_find("MPC_THR_MAX"); - h->z_p = param_find("MPC_Z_P"); - h->z_d = param_find("MPC_Z_D"); - h->z_vel_p = param_find("MPC_Z_VEL_P"); - h->z_vel_i = param_find("MPC_Z_VEL_I"); - h->z_vel_d = param_find("MPC_Z_VEL_D"); - h->z_vel_max = param_find("MPC_Z_VEL_MAX"); - h->xy_p = param_find("MPC_XY_P"); - h->xy_d = param_find("MPC_XY_D"); - h->xy_vel_p = param_find("MPC_XY_VEL_P"); - h->xy_vel_i = param_find("MPC_XY_VEL_I"); - h->xy_vel_d = param_find("MPC_XY_VEL_D"); - h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->tilt_max = param_find("MPC_TILT_MAX"); - - h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); - h->rc_scale_roll = param_find("RC_SCALE_ROLL"); - h->rc_scale_yaw = param_find("RC_SCALE_YAW"); - - return OK; -} - -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) -{ - param_get(h->takeoff_alt, &(p->takeoff_alt)); - param_get(h->takeoff_gap, &(p->takeoff_gap)); - param_get(h->thr_min, &(p->thr_min)); - param_get(h->thr_max, &(p->thr_max)); - param_get(h->z_p, &(p->z_p)); - param_get(h->z_d, &(p->z_d)); - param_get(h->z_vel_p, &(p->z_vel_p)); - param_get(h->z_vel_i, &(p->z_vel_i)); - param_get(h->z_vel_d, &(p->z_vel_d)); - param_get(h->z_vel_max, &(p->z_vel_max)); - param_get(h->xy_p, &(p->xy_p)); - param_get(h->xy_d, &(p->xy_d)); - param_get(h->xy_vel_p, &(p->xy_vel_p)); - param_get(h->xy_vel_i, &(p->xy_vel_i)); - param_get(h->xy_vel_d, &(p->xy_vel_d)); - param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->tilt_max, &(p->tilt_max)); - - param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); - param_get(h->rc_scale_roll, &(p->rc_scale_roll)); - param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); - - return OK; -} diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h deleted file mode 100644 index fc658dadb..000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * 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 multirotor_pos_control_params.h - * - * Parameters for multirotor_pos_control - */ - -#include <systemlib/param/param.h> - -struct multirotor_position_control_params { - float takeoff_alt; - float takeoff_gap; - float thr_min; - float thr_max; - float z_p; - float z_d; - float z_vel_p; - float z_vel_i; - float z_vel_d; - float z_vel_max; - float xy_p; - float xy_d; - float xy_vel_p; - float xy_vel_i; - float xy_vel_d; - float xy_vel_max; - float tilt_max; - - float rc_scale_pitch; - float rc_scale_roll; - float rc_scale_yaw; -}; - -struct multirotor_position_control_param_handles { - param_t takeoff_alt; - param_t takeoff_gap; - param_t thr_min; - param_t thr_max; - param_t z_p; - param_t z_d; - param_t z_vel_p; - param_t z_vel_i; - param_t z_vel_d; - param_t z_vel_max; - param_t xy_p; - param_t xy_d; - param_t xy_vel_p; - param_t xy_vel_i; - param_t xy_vel_d; - param_t xy_vel_max; - param_t tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; - param_t rc_scale_yaw; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct multirotor_position_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c deleted file mode 100644 index b985630ae..000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ /dev/null @@ -1,189 +0,0 @@ -/**************************************************************************** - * - * 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 thrust_pid.c - * - * Implementation of thrust control PID. - * - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#include "thrust_pid.h" -#include <math.h> - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) -{ - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->limit_min = limit_min; - pid->limit_max = limit_max; - pid->mode = mode; - pid->dt_min = dt_min; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; - pid->integral = 0.0f; -} - -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) -{ - int ret = 0; - - if (isfinite(kp)) { - pid->kp = kp; - - } else { - ret = 1; - } - - if (isfinite(ki)) { - pid->ki = ki; - - } else { - ret = 1; - } - - if (isfinite(kd)) { - pid->kd = kd; - - } else { - ret = 1; - } - - if (isfinite(limit_min)) { - pid->limit_min = limit_min; - - } else { - ret = 1; - } - - if (isfinite(limit_max)) { - pid->limit_max = limit_max; - - } else { - ret = 1; - } - - return ret; -} - -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) -{ - /* Alternative integral component calculation - * - * start: - * error = setpoint - current_value - * integral = integral + (Ki * error * dt) - * derivative = (error - previous_error) / dt - * previous_error = error - * output = (Kp * error) + integral + (Kd * derivative) - * wait(dt) - * goto start - */ - - if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { - return pid->last_output; - } - - float i, d; - pid->sp = sp; - - // Calculated current error value - float error = pid->sp - val; - - // Calculate or measured current error derivative - if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; - - } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else { - d = 0.0f; - } - - if (!isfinite(d)) { - d = 0.0f; - } - - /* calculate the error integral */ - i = pid->integral + (pid->ki * error * dt); - - /* attitude-thrust compensation - * r22 is (2, 2) componet of rotation matrix for current attitude */ - float att_comp; - - if (r22 > 0.8f) - att_comp = 1.0f / r22; - else if (r22 > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; - else - att_comp = 1.0f; - - /* calculate output */ - float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; - - /* check for saturation */ - if (output < pid->limit_min || output > pid->limit_max) { - /* saturated, recalculate output with old integral */ - output = (error * pid->kp) + pid->integral + (d * pid->kd); - - } else { - if (isfinite(i)) { - pid->integral = i; - } - } - - if (isfinite(output)) { - if (output > pid->limit_max) { - output = pid->limit_max; - - } else if (output < pid->limit_min) { - output = pid->limit_min; - } - - pid->last_output = output; - } - - return pid->last_output; -} - -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) -{ - pid->integral = i; -} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h deleted file mode 100644 index 5e169c1ba..000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * 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 thrust_pid.h - * - * Definition of thrust control PID interface. - * - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#ifndef THRUST_PID_H_ -#define THRUST_PID_H_ - -#include <stdint.h> - -__BEGIN_DECLS - -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ -#define THRUST_PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ -#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 - -typedef struct { - float kp; - float ki; - float kd; - float sp; - float integral; - float error_previous; - float last_output; - float limit_min; - float limit_max; - float dt_min; - uint8_t mode; -} thrust_pid_t; - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); - -__END_DECLS - -#endif /* THRUST_PID_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 63952be6e..9c5e62be7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -62,7 +62,7 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h> @@ -150,7 +150,7 @@ private: int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -158,16 +158,18 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct mission_item_s _mission_item; /**< current mission item */ + bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -184,6 +186,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + char *nav_states_str[NAV_STATE_MAX]; + struct { float min_altitude; float acceptance_radius; @@ -192,6 +196,7 @@ private: float takeoff_alt; float land_alt; float rtl_alt; + float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { @@ -202,6 +207,7 @@ private: param_t takeoff_alt; param_t land_alt; param_t rtl_alt; + param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -210,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -286,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -311,7 +318,7 @@ private: /** * Move to next waypoint */ - void advance_mission(); + void set_mission_item(); /** * Switch to next RTL state @@ -319,35 +326,24 @@ private: void set_rtl_item(); /** - * Helper function to get a loiter item + * Set position setpoint for mission item */ - void get_loiter_item(mission_item_s *item); + void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); /** * Helper function to get a takeoff item */ - void get_takeoff_item(mission_item_s *item); + void get_takeoff_setpoint(position_setpoint_s *pos_sp); /** * Publish a new mission item triplet for position controller */ - void publish_mission_item_triplet(); + void publish_position_setpoint_triplet(); /** * Publish vehicle_control_mode topic for controllers */ void publish_control_mode(); - - - /** - * Compare two mission items if they are equivalent - * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. - * - * @return true if equivalent, false otherwise - */ - bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); - - void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -362,7 +358,7 @@ namespace navigator Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -381,7 +377,7 @@ Navigator::Navigator() : _capabilities_sub(-1), /* publications */ - _triplet_pub(-1), + _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -398,6 +394,7 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), + _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), _geofence_violation_warning_sent(false) @@ -409,15 +406,19 @@ Navigator::Navigator() : _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); + _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); - + memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_mission_item, 0, sizeof(struct mission_item_s)); + + memset(&nav_states_str, 0, sizeof(nav_states_str)); + nav_states_str[0] = "NONE"; + nav_states_str[1] = "READY"; + nav_states_str[2] = "LOITER"; + nav_states_str[3] = "MISSION"; + nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -463,6 +464,7 @@ Navigator::parameters_update() param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); + param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); @@ -472,7 +474,6 @@ Navigator::parameters_update() void Navigator::global_position_update() { - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } @@ -493,16 +494,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -519,6 +524,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -561,11 +567,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -581,7 +589,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -647,87 +655,122 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - stick_mode = true; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + stick_mode = true; + } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + stick_mode = true; + } + + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } } - stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - break; + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - break; + } else { + dispatch(EVENT_LOITER_REQUESTED); + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); } else { - dispatch(EVENT_LOITER_REQUESTED); + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } } + + } else { + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); + } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + + dispatch(EVENT_RTL_REQUESTED); + } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); } + + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -746,7 +789,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -767,6 +810,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -775,15 +819,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -792,7 +836,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } @@ -833,128 +877,153 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* NAV_STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + { + /* NAV_STATE_READY */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* NAV_STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* NAV_STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* NAV_STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + }, + { + /* NAV_STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, }, }; void Navigator::start_none() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_ready() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; - // TODO check if (_rtl_state != RTL_STATE_LAND) { + /* allow RTL if landed not at home */ _rtl_state = RTL_STATE_NONE; } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -963,33 +1032,41 @@ Navigator::start_loiter() _do_takeoff = false; /* set loiter position if needed */ - if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _mission_item_triplet.current.altitude_is_relative = false; - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { - _mission_item_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - get_loiter_item(&_mission_item_triplet.current); + if (_rtl_state == RTL_STATE_LAND) { + /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ + _rtl_state = RTL_STATE_DESCEND; + } + } - publish_mission_item_triplet(); + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; + + publish_position_setpoint_triplet(); } void @@ -997,16 +1074,14 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); - advance_mission(); + set_mission_item(); } void -Navigator::advance_mission() +Navigator::set_mission_item() { /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _reset_loiter_pos = true; _do_takeoff = false; @@ -1015,36 +1090,34 @@ Navigator::advance_mission() bool onboard; unsigned index; - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - takeoff_alt_amsl += _home_pos.altitude; + float takeoff_alt_amsl = _pos_sp_triplet.current.alt; if (_vstatus.condition_landed) { /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ @@ -1052,210 +1125,305 @@ Navigator::advance_mission() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + /* move current position setpoint to next */ + memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - /* set current item to TAKEOFF */ - get_takeoff_item(&_mission_item_triplet.current); + /* set current setpoint to takeoff */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = takeoff_alt_amsl; - _mission_item_triplet.current.altitude_is_relative = false; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = takeoff_alt_amsl; + _pos_sp_triplet.current.yaw = NAN; + _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_triplet.current_valid = false; + _mission_item_valid = false; + _pos_sp_triplet.current.valid = false; warnx("ERROR: current WP can't be set"); } if (!_do_takeoff) { - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + mission_item_s item_next; + ret = _mission.get_next_mission_item(&item_next); if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + } else { /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.next.valid = false; } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_rtl() { - _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state == RTL_STATE_NONE) - _rtl_state = RTL_STATE_CLIMB; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); + if (_rtl_state == RTL_STATE_NONE) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + + if (_reset_loiter_pos) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + } + } + + _reset_loiter_pos = true; set_rtl_item(); } void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; +} + +void Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); - break; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + float climb_alt = _home_pos.alt + _parameters.rtl_alt; + + if (_vstatus.condition_landed) { + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + break; + } + case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } + case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - float descend_alt = _home_pos.altitude + _parameters.land_alt; - - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + break; + } + case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } + default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } + + publish_position_setpoint_triplet(); +} + +void +Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +{ + sp->valid = true; + + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* set home position for RTL item */ + sp->lat = _home_pos.lat; + sp->lon = _home_pos.lon; + sp->alt = _home_pos.alt + _parameters.rtl_alt; + + } else { + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; } - publish_mission_item_triplet(); + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_NORMAL; + } } bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ - if (!_mission_item_triplet.current_valid) { + if (!_mission_item_valid) { return false; } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + if (_vstatus.is_rotary_wing) { + return _vstatus.condition_landed; + + } else { + /* For fw there is currently no landing detector: + * make sure control is not stopped when overshooting the landing waypoint */ + return false; + } } /* XXX TODO count turns */ - if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); if (!_waypoint_position_reached) { float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; @@ -1265,20 +1433,22 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1287,12 +1457,14 @@ Navigator::check_mission_item_reached() } if (!_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1302,20 +1474,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - if (_mission_item_triplet.current.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + + if (_mission_item.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1328,30 +1502,36 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); } if (_mission.current_mission_available()) { - advance_mission(); + set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1361,67 +1541,16 @@ Navigator::on_mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - //item->yaw - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; - -} - -void -Navigator::get_takeoff_item(mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_TAKEOFF; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* append the home position to RTL item */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; - new_mission_item->altitude_is_relative = false; - new_mission_item->loiter_radius = _parameters.loiter_radius; - new_mission_item->acceptance_radius = _parameters.acceptance_radius; - } -} - -void -Navigator::publish_mission_item_triplet() +Navigator::publish_position_setpoint_triplet() { /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { + if (_pos_sp_triplet_pub > 0) { /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } @@ -1436,42 +1565,85 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + break; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; + default: break; + } - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; - if (myState == NAV_STATE_READY) { + + switch (myState) { + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1479,18 +1651,28 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - } else { + break; + + case NAV_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; + break; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); @@ -1506,24 +1688,6 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (a.altitude_is_relative == b.altitude_is_relative && - fabs(a.lat - b.lat) < FLT_EPSILON && - fabs(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - a.autocontinue == b.autocontinue) { - return true; - } else { - return false; - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); @@ -1579,8 +1743,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 8df47fc3b..af1d9d7d5 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @autho Lorenz Meier <lm@inf.ethz.ch> + * Author: @author Lorenz Meier <lm@inf.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> + * @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 @@ -40,6 +41,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -55,6 +57,7 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5bf0fba30..af04bb0bc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,15 +71,21 @@ #include "position_estimator_inav_params.h" #include "inertial_filter.h" +#define MIN_VALID_W 0.00001f + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s +static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms +static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss +static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz +static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz +static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -95,8 +101,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -115,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("position_estimator_inav already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -135,16 +140,23 @@ int position_estimator_inav_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tposition_estimator_inav is running\n"); + warnx("app is running"); } else { - printf("\tposition_estimator_inav not started\n"); + warnx("app not started"); } exit(0); @@ -154,32 +166,92 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { + FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { + char *s = malloc(256); + snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fputs(f, s); + snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fputs(f, s); + free(s); + } + fclose(f); +} + /**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started."); + warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_init_cnt = 0; int baro_init_num = 200; - float baro_alt0 = 0.0f; /* to determine while start up */ + float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted + float surface_offset = 0.0f; // ground level offset from reference altitude + float surface_offset_rate = 0.0f; // surface offset change rate float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; + bool ref_inited = false; + hrt_abstime ref_init_start = 0; + const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + + uint16_t accel_updates = 0; + uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; + + hrt_abstime updates_counter_start = hrt_absolute_time(); + hrt_abstime pub_last = hrt_absolute_time(); + + hrt_abstime t_prev = 0; + + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float corr_baro = 0.0f; // D + float corr_gps[3][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) + }; + float w_gps_xy = 1.0f; + float w_gps_z = 1.0f; + float corr_sonar = 0.0f; + float corr_sonar_filtered = 0.0f; + + float corr_flow[] = { 0.0f, 0.0f }; // N E + float w_flow = 0.0f; + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) + hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) + hrt_abstime xy_src_time = 0; // time of last available position data + + bool gps_valid = false; // GPS is valid + bool sonar_valid = false; // sonar is valid + bool flow_valid = false; // flow is valid + bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); @@ -247,75 +319,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_alt0 += sensor.baro_alt_meter; + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } else { wait_baro = false; - baro_alt0 /= (float) baro_init_cnt; - warnx("init baro: alt = %.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + baro_offset /= (float) baro_init_cnt; + warnx("baro offs: %.2f", baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.z_global = true; } } } } } - bool ref_xy_inited = false; - hrt_abstime ref_xy_init_start = 0; - const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix - - hrt_abstime t_prev = 0; - - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; - uint16_t gps_updates = 0; - uint16_t attitude_updates = 0; - uint16_t flow_updates = 0; - - hrt_abstime updates_counter_start = hrt_absolute_time(); - hrt_abstime pub_last = hrt_absolute_time(); - - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D - float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float baro_corr = 0.0f; // D - float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - }; - float sonar_corr = 0.0f; - float sonar_corr_filtered = 0.0f; - float flow_corr[] = { 0.0f, 0.0f }; // X, Y - - float sonar_prev = 0.0f; - hrt_abstime sonar_time = 0; - /* main loop */ - struct pollfd fds[7] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = actuator_sub, .events = POLLIN }, - { .fd = armed_sub, .events = POLLIN }, + struct pollfd fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - if (!thread_should_exit) { - warnx("main loop started."); - } - while (!thread_should_exit) { - int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -324,40 +350,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) continue; } else if (ret > 0) { + /* act on attitude updates */ + + /* vehicle attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; + + bool updated; + /* parameter update */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ + orb_check(parameter_update_sub, &updated); + + if (updated) { struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); - /* update parameters */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ - if (fds[1].revents & POLLIN) { + orb_check(actuator_sub, &updated); + + if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ - if (fds[2].revents & POLLIN) { + orb_check(armed_sub, &updated); + + if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - } - /* vehicle attitude */ - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + flag_armed = armed.armed; + baro_offset -= z_est[0]; + corr_baro = 0.0f; + local_pos.ref_alt -= z_est[0]; + local_pos.ref_timestamp = t; + z_est[0] = 0.0f; + alt_avg = 0.0f; + } } /* sensor combined */ - if (fds[4].revents & POLLIN) { + orb_check(sensor_combined_sub, &updated); + + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { - /* correct accel bias, now only for Z */ - sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* correct accel bias */ + sensor.accelerometer_m_s2[0] -= acc_bias[0]; + sensor.accelerometer_m_s2[1] -= acc_bias[1]; + sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { @@ -368,12 +414,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_corr[0] = accel_NED[0] - x_est[2]; - accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + corr_acc[0] = accel_NED[0] - x_est[2]; + corr_acc[1] = accel_NED[1] - y_est[2]; + corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { - memset(accel_corr, 0, sizeof(accel_corr)); + memset(corr_acc, 0, sizeof(corr_acc)); } accel_counter = sensor.accelerometer_counter; @@ -381,180 +427,352 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter != baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; + corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } } /* optical flow */ - if (fds[5].revents & POLLIN) { + orb_check(optical_flow_sub, &updated); + + if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { - if (flow.ground_distance_m != sonar_prev) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - sonar_corr = -flow.ground_distance_m - z_est[0]; - sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; - - if (fabsf(sonar_corr) > params.sonar_err) { - // correction is too large: spike or new ground level? - if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { - // spike detected, ignore - sonar_corr = 0.0f; - - } else { - // new ground level - baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - z_est[0] += sonar_corr; - sonar_corr = 0.0f; - sonar_corr_filtered = 0.0f; - } + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; + corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; + + if (fabsf(corr_sonar) > params.sonar_err) { + /* correction is too large: spike or new ground level? */ + if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { + /* spike detected, ignore */ + corr_sonar = 0.0f; + sonar_valid = false; + + } else { + /* new ground level */ + surface_offset -= corr_sonar; + surface_offset_rate = 0.0f; + corr_sonar = 0.0f; + corr_sonar_filtered = 0.0f; + sonar_valid_time = t; + sonar_valid = true; + local_pos.surface_bottom_timestamp = t; + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); } + + } else { + /* correction is ok, use it */ + sonar_valid_time = t; + sonar_valid = true; + } + } + + float flow_q = flow.quality / 255.0f; + float dist_bottom = - z_est[0] - surface_offset; + + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + /* distance to surface */ + float flow_dist = dist_bottom / att.R[2][2]; + /* check if flow if too large for accurate measurements */ + /* calculate estimated velocity in body frame */ + float body_v_est[2] = { 0.0f, 0.0f }; + + for (int i = 0; i < 2; i++) { + body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; } + /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ + flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && + fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; + + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; + } + } + + /* velocity correction */ + corr_flow[0] = flow_v[0] - x_est[1]; + corr_flow[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + + /* if flow is not accurate, reduce weight for it */ + // TODO make this more fuzzy + if (!flow_accurate) + w_flow *= 0.05f; + + flow_valid = true; + } else { - sonar_corr = 0.0f; + w_flow = 0.0f; + flow_valid = false; } flow_updates++; } /* vehicle GPS position */ - if (fds[6].revents & POLLIN) { + orb_check(vehicle_gps_position_sub, &updated); + + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps.fix_type >= 3) { + /* hysteresis for GPS quality */ + if (gps_valid) { + if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + gps_valid = false; + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); + } + + } else { + if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + gps_valid = true; + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); + } + } + + } else { + gps_valid = false; + } + + if (gps_valid) { /* initialize reference position if needed */ - if (!ref_xy_inited) { - /* require EPH < 10m */ - if (gps.eph_m < 10.0f) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; - - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); - } - } else { - ref_xy_init_start = 0; + if (!ref_inited) { + if (ref_init_start == 0) { + ref_init_start = t; + + } else if (t > ref_init_start + ref_init_delay) { + ref_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_alt = alt + z_est[0]; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } } - if (ref_xy_inited) { + if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; + corr_gps[0][0] = gps_proj[0] - x_est[0]; + corr_gps[1][0] = gps_proj[1] - y_est[0]; + corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + corr_gps[0][1] = gps.vel_n_m_s - x_est[1]; + corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; + corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + corr_gps[0][1] = 0.0f; + corr_gps[1][1] = 0.0f; + corr_gps[2][1] = 0.0f; } + + w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); + w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); } } else { /* no GPS lock */ - memset(gps_corr, 0, sizeof(gps_corr)); - ref_xy_init_start = 0; + memset(corr_gps, 0, sizeof(corr_gps)); + ref_init_start = 0; } gps_updates++; } } - /* end of poll return value check */ + /* check for timeout on FLOW topic */ + if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { + flow_valid = false; + sonar_valid = false; + warnx("FLOW timeout"); + mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); + } + + /* check for timeout on GPS topic */ + if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + gps_valid = false; + warnx("GPS timeout"); + mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); + } + + /* check for sonar measurement timeout */ + if (sonar_valid && t > sonar_time + sonar_timeout) { + corr_sonar = 0.0f; + sonar_valid = false; + } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - baro_alt0 -= z_est[0]; - z_est[0] = 0.0f; - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + /* use GPS if it's valid and reference position initialized */ + bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; + bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use flow if it's valid and (accurate or no GPS available) */ + bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); + + /* try to estimate position during some time after position sources lost */ + if (use_gps_xy || use_flow) { + xy_src_time = t; } - /* accel bias correction, now only for Z - * not strictly correct, but stable and works */ - accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + + bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + + if (dist_bottom_valid) { + /* surface distance prediction */ + surface_offset += surface_offset_rate * dt; + + /* surface distance correction */ + if (sonar_valid) { + surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; + surface_offset -= corr_sonar * params.w_z_sonar * dt; + } + } + + float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; + float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; + float w_z_gps_p = params.w_z_gps_p * w_gps_z; + + /* reduce GPS weight if optical flow is good */ + if (use_flow && flow_accurate) { + w_xy_gps_p *= params.w_gps_flow; + w_xy_gps_v *= params.w_gps_flow; + } + + /* baro offset correction */ + if (use_gps_z) { + float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; + baro_offset += offs_corr; + baro_counter += offs_corr; + } + + /* accelerometer bias correction */ + float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + + if (use_gps_xy) { + accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; + accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; + } + + if (use_gps_z) { + accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + } + + if (use_flow) { + accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; + accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; + } + + accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += att.R[j][i] * accel_bias_corr[j]; + } + + acc_bias[i] += c * params.w_acc_bias * dt; + } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - baro_alt0 += sonar_corr * params.w_alt_sonar * dt; - inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); - inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - - bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; - bool flow_valid = false; // TODO implement opt flow - - /* try to estimate xy even if no absolute position source available, - * if using optical flow velocity will be correct in this case */ - bool can_estimate_xy = gps_valid || flow_valid; + inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + thread_should_exit = true; + } + /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); + inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); + + if (use_flow) { + inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); + inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); + } - if (gps_valid) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (use_gps_xy) { + inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); + inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); - if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { + inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); + inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } + + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + thread_should_exit = true; + } } /* detect land */ - alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp = z_est[0] - alt_avg; - alt_disp = alt_disp * alt_disp; + alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp2 = - z_est[0] - alt_avg; + alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; /* get actual thrust output */ float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 && thrust > params.land_thr) { landed = false; landed_time = 0; } } else { - if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { landed_time = t; // land detected first time @@ -593,10 +811,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.timestamp = t; - local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.xy_valid = can_estimate_xy && use_gps_xy; local_pos.v_xy_valid = can_estimate_xy; - local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && use_gps_xy; + local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; @@ -605,6 +823,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; + local_pos.dist_bottom_valid = dist_bottom_valid; + + if (local_pos.dist_bottom_valid) { + local_pos.dist_bottom = -z_est[0] - surface_offset; + local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; + } + + local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -614,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; } /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { @@ -634,19 +856,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (local_pos.v_z_valid) { - global_pos.vz = local_pos.vz; + global_pos.vel_d = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } - flag_armed = armed.armed; } - warnx("exiting."); - mavlink_log_info(mavlink_fd, "[inav] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 4f9ddd009..e1bbd75a6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,16 +40,19 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); -PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); @@ -57,15 +60,18 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->w_alt_baro = param_find("INAV_W_ALT_BARO"); - h->w_alt_acc = param_find("INAV_W_ALT_ACC"); - h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); - h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); - h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); - h->w_pos_acc = param_find("INAV_W_POS_ACC"); - h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_z_baro = param_find("INAV_W_Z_BARO"); + h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_acc = param_find("INAV_W_Z_ACC"); + h->w_z_sonar = param_find("INAV_W_Z_SONAR"); + h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); + h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_acc = param_find("INAV_W_XY_ACC"); + h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); + h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); h->land_t = param_find("INAV_LAND_T"); @@ -77,15 +83,18 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->w_alt_baro, &(p->w_alt_baro)); - param_get(h->w_alt_acc, &(p->w_alt_acc)); - param_get(h->w_alt_sonar, &(p->w_alt_sonar)); - param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); - param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); - param_get(h->w_pos_acc, &(p->w_pos_acc)); - param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_z_baro, &(p->w_z_baro)); + param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_acc, &(p->w_z_acc)); + param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); + param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_acc, &(p->w_xy_acc)); + param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); + param_get(h->flow_q_min, &(p->flow_q_min)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); param_get(h->land_t, &(p->land_t)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 61570aea7..e2be079d3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,15 +41,18 @@ #include <systemlib/param/param.h> struct position_estimator_inav_params { - float w_alt_baro; - float w_alt_acc; - float w_alt_sonar; - float w_pos_gps_p; - float w_pos_gps_v; - float w_pos_acc; - float w_pos_flow; + float w_z_baro; + float w_z_gps_p; + float w_z_acc; + float w_z_sonar; + float w_xy_gps_p; + float w_xy_gps_v; + float w_xy_acc; + float w_xy_flow; + float w_gps_flow; float w_acc_bias; float flow_k; + float flow_q_min; float sonar_filt; float sonar_err; float land_t; @@ -58,15 +61,18 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t w_alt_baro; - param_t w_alt_acc; - param_t w_alt_sonar; - param_t w_pos_gps_p; - param_t w_pos_gps_v; - param_t w_pos_acc; - param_t w_pos_flow; + param_t w_z_baro; + param_t w_z_gps_p; + param_t w_z_acc; + param_t w_z_sonar; + param_t w_xy_gps_p; + param_t w_xy_gps_v; + param_t w_xy_acc; + param_t w_xy_flow; + param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; + param_t flow_q_min; param_t sonar_filt; param_t sonar_err; param_t land_t; diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a..2f5908ac5 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1..5e2c92bf4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,20 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d0..60eda2319 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a..2e79f0ac6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705..d4c25911e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -177,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); @@ -186,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); @@ -204,24 +231,41 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); - phase = !phase; - usleep(300000); + phase = !phase; + } } -#endif + + /* Start the failsafe led init */ + failsafe_led_init(); /* * Run everything in a tight loop. @@ -249,11 +293,14 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } -#if 0 - /* check for debug activity */ + check_reboot(); + + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); @@ -266,7 +313,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663..393e0560e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) @@ -177,12 +178,13 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications @@ -220,3 +222,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index bfc0337f6..2c437d2c0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -382,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -518,16 +520,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) break; - - // note that we don't set BL_WAIT_MAGIC in - // BKP_DR1 as that is not necessary given the - // timing of the forceupdate command. The - // bootloader on px4io waits for enough time - // anyway, and this method works with older - // bootloader versions (tested with both - // revision 3 and revision 4). - - up_systemreset(); + + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -545,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } @@ -606,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 83bd3026e..ff2e4af6e 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -83,7 +83,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -164,8 +168,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356..495447740 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -218,11 +218,33 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { sbus_frame_drops++; return false; } + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; + } + /* we have received something we think is a frame */ last_frame_time = frame_time; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6833ec43f..9bac2958e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> * Anton Babushkin <anton.babushkin@me.com> * @@ -62,6 +62,7 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -72,7 +73,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> @@ -85,13 +86,13 @@ #include <systemlib/systemlib.h> #include <systemlib/param/param.h> +#include <version/version.h> #include <mavlink/mavlink_log.h> #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" -#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ @@ -108,13 +109,13 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -122,14 +123,17 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char folder_path[64]; +static char log_dir[32]; /* statistics counters */ -static unsigned long log_bytes_written = 0; static uint64_t start_time = 0; +static unsigned long log_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; +/* GPS time, used for log files naming */ +static uint64_t gps_time = 0; + /* current state of logging */ static bool logging_enabled = false; /* enable logging on start (-e option) */ @@ -138,11 +142,14 @@ static bool log_on_start = false; static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ static useconds_t sleep_delay = 0; +/* use date/time for naming directories and files (-t option) */ +static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; static pthread_t logwriter_pthread = 0; +static pthread_attr_t logwriter_attr; /** * Log buffer writing thread. Open and close file here. @@ -203,14 +210,14 @@ static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. Store folder name in 'log_folder'. + * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logfolder(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -218,11 +225,12 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n"); + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -280,82 +288,112 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } -int create_logfolder() +int create_log_dir() { - /* make folder on sdcard */ - uint16_t folder_number = 1; // start with folder sess001 + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - /* look for the next folder that does not exist */ - while (folder_number <= MAX_NO_LOGFOLDER) { - /* set up folder path: e.g. /fs/microsd/sess001 */ - sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ + if (log_name_timestamp && gps_time != 0) { + /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret == 0) { - /* folder does not exist, success */ - break; + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); + + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; + } + + } else { + /* look for the next dir that does not exist */ + while (dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + sprintf(log_dir, "%s/sess%03u", log_root, dir_number); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; + } - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; + /* dir exists already */ + dir_number++; continue; + } - } else { - warn("failed creating new folder"); + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } - if (folder_number >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); - return -1; - } - + /* print logging path, important to find log file later */ + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { - /* make folder on sdcard */ - uint16_t file_number = 1; // start with file log001 - /* string to hold the path to the log */ - char path_buf[64] = ""; - - int fd = 0; - - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ - sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); + char log_file_name[16] = ""; + char log_file_path[48] = ""; + + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + + } else { + uint16_t file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + + if (!file_exist(log_file_path)) { + break; + } - if (file_exist(path_buf)) { file_number++; - continue; } - fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); - - if (fd == 0) { - warn("opening %s failed", path_buf); + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warnx("all %d possible files exist already", MAX_NO_LOGFILE); + return -1; } + } - warnx("logging to: %s.", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); - return fd; - } + if (fd < 0) { + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); - return -1; + } else { + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } - return 0; + return fd; } static void *logwriter_thread(void *arg) @@ -363,9 +401,12 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + int log_fd = open_log_file(); + + if (log_fd < 0) + return; - int log_fd = open_logfile(); + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); @@ -443,14 +484,20 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return OK; + return; } void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + /* create log dir if needed */ + if (create_log_dir() != 0) { + mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + errx(1, "error creating log dir"); + } + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -458,30 +505,28 @@ void sdlog2_start_log() log_msgs_skipped = 0; /* initialize log buffer emptying thread */ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); + pthread_attr_init(&logwriter_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&logwriter_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; - // XXX we have to destroy the attr at some point } void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -501,6 +546,7 @@ void sdlog2_stop_log() } logwriter_pthread = 0; + pthread_attr_destroy(&logwriter_attr); sdlog2_status(); } @@ -569,8 +615,8 @@ int write_parameters(int fd) } case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; + param_get(param, &value); + break; default: break; @@ -588,18 +634,25 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + logging_enabled = false; + log_on_start = false; + log_when_armed = false; + log_name_timestamp = false; + + flag_system_armed = false; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -632,49 +685,52 @@ int sdlog2_thread_main(int argc, char *argv[]) log_when_armed = true; break; + case 't': + log_name_timestamp = true; + break; + case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); - } + gps_time = 0; + + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", folder_path); + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); } free(converter_out); - /* only print logging path, important to find log file later */ - warnx("logging to directory: %s", folder_path); - /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -684,6 +740,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; + struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -693,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -702,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct airspeed_s airspeed; struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; + struct battery_status_s battery; } buf; memset(&buf, 0, sizeof(buf)); @@ -709,6 +767,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -726,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int airspeed_sub; int esc_sub; int global_vel_sp_sub; + int battery_sub; } subs; /* log message buffer: header + body */ @@ -752,6 +812,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; struct log_GVSP_s log_GVSP; + struct log_BATT_s log_BATT; + struct log_DIST_s log_DIST; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -760,9 +822,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of subscriptions */ - const ssize_t fdsc = 19; - /* sanity check variable and index */ + /* number of messages */ + const ssize_t fdsc = 25; + /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -785,6 +847,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- VEHICLE CONTROL MODE --- */ + subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + fds[fdsc_count].fd = subs.control_mode_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -840,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -881,12 +949,18 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- BATTERY --- */ + subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); + fds[fdsc_count].fd = subs.battery_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -909,20 +983,31 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t baro_counter = 0; uint16_t differential_pressure_counter = 0; + /* track changes in distance status */ + bool dist_bottom_present = false; + /* enable logging on start if needed */ - if (log_on_start) + if (log_on_start) { + /* check GPS topic to get GPS time */ + if (log_name_timestamp) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { + gps_time = buf.gps_pos.time_gps_usec; + } + } + sdlog2_start_log(); + } while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -936,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + handle_command(&buf.cmd); handled_topics++; } @@ -951,11 +1037,22 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- GPS POSITION - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + if (log_name_timestamp) { + gps_time = buf.gps_pos.time_gps_usec; + } + + handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // begin from fds[1] again + ifds = 1; // begin from VEHICLE STATUS again pthread_mutex_lock(&logbuffer_mutex); @@ -966,14 +1063,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { - // Don't orb_copy, it's already done few lines above + /* don't orb_copy, it's already done few lines above */ + /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ + orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - // TODO use control_mode topic - //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; - log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -998,6 +1094,8 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } + ifds++; // skip CONTROL MODE, already handled + /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1063,6 +1161,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.gx = buf.att.g_comp[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); } @@ -1123,6 +1224,17 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); + + if (buf.local_pos.dist_bottom_valid) { + dist_bottom_present = true; + } + if (dist_bottom_present) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; + log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; + log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } } /* --- LOCAL POSITION SETPOINT --- */ @@ -1140,29 +1252,26 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); + orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; - log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } @@ -1238,6 +1347,17 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GVSP); } + /* --- BATTERY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery); + log_msg.msg_type = LOG_BATT_MSG; + log_msg.body.log_BATT.voltage = buf.battery.voltage_v; + log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; + log_msg.body.log_BATT.current = buf.battery.current_a; + log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; + LOGBUFFER_WRITE_AND_COUNT(BATT); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ @@ -1261,7 +1381,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1274,8 +1394,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1294,7 +1414,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1302,7 +1422,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1313,7 +1433,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3afaaa2ad..98736dd21 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -57,6 +57,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float gx; + float gy; + float gz; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -148,8 +151,6 @@ struct log_STAT_s { uint8_t main_state; uint8_t navigation_state; uint8_t arming_state; - float battery_voltage; - float battery_current; float battery_remaining; uint8_t battery_warning; uint8_t landed; @@ -209,16 +210,13 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t altitude_is_relative; int32_t lat; int32_t lon; - float altitude; + float alt; float yaw; - uint8_t nav_cmd; + uint8_t type; float loiter_radius; int8_t loiter_direction; - float acceptance_radius; - float time_inside; float pitch_min; }; @@ -247,6 +245,25 @@ struct log_GVSP_s { float vz; }; +/* --- BATT - BATTERY --- */ +#define LOG_BATT_MSG 20 +struct log_BATT_s { + float voltage; + float voltage_filtered; + float current; + float discharged; +}; + +/* --- DIST - DISTANCE TO SURFACE --- */ +#define LOG_DIST_MSG 21 +struct log_DIST_s { + float bottom; + float bottom_rate; + uint8_t flags; +}; + +/********** SYSTEM MESSAGES, ID > 0x80 **********/ + /* --- TIME - TIME STAMP --- */ #define LOG_TIME_MSG 129 struct log_TIME_s { @@ -272,7 +289,7 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), @@ -280,16 +297,18 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), 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(BATT, "ffff", "V,VFilt,C,Discharged"), + LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index ebbc580e1..aa538fd6b 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 763723554..30659fd3a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +35,10 @@ * @file sensor_params.c * * Parameters defined by the sensors task. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include <nuttx/config.h> @@ -45,41 +46,98 @@ #include <systemlib/param/param.h> /** - * Gyro X offset FIXME + * Gyro X offset * - * This is an X-axis offset for the gyro. - * Adjust it according to the calibration data. + * 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 + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset FIXME with dot. + * Gyro Y offset * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset FIXME + * Gyro Z offset * * @min -5.0 * @max 5.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +/** + * Gyro X scaling + * + * X-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); + +/** + * Gyro Y scaling + * + * Y-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); + +/** + * Gyro Z scaling + * + * Z-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * Magnetometer X offset + * + * This is an X-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); + +/** + * Magnetometer Y offset + * + * This is an Y-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); + +/** + * Magnetometer Z offset + * + * This is an Z-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); @@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** + * RC Channel 1 Minimum + * + * Minimum value for RC channel 1 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); + +/** + * RC Channel 1 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); + +/** + * RC Channel 1 Maximum + * + * Maximum value for RC channel 1 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); + +/** + * RC Channel 1 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +/** + * RC Channel 1 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); -PARAM_DEFINE_FLOAT(RC2_MIN, 1000); -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +/** + * RC Channel 2 Minimum + * + * Minimum value for RC channel 2 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); + +/** + * RC Channel 2 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC Channel 2 Maximum + * + * Maximum value for RC channel 2 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC Channel 2 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC Channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); @@ -223,15 +379,75 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9baf1a6af..ea864390d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -114,6 +114,7 @@ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #endif @@ -124,10 +125,8 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif -#define BAT_VOL_INITIAL 0.f -#define BAT_VOL_LOWPASS_1 0.99f -#define BAT_VOL_LOWPASS_2 0.01f -#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f +#define BATT_V_LOWPASS 0.001f +#define BATT_V_IGNORE_THRESHOLD 3.5f /** * HACK - true temperature is much less than indicated temperature in baro, @@ -211,10 +210,13 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ + hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -265,6 +267,7 @@ private: float rc_fs_thr; float battery_voltage_scaling; + float battery_current_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -314,6 +317,7 @@ private: param_t rc_fs_thr; param_t battery_voltage_scaling; + param_t battery_current_scaling; param_t board_rotation; param_t external_mag_rotation; @@ -465,9 +469,9 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), - _mag_is_external(false) + _mag_is_external(false), + _battery_discharged(0), + _battery_current_timestamp(0) { /* basic r/c parameters */ @@ -560,6 +564,7 @@ Sensors::Sensors() : _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -740,6 +745,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* scaling of ADC ticks to battery current */ + if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { + warnx("Failed updating current scaling param"); + } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); @@ -785,7 +795,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -825,7 +834,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -920,7 +928,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +954,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -972,7 +980,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; @@ -1157,17 +1165,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (!_publishing) return; + hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ - if (hrt_absolute_time() - _last_adc >= 10000) { + if (t - _last_adc >= 10000) { /* make space for a maximum of eight channels */ struct adc_msg_s buf_adc[8]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - - if (ret >= (int)sizeof(buf_adc[0])) { - + if (ret >= (int)sizeof(buf_adc[0])) { + for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); @@ -1178,27 +1185,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); - if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - + if (voltage > BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_v = voltage; /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_v < 3.0f) { - _battery_status.voltage_v = voltage; + if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_filtered_v = voltage; } - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; + _battery_status.timestamp = t; + _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; - /* announce the battery voltage if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + } else { + /* mark status as invalid */ + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + } - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { + /* handle current only if voltage is valid */ + if (_battery_status.voltage_v > 0.0f) { + float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ + if (current >= 0.0f) { + _battery_status.timestamp = t; + _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { + /* initialize discharged value */ + if (_battery_status.discharged_mah < 0.0f) + _battery_status.discharged_mah = 0.0f; + _battery_discharged += current * (t - _battery_current_timestamp); + _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; + } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1214,7 +1234,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor - _diff_pres.timestamp = hrt_absolute_time(); + _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.voltage = voltage; @@ -1227,8 +1247,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } - - _last_adc = hrt_absolute_time(); + } + _last_adc = t; + if (_battery_status.voltage_v > 0.0f) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } } } @@ -1475,9 +1503,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); @@ -1516,7 +1541,10 @@ Sensors::task_main() raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); - _battery_status.voltage_v = BAT_VOL_INITIAL; + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; /* get a set of initial values */ accel_poll(raw); diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/modules/systemlib/board_serial.c index 568d9669a..ad8c2bf83 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/modules/systemlib/board_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,45 +32,29 @@ ****************************************************************************/ /** - * @file Vector3.hpp + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> * - * math 3 vector */ -#pragma once - -#include "Vector.hpp" +#include "otp.h" +#include "board_config.h" +#include "board_serial.h" -namespace math +int get_board_serial(char *serialid) { + const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + union udid id; + val_read((unsigned *)&id, udid_ptr, sizeof(id)); -class __EXPORT Vector3 : - public Vector -{ -public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ -}; -int __EXPORT vector3Test(); -} // math + /* Copy the serial from the chips non-write memory and swap endianess */ + serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0]; + serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4]; + serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; + return 0; +}
\ No newline at end of file diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/modules/systemlib/board_serial.h index 21661622a..b14bb4376 100644 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ b/src/modules/systemlib/board_serial.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,18 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> * - * matrix code */ -#include "Matrix.hpp" +#pragma once + +__BEGIN_DECLS + +__EXPORT int get_board_serial(char *serialid); + +__END_DECLS diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 8aca6a25d..49403c98b 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder) memcpy(encoder->buf, &len, sizeof(len)); } + /* sync file */ + fsync(encoder->fd); + return 0; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 843cda722..3953b757d 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -49,4 +49,8 @@ SRCS = err.c \ airspeed.c \ system_params.c \ mavlink_log.c \ - rc_check.c + rc_check.c \ + otp.c \ + board_serial.c \ + pwm_limit/pwm_limit.c + diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c new file mode 100644 index 000000000..695574fdc --- /dev/null +++ b/src/modules/systemlib/otp.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Authors: + * Lorenz Meier <lm@inf.ethz.ch> + * David "Buzz" Bussenschutt <davidbuzz@gmail.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 otp.c + * otp estimation + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + */ + +#include <nuttx/config.h> +#include <board_config.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <string.h> // memset +#include "conversions.h" +#include "otp.h" +#include "err.h" // warnx +#include <assert.h> + + +int val_read(void *dest, volatile const void *src, int bytes) +{ + + int i; + + for (i = 0; i < bytes / 4; i++) { + *(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i); + } + + return i * 4; +} + + +int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) +{ + + warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid); + + int errors = 0; + + // descriptor + if (F_write_byte(ADDR_OTP_START, 'P')) + errors++; + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) + errors++; // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 2, '4')) + errors++; + if (F_write_byte(ADDR_OTP_START + 3, '\0')) + errors++; + //id_type + if (F_write_byte(ADDR_OTP_START + 4, id_type)) + errors++; + // vid and pid are 4 bytes each + if (F_write_word(ADDR_OTP_START + 5, vid)) + errors++; + if (F_write_word(ADDR_OTP_START + 9, pid)) + errors++; + + // leave some 19 bytes of space, and go to the next block... + // then the auth sig starts + for (int i = 0 ; i < 128 ; i++) { + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + errors++; + } + + return errors; +} + +int lock_otp(void) +{ + //determine the required locking size - can only write full lock bytes */ +// int size = sizeof(struct otp) / 32; +// +// struct otp_lock otp_lock_mem; +// +// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); +// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) +// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; + //XXX add the actual call here to write the OTP_LOCK bytes only at final stage + // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); + + int locksize = 5; + + int errors = 0; + + // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. + for (int i = 0 ; i < locksize ; i++) { + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + errors++; + } + + return errors; +} + + + +// COMPLETE, BUSY, or other flash error? +int F_GetStatus(void) +{ + int fs = F_COMPLETE; + + if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { + + if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { + + if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { + + if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { + fs = F_COMPLETE; + } + } + } + } + + return fs; +} + + +// enable FLASH Registers +void F_unlock(void) +{ + if ((FLASH->control & F_CR_LOCK) != 0) { + FLASH->key = F_KEY1; + FLASH->key = F_KEY2; + } +} + +// lock the FLASH Registers +void F_lock(void) +{ + FLASH->control |= F_CR_LOCK; +} + +// flash write word. +int F_write_word(uint32_t Address, uint32_t Data) +{ + unsigned char octet[4] = {0, 0, 0, 0}; + + int ret = 0; + + for (int i = 0; i < 4; i++) { + octet[i] = (Data >> (i * 8)) & 0xFF; + ret = F_write_byte(Address + i, octet[i]); + } + + return ret; +} + +// flash write byte +int F_write_byte(uint32_t Address, uint8_t Data) +{ + volatile int status = F_COMPLETE; + + //warnx("F_write_byte: %08X %02d", Address , Data ) ; + + //Check the parameters + assert(IS_F_ADDRESS(Address)); + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + if (status == F_COMPLETE) { + //if the previous operation is completed, proceed to program the new data + FLASH->control &= CR_PSIZE_MASK; + FLASH->control |= F_PSIZE_BYTE; + FLASH->control |= F_CR_PG; + + *(volatile uint8_t *)Address = Data; + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + //if the program operation is completed, disable the PG Bit + FLASH->control &= (~F_CR_PG); + } + + //Return the Program Status + return !(status == F_COMPLETE); +} + + + diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h new file mode 100644 index 000000000..f10e129d8 --- /dev/null +++ b/src/modules/systemlib/otp.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 otp.h + * One TIme Programmable ( OTP ) Flash routine/s. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + */ + +#ifndef OTP_H_ +#define OTP_H_ + +__BEGIN_DECLS + +#define ADDR_OTP_START 0x1FFF7800 +#define ADDR_OTP_LOCK_START 0x1FFF7A00 + +#define OTP_LOCK_LOCKED 0x00 +#define OTP_LOCK_UNLOCKED 0xFF + + + +#include <unistd.h> +#include <stdio.h> + +// possible flash statuses +#define F_BUSY 1 +#define F_ERROR_WRP 2 +#define F_ERROR_PROGRAM 3 +#define F_ERROR_OPERATION 4 +#define F_COMPLETE 5 + +typedef struct { + volatile uint32_t accesscontrol; // 0x00 + volatile uint32_t key; // 0x04 + volatile uint32_t optionkey; // 0x08 + volatile uint32_t status; // 0x0C + volatile uint32_t control; // 0x10 + volatile uint32_t optioncontrol; //0x14 +} flash_registers; + +#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) +#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) +#define FLASH ((flash_registers *) F_R_BASE) + +#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) +#define F_PSIZE_WORD ((uint32_t)0x00000200) +#define F_PSIZE_BYTE ((uint32_t)0x00000000) +#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. + +#define F_KEY1 ((uint32_t)0x45670123) +#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) + + + +#pragma pack(push, 1) + +/* + * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes. + * The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15) + * to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed + * until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only + * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly. + */ + +struct otp { + // first 32 bytes = the '0' Block + char id[4]; ///4 bytes < 'P' 'X' '4' '\n' + uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID + uint32_t vid; ///4 bytes + uint32_t pid; ///4 bytes + char unused[19]; ///19 bytes + // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) + char signature[128]; + // insert extras here + uint32_t lock_bytes[4]; +}; + +struct otp_lock { + uint8_t lock_bytes[16]; +}; +#pragma pack(pop) + +#define ADDR_F_SIZE 0x1FFF7A22 + +#pragma pack(push, 1) +union udid { + uint32_t serial[3]; + char data[12]; +}; +#pragma pack(pop) + + +/** + * s + */ +//__EXPORT float calc_indicated_airspeed(float differential_pressure); + +__EXPORT void F_unlock(void); +__EXPORT void F_lock(void); +__EXPORT int val_read(void *dest, volatile const void *src, int bytes); +__EXPORT int val_write(volatile void *dest, const void *src, int bytes); +__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature); +__EXPORT int lock_otp(void); + + +__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); +__EXPORT int F_write_word(uint32_t Address, uint32_t Data); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 398657dd7..2d773fd25 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -61,7 +61,7 @@ #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" -#if 1 +#if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else # define debug(fmt, args...) do { } while(0) @@ -512,6 +512,28 @@ param_save_default(void) int fd; const char *filename = param_get_default_file(); + + /* write parameters to temp file */ + fd = open(filename, O_WRONLY | O_CREAT); + + if (fd < 0) { + warn("failed to open param file: %s", filename); + return ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); + } + } + + close(fd); + + return res; + +#if 0 const char *filename_tmp = malloc(strlen(filename) + 5); sprintf(filename_tmp, "%s.tmp", filename); @@ -565,6 +587,7 @@ param_save_default(void) free(filename_tmp); return res; +#endif } /** @@ -573,9 +596,9 @@ param_save_default(void) int param_load_default(void) { - int fd = open(param_get_default_file(), O_RDONLY); + int fd_load = open(param_get_default_file(), O_RDONLY); - if (fd < 0) { + if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { warn("open '%s' for reading failed", param_get_default_file()); @@ -584,8 +607,8 @@ param_load_default(void) return 1; } - int result = param_load(fd); - close(fd); + int result = param_load(fd_load); + close(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 77c952f52..6a4e9392a 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -39,7 +39,7 @@ /** * @file pid.c * - * Implementation of generic PID control interface. + * Implementation of generic PID controller. * * @author Laurens Mackay <mackayl@student.ethz.ch> * @author Tobias Naegeli <naegelit@student.ethz.ch> @@ -53,24 +53,21 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->limit = limit; pid->mode = mode; pid->dt_min = dt_min; - pid->count = 0.0f; - pid->saturated = 0.0f; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) + +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) { int ret = 0; @@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(intmax)) { - pid->intmax = intmax; + if (isfinite(integral_limit)) { + pid->integral_limit = integral_limit; } else { ret = 1; } - if (isfinite(limit)) { - pid->limit = limit; + if (isfinite(output_limit)) { + pid->output_limit = output_limit; } else { ret = 1; @@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float return ret; } -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* current error value */ + float error = sp - val; - // Calculate or measured current error derivative + /* current error derivative */ if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); pid->error_previous = error; @@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - if (pid->ki > 0.0f) { + /* calculate PD output */ + float output = (error * pid->kp) + (d * pid->kd); + + if (pid->ki > SIGMA) { // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; - - } else { - if (!isfinite(i)) { - i = 0.0f; + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ + pid->integral = i; } - - pid->integral = i; - pid->saturated = 0; } - } else { - i = 0.0f; - pid->saturated = 0; + /* add I component to output */ + output += pid->integral * pid->ki; } - // Calculate the output. Limit output magnitude to pid->limit - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - + /* limit output */ if (isfinite(output)) { - if (pid->limit > SIGMA) { - if (output > pid->limit) { - output = pid->limit; + if (pid->output_limit > SIGMA) { + if (output > pid->output_limit) { + output = pid->output_limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; } } @@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo __EXPORT void pid_reset_integral(PID_t *pid) { - pid->integral = 0; + pid->integral = 0.0f; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eca228464..e8b1aac4f 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -39,7 +39,7 @@ /** * @file pid.h * - * Definition of generic PID control interface. + * Definition of generic PID controller. * * @author Laurens Mackay <mackayl@student.ethz.ch> * @author Tobias Naegeli <naegelit@student.ethz.ch> @@ -55,38 +55,35 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC_NO_SP 1 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 2 -// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) -#define PID_MODE_DERIVATIV_NONE 9 +typedef enum PID_MODE { + /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ + PID_MODE_DERIVATIV_NONE = 0, + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, + * val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC, + /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, + * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC_NO_SP, + /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ + PID_MODE_DERIVATIV_SET +} pid_mode_t; typedef struct { + pid_mode_t mode; + float dt_min; float kp; float ki; float kd; - float intmax; - float sp; float integral; + float integral_limit; + float output_limit; float error_previous; float last_output; - float limit; - float dt_min; - uint8_t mode; - uint8_t count; - uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); -//void pid_set(PID_t *pid, float sp); +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - __EXPORT void pid_reset_integral(PID_t *pid); __END_DECLS diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cac3dc82a..190b315f1 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -44,38 +44,53 @@ #include <math.h> #include <stdbool.h> #include <drivers/drv_hrt.h> +#include <stdio.h> void pwm_limit_init(pwm_limit_t *limit) { - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_INIT; limit->time_armed = 0; return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { + /* first evaluate state changes */ switch (limit->state) { - case LIMIT_STATE_OFF: - if (armed) - limit->state = LIMIT_STATE_RAMP; - limit->time_armed = hrt_absolute_time(); + case PWM_LIMIT_STATE_INIT: + + if (armed) { + + /* set arming time for the first call */ + if (limit->time_armed == 0) { + limit->time_armed = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { + limit->state = PWM_LIMIT_STATE_OFF; + } + } break; - case LIMIT_STATE_INIT: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) - limit->state = LIMIT_STATE_RAMP; + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } break; - case LIMIT_STATE_RAMP: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) - limit->state = LIMIT_STATE_ON; + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } break; - case LIMIT_STATE_ON: - if (!armed) - limit->state = LIMIT_STATE_OFF; + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } break; default: break; @@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ /* then set effective_pwm based on state */ switch (limit->state) { - case LIMIT_STATE_OFF: - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = disarmed_pwm[i]; - output[i] = 0.0f; } break; - case LIMIT_STATE_RAMP: + case PWM_LIMIT_STATE_RAMP: + { + hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; i<num_channels; i++) { - - uint16_t ramp_min_pwm; - - /* if a disarmed pwm value was set, blend between disarmed and min */ - if (disarmed_pwm[i] > 0) { - - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; - - uint16_t disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; - } + progress = diff * 10000 / RAMP_TIME_US; + + for (unsigned i=0; i<num_channels; i++) { + + uint16_t ramp_min_pwm; + + /* if a disarmed pwm value was set, blend between disarmed and min */ + if (disarmed_pwm[i] > 0) { - effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - output[i] = (float)progress/10000.0f * output[i]; + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + } } break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; - /* effective_output stays the same */ } break; default: diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 9974770be..6a667ac6f 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -46,6 +46,8 @@ #include <stdint.h> #include <stdbool.h> +__BEGIN_DECLS + /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -56,21 +58,21 @@ */ #define RAMP_TIME_US 2500000 +enum pwm_limit_state { + PWM_LIMIT_STATE_OFF = 0, + PWM_LIMIT_STATE_INIT, + PWM_LIMIT_STATE_RAMP, + PWM_LIMIT_STATE_ON +}; + typedef struct { - enum { - LIMIT_STATE_OFF = 0, - LIMIT_STATE_INIT, - LIMIT_STATE_RAMP, - LIMIT_STATE_ON - } state; + enum pwm_limit_state state; uint64_t time_armed; } pwm_limit_t; -__BEGIN_DECLS - __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 79a820c06..4c84c1f25 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/mission_item_triplet.h" -ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); +#include "topics/position_setpoint_triplet.h" +ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h index c40d0d4e5..d473dff3f 100644 --- a/src/modules/uORB/topics/battery_status.h +++ b/src/modules/uORB/topics/battery_status.h @@ -53,9 +53,10 @@ */ struct battery_status_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float voltage_v; /**< Battery voltage in volts, filtered */ - float current_a; /**< Battery current in amperes, filtered, -1 if unknown */ - float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */ + float voltage_v; /**< Battery voltage in volts, 0 if unknown */ + float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */ + float current_a; /**< Battery current in amperes, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */ }; /** @@ -65,4 +66,4 @@ struct battery_status_s { /* register this as object request broker structure */ ORB_DECLARE(battery_status); -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e..08d11abae 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index b35eae607..4b57833b6 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -46,31 +46,42 @@ #include <stdbool.h> #include "../uORB.h" -#include "mission.h" - /** * @addtogroup topics * @{ */ +enum SETPOINT_TYPE +{ + SETPOINT_TYPE_NORMAL = 0, + SETPOINT_TYPE_LOITER, + SETPOINT_TYPE_TAKEOFF, + SETPOINT_TYPE_LAND, +}; + +struct position_setpoint_s +{ + bool valid; /**< true if setpoint is valid */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + double lat; /**< latitude, in deg */ + double lon; /**< longitude, in deg */ + float alt; /**< altitude AMSL, in m */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float loiter_radius; /**< loiter radius (only for fixed wing), in m */ + int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ +}; + /** * Global position setpoint triplet in WGS84 coordinates. * * This are the three next waypoints (or just the next two or one). */ -struct mission_item_triplet_s +struct position_setpoint_triplet_s { - bool previous_valid; - bool current_valid; /**< flag indicating previous mission item is valid */ - bool next_valid; /**< flag indicating next mission item is valid */ - - struct mission_item_s previous; - struct mission_item_s current; - struct mission_item_s next; - - int previous_index; - int current_index; - int next_index; + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; }; /** @@ -78,6 +89,6 @@ struct mission_item_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); +ORB_DECLARE(position_setpoint_triplet); #endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 4380a5ee7..e5a35ff9b 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,6 +76,7 @@ struct vehicle_attitude_s { float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ + float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a..7596f944f 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -61,7 +61,7 @@ struct vehicle_attitude_setpoint_s float yaw_body; /**< body angle in NED frame */ //float body_valid; /**< Set to true if body angles are valid */ - float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ //! For quaternion-based attitude control diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985..5aecac898 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; @@ -92,7 +93,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e37..ae771ca00 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,7 +54,7 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. @@ -65,14 +65,13 @@ struct vehicle_global_position_s uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ - float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 427153782..d567f2e02 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,6 +77,11 @@ struct vehicle_local_position_s int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ + /* Distance to surface */ + float dist_bottom; /**< Distance to bottom surface (ground) */ + float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ + uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ + bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..a5988d3ba 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { @@ -82,9 +84,12 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +178,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +229,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 2aed80e01..000000000 --- a/src/systemcmds/eeprom/eeprom.c +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 eeprom.c - * - * EEPROM service and utility app. - */ - -#include <nuttx/config.h> - -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <stdbool.h> -#include <unistd.h> -#include <fcntl.h> -#include <sys/mount.h> -#include <sys/ioctl.h> -#include <sys/stat.h> - -#include <nuttx/i2c.h> -#include <nuttx/mtd.h> -#include <nuttx/fs/nxffs.h> -#include <nuttx/fs/ioctl.h> - -#include <board_config.h> - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -#ifndef PX4_I2C_BUS_ONBOARD -# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM -#endif - -__EXPORT int eeprom_main(int argc, char *argv[]); - -static void eeprom_attach(void); -static void eeprom_start(void); -static void eeprom_erase(void); -static void eeprom_ioctl(unsigned operation); -static void eeprom_save(const char *name); -static void eeprom_load(const char *name); -static void eeprom_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *eeprom_mtd; - -int eeprom_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - eeprom_start(); - - if (!strcmp(argv[1], "save_param")) - eeprom_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - eeprom_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - eeprom_erase(); - - if (!strcmp(argv[1], "test")) - eeprom_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - eeprom_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - eeprom_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); -} - - -static void -eeprom_attach(void) -{ - /* find the right I2C */ - struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - /* this resets the I2C bus, set correct bus speed again */ - I2C_SETFREQUENCY(i2c, 400000); - - if (i2c == NULL) - errx(1, "failed to locate I2C bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - eeprom_mtd = at24c_initialize(i2c); - if (eeprom_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (eeprom_mtd == NULL) - errx(1, "failed to initialize EEPROM driver"); - - attached = true; -} - -static void -eeprom_start(void) -{ - int ret; - - if (started) - errx(1, "EEPROM already mounted"); - - if (!attached) - eeprom_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(eeprom_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - - /* mount the EEPROM */ - ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - - started = true; - warnx("mounted EEPROM at /eeprom"); - exit(0); -} - -extern int at24c_nuke(void); - -static void -eeprom_erase(void) -{ - if (!attached) - eeprom_attach(); - - if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -eeprom_ioctl(unsigned operation) -{ - int fd; - - fd = open("/eeprom/.", 0); - - if (fd < 0) - err(1, "open /eeprom"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -eeprom_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -eeprom_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -extern void at24c_test(void); - -static void -eeprom_test(void) -{ - at24c_test(); - exit(0); -} diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/systemcmds/hw_ver/hw_ver.c index 21661622a..4b84523cc 100644 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ b/src/systemcmds/hw_ver/hw_ver.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,42 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file hw_ver.c * - * matrix code + * Show and test hardware version. */ -#include "Matrix.hpp" +#include <nuttx/config.h> + +#include <stdio.h> +#include <string.h> +#include <errno.h> +#include <version/version.h> + +__EXPORT int hw_ver_main(int argc, char *argv[]); + +int +hw_ver_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "show")) { + printf(HW_ARCH "\n"); + exit(0); + } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 3) { + int ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + exit(ret); + + } else { + errx(1, "not enough arguments, try 'compare PX4FMU_1'"); + } + } + } + + errx(1, "expected a command, try 'show' or 'compare'"); +} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk new file mode 100644 index 000000000..3cc08b6a1 --- /dev/null +++ b/src/systemcmds/hw_ver/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = hw_ver +SRCS = hw_ver.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index e34be44e3..e34be44e3 100644 --- a/src/systemcmds/eeprom/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk new file mode 100644 index 000000000..b3fceceb5 --- /dev/null +++ b/src/systemcmds/mtd/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = mtd +SRCS = mtd.c 24xxxx_mtd.c diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c new file mode 100644 index 000000000..a2a0c109c --- /dev/null +++ b/src/systemcmds/mtd/mtd.c @@ -0,0 +1,493 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 mtd.c + * + * mtd service and utility app. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/spi.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +#include <board_config.h> + +__EXPORT int mtd_main(int argc, char *argv[]); + +#ifndef CONFIG_MTD + +/* create a fake command with decent warnx to not confuse users */ +int mtd_main(int argc, char *argv[]) +{ + errx(1, "MTD not enabled, skipping."); +} + +#else + +#ifdef CONFIG_MTD_RAMTRON +static void ramtron_attach(void); +#else + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM +#endif + +static void at24xxx_attach(void); +#endif +static void mtd_start(char *partition_names[], unsigned n_partitions); +static void mtd_test(void); +static void mtd_erase(char *partition_names[], unsigned n_partitions); +static void mtd_readtest(char *partition_names[], unsigned n_partitions); +static void mtd_rwtest(char *partition_names[], unsigned n_partitions); +static void mtd_print_info(); +static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *mtd_dev; +static unsigned n_partitions_current = 0; + +/* note, these will be equally sized */ +static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; +static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); + +int mtd_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) { + + /* start mapping according to user request */ + if (argc >= 3) { + mtd_start(argv + 2, argc - 2); + } else { + mtd_start(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "test")) + mtd_test(); + + if (!strcmp(argv[1], "readtest")) { + if (argc >= 3) { + mtd_readtest(argv + 2, argc - 2); + } else { + mtd_readtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "rwtest")) { + if (argc >= 3) { + mtd_rwtest(argv + 2, argc - 2); + } else { + mtd_rwtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "status")) + mtd_status(); + + if (!strcmp(argv[1], "erase")) { + if (argc >= 3) { + mtd_erase(argv + 2, argc - 2); + } else { + mtd_erase(partition_names_default, n_partitions_default); + } + } + } + + errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); +struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, + off_t firstblock, off_t nblocks); + +#ifdef CONFIG_MTD_RAMTRON +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the RAMTRON driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = ramtron_initialize(spi); + + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: mtd needed %d attempts to attach", i + 1); + } + + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize mtd driver"); + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); + if (ret != OK) + warnx(1, "failed to set bus speed"); + + attached = true; +} +#else + +static void +at24xxx_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = at24c_initialize(i2c); + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} +#endif + +static void +mtd_start(char *partition_names[], unsigned n_partitions) +{ + int ret; + + if (started) + errx(1, "mtd already mounted"); + + if (!attached) { + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + at24xxx_attach(); + #else + ramtron_attach(); + #endif + } + + if (!mtd_dev) { + warnx("ERROR: Failed to create RAMTRON FRAM MTD instance"); + exit(1); + } + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions); + if (ret) + exit(3); + + /* Now create MTD FLASH partitions */ + + warnx("Creating partitions"); + FAR struct mtd_dev_s *part[n_partitions]; + char blockname[32]; + + unsigned offset; + unsigned i; + + for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) { + + warnx(" Partition %d. Block offset=%lu, size=%lu", + i, (unsigned long)offset, (unsigned long)nblocks); + + /* Create the partition */ + + part[i] = mtd_partition(mtd_dev, offset, nblocks); + + if (!part[i]) { + warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu", + (unsigned long)offset, (unsigned long)nblocks); + exit(4); + } + + /* Initialize to provide an FTL block driver on the MTD FLASH interface */ + + snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i); + + ret = ftl_initialize(i, part[i]); + + if (ret < 0) { + warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret); + exit(5); + } + + /* Now create a character device on the block device */ + + ret = bchdev_register(blockname, partition_names[i], false); + + if (ret < 0) { + warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret); + exit(6); + } + } + + n_partitions_current = n_partitions; + + started = true; + exit(0); +} + +int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) +{ + /* Get the geometry of the FLASH device */ + + FAR struct mtd_geometry_s geo; + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + + if (ret < 0) { + warnx("ERROR: mtd->ioctl failed: %d", ret); + return ret; + } + + *blocksize = geo.blocksize; + *erasesize = geo.blocksize; + *neraseblocks = geo.neraseblocks; + + /* Determine the size of each partition. Make each partition an even + * multiple of the erase block size (perhaps not using some space at the + * end of the FLASH). + */ + + *blkpererase = geo.erasesize / geo.blocksize; + *nblocks = (geo.neraseblocks / n_partitions) * *blkpererase; + *partsize = *nblocks * geo.blocksize; + + return ret; +} + +/* + get partition size in bytes + */ +static ssize_t mtd_get_partition_size(void) +{ + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize = 0; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret != OK) { + errx(1, "Failed to get geometry"); + } + return partsize; +} + +void mtd_print_info() +{ + if (!attached) + exit(1); + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret) + exit(3); + + warnx("Flash Geometry:"); + + printf(" blocksize: %lu\n", blocksize); + printf(" erasesize: %lu\n", erasesize); + printf(" neraseblocks: %lu\n", neraseblocks); + printf(" No. partitions: %u\n", n_partitions_current); + printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize); + printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024); + +} + +void +mtd_test(void) +{ + warnx("This test routine does not test anything yet!"); + exit(1); +} + +void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + +void +mtd_erase(char *partition_names[], unsigned n_partitions) +{ + uint8_t v[64]; + memset(v, 0xFF, sizeof(v)); + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("Erasing %s\n", partition_names[i]); + int fd = open(partition_names[i], O_WRONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (write(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + printf("Erased %lu bytes\n", (unsigned long)count); + close(fd); + } + exit(0); +} + +/* + readtest is useful during startup to validate the device is + responding on the bus. It relies on the driver returning an error on + bad reads (the ramtron driver does return an error) + */ +void +mtd_readtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("readtest OK\n"); + exit(0); +} + +/* + rwtest is useful during startup to validate the device is + responding on the bus for both reads and writes. It reads data in + blocks and writes the data back, then reads it again, failing if the + data isn't the same + */ +void +mtd_rwtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128], v2[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + off_t offset = 0; + printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDWR); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (write(fd, v, sizeof(v)) != sizeof(v)) { + errx(1, "write failed"); + } + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { + errx(1, "read failed"); + } + if (memcmp(v, v2, sizeof(v2)) != 0) { + errx(1, "memcmp failed"); + } + offset += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("rwtest OK\n"); + exit(0); +} + +#endif diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 458bb2259..7d9484d3e 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,9 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 50) { + + /* try the first 30 seconds */ + while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 65f291f40..580fdc62f 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -72,7 +72,12 @@ param_main(int argc, char *argv[]) if (argc >= 3) { do_save(argv[2]); } else { - do_save(param_get_default_file()); + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } } } @@ -133,11 +138,8 @@ param_main(int argc, char *argv[]) static void do_save(const char* param_file_name) { - /* delete the parameter file in case it exists */ - unlink(param_file_name); - /* create the file */ - int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL); + int fd = open(param_file_name, O_WRONLY | O_CREAT); if (fd < 0) err(1, "opening '%s' failed", param_file_name); @@ -146,7 +148,7 @@ do_save(const char* param_file_name) close(fd); if (result < 0) { - unlink(param_file_name); + (void)unlink(param_file_name); errx(1, "error exporting to '%s'", param_file_name); } @@ -203,11 +205,38 @@ do_show_print(void *arg, param_t param) int32_t i; float f; const char *search_string = (const char*)arg; + const char *p_name = (const char*)param_name(param); /* print nothing if search string is invalid and not matching */ - if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { - /* param not found */ - return; + if (!(arg == NULL)) { + + /* start search */ + char *ss = search_string; + char *pp = p_name; + bool mismatch = false; + + /* XXX this comparison is only ok for trailing wildcards */ + while (*ss != '\0' && *pp != '\0') { + + if (*ss == *pp) { + ss++; + pp++; + } else if (*ss == '*') { + if (*(ss + 1) != '\0') { + warnx("* symbol only allowed at end of search string."); + exit(1); + } + + pp++; + } else { + /* param not found */ + return; + } + } + + /* the search string must have been consumed */ + if (!(*ss == '\0' || *ss == '*')) + return; } printf("%c %s: ", diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk deleted file mode 100644 index e4eb1d143..000000000 --- a/src/systemcmds/ramtron/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# RAMTRON file system driver -# - -MODULE_COMMAND = ramtron -SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c deleted file mode 100644 index 03c713987..000000000 --- a/src/systemcmds/ramtron/ramtron.c +++ /dev/null @@ -1,279 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 ramtron.c - * - * ramtron service and utility app. - */ - -#include <nuttx/config.h> - -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <stdbool.h> -#include <unistd.h> -#include <fcntl.h> -#include <sys/mount.h> -#include <sys/ioctl.h> -#include <sys/stat.h> - -#include <nuttx/spi.h> -#include <nuttx/mtd.h> -#include <nuttx/fs/nxffs.h> -#include <nuttx/fs/ioctl.h> - -#include <arch/board/board.h> - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -__EXPORT int ramtron_main(int argc, char *argv[]); - -#ifndef CONFIG_MTD_RAMTRON - -/* create a fake command with decent message to not confuse users */ -int ramtron_main(int argc, char *argv[]) -{ - errx(1, "RAMTRON not enabled, skipping."); -} -#else - -static void ramtron_attach(void); -static void ramtron_start(void); -static void ramtron_erase(void); -static void ramtron_ioctl(unsigned operation); -static void ramtron_save(const char *name); -static void ramtron_load(const char *name); -static void ramtron_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *ramtron_mtd; - -int ramtron_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - ramtron_start(); - - if (!strcmp(argv[1], "save_param")) - ramtron_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - ramtron_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - ramtron_erase(); - - if (!strcmp(argv[1], "test")) - ramtron_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - ramtron_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - ramtron_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); -} - -struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); - - -static void -ramtron_attach(void) -{ - /* find the right spi */ - struct spi_dev_s *spi = up_spiinitialize(2); - /* this resets the spi bus, set correct bus speed again */ - // xxx set in ramtron driver, leave this out -// SPI_SETFREQUENCY(spi, 4000000); - SPI_SETFREQUENCY(spi, 375000000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SELECT(spi, SPIDEV_FLASH, false); - - if (spi == NULL) - errx(1, "failed to locate spi bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - ramtron_mtd = ramtron_initialize(spi); - if (ramtron_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: ramtron needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (ramtron_mtd == NULL) - errx(1, "failed to initialize ramtron driver"); - - attached = true; -} - -static void -ramtron_start(void) -{ - int ret; - - if (started) - errx(1, "ramtron already mounted"); - - if (!attached) - ramtron_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(ramtron_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); - - /* mount the ramtron */ - ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /ramtron - erase ramtron to reformat"); - - started = true; - warnx("mounted ramtron at /ramtron"); - exit(0); -} - -//extern int at24c_nuke(void); - -static void -ramtron_erase(void) -{ - if (!attached) - ramtron_attach(); - -// if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -ramtron_ioctl(unsigned operation) -{ - int fd; - - fd = open("/ramtron/.", 0); - - if (fd < 0) - err(1, "open /ramtron"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -ramtron_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -ramtron_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -//extern void at24c_test(void); - -static void -ramtron_test(void) -{ -// at24c_test(); - exit(0); -} - -#endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 68a080c77..acf28c35b 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,8 +24,12 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ - test_rc.c + test_rc.c \ + test_conv.cpp \ + test_mount.c \ + test_mtd.c diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/systemcmds/tests/test_conv.cpp index 399eecfa7..50dece816 100644 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -32,43 +32,45 @@ ****************************************************************************/ /** - * @file Vector.h + * @file test_conv.cpp + * Tests conversions used across the system. * - * math vector */ -#pragma once +#include <nuttx/config.h> -#include "Vector.hpp" +#include <sys/types.h> -namespace math -{ - -class Quaternion; -class Dcm; +#include <stdio.h> +#include <poll.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); +#include "tests.h" - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } +#include <math.h> +#include <float.h> - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } +#include <systemlib/err.h> +#include <unit_test/unit_test.h> +#include <px4iofirmware/protocol.h> -}; +int test_conv(int argc, char *argv[]) +{ + warnx("Testing system conversions"); -int __EXPORT eulerAnglesTest(); + for (int i = -10000; i <= 10000; i+=1) { + float f = i/10000.0f; + float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); + if (fabsf(f - fres) > 0.0001f) { + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + return 1; + } + } -} // math + warnx("All conversions clean"); + return 0; +} diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1..96be1e8df 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +35,12 @@ * @file test_file.c * * File write test. + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <sys/stat.h> +#include <poll.h> #include <dirent.h> #include <stdio.h> #include <stddef.h> @@ -51,6 +54,40 @@ #include "tests.h" +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + int test_file(int argc, char *argv[]) { @@ -86,7 +123,6 @@ test_file(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); @@ -94,28 +130,25 @@ test_file(int argc, char *argv[]) start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { - //perf_begin(wperf); int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) - errx(1, "memory is unaligned, align shift: %d", a); + warnx("memory is unaligned, align shift: %d", a); + return 1; } fsync(fd); - //perf_end(wperf); + + if (!check_user_abort(fd)) + return OK; } end = hrt_absolute_time(); - //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - //perf_print_counter(wperf); - //perf_free(wperf); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -124,7 +157,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - errx(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -139,9 +173,13 @@ test_file(int argc, char *argv[]) } if (!compare_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } + if (!check_user_abort(fd)) + return OK; + } /* @@ -152,16 +190,20 @@ test_file(int argc, char *argv[]) int ret = unlink("/fs/microsd/testfile"); fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned writes - please wait.."); + warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); if (wret != chunk_sizes[c]) { - err(1, "WRITE ERROR!"); + warnx("WRITE ERROR!"); + return 1; } + if (!check_user_abort(fd)) + return OK; + } fsync(fd); @@ -178,7 +220,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -190,10 +233,14 @@ test_file(int argc, char *argv[]) align_read_ok = false; break; } + + if (!check_user_abort(fd)) + return OK; } if (!align_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -215,7 +262,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf + a, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } for (int j = 0; j < chunk_sizes[c]; j++) { @@ -228,10 +276,14 @@ test_file(int argc, char *argv[]) if (unalign_read_err_count > 10) break; } + + if (!check_user_abort(fd)) + return OK; } if (!unalign_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -239,9 +291,10 @@ test_file(int argc, char *argv[]) ret = unlink("/fs/microsd/testfile"); close(fd); - if (ret) - err(1, "UNLINKING FILE FAILED"); - + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } } } @@ -261,75 +314,9 @@ test_file(int argc, char *argv[]) } else { /* failed opening dir */ - err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); - } - - return 0; -} -#if 0 -int -test_file(int argc, char *argv[]) -{ - const iterations = 1024; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); return 1; } - uint8_t buf[512]; - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, buf, sizeof(buf)); - perf_end(wperf); - } - end = hrt_absolute_time(); - - close(fd); - - unlink("/fs/microsd/testfile"); - - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - perf_print_counter(wperf); - perf_free(wperf); - - warnx("running unlink test"); - - /* ensure that common commands do not run against file count limits */ - for (unsigned i = 0; i < 64; i++) { - - warnx("unlink iteration #%u", i); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file before unlink()"); - int ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file before unlink()"); - close(fd); - - ret = unlink("/fs/microsd/testfile"); - if (ret != OK) - errx(1, "failed unlinking test file"); - - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file after unlink()"); - ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file after unlink()"); - close(fd); - } - return 0; } -#endif diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp new file mode 100644 index 000000000..693a208ba --- /dev/null +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 test_mathlib.cpp + * + * Mathlib test + */ + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <time.h> +#include <mathlib/mathlib.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +#include "tests.h" + +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + +using namespace math; + +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; +} + +int test_mathlib(int argc, char *argv[]) +{ + warnx("testing mathlib"); + + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector<2>()", Vector<2> v3); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f)); + TEST_OP("Vector<2> = Vector<2>", v = v1); + TEST_OP("Vector<2> + Vector<2>", v + v1); + TEST_OP("Vector<2> - Vector<2>", v - v1); + TEST_OP("Vector<2> += Vector<2>", v += v1); + TEST_OP("Vector<2> -= Vector<2>", v -= v1); + TEST_OP("Vector<2> * Vector<2>", v * v1); + TEST_OP("Vector<2> %% Vector<2>", v1 % v2); + } + + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector<3>()", Vector<3> v3); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector<3> = Vector<3>", v = v1); + TEST_OP("Vector<3> + Vector<3>", v + v1); + TEST_OP("Vector<3> - Vector<3>", v - v1); + TEST_OP("Vector<3> += Vector<3>", v += v1); + TEST_OP("Vector<3> -= Vector<3>", v -= v1); + TEST_OP("Vector<3> * float", v1 * 2.0f); + TEST_OP("Vector<3> / float", v1 / 2.0f); + TEST_OP("Vector<3> *= float", v1 *= 2.0f); + TEST_OP("Vector<3> /= float", v1 /= 2.0f); + TEST_OP("Vector<3> * Vector<3>", v * v1); + TEST_OP("Vector<3> %% Vector<3>", v1 % v2); + TEST_OP("Vector<3> length", v1.length()); + TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); + } + + { + Vector<4> v; + Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); + Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Vector<4> v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v * v1); + } + + { + Vector<10> v1; + v1.zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Vector<10> v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + } + + { + Matrix<3, 3> m1; + m1.identity(); + Matrix<3, 3> m2; + m2.identity(); + Vector<3> v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1); + TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); + TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); + } + + { + Matrix<10, 10> m1; + m1.identity(); + Matrix<10, 10> m2; + m2.identity(); + Vector<10> v1; + v1.zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + return 0; +} diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 4da86042d..2a47551ee 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -48,9 +48,13 @@ #include <errno.h> #include <string.h> #include <time.h> +#include <math.h> #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> +#include <systemlib/pwm_limit/pwm_limit.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include "tests.h" @@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); +const unsigned output_max = 8; +static float actuator_controls[output_max]; +static bool should_arm = false; +uint16_t r_page_servo_disarmed[output_max]; +uint16_t r_page_servo_control_min[output_max]; +uint16_t r_page_servo_control_max[output_max]; +uint16_t r_page_servos[output_max]; +uint16_t servo_predicted[output_max]; + +/* + * PWM limit structure + */ +pwm_limit_t pwm_limit; + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[]) if (mixer_group.count() != 8) return 1; + /* execute the mixer */ + + float outputs[output_max]; + unsigned mixed; + const int jmax = 5; + + pwm_limit_init(&pwm_limit); + should_arm = true; + + /* run through arming phase */ + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + } + + warnx("ARMING TEST: STARTING RAMP"); + unsigned sleep_quantum_us = 10000; + + hrt_abstime starttime = hrt_absolute_time(); + unsigned sleepcount = 0; + + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (hrt_elapsed_time(&starttime) < INIT_TIME_US && + r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && + r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { + warnx("ramp servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: NORMAL OPERATION"); + + for (int j = -jmax; j <= jmax; j++) { + + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = j/10.0f + 0.1f * i; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + } + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + warnx("mixed %d outputs (max %d)", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + } + } + + warnx("ARMING TEST: DISARMING"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = false; + + while (hrt_elapsed_time(&starttime) < 600000) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: REARMING: STARTING RAMP"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = true; + + while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* predict value */ + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + + /* check ramp */ + + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && + (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || + r_page_servos[i] > servo_predicted[i])) { + warnx("ramp servo value mismatch"); + return 1; + } + + /* check post ramp phase */ + if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && + fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + /* load multirotor at once test */ mixer_group.reset(); @@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[]) unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + if (mixer_group.count() != 5) { + warnx("FAIL: Quad W mixer load failed"); return 1; + } + + warnx("SUCCESS: No errors in mixer test"); } static int @@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle, if (control_group != 0) return -1; - control = 0.0f; + if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) + return -1; + + control = actuator_controls[control_index]; return 0; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c new file mode 100644 index 000000000..44e34d9ef --- /dev/null +++ b/src/systemcmds/tests/test_mount.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 test_mount.c + * + * Device mount / unmount stress test + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <sys/stat.h> +#include <dirent.h> +#include <stdio.h> +#include <stddef.h> +#include <unistd.h> +#include <fcntl.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "tests.h" + +const int fsync_tries = 1; +const int abort_tries = 10; + +int +test_mount(int argc, char *argv[]) +{ + const unsigned iterations = 2000; + const unsigned alignments = 10; + + const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + + if (stat(cmd_filename, &buffer) == OK) { + (void)unlink(cmd_filename); + } + + return 1; + } + + /* read current test status from file, write test instructions for next round */ + + /* initial values */ + int it_left_fsync = fsync_tries; + int it_left_abort = abort_tries; + + int cmd_fd; + if (stat(cmd_filename, &buffer) == OK) { + + /* command file exists, read off state */ + cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK); + char buf[64]; + int ret = read(cmd_fd, buf, sizeof(buf)); + + if (ret > 0) { + int count = 0; + ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count); + } else { + buf[0] = '\0'; + } + + if (it_left_fsync > fsync_tries) + it_left_fsync = fsync_tries; + + if (it_left_abort > abort_tries) + it_left_abort = abort_tries; + + warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, + fsync_tries, abort_tries, buf); + + int it_left_fsync_prev = it_left_fsync; + + /* now write again what to do next */ + if (it_left_fsync > 0) + it_left_fsync--; + + if (it_left_fsync == 0 && it_left_abort > 0) { + + it_left_abort--; + + /* announce mode switch */ + if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { + warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); + fsync(stdout); + fsync(stderr); + usleep(20000); + } + + } + + if (it_left_abort == 0) { + (void)unlink(cmd_filename); + return 0; + } + + } else { + + /* this must be the first iteration, do something */ + cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT); + + warnx("First iteration of file test\n"); + } + + char buf[64]; + int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + lseek(cmd_fd, 0, SEEK_SET); + write(cmd_fd, buf, strlen(buf) + 1); + fsync(cmd_fd); + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); + fsync(stdout); + fsync(stderr); + usleep(50000); + + for (unsigned a = 0; a < alignments; a++) { + + printf("."); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + warnx("memory is unaligned, align shift: %d", a); + + return 1; + + } + + if (it_left_fsync > 0) { + fsync(fd); + } else { + printf("#"); + fsync(stdout); + fsync(stderr); + } + } + + if (it_left_fsync > 0) { + printf("#"); + } + + printf("."); + fsync(stdout); + fsync(stderr); + usleep(200000); + + end = hrt_absolute_time(); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; + } + + } + + int ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } + + } + } + + fsync(stdout); + fsync(stderr); + usleep(20000); + + + + /* we always reboot for the next test if we get here */ + warnx("Iteration done, rebooting.."); + fsync(stdout); + fsync(stderr); + usleep(50000); + systemreset(false); + + /* never going to get here */ + return 0; +} diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c new file mode 100644 index 000000000..bac9efbdb --- /dev/null +++ b/src/systemcmds/tests/test_mtd.c @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 test_mtd.c + * + * Param storage / file test. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <sys/stat.h> +#include <poll.h> +#include <dirent.h> +#include <stdio.h> +#include <stddef.h> +#include <unistd.h> +#include <fcntl.h> +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "tests.h" + +#define PARAM_FILE_NAME "/fs/mtd_params" + +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + +void print_fail() +{ + printf("<[T]: MTD: FAIL>\n"); +} + +void print_success() +{ + printf("<[T]: MTD: OK>\n"); +} + +int +test_mtd(int argc, char *argv[]) +{ + unsigned iterations= 0; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat(PARAM_FILE_NAME, &buffer)) { + warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); + print_fail(); + return 1; + } + + // XXX get real storage space here + unsigned file_size = 4096; + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {256, 512, 4096}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open(PARAM_FILE_NAME, O_RDONLY); + int rret = read(fd, read_buf, chunk_sizes[c]); + close(fd); + + fd = open(PARAM_FILE_NAME, O_WRONLY); + + printf("printing 2 percent of the first chunk:\n"); + for (int i = 0; i < sizeof(read_buf) / 50; i++) { + printf("%02X", read_buf[i]); + } + printf("\n"); + + iterations = file_size / chunk_sizes[c]; + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != (int)chunk_sizes[c]) { + warn("WRITE ERROR!"); + print_fail(); + return 1; + } + + fsync(fd); + + if (!check_user_abort(fd)) + return OK; + + } + end = hrt_absolute_time(); + + close(fd); + fd = open(PARAM_FILE_NAME, O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + print_fail(); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d", j); + print_fail(); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + print_fail(); + return 1; + } + + if (!check_user_abort(fd)) + return OK; + + } + + + close(fd); + + } + + /* fill the file with 0xFF to make it look new again */ + char ffbuf[64]; + memset(ffbuf, 0xFF, sizeof(ffbuf)); + int fd = open(PARAM_FILE_NAME, O_WRONLY); + for (int i = 0; i < file_size / sizeof(ffbuf); i++) { + int ret = write(fd, ffbuf, sizeof(ffbuf)); + + if (ret != sizeof(ffbuf)) { + warnx("ERROR! Could not fill file with 0xFF"); + close(fd); + print_fail(); + return 1; + } + } + + (void)close(fd); + print_success(); + + return 0; +} diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 72619fc8b..6a602ecfc 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file test_ppm_loopback.c - * Tests the PWM outputs and PPM input + * @file test_rc.c + * Tests RC input. * */ diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index a57d04be3..ac64ad33d 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -109,6 +109,10 @@ extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); +extern int test_conv(int argc, char *argv[]); +extern int test_mount(int argc, char *argv[]); +extern int test_mtd(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 1088a4407..73827b7cf 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -106,6 +106,10 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mtd", test_mtd, 0}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; |