diff options
55 files changed, 466 insertions, 1036 deletions
diff --git a/NuttX b/NuttX -Subproject 89a6776fc1b31d242c637d3f19072609bb98ec6 +Subproject c7b06385926349e10b3739314d1d56eec7efb8b diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c8379e3a1..c1b366de8 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO review MC_YAWRATE_I param set MC_ROLL_P 8.0 @@ -26,5 +26,5 @@ fi set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 0b422de7e..3879737a8 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 @@ -29,7 +29,7 @@ fi set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 set PWM_MIN 1200 set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index a621de7ce..57f77754c 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 @@ -31,4 +31,4 @@ set MIXER FMU_quad_w set PWM_MIN 1210 set PWM_MAX 2100 -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 00b97d675..f208b692a 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set FW_AIRSPD_MIN 12 param set FW_AIRSPD_TRIM 25 diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index e4d96fbd5..50f717e3d 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_cox # Need to set all 8 channels -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index f820251ad..e0a838185 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_cox -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index dcc5db824..906f4b1cc 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -2,4 +2,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_AERT
\ No newline at end of file +set MIXER skywalker
\ 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 0f0cb3a89..fe0269557 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -4,5 +4,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER FMU_Q # Provide ESC a constant 1000 us pulse while disarmed -set PWM_OUTPUTS 4 +set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a7749cba6..c7dd1dfc5 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set FW_AIRSPD_MIN 13 param set FW_AIRSPD_TRIM 15 @@ -36,5 +36,5 @@ fi set MIXER phantom # Provide ESC a constant 1000 us pulse -set PWM_OUTPUTS 4 +set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 26c7c95e6..94363bf6a 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set FW_AIRSPD_MIN 15 param set FW_AIRSPD_TRIM 20 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 919eefb4a..add905b11 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set BAT_N_CELLS 2 param set FW_AIRSPD_MAX 15 @@ -44,5 +44,5 @@ fi set MIXER wingwing # Provide ESC a constant 1000 us pulse -set PWM_OUTPUTS 4 +set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9bfd9d9ed..9eafac1c5 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -7,9 +7,9 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then - + # TODO: these are the X5 default parameters, update them to the caipi param set FW_AIRSPD_MIN 15 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 8fe8961c5..4677f9fc3 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_x -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 0488e3928..c3358ef4c 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -8,7 +8,7 @@ sh /etc/init.d/rc.mc_defaults # # Load default params for this platform # -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # Set all params here, then disable autoconfig param set MC_ROLL_P 6.0 @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.8 - + param set BAT_V_SCALING 0.00838095238 fi diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index f0cc05207..8e5dc76b1 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -7,7 +7,7 @@ sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 1ca716a6b..ea35b3ba9 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -7,7 +7,7 @@ sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 5c4a6497a..b1db1dd9a 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -7,7 +7,7 @@ sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 9fe310dde..a1de19d5d 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -9,7 +9,7 @@ echo "HK Micro PCB Quad" sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 5512aa738..c78911391 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 1043ad8ad..0df25b11a 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x # Need to set all 8 channels -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 84ab88883..16c772ee1 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ # Need to set all 8 channels -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 74e304cd9..bae36737f 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_x -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index f7c06c6c8..ca5439f68 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_+ -set PWM_OUTPUTS 12345678
\ No newline at end of file +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3c336f295..fab2a7f18 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -2,7 +2,7 @@ set VEHICLE_TYPE fw -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # # Default parameters for FW @@ -15,4 +15,4 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 -fi
\ No newline at end of file +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 41e0b149b..bab71be93 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -8,12 +8,11 @@ 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 ] + #Use the mixer file from the SD-card if it exists + if [ -f /fs/microsd/etc/mixers/$MIXER.mix ] then - set MIXER_FILE $MIXERSD + set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix else set MIXER_FILE /etc/mixers/$MIXER.mix fi @@ -32,29 +31,31 @@ then if mixer load $OUTPUT_DEV $MIXER_FILE then - echo "[init] Mixer loaded: $MIXER_FILE" + echo "[i] Mixer: $MIXER_FILE" else - echo "[init] Error loading mixer: $MIXER_FILE" - tone_alarm $TUNE_OUT_ERROR + echo "[i] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_ERR fi + + unset MIXER_FILE else if [ $MIXER != skip ] then - echo "[init] Mixer not defined" - tone_alarm $TUNE_OUT_ERROR + echo "[i] Mixer not defined" + tone_alarm $TUNE_ERR fi fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then - if [ $PWM_OUTPUTS != none ] + if [ $PWM_OUT != none ] then # # Set PWM output frequency # if [ $PWM_RATE != none ] then - pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + pwm rate -c $PWM_OUT -r $PWM_RATE fi # @@ -62,15 +63,15 @@ then # if [ $PWM_DISARMED != none ] then - pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + pwm disarmed -c $PWM_OUT -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - pwm min -c $PWM_OUTPUTS -p $PWM_MIN + pwm min -c $PWM_OUT -p $PWM_MIN fi if [ $PWM_MAX != none ] then - pwm max -c $PWM_OUTPUTS -p $PWM_MAX + pwm max -c $PWM_OUT -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index e23aebd87..e957626ce 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -16,5 +16,5 @@ then set PX4IO_LIMIT 200 fi -echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 0df320f49..307a64c4d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -2,7 +2,7 @@ set VEHICLE_TYPE mc -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index fbac50cf7..e6fc535a6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -56,7 +56,7 @@ fi if meas_airspeed start then - echo "[init] Using MEAS airspeed sensor" + echo "[i] Using MEAS airspeed sensor" else if ets_airspeed start then @@ -71,6 +71,10 @@ if px4flow start then fi +if ll40ls start +then +fi + # # Start sensors -> preflight_check # diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 9a470a6b8..08ba86d78 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -10,9 +10,9 @@ then # First sensor publisher to initialize takes lowest instance ID # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs sleep 1 - echo "[init] UAVCAN started" + echo "[i] UAVCAN started" else - echo "[init] ERROR: Could not start UAVCAN" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: Could not start UAVCAN" + tone_alarm $TUNE_ERR fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 353f44877..b027107c8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -1,46 +1,46 @@ #!nsh # # PX4FMU startup script. +# +# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. +# # # Default to auto-start mode. # set MODE autostart -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 FRC /fs/microsd/etc/rc.txt +set FCONFIG /fs/microsd/etc/config.txt +set FEXTRAS /fs/microsd/etc/extras.txt -set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4 +set TUNE_ERR ML<<CP4CP4CP4CP4CP4 # # Try to mount the microSD card. # -echo "[init] Looking for microSD..." +echo "[i] Looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then set LOG_FILE /fs/microsd/bootlog.txt - echo "[init] microSD mounted: /fs/microsd" + echo "[i] microSD mounted: /fs/microsd" # Start playing the startup tune tone_alarm start else 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. # -if [ -f $RC_FILE ] +if [ -f $FRC ] then - echo "[init] Executing init script: $RC_FILE" - sh $RC_FILE + echo "[i] Executing init script: $FRC" + sh $FRC set MODE custom else - echo "[init] Init script not found: $RC_FILE" + echo "[i] Init script not found: $FRC" fi # if this is an APM build then there will be a rc.APM script @@ -49,17 +49,17 @@ if [ -f /etc/init.d/rc.APM ] then if sercon then - echo "[init] USB interface connected" + echo "[i] USB interface connected" fi - echo "[init] Running rc.APM" + echo "[i] 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" + echo "[i] AUTOSTART mode" # # Start CDC/ACM serial driver @@ -117,31 +117,31 @@ then set VEHICLE_TYPE none set MIXER none set OUTPUT_MODE none - set PWM_OUTPUTS none + set PWM_OUT none set PWM_RATE none set PWM_DISARMED none set PWM_MIN none set PWM_MAX none - set MKBLCTRL_MODE none + set MK_MODE none set FMU_MODE pwm - set MAVLINK_FLAGS default + set MAVLINK_F default set EXIT_ON_END no set MAV_TYPE none - set LOAD_DEFAULT_APPS yes + set LOAD_DAPPS yes set GPS yes set GPS_FAKE no set FAILSAFE none # - # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts + # Set AUTOCNF flag to use it in AUTOSTART scripts # if param compare SYS_AUTOCONFIG 1 then # Wipe out params param reset_nostart - set DO_AUTOCONFIG yes + set AUTOCNF yes else - set DO_AUTOCONFIG no + set AUTOCNF no fi # @@ -159,7 +159,7 @@ then # if param compare SYS_AUTOSTART 0 then - echo "[init] No autostart" + echo "[i] No autostart" else sh /etc/init.d/rc.autostart fi @@ -167,18 +167,19 @@ then # # Override parameters from user configuration file # - if [ -f $CONFIG_FILE ] + if [ -f $FCONFIG ] then - echo "[init] Config: $CONFIG_FILE" - sh $CONFIG_FILE + echo "[i] Config: $FCONFIG" + sh $FCONFIG else - echo "[init] Config not found: $CONFIG_FILE" + echo "[i] Config not found: $FCONFIG" fi + unset FCONFIG # # If autoconfig parameter was set, reset it and save parameters # - if [ $DO_AUTOCONFIG == yes ] + if [ $AUTOCNF == yes ] then param set SYS_AUTOCONFIG 0 param save @@ -219,18 +220,18 @@ then set IO_PRESENT yes else echo "PX4IO update failed" >> $LOG_FILE - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi else echo "PX4IO update failed" >> $LOG_FILE - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi fi if [ $IO_PRESENT == no ] then - echo "[init] ERROR: PX4IO not found" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: PX4IO not found" + tone_alarm $TUNE_ERR fi fi @@ -251,7 +252,7 @@ then then # Need IO for output but it not present, disable output set OUTPUT_MODE none - echo "[init] ERROR: PX4IO not found, disabling output" + echo "[i] ERROR: PX4IO not found, disabling output" # Avoid using ttyS0 for MAVLink on FMUv1 if ver hwcmp PX4FMU_V1 @@ -294,7 +295,7 @@ then then if param compare UAVCAN_ENABLE 0 then - echo "[init] OVERRIDING UAVCAN_ENABLE = 1" + echo "[i] OVERRIDING UAVCAN_ENABLE = 1" param set UAVCAN_ENABLE 1 fi fi @@ -303,11 +304,11 @@ then then if px4io start then - echo "[init] PX4IO started" + echo "[i] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: PX4IO start failed" + tone_alarm $TUNE_ERR fi fi @@ -315,10 +316,10 @@ then then if fmu mode_$FMU_MODE then - echo "[init] FMU mode_$FMU_MODE started" + echo "[i] FMU mode_$FMU_MODE started" else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_ERR fi if ver hwcmp PX4FMU_V1 @@ -337,21 +338,21 @@ then if [ $OUTPUT_MODE == mkblctrl ] then set MKBLCTRL_ARG "" - if [ $MKBLCTRL_MODE == x ] + if [ $MK_MODE == x ] then set MKBLCTRL_ARG "-mkmode x" fi - if [ $MKBLCTRL_MODE == + ] + if [ $MK_MODE == + ] then set MKBLCTRL_ARG "-mkmode +" fi if mkblctrl $MKBLCTRL_ARG then - echo "[init] MKBLCTRL started" + echo "[i] MK started" else - echo "[init] ERROR: MKBLCTRL start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: MK start failed" + tone_alarm $TUNE_ERR fi fi @@ -360,10 +361,10 @@ then then if hil mode_port2_pwm8 then - echo "[init] HIL output started" + echo "[i] HIL output started" else - echo "[init] ERROR: HIL output start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: HIL output start failed" + tone_alarm $TUNE_ERR fi fi @@ -376,10 +377,11 @@ then then if px4io start then + echo "[i] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: PX4IO start failed" + tone_alarm $TUNE_ERR fi fi else @@ -387,10 +389,10 @@ then then if fmu mode_$FMU_MODE then - echo "[init] FMU mode_$FMU_MODE started" + echo "[i] FMU mode_$FMU_MODE started" else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_ERR fi if ver hwcmp PX4FMU_V1 @@ -408,23 +410,24 @@ then fi fi - if [ $MAVLINK_FLAGS == default ] + if [ $MAVLINK_F == default ] then # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + set MAVLINK_F "-r 1000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_FLAGS "-r 1000" + set MAVLINK_F "-r 1000" fi fi - mavlink start $MAVLINK_FLAGS + mavlink start $MAVLINK_F + unset MAVLINK_F # # UAVCAN @@ -439,14 +442,16 @@ then if [ $GPS == yes ] then + echo "[i] Start GPS" if [ $GPS_FAKE == yes ] then - echo "[init] Faking GPS" + echo "[i] Faking GPS" gps start -f else gps start fi fi + unset GPS_FAKE # # Start up ARDrone Motor interface @@ -461,7 +466,7 @@ then # if [ $VEHICLE_TYPE == fw ] then - echo "[init] Vehicle type: FIXED WING" + echo "[i] FIXED WING" if [ $MIXER == none ] then @@ -481,7 +486,7 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ $LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DAPPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -492,11 +497,11 @@ then # if [ $VEHICLE_TYPE == mc ] then - echo "[init] Vehicle type: MULTICOPTER" + echo "[i] MULTICOPTER" if [ $MIXER == none ] then - echo "Default mixer for multicopter not defined" + echo "Mixer undefined" fi if [ $MAV_TYPE == none ] @@ -540,7 +545,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ $LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DAPPS == yes ] then sh /etc/init.d/rc.mc_apps fi @@ -556,24 +561,38 @@ then # if [ $VEHICLE_TYPE == none ] then - echo "[init] Vehicle type: No autostart ID found" + echo "[i] No autostart ID found" fi # Start any custom addons - if [ -f $EXTRAS_FILE ] + if [ -f $FEXTRAS ] then - echo "[init] Starting addons script: $EXTRAS_FILE" - sh $EXTRAS_FILE + echo "[i] Addons script: $FEXTRAS" + sh $FEXTRAS else - echo "[init] No addons script: $EXTRAS_FILE" + echo "[i] No addons script: $FEXTRAS" fi + unset FEXTRAS if [ $EXIT_ON_END == yes ] then - echo "[init] Exit from nsh" + echo "Exit from nsh" exit fi + unset EXIT_ON_END + + # Run no SD alarm last + if [ $LOG_FILE == /dev/null ] + then + echo "[i] No microSD card found" + # Play SOS + tone_alarm error + fi # End of autostart fi + +# There is no further processing, so we can free some RAM +# XXX potentially unset all script variables. +unset TUNE_ERR diff --git a/ROMFS/px4fmu_common/mixers/skywalker.mix b/ROMFS/px4fmu_common/mixers/skywalker.mix new file mode 100644 index 000000000..04d677e56 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/skywalker.mix @@ -0,0 +1,64 @@ +Mixer for Skywalker Airframe +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the rudder to output 2 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 -10000 -10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone index 67e95215b..5c7470d12 100644 --- a/ROMFS/px4fmu_test/init.d/rc.standalone +++ b/ROMFS/px4fmu_test/init.d/rc.standalone @@ -3,11 +3,11 @@ # Flight startup script for PX4FMU standalone configuration. # -echo "[init] doing standalone PX4FMU startup..." +echo "[i] doing standalone PX4FMU startup..." # # Start the ORB # uorb start -echo "[init] startup done" +echo "[i] startup done" diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index bc248ac04..ef032de5c 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -6,7 +6,7 @@ uorb start if sercon then - echo "[init] USB interface connected" + echo "[i] USB interface connected" # Try to get an USB console nshterm /dev/ttyACM0 & @@ -15,14 +15,14 @@ fi # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[i] looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + echo "[i] card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + echo "[i] no microSD card found" # Play SOS tone_alarm error fi @@ -104,4 +104,4 @@ then else echo echo "Some Unit Tests FAILED:${unit_test_failure_list}" -fi
\ No newline at end of file +fi diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 844f1236a..fa2c83262 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -72,14 +72,24 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += platforms/nuttx # -# Modules to test-build, but not useful for test environment +# Example modules to test-build +# +MODULES += examples/flow_position_estimator +MODULES += examples/fixedwing_control +MODULES += examples/hwtest +MODULES += examples/matlab_csv_serial +MODULES += examples/px4_daemon_app +MODULES += examples/px4_mavlink_debug +MODULES += examples/px4_simple_app + +# +# Drivers / modules to test build, but not useful for test environment # MODULES += modules/attitude_estimator_so3 MODULES += drivers/pca8574 -MODULES += examples/flow_position_estimator # -# Libraries +# Tests # MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 6793acd81..4676c6ad7 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -89,7 +89,7 @@ /* Device limits */ #define LL40LS_MIN_DISTANCE (0.00f) -#define LL40LS_MAX_DISTANCE (14.00f) +#define LL40LS_MAX_DISTANCE (60.00f) #define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */ @@ -233,11 +233,11 @@ LL40LS::~LL40LS() if (_reports != nullptr) { delete _reports; } - + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -263,7 +263,7 @@ LL40LS::init() _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { + if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct range_finder_report rf_report; measure(); @@ -314,9 +314,9 @@ LL40LS::probe() goto ok; } - debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", - (unsigned)who_am_i, - LL40LS_WHO_AM_I_REG_VAL, + debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", + (unsigned)who_am_i, + LL40LS_WHO_AM_I_REG_VAL, (unsigned)val); } @@ -581,6 +581,8 @@ LL40LS::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { report.valid = 1; } @@ -704,7 +706,7 @@ LL40LS::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", + printf("distance: %ucm (0x%04x)\n", (unsigned)_last_distance, (unsigned)_last_distance); } @@ -969,8 +971,8 @@ ll40ls_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; - + const char *verb = argv[optind]; + /* * Start/load the driver. */ diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index beb6c8e35..9cf568658 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -520,6 +520,8 @@ MB12XX::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it, if we are the primary */ diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index fdd524189..4301750f0 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -545,7 +545,7 @@ SF0X::collect() float si_units; bool valid = false; - + for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { valid = true; @@ -564,6 +564,8 @@ SF0X::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 2f2f692a1..83b5c987e 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -256,11 +256,11 @@ TRONE::~TRONE() if (_reports != nullptr) { delete _reports; } - + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -286,7 +286,7 @@ TRONE::init() _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { + if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct range_finder_report rf_report; measure(); @@ -558,8 +558,10 @@ TRONE::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - + /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 067d77364..6a5cbcd30 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.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 @@ -31,6 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + /** * @file main.c * @@ -55,7 +55,7 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -106,11 +106,9 @@ static void usage(const char *reason); * * @param att_sp The current attitude setpoint - the values the system would like to reach. * @param att The current attitude. The controller should make the attitude match the setpoint - * @param speed_body The velocity of the system. Currently unused. * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); /** @@ -125,7 +123,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * @param att The current attitude * @param att_sp The attitude setpoint. This is the output of the controller */ -void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, +void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); /* Variables */ @@ -135,8 +133,7 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { @@ -173,7 +170,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st actuators->control[1] = pitch_err * p.pitch_p; } -void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, +void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) { @@ -186,7 +183,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v /* calculate heading error */ float yaw_err = att->yaw - bearing; /* apply control gain */ - float roll_command = yaw_err * p.hdng_p; + att_sp->roll_body = yaw_err * p.hdng_p; /* limit output, this commonly is a tuning parameter, too */ if (att_sp->roll_body < -0.6f) { @@ -253,7 +250,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) memset(&manual_sp, 0, sizeof(manual_sp)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); - struct vehicle_global_position_setpoint_s global_sp; + struct position_setpoint_s global_sp; memset(&global_sp, 0, sizeof(global_sp)); /* output structs - this is what is sent to the mixer */ @@ -275,17 +272,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - float speed_body[3] = {0.0f, 0.0f, 0.0f}; - /* RC failsafe check */ - bool throttle_half_once = false; + struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, { .fd = att_sub, .events = POLLIN }}; @@ -339,25 +333,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp); - - /* currently speed in body frame is not used, but here for reference */ - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (att.R_valid) { - speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; - speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; - speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; - - } else { - speed_body[0] = 0; - speed_body[1] = 0; - speed_body[2] = 0; - - warnx("Did not get a valid R\n"); - } + if (global_sp_updated) { + struct position_setpoint_triplet_s triplet; + orb_copy(ORB_ID(position_setpoint_triplet), global_sp_sub, &triplet); + memcpy(&global_sp, &triplet.current, sizeof(global_sp)); } if (manual_sp_updated) @@ -365,106 +344,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.6f) && - (manual_sp.throttle <= 1.0f)) { - throttle_half_once = true; + if (isfinite(manual_sp.z) && + (manual_sp.z >= 0.6f) && + (manual_sp.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* control */ - -#warning fix this -#if 0 - if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ || - vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) { - - /* simple heading control */ - control_heading(&global_pos, &global_sp, &att, &att_sp); - - /* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */ - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - - /* simple attitude control */ - control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) { - /* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */ - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost && throttle_half_once) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } -#endif - /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); @@ -474,6 +361,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) isfinite(actuators.control[2]) && isfinite(actuators.control[3])) { orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + if (verbose) { + warnx("published"); + } } } } diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c deleted file mode 100644 index feed0d23c..000000000 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ /dev/null @@ -1,387 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann <samuezih@ee.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 flow_speed_control.c - * - * Optical flow speed controller - */ - -#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 <termios.h> -#include <time.h> -#include <math.h> -#include <sys/prctl.h> -#include <drivers/drv_hrt.h> -#include <uORB/uORB.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h> -#include <uORB/topics/filtered_bottom_flow.h> -#include <systemlib/systemlib.h> -#include <systemlib/perf_counter.h> -#include <systemlib/err.h> -#include <poll.h> -#include <mavlink/mavlink_log.h> - -#include "flow_speed_control_params.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 flow_speed_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int flow_speed_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\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_cmd(). - */ -int flow_speed_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { - printf("flow speed control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("flow_speed_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 4096, - flow_speed_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) - { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) - { - if (thread_running) - printf("\tflow speed control app is running\n"); - else - printf("\tflow speed control app not started\n"); - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int -flow_speed_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - thread_running = true; - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd,"[fsc] started"); - - uint32_t counter = 0; - - /* structures */ - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct filtered_bottom_flow_s filtered_flow; - memset(&filtered_flow, 0, sizeof(filtered_flow)); - struct vehicle_bodyframe_speed_setpoint_s speed_sp; - memset(&speed_sp, 0, sizeof(speed_sp)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); - int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); - - orb_advert_t att_sp_pub; - bool attitude_setpoint_adverted = false; - - /* parameters init*/ - struct flow_speed_control_params params; - struct flow_speed_control_param_handles param_handles; - parameters_init(¶m_handles); - parameters_update(¶m_handles, ¶ms); - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); - - static bool sensors_ready = false; - static bool status_changed = false; - - while (!thread_should_exit) - { - /* wait for first attitude msg to be sure all data are available */ - if (sensors_ready) - { - /* polling */ - struct pollfd fds[2] = { - { .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller - { .fd = parameter_update_sub, .events = POLLIN } - }; - - /* wait for a position update, check for exit condition every 5000 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ -// printf("[flow speed control] no bodyframe speed setpoints updates\n"); - } - else - { - /* parameter update available? */ - if (fds[1].revents & POLLIN) - { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - - parameters_update(¶m_handles, ¶ms); - mavlink_log_info(mavlink_fd,"[fsp] parameters updated."); - } - - /* only run controller if position/speed changed */ - if (fds[0].revents & POLLIN) - { - perf_begin(mc_loop_perf); - - /* get a local copy of the armed topic */ - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - /* get a local copy of the control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* get a local copy of filtered bottom flow */ - orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); - /* get a local copy of bodyframe speed setpoint */ - orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); - /* get a local copy of control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - - if (control_mode.flag_control_velocity_enabled) - { - /* calc new roll/pitch */ - float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; - float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; - - if(status_changed == false) - mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged"); - - status_changed = true; - - /* limit roll and pitch corrections */ - if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) - { - att_sp.pitch_body = pitch_body; - } - else - { - if(pitch_body > params.limit_pitch) - att_sp.pitch_body = params.limit_pitch; - if(pitch_body < -params.limit_pitch) - att_sp.pitch_body = -params.limit_pitch; - } - - if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll)) - { - att_sp.roll_body = roll_body; - } - else - { - if(roll_body > params.limit_roll) - att_sp.roll_body = params.limit_roll; - if(roll_body < -params.limit_roll) - att_sp.roll_body = -params.limit_roll; - } - - /* set yaw setpoint forward*/ - att_sp.yaw_body = speed_sp.yaw_sp; - - /* add trim from parameters */ - att_sp.roll_body = att_sp.roll_body + params.trim_roll; - att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch; - - att_sp.thrust = speed_sp.thrust_sp; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust)) - { - if (attitude_setpoint_adverted) - { - 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); - attitude_setpoint_adverted = true; - } - } - else - { - warnx("NaN in flow speed controller!"); - } - } - else - { - if(status_changed == true) - mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged."); - - status_changed = false; - - /* reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.thrust = 0.0f; - att_sp.yaw_body = 0.0f; - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - perf_end(mc_loop_perf); - } - } - - counter++; - } - else - { - /* sensors not ready waiting for first attitude msg */ - - /* polling */ - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - /* wait for a flow msg, check for exit condition every 5 s */ - int ret = poll(fds, 1, 5000); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ - mavlink_log_info(mavlink_fd,"[fsc] no attitude received."); - } - else - { - if (fds[0].revents & POLLIN) - { - sensors_ready = true; - mavlink_log_info(mavlink_fd,"[fsp] initialized."); - } - } - } - } - - mavlink_log_info(mavlink_fd,"[fsc] ending now..."); - - thread_running = false; - - close(parameter_update_sub); - close(vehicle_attitude_sub); - close(vehicle_bodyframe_speed_setpoint_sub); - close(filtered_bottom_flow_sub); - close(armed_sub); - close(control_mode_sub); - close(att_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - return 0; -} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c deleted file mode 100644 index 8dfe54173..000000000 --- a/src/examples/flow_speed_control/flow_speed_control_params.c +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann <samuezih@ee.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 flow_speed_control_params.c - * - */ - -#include "flow_speed_control_params.h" - -/* controller parameters */ -PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f); -PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f); - -int parameters_init(struct flow_speed_control_param_handles *h) -{ - /* PID parameters */ - h->speed_p = param_find("FSC_S_P"); - h->limit_pitch = param_find("FSC_L_PITCH"); - h->limit_roll = param_find("FSC_L_ROLL"); - h->trim_roll = param_find("TRIM_ROLL"); - h->trim_pitch = param_find("TRIM_PITCH"); - - - return OK; -} - -int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p) -{ - param_get(h->speed_p, &(p->speed_p)); - param_get(h->limit_pitch, &(p->limit_pitch)); - param_get(h->limit_roll, &(p->limit_roll)); - param_get(h->trim_roll, &(p->trim_roll)); - param_get(h->trim_pitch, &(p->trim_pitch)); - - return OK; -} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.h b/src/examples/flow_speed_control/flow_speed_control_params.h deleted file mode 100644 index eec27a2bf..000000000 --- a/src/examples/flow_speed_control/flow_speed_control_params.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann <samuezih@ee.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 flow_speed_control_params.h - * - * Parameters for speed controller - */ - -#include <systemlib/param/param.h> - -struct flow_speed_control_params { - float speed_p; - float limit_pitch; - float limit_roll; - float trim_roll; - float trim_pitch; -}; - -struct flow_speed_control_param_handles { - param_t speed_p; - param_t limit_pitch; - param_t limit_roll; - param_t trim_roll; - param_t trim_pitch; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct flow_speed_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p); diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk deleted file mode 100644 index 5a4182146..000000000 --- a/src/examples/flow_speed_control/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# 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 -# 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. -# -############################################################################ - -# -# Build flow speed control -# - -MODULE_COMMAND = flow_speed_control - -SRCS = flow_speed_control_main.c \ - flow_speed_control_params.c diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 06337be32..d3b10f46e 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.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 @@ -34,7 +33,8 @@ /** * @file hwtest.c * - * Simple functional hardware test. + * Simple output test. + * @ref Documentation https://pixhawk.org/dev/examples/write_output * * @author Lorenz Meier <lm@inf.ethz.ch> */ @@ -45,30 +45,80 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> __EXPORT int ex_hwtest_main(int argc, char *argv[]); int ex_hwtest_main(int argc, char *argv[]) { - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); - - int i; - float rcvalue = -1.0f; - hrt_abstime stime; - - while (true) { - stime = hrt_absolute_time(); - while (hrt_absolute_time() - stime < 1000000) { - for (i=0; i<8; i++) - actuators.control[i] = rcvalue; - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); - } - warnx("servos set to %.1f", rcvalue); - rcvalue *= -1.0f; - } - - return OK; + warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); + warnx("(run <commander stop> to do so)"); + warnx("usage: http://px4.io/dev/examples/write_output"); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + + struct actuator_armed_s arm; + memset(&arm, 0 , sizeof(arm)); + + arm.timestamp = hrt_absolute_time(); + arm.ready_to_arm = true; + arm.armed = true; + orb_advert_t arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm); + orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm); + + /* read back values to validate */ + int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm); + + if (arm.ready_to_arm && arm.armed) { + warnx("Actuator armed"); + + } else { + errx(1, "Arming actuators failed"); + } + + hrt_abstime stime; + + int count = 0; + + while (count != 36) { + stime = hrt_absolute_time(); + + while (hrt_absolute_time() - stime < 1000000) { + for (int i = 0; i != 2; i++) { + if (count <= 5) { + actuators.control[i] = -1.0f; + + } else if (count <= 10) { + actuators.control[i] = -0.7f; + + } else if (count <= 15) { + actuators.control[i] = -0.5f; + + } else if (count <= 20) { + actuators.control[i] = -0.3f; + + } else if (count <= 25) { + actuators.control[i] = 0.0f; + + } else if (count <= 30) { + actuators.control[i] = 0.3f; + + } else { + actuators.control[i] = 0.5f; + } + } + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + usleep(10000); + } + + warnx("count %i", count); + count++; + } + + return OK; } diff --git a/src/examples/math_demo/math_demo.cpp b/src/examples/math_demo/math_demo.cpp deleted file mode 100644 index 36d3c2e84..000000000 --- a/src/examples/math_demo/math_demo.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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 math_demo.cpp - * @author James Goppert - * Demonstration of math library - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <systemlib/systemlib.h> -#include <mathlib/mathlib.h> - -/** - * Management function. - */ -extern "C" __EXPORT int math_demo_main(int argc, char *argv[]); - -/** - * Test function - */ -void test(); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: math_demo {test}\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_create(). - */ -int math_demo_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "test")) { - test(); - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -void test() -{ - printf("beginning math lib test\n"); - using namespace math; - vectorTest(); - matrixTest(); - vector3Test(); - eulerAnglesTest(); - quaternionTest(); - dcmTest(); -} diff --git a/src/examples/math_demo/module.mk b/src/examples/math_demo/module.mk deleted file mode 100644 index deba13fd0..000000000 --- a/src/examples/math_demo/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# 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 -# 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. -# -############################################################################ - -# -# Mathlib / operations demo application -# - -MODULE_COMMAND = math_demo -MODULE_STACKSIZE = 12000 - -SRCS = math_demo.cpp diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c9c8d16b5..6d6b3a36c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1668,7 +1668,7 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - mavlink_log_critical(mavlink_fd, "data link %i regained", i); + mavlink_log_info(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; @@ -1682,7 +1682,7 @@ int commander_thread_main(int argc, char *argv[]) telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "data link %i lost", i); + mavlink_log_info(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); + mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status.data_link_lost_counter++; status_changed = true; 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 57c1e72f3..6bb9bdee3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -752,8 +752,7 @@ FixedwingAttitudeControl::task_main() * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ - if (_vcontrol_mode.flag_control_velocity_enabled || - _vcontrol_mode.flag_control_position_enabled || + if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; @@ -770,6 +769,25 @@ FixedwingAttitudeControl::task_main() if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } + } else if (_vcontrol_mode.flag_control_velocity_enabled) { + /* + * Velocity should be controlled and manual is enabled. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } } else { /* * Scale down roll and pitch as the setpoints are radians 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 f8399d10b..4c969a5ca 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 @@ -165,6 +165,9 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + float _hold_alt; /**< hold altitude for velocity mode */ + hrt_abstime _control_position_last_called; /**<last call of control_position */ + /* land states */ bool land_noreturn_horizontal; bool land_noreturn_vertical; @@ -200,6 +203,8 @@ private: TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; + bool _was_velocity_control_mode; + bool _was_alt_control_mode; struct { float l1_period; @@ -319,6 +324,11 @@ private: void vehicle_status_poll(); /** + * Check for manual setpoint updates. + */ + bool vehicle_manual_control_setpoint_poll(); + + /** * Check for airspeed updates. */ bool vehicle_airspeed_poll(); @@ -441,6 +451,9 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + _hold_alt(0.0f), + _control_position_last_called(0), + land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -694,6 +707,22 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::vehicle_manual_control_setpoint_poll() +{ + bool manual_updated; + + /* Check if manual setpoint has changed */ + orb_check(_manual_control_sub, &manual_updated); + + if (manual_updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + } + + return manual_updated; +} + + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -854,6 +883,12 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { + float dt = FLT_MIN; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { + dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; + } + _control_position_last_called = hrt_absolute_time(); + bool setpoint = true; math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; @@ -890,6 +925,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _was_pos_control_mode = true; + _was_velocity_control_mode = false; + + /* reset hold altitude */ + _hold_alt = _global_pos.alt; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1211,12 +1250,59 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } + } else if (_control_mode.flag_control_velocity_enabled) { + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + if (!_was_velocity_control_mode) { + _hold_alt = _global_pos.alt; + _was_alt_control_mode = false; + } + _was_velocity_control_mode = true; + _was_pos_control_mode = false; + // Get demanded airspeed + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + // Get demanded vertical velocity from pitch control + float pitch = 0.0f; + if (_manual.x > deadBand) { + pitch = (_manual.x - deadBand) / factor; + } else if (_manual.x < - deadBand) { + pitch = (_manual.x + deadBand) / factor; + } + if (pitch > 0.0f) { + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + _was_alt_control_mode = false; + } else if (pitch < 0.0f) { + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + _was_alt_control_mode = false; + } else if (!_was_alt_control_mode) { + _hold_alt = _global_pos.alt; + _was_alt_control_mode = true; + } + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); } else { - + _was_velocity_control_mode = false; _was_pos_control_mode = false; /** MANUAL FLIGHT **/ + /* reset hold altitude */ + _hold_alt = _global_pos.alt; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -1352,6 +1438,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a8f956ad0..5908279d5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -810,9 +810,6 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; - MavlinkOrbSubscription *_sensor_combined_sub; - uint64_t _sensor_combined_time; - /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -828,9 +825,7 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0), - _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), - _sensor_combined_time(0) + _airspeed_time(0) {} void send(const hrt_abstime t) @@ -840,14 +835,12 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; - struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); - updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -856,7 +849,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = sensor_combined.baro_alt_meter; + msg.alt = pos.alt; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); @@ -2169,7 +2162,7 @@ protected: msg.id = 0; msg.orientation = 0; msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.minimum_distance * 100; + msg.max_distance = range.maximum_distance * 100; msg.current_distance = range.distance * 100; msg.covariance = 20; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 70071130d..d747b5f43 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -61,7 +61,7 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude in meters (AMSL) */ float x; /**< X coordinate in meters */ float y; /**< Y coordinate in meters */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index e4b72f87c..22a8f3ecb 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -83,7 +83,7 @@ struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ - float altitude; /**< altitude in meters */ + float altitude; /**< altitude in meters (AMSL) */ float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index f264accbb..6b4cb483b 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -65,7 +65,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ diff --git a/uavcan b/uavcan -Subproject 4de0338824972de4d3a8e29697ea1a2d95a926c +Subproject 1efd24427539fa332a15151143466ec760fa5ff |