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 40b9ed8df5..1e3dc6c5f6 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -6,5 +6,30 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set BAT_N_CELLS 3 + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 14 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.1 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.2 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.1 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.3 +fi + set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index d46147ede5..9f45c636b0 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -7,4 +7,12 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set NAV_LOITER_RAD 150 + param set FW_AIRSPD_MAX 30 + param set FW_AIRSPD_MIN 13 + param set FW_AIRSPD_TRIM 15 +fi + set MIXER FX79 diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9e1c1c170a..1fd96d6d3d 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -2,33 +2,39 @@ # # TBS Caipirinha # -# Thomas Gubler +# Lorenz Meier # sh /etc/init.d/rc.fw_defaults if [ $AUTOCNF == yes ] then - - # TODO: these are the X5 default parameters, update them to the caipi - - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 40 + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 16 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.01 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.01 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + param set FW_RR_P 0.04 + param set SYS_COMPANION 157600 + param set PWM_MAIN_REV0 1 + param set PWM_MAIN_REV1 1 fi -set MIXER Q +set MIXER caipi +# Provide ESC a constant 1000 us pulse +set PWM_OUT 4 +set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 6bfbfea396..5d374745a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,7 +11,7 @@ then then fi else - if sdlog2 start -r 100 -a -b 22 -t + if sdlog2 start -r 100 -a -b 12 -t then fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e40121eec6..8a93737ef8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -485,6 +485,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 fi + if param compare SYS_COMPANION 157600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 + fi fi # UAVCAN diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix index b8879af9ee..3223eff133 100755 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix @@ -27,13 +27,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 9500 9500 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 -9500 -9500 0 -10000 10000 Output 2 -------- @@ -51,19 +51,3 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - -Inputs to the mixer come from channel group 2 (payload), channels 0 -(bay servo 1), 1 (bay servo 2) and 3 (drop release). ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 0 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix new file mode 100644 index 0000000000..af053deab4 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/caipi.main.mix @@ -0,0 +1,51 @@ +Delta-wing mixer +=========================== + +Designed for TBS Caipirinha + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 3000 -10000 10000 +S: 0 0 8000 8000 0 -10000 10000 +S: 0 1 9000 9000 0 -10000 10000 + +M: 2 +O: 10000 10000 -3000 -10000 10000 +S: 0 0 8000 8000 0 -10000 10000 +S: 0 1 -9000 -9000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +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/makefiles/nuttx/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 3c2898cb0c..27be31da8e 100644 --- a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -132,6 +132,12 @@ LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a) ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ +# +# Provide defaults, but allow for module override +WFRAME_LARGER_THAN ?= 1024 + + + # Generic warnings # ARCHWARNINGS = -Wall \ diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg new file mode 100644 index 0000000000..235b5df03b --- /dev/null +++ b/msg/navigation_capabilities.msg @@ -0,0 +1,5 @@ +uint64 timestamp # timestamp this topic was emitted +float32 turn_distance # the optimal distance to a waypoint to switch to the next +float32 landing_horizontal_slope_displacement +float32 landing_slope_angle_rad +float32 landing_flare_length diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 07484425b3..68e4aa9dc6 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -7,7 +7,8 @@ uint8 MAIN_STATE_AUTO_LOITER = 4 uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 -uint8 MAIN_STATE_MAX = 8 +uint8 MAIN_STATE_STAB = 8 +uint8 MAIN_STATE_MAX = 9 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -39,7 +40,8 @@ uint8 NAVIGATION_STATE_LAND = 11 # Land mode uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 -uint8 NAVIGATION_STATE_MAX = 15 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_MAX = 16 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 59e1824dc8..a247c2596e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1114,6 +1114,27 @@ PX4IO::task_main() } (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); + + float trim_val; + param_t trim_parm; + + trim_parm = param_find("TRIM_ROLL"); + if (trim_parm != PARAM_INVALID) { + param_get(trim_parm, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val)); + } + + trim_parm = param_find("TRIM_PITCH"); + if (trim_parm != PARAM_INVALID) { + param_get(trim_parm, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val)); + } + + trim_parm = param_find("TRIM_YAW"); + if (trim_parm != PARAM_INVALID) { + param_get(trim_parm, &trim_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val)); + } } } @@ -1240,7 +1261,7 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (have_armed == OK) { + if (have_armed == OK) { _in_esc_calibration_mode = armed.in_esc_calibration_mode; if (armed.armed || _in_esc_calibration_mode) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; @@ -2127,7 +2148,14 @@ PX4IO::print_status(bool extended_status) for (unsigned i = 0; i < _max_actuators; i++) { printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); } - printf("]\n"); + printf("]"); + + float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL)); + float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH)); + float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW)); + + printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n", + (double)trim_roll, (double)trim_pitch, (double)trim_yaw); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd33eca354..d47b45d89f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -37,9 +37,9 @@ * * @author Petri Tanskanen * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -93,7 +93,7 @@ #include #include #include - #include +#include #include #include @@ -148,7 +148,12 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 -#define PRINT_MODE_REJECT_INTERVAL 2000000 +#define PRINT_MODE_REJECT_INTERVAL 10000000 + +#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 + +#define HIL_ID_MIN 1000 +#define HIL_ID_MAX 1999 enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -174,6 +179,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ @@ -189,6 +195,7 @@ static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; +static struct home_position_s _home; /** * The daemon app only briefly exists to start @@ -212,7 +219,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -254,6 +261,15 @@ void *commander_low_prio_loop(void *arg); void answer_command(struct vehicle_command_s &cmd, unsigned result); +/** + * check whether autostart ID is in the reserved range for HIL setups + */ +bool is_hil_setup(int id); + +bool is_hil_setup(int id) { + return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); +} + int commander_main(int argc, char *argv[]) { @@ -354,6 +370,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); + warnx("note: not updating home position on commandline arming!"); close(mavlink_fd_local); return 0; } @@ -383,6 +400,8 @@ void print_status() warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt); + warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -437,6 +456,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char { transition_result_t arming_res = TRANSITION_NOT_CHANGED; + // For HIL platforms, require that simulated sensors are connected + if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); + return TRANSITION_DENIED; + } + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, @@ -454,7 +479,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct home_position_s *home, struct vehicle_global_position_s *global_pos, + struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -480,7 +506,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state - arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; + + arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { + + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); + } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ @@ -504,6 +539,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* ACRO */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); @@ -521,6 +560,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* STABILIZED */ + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB); + } else { /* MANUAL */ main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } @@ -845,6 +887,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB"; main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; @@ -858,6 +901,7 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -955,8 +999,7 @@ int commander_thread_main(int argc, char *argv[]) /* home position */ orb_advert_t home_pub = nullptr; - struct home_position_s home; - memset(&home, 0, sizeof(home)); + memset(&_home, 0, sizeof(_home)); /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = nullptr; @@ -1141,15 +1184,22 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + param_get(_param_autostart_id, &autostart_id); + if (is_hil_setup(autostart_id)) { + // HIL configuration selected: real sensors will be disabled + status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } else { + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); + if (!status.condition_system_sensors_initialized) { + set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune + } + else { + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } } - const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); + commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1317,8 +1367,15 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + if (is_hil_setup(autostart_id)) { + /* HIL configuration: check only RC input */ + (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + } else { + /* check sensors also */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + } } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1589,6 +1646,8 @@ int commander_thread_main(int argc, char *argv[]) low_battery_voltage_actions_done = true; if (armed.armed) { mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + } else { + mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED"); } status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; @@ -1676,7 +1735,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.gps_failure && !gpsIsNoisy) { status.gps_failure = false; status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); + mavlink_log_critical(mavlink_fd, "gps fix regained"); } } else if (!status.gps_failure) { @@ -1710,12 +1769,12 @@ int commander_thread_main(int argc, char *argv[]) if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + mavlink_log_critical(mavlink_fd, "Flight termination active"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1725,7 +1784,7 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_info(mavlink_fd, "Detected RC signal first time"); + mavlink_log_info(mavlink_fd, "Detected radio control"); status_changed = true; } else { @@ -1742,7 +1801,10 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state == vehicle_status_s::MAIN_STATE_ACRO || + status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1775,7 +1837,8 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { + if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) && + (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { @@ -1932,7 +1995,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { status_changed = true; } } @@ -1944,6 +2007,7 @@ int commander_thread_main(int argc, char *argv[]) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || @@ -1968,6 +2032,7 @@ int commander_thread_main(int argc, char *argv[]) * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || @@ -1987,17 +2052,17 @@ int commander_thread_main(int argc, char *argv[]) } } - //Get current timestamp + /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); - //First time home position update - if (!status.condition_home_position_valid) { - commander_set_home_position(home_pub, home, local_position, global_position); + /* First time home position update - but only if disarmed */ + if (!status.condition_home_position_valid && !armed.armed) { + commander_set_home_position(home_pub, _home, local_position, global_position); } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { - commander_set_home_position(home_pub, home, local_position, global_position); + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + commander_set_home_position(home_pub, _home, local_position, global_position); } /* print new state */ @@ -2014,14 +2079,6 @@ int commander_thread_main(int argc, char *argv[]) mission_result.finished, mission_result.stay_in_failsafe); - // TODO handle mode changes by commands - if (main_state_changed) { - status_changed = true; - warnx("main state: %s", main_states_str[status.main_state]); - mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); - main_state_changed = false; - } - if (status.failsafe != failsafe_old) { status_changed = true; @@ -2035,10 +2092,12 @@ int commander_thread_main(int argc, char *argv[]) failsafe_old = status.failsafe; } - if (nav_state_changed) { + // TODO handle mode changes by commands + if (main_state_changed || nav_state_changed) { status_changed = true; - warnx("nav state: %s", nav_states_str[status.nav_state]); - mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); + warnx("main state: %s nav state: %s", main_states_str[status.main_state], nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]); + main_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -2061,11 +2120,13 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && + status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && + (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -2292,7 +2353,15 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); + + /* manual mode is stabilized already for multirotors, so switch to acro + * for any non-manual mode + */ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } } else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); @@ -2403,6 +2472,20 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + 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; + control_mode.flag_control_termination_enabled = false; + /* override is not ok in stabilized mode */ + control_mode.flag_external_manual_override_ok = false; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index eaf309288f..baf651f08c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -1,8 +1,41 @@ -/* - * px4_custom_mode.h +/**************************************************************************** * - * Created on: 09.08.2013 - * Author: ton + * Copyright (c) 2013-2015 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 px4_custom_mode.h + * PX4 custom flight modes + * + * @author Anton Babushkin */ #ifndef PX4_CUSTOM_MODE_H_ @@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, + PX4_CUSTOM_MAIN_MODE_STABILIZED }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 8c51e95b9d..484087a234 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -59,7 +59,7 @@ static const int ERROR = -1; int do_trim_calibration(int mavlink_fd) { int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - usleep(200000); + usleep(400000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); @@ -71,20 +71,34 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - /* set parameters */ - float p = sp.y; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.x; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.r; - param_set(param_find("TRIM_YAW"), &p); + /* load trim values which are active */ + float roll_trim_active; + param_get(param_find("TRIM_ROLL"), &roll_trim_active); + float pitch_trim_active; + param_get(param_find("TRIM_PITCH"), &pitch_trim_active); + float yaw_trim_active; + param_get(param_find("TRIM_YAW"), &yaw_trim_active); + + /* set parameters: the new trim values are the combination of active trim values + and the values coming from the remote control of the user + */ + float p = sp.y + roll_trim_active; + int p1r = param_set(param_find("TRIM_ROLL"), &p); + /* + we explicitly swap sign here because the trim is added to the actuator controls + which are moving in an inverse sense to manual pitch inputs + */ + p = -sp.x + pitch_trim_active; + int p2r = param_set(param_find("TRIM_PITCH"), &p); + p = sp.r + yaw_trim_active; + int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ /* auto-save */ int save_ret = param_save_default(); - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); + if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) { + mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL"); px4_close(sub_man); return ERROR; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fdbcaa3256..8d49c305e1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -126,7 +126,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -304,6 +305,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -538,10 +540,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -564,6 +567,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_STAB: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; + break; + case vehicle_status_s::MAIN_STATE_ALTCTL: status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ee6fcb3ac5..7084f716c0 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,7 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -193,7 +193,6 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; - float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; @@ -333,6 +332,12 @@ private: **/ void initializeGPS(); + /** + * Initialize the reference position for the local coordinate frame + */ + void initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt); + /** * @brief * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3eea623776..1c8188110f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -75,6 +75,9 @@ static uint64_t IMUusec = 0; static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure +static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) +static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) +static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) /** * estimator app start / stop handling function @@ -157,7 +160,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined {}, _pos_ref{}, - _baro_ref_offset(0.0f), + _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), /* performance counters */ @@ -177,7 +180,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _gpsIsGood(false), _previousGPSTimestamp(0), _baro_init(false), - _baroAltRef(0.0f), _gps_initialized(false), _filter_start_time(0), _last_sensor_timestamp(0), @@ -408,6 +410,15 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Count the reset condition perf_count(_perf_reset); + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -590,6 +601,7 @@ void AttitudePositionEstimatorEKF::task_main() _baro_init = false; _gps_initialized = false; + _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; @@ -626,7 +638,10 @@ void AttitudePositionEstimatorEKF::task_main() // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors - _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + + if (_gpsIsGood) { + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + } // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); @@ -648,12 +663,15 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = 0.0f; _local_pos.ref_alt = 0.0f; - _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _filter_ref_offset = -_baro.altitude; + + warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); + } else { if (!_gps_initialized && _gpsIsGood) { @@ -705,6 +723,32 @@ void AttitudePositionEstimatorEKF::task_main() return; } +void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, + double lat, double lon, float gps_alt, float baro_alt) +{ + // Reference altitude + if (isfinite(_ekf->states[9])) { + _filter_ref_offset = _ekf->states[9]; + } else if (isfinite(-_ekf->hgtMea)) { + _filter_ref_offset = -_ekf->hgtMea; + } else { + _filter_ref_offset = -_baro.altitude; + } + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = baro_alt; + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = timestamp; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); +} + void AttitudePositionEstimatorEKF::initializeGPS() { // GPS is in scaled integers, convert @@ -714,11 +758,6 @@ void AttitudePositionEstimatorEKF::initializeGPS() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - - // init filtered gps and baro altitudes - _gps_alt_filt = gps_alt; - _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = _ekf->baroHgt; @@ -740,20 +779,13 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - // Initialize projection - _local_pos.ref_lat = lat; - _local_pos.ref_lon = lon; - _local_pos.ref_alt = gps_alt; - _local_pos.ref_timestamp = _gps.timestamp_position; - - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_baro_ref_offset); + (double)_filter_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -814,7 +846,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; + _local_pos.z = _ekf->states[9] - _filter_ref_offset; + //_local_pos.z_stable = _ekf->states[9]; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -829,6 +862,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.z_global = false; _local_pos.yaw = _att.yaw; + if (!isfinite(_local_pos.x) || + !isfinite(_local_pos.y) || + !isfinite(_local_pos.z) || + !isfinite(_local_pos.vx) || + !isfinite(_local_pos.vy) || + !isfinite(_local_pos.vz)) + { + // bad data, abort publication + return; + } + /* lazily publish the local position only once available */ if (_local_pos_pub != nullptr) { /* publish the attitude setpoint */ @@ -862,7 +906,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -877,6 +921,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; + if (!isfinite(_global_pos.lat) || + !isfinite(_global_pos.lon) || + !isfinite(_global_pos.alt) || + !isfinite(_global_pos.vel_n) || + !isfinite(_global_pos.vel_e) || + !isfinite(_global_pos.vel_d)) + { + // bad data, abort publication + return; + } + /* lazily publish the global position only once available */ if (_global_pos_pub != nullptr) { /* publish the global position */ @@ -1073,8 +1128,9 @@ void AttitudePositionEstimatorEKF::print_status() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, + printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt); + printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt); + printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1173,7 +1229,7 @@ void AttitudePositionEstimatorEKF::pollData() if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) && PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) && PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; @@ -1212,7 +1268,7 @@ void AttitudePositionEstimatorEKF::pollData() int last_accel_main = _accel_main; /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { + if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; @@ -1320,7 +1376,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); // update LPF - _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + + if (isfinite(filter_step)) { + _gps_alt_filt += filter_step; + } } //check if we had a GPS outage for a long time @@ -1403,15 +1463,19 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; - if(!_baro_init) { + if (!_baro_init) { _baro_init = true; - _baroAltRef = _baro.altitude; + _baro_alt_filt = _baro.altitude; } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); + + if (isfinite(filter_step)) { + _baro_alt_filt += filter_step; + } perf_count(_perf_baro); } @@ -1435,7 +1499,7 @@ void AttitudePositionEstimatorEKF::pollData() /* fail over to the 2nd mag if we know the first is down */ if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && - _sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount && + _sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) && mag0.length() > 0.1f) { _ekf->magData.x = mag0.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset @@ -1463,7 +1527,7 @@ void AttitudePositionEstimatorEKF::pollData() } if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main); + mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main); } } @@ -1497,30 +1561,34 @@ int AttitudePositionEstimatorEKF::trip_nan() float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN values"); + warnx("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN values"); + warnx("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN values"); + warnx("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN values"); + warnx("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN values"); + warnx("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN values"); + warnx("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); + warnx("tripping states[9] with NaN"); + _ekf->states[9] = nan_val; + usleep(100000); + warnx("\nDONE - FILTER STATE:"); print_status(); } 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 7ec5148ccc..fe0d057222 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -859,11 +859,36 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { + + /* the pilot does not want to change direction, + * take straight attitude setpoint from position controller + */ + if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { + roll_sp = (_manual.y * _parameters.man_roll_max) + + _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 if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; @@ -890,10 +915,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) - + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) - + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; @@ -1060,9 +1083,9 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.y; - _actuators.control[1] = -_manual.x; - _actuators.control[2] = _manual.r; + _actuators.control[0] = _manual.y + _parameters.trim_roll; + _actuators.control[1] = -_manual.x + _parameters.trim_pitch; + _actuators.control[2] = _manual.r + _parameters.trim_yaw; _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } @@ -1079,8 +1102,8 @@ FixedwingAttitudeControl::task_main() /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { 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 9453b51aed..48e78adaf0 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 @@ -94,6 +94,13 @@ #include static int _control_task = -1; /**< task handle for sensor task */ +#define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode +#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane +#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode +#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading + +#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes /** @@ -165,7 +172,13 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - float _hold_alt; /**< hold altitude for velocity mode */ + float _hold_alt; /**< hold altitude for altitude mode */ + float _ground_alt; /**< ground altitude at which plane was launched */ + float _hdg_hold_yaw; /**< hold heading for velocity mode */ + bool _hdg_hold_enabled; /**< heading hold enabled */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ + struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); @@ -454,6 +493,12 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), + _ground_alt(0.0f), + _hdg_hold_yaw(0.0f), + _hdg_hold_enabled(false), + _yaw_lock_engaged(false), + _hdg_hold_prev_wp{}, + _hdg_hold_curr_wp{}, _control_position_last_called(0), land_noreturn_horizontal(false), @@ -462,6 +507,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _was_in_air(false), + _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -855,6 +902,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c void FixedwingPositionControl::navigation_capabilities_publish() { + _nav_capabilities.timestamp = hrt_absolute_time(); + if (_nav_capabilities_pub != nullptr) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); } else { @@ -862,6 +911,31 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance, + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) +{ + waypoint_prev.valid = true; + waypoint_prev.alt = _hold_alt; + + if (flag_init) { + // on init set first waypoint to momentary position + waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + } else { + /* + for previous waypoint use the one still in front of us but shift it such that it is + located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us + */ + waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; + } + + waypoint_next.valid = true; + waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; + waypoint_next.alt = _hold_alt; +} + float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { if (!isfinite(global_pos.terrain_alt)) { @@ -881,6 +955,41 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +void FixedwingPositionControl::update_desired_altitude(float dt) +{ + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + static bool was_in_deadband = false; + + if (_manual.x > deadBand) { + float pitch = (_manual.x - deadBand) / factor; + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ + _hold_alt = _global_pos.alt; + was_in_deadband = true; + } +} + +void FixedwingPositionControl::do_takeoff_help() +{ + const hrt_abstime delta_takeoff = 10000000; + const float throttle_threshold = 0.3f; + const float delta_alt_takeoff = 30.0f; + + /* demand 30 m above ground if user switched into this mode during takeoff */ + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { + _hold_alt = _ground_alt + delta_alt_takeoff; + } +} + 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) @@ -912,6 +1021,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no throttle limit as default */ float throttle_max = 1.0f; + /* save time when airplane is in air */ + if (!_was_in_air && !_vehicle_status.condition_landed) { + _was_in_air = true; + _time_went_in_air = hrt_absolute_time(); + _ground_alt = _global_pos.alt; + } + /* reset flag when airplane landed */ + if (_vehicle_status.condition_landed) { + _was_in_air = false; + } + if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ @@ -928,6 +1048,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; + /* reset hold yaw */ + _hdg_hold_yaw = _att.yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1251,13 +1373,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { - /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, + heading is set to a distant waypoint */ - const float deadBand = (60.0f/1000.0f); - const float factor = 1.0f - deadBand; if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; + _hdg_hold_yaw = _att.yaw; + _hdg_hold_enabled = false; // this makes sure the waypoints are reset below + _yaw_lock_engaged = false; } /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { @@ -1274,30 +1398,118 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - /* Get demanded vertical velocity from pitch control */ - static bool was_in_deadband = false; - if (_manual.x > deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - was_in_deadband = false; - } else if (!was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ - _hold_alt = _global_pos.alt; - was_in_deadband = true; + /* update desired altitude based on user pitch stick input */ + update_desired_altitude(dt); + + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ + do_takeoff_help(); + + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { + throttle_max = 0.0f; } + 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, + throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); + + /* heading control */ + + if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { + /* heading / roll is zero, lock onto current heading */ + if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + // little yaw movement, lock to current heading + _yaw_lock_engaged = true; + + } + + if (_yaw_lock_engaged) { + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _att.yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } + } else { + _hdg_hold_enabled = false; + _yaw_lock_engaged = false; + } + + } else if (_control_mode.flag_control_altitude_enabled) { + /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + + if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _global_pos.alt; + } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; + + /* Get demanded airspeed */ + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + /* update desired altitude based on user pitch stick input */ + update_desired_altitude(dt); + + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ + do_takeoff_help(); + + /* throttle limiting */ + throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { + throttle_max = 0.0f; + } + + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), @@ -1476,7 +1688,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + && turn_distance > 0)) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index e26954d1a6..5f7ded9cb2 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -84,11 +84,22 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - // TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { + float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + if (isfinite(val)) { + _velocity_xy_filtered = val; + } + val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + + if (isfinite(val)) { + _velocity_z_filtered = val; + } + } + + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 4e1f30ca49..f5ca4f3265 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -87,7 +87,7 @@ protected: virtual void initialize() = 0; /** - * @brief Convinience function for polling uORB subscriptions + * @brief Convenience function for polling uORB subscriptions * @return true if there was new data and it was successfully copied **/ bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 9cf57b5fc9..b670dcc035 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -45,6 +45,8 @@ * * Maximum vertical velocity allowed to trigger a land (m/s up and down) * + * @unit m/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); @@ -54,6 +56,8 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); * * Maximum horizontal velocity allowed to trigger a land (m/s) * + * @unit m/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); @@ -63,6 +67,8 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); * * Maximum allowed around each axis to trigger a land (degrees per second) * + * @unit deg/s + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); @@ -72,6 +78,9 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); * * Maximum actuator output on throttle before triggering a land * + * @min 0.1 + * @max 0.5 + * * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); @@ -81,24 +90,36 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * Maximum horizontal velocity allowed to trigger a land (m/s) * + * @min 0.5 + * @max 10 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f); /** * Fixedwing max climb rate * * Maximum vertical velocity allowed to trigger a land (m/s up and down) * + * @min 5 + * @max 20 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); /** * Airspeed max * * Maximum airspeed allowed to trigger a land (m/s) * + * @min 4 + * @max 20 + * @unit m/s + * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 44127f586e..6756b9dcbc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -299,7 +299,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) /// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(PayloadHeader* payload) +MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden) { char dirPath[kMaxDataLength]; strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); @@ -383,7 +383,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) #else case DT_DIR: #endif - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) || + strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 33b8996f71..998c6b3cc6 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -130,7 +130,7 @@ private: void _reply(mavlink_file_transfer_protocol_t* ftp_req); int _copy_file(const char *src_path, const char *dst_path, size_t length); - ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workBurst(PayloadHeader* payload, uint8_t target_system_id); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1a117fb8a7..5b43bc02e0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1421,6 +1421,8 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "osd") == 0) { + _mode = MAVLINK_MODE_OSD; } break; @@ -1593,7 +1595,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); configure_stream("HIGHRES_IMU", 50.0f); - configure_stream("VFR_HUD", 5.0f); configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); @@ -1610,6 +1611,18 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); break; + case MAVLINK_MODE_OSD: + configure_stream("SYS_STATUS", 5.0f); + configure_stream("ATTITUDE", 25.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 10.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("BATTERY_STATUS", 1.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("RC_CHANNELS_RAW", 5.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 326182bbef..a5468a86cc 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -148,7 +148,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD + MAVLINK_MODE_ONBOARD, + MAVLINK_MODE_OSD }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ffdc0e0730..9d16363e66 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -131,6 +131,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; + case vehicle_status_s::NAVIGATION_STATE_STAB: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e76f907176..90decb9e47 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1007,7 +1007,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) * which makes the corner positions unreachable. * scale yaw up and clip it to overcome this. */ - rc.values[2] = man.r / 1.2f + 1500; + rc.values[2] = man.r / 1.1f + 1500; if (rc.values[2] > 2000) { rc.values[2] = 2000; } else if (rc.values[2] < 1000) { @@ -1015,7 +1015,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } /* throttle */ - rc.values[3] = man.z + 1000; + rc.values[3] = man.z / 0.9f + 1000; + if (rc.values[3] > 2000) { + rc.values[3] = 2000; + } else if (rc.values[3] < 1000) { + rc.values[3] = 1000; + } rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000; @@ -1346,6 +1351,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa; hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 5b3a4a3503..94bb5f24c8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -100,6 +100,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f class MulticopterAttitudeControl { @@ -834,7 +835,7 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp.z; + _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 766ede403c..d92ee38414 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -86,6 +86,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f +#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f /** * Multicopter position control app start / stop handling function @@ -1390,7 +1391,7 @@ MulticopterPositionControl::task_main() //Control climb rate directly if no aiding altitude controller is active if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = _manual.z; + _att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER); } //Construct attitude setpoint rotation matrix diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index fc73b31f59..ade43ffb91 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -55,13 +55,16 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); /** * Maximum thrust * - * Limit max allowed thrust. + * Limit max allowed thrust. Setting a value of one can put + * the system into actuator saturation as no spread between + * the motors is possible any more. A value of 0.8 - 0.9 + * is recommended. * * @min 0.0 * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); /** * Proportional gain for vertical position error diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8b4e889abf..9362d7af4d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -230,9 +230,9 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt); + _navigator->get_home_position()->alt, _navigator->home_position_valid()); } else { warnx("offboard mission update failed"); @@ -271,6 +271,16 @@ Mission::advance_mission() } } +int +Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) +{ + if (_mission_item.altitude_is_relative) { + return _mission_item.altitude + _navigator->get_home_position()->alt; + } else { + return _mission_item.altitude; + } +} + bool Mission::check_dist_1wp() { @@ -297,10 +307,10 @@ Mission::check_dist_1wp() mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - /* check distance from home to item */ + /* check distance from current position to item */ float dist_to_1wp = get_distance_to_next_waypoint( mission_item.lat, mission_item.lon, - _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; @@ -354,7 +364,7 @@ Mission::set_mission_items() /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ if (pos_sp_triplet->previous.valid) { - _mission_item_previous_alt = _mission_item.altitude; + _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } /* get home distance state */ @@ -374,13 +384,14 @@ Mission::set_mission_items() } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -388,8 +399,9 @@ Mission::set_mission_items() } else if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet + * https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); } _mission_type = MISSION_TYPE_NONE; @@ -440,10 +452,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); /* calculate takeoff altitude */ - float takeoff_alt = _mission_item.altitude; - if (_mission_item.altitude_is_relative) { - takeoff_alt += _navigator->get_home_position()->alt; - } + float takeoff_alt = get_absolute_altitude_for_item(_mission_item); /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { @@ -586,7 +595,7 @@ Mission::altitude_sp_foh_update() } /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ - if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) { return; } @@ -608,16 +617,16 @@ Mission::altitude_sp_foh_update() /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ - if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { - pos_sp_triplet->current.alt = _mission_item.altitude; + if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { + pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item); } else { /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp * of the mission item as it is used to check if the mission item is reached * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance * radius around the current waypoint **/ - float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); - float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt); + float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius)); float a = _mission_item_previous_alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index e9f78e8fdc..bc9a2c6c82 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -132,6 +132,8 @@ private: */ void altitude_sp_foh_reset(); + int get_absolute_altitude_for_item(struct mission_item_s &mission_item); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 077404b574..af85799c62 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ - if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ - if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) { _waypoint_position_reached = true; } } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d9297f25bf..9d1dc7c7e6 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -63,42 +63,43 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { bool failed = false; /* Init if not done yet */ init(); - /* Open mavlink fd */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } + _mavlink_fd = mavlink_fd; // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - if (isRotarywing) - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); - else - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + if (isRotarywing) { + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + } else { + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + } + + if (!failed) { + mavlink_log_info(_mavlink_fd, "Mission checked and ready."); + } return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Perform checks and issue feedback to the user for all checks */ bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resGeofence && resHomeAltitude); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); @@ -107,7 +108,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resLanding && resGeofence && resHomeAltitude); @@ -136,7 +137,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -152,6 +153,12 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } + /* always reject relative alt without home set */ + if (missionitem.altitude_is_relative && !home_valid) { + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + return false; + } + /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; @@ -197,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } } - mavlink_log_info(_mavlink_fd, "Mission ready."); return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 94b6b29226..9c9511be3d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -61,16 +61,16 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); - bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); public: MissionFeasibilityChecker(); @@ -79,8 +79,8 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt); + bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt, bool home_valid); }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 8e501fd8ef..b2094f62e3 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); /** * Altitude setpoint mode @@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @max 1 * @group Mission */ -PARAM_DEFINE_INT32(MIS_ALTMODE, 0); +PARAM_DEFINE_INT32(MIS_ALTMODE, 1); /** * Multirotor only. Yaw setpoint mode. diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d2acce7899..782a297fbb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -134,6 +134,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } + bool home_position_valid() { return _home_position_set; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } @@ -144,7 +145,22 @@ public: Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } - float get_acceptance_radius() { return _param_acceptance_radius.get(); } + + /** + * Get the acceptance radius + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(); + + /** + * Get the acceptance radius given the mission item preset radius + * + * @param mission_item_radius the radius to use in case the controller-derived radius is smaller + * + * @return the distance at which the next waypoint should be used + */ + float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 28bd482cc2..d7f971f067 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -212,7 +212,10 @@ Navigator::home_position_update() if (updated) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - _home_position_set = true; + + if (_home_pos.timestamp > 0) { + _home_position_set = true; + } } } @@ -574,6 +577,26 @@ Navigator::publish_position_setpoint_triplet() } } +float +Navigator::get_acceptance_radius() +{ + return get_acceptance_radius(_param_acceptance_radius.get()); +} + +float +Navigator::get_acceptance_radius(float mission_item_radius) +{ + float radius = mission_item_radius; + + if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { + if (_nav_caps.turn_distance > radius) { + radius = _nav_caps.turn_distance; + } + } + + return radius; +} + void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index a0722ac4d6..65d49a388a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -64,6 +64,10 @@ static int _dsm_fd; static uint16_t rc_value_override = 0; +#ifdef ADC_RSSI +static unsigned _rssi_adc_counts = 0; +#endif + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { perf_begin(c_gather_dsm); @@ -71,17 +75,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint8_t n_bytes = 0; uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; - if (temp_count & 0x8000) + + if (temp_count & 0x8000) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - else + + } else { r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + } r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + perf_end(c_gather_dsm); /* get data from FD and attempt to parse with DSM and ST24 libs */ @@ -94,7 +103,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* set updated flag if one complete packet was parsed */ st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, - &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*st24_updated) { @@ -121,7 +130,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool /* set updated flag if one complete packet was parsed */ sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, - &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { @@ -170,7 +179,8 @@ controls_init(void) } void -controls_tick() { +controls_tick() +{ /* * Gather R/C control inputs from supported sources. @@ -184,19 +194,24 @@ controls_tick() { uint16_t rssi = 0; #ifdef ADC_RSSI + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { unsigned counts = adc_measure(ADC_RSSI); - if (counts != 0xffff) { - /* use 1:1 scaling on 3.3V ADC input */ - unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 and lowpass */ - rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (counts != 0xffff) { + /* low pass*/ + _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); + /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ + unsigned mV = _rssi_adc_counts * 3300 / 4095; + /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ + rssi = (mV * RC_INPUT_RSSI_MAX / 3300); + if (rssi > RC_INPUT_RSSI_MAX) { rssi = RC_INPUT_RSSI_MAX; } } } + #endif /* zero RSSI if signal is lost */ @@ -207,21 +222,26 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; } + if (st24_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; } + if (sumd_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; } + perf_end(c_gather_dsm); perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; @@ -231,17 +251,19 @@ controls_tick() { if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; sbus_rssi = RC_INPUT_RSSI_MAX / 2; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - /* set RSSI to an emulated value if ADC RSSI is off */ + /* set RSSI to an emulated value if ADC RSSI is off */ if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { rssi = sbus_rssi; } @@ -257,17 +279,20 @@ controls_tick() { */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); + if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + perf_end(c_gather_ppm); /* limit number of channels to allowable data size */ - if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) { r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + } /* store RSSI */ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; @@ -308,10 +333,13 @@ controls_tick() { /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) { raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + } + + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) { raw = conf[PX4IO_P_RC_CONFIG_MAX]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -330,10 +358,12 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( + conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ @@ -347,6 +377,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ @@ -360,6 +391,7 @@ controls_tick() { if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } @@ -389,6 +421,7 @@ controls_tick() { /* if we have enough channels (5) to control the vehicle, the mapping is ok */ if (assigned_channels > 4) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } @@ -408,9 +441,9 @@ controls_tick() { /* clear the input-kind flags here */ r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); } @@ -427,8 +460,8 @@ controls_tick() { if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK); + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); @@ -451,9 +484,9 @@ controls_tick() { * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -464,8 +497,9 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { override = true; + } /* if the FMU is dead then enable override if we have a @@ -473,20 +507,23 @@ controls_tick() { */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) - override = true; + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + override = true; + } if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { mixer_tick(); + } } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } @@ -512,8 +549,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > PX4IO_RC_INPUT_CHANNELS) + + if (*num_values > PX4IO_RC_INPUT_CHANNELS) { *num_values = PX4IO_RC_INPUT_CHANNELS; + } for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index eaff89e7c2..516eb59b86 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -318,6 +318,13 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE: if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } return -1; @@ -326,6 +333,13 @@ mixer_callback(uintptr_t handle, /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); @@ -339,6 +353,13 @@ mixer_callback(uintptr_t handle, return -1; } + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + } else if (control < -1.0f) { + control = -1.0f; + } + return 0; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a649920320..64ba0f93c4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -234,6 +234,9 @@ enum { /* DSM bind states */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ #define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 33134b1084..a5e7a5500c 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -113,6 +113,10 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] +#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] +#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] +#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] + #define r_control_values (&r_page_controls[0]) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 4ad7ba3182..8adc91cda9 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -174,6 +174,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, + [PX4IO_P_SETUP_TRIM_ROLL] = 0, + [PX4IO_P_SETUP_TRIM_PITCH] = 0, + [PX4IO_P_SETUP_TRIM_YAW] = 0 }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 @@ -627,6 +630,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value; break; + case PX4IO_P_SETUP_TRIM_ROLL: + case PX4IO_P_SETUP_TRIM_PITCH: + case PX4IO_P_SETUP_TRIM_YAW: + r_page_setup[offset] = value; + break; + default: return -1; } diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 454e5b5aa6..a1632e53c9 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -88,9 +88,9 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); * Companion computer interface * * Configures the baud rate of the companion computer interface. -* Set to zero to disable, set to 921600 to enable. -* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for -* other baud rates. +* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) +* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. +* 157600: enables OSD mode at 57600 baud, 8N1. * * @min 0 * @max 921600 diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h deleted file mode 100644 index 7a5ae98914..0000000000 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ /dev/null @@ -1,70 +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. - * - ****************************************************************************/ - -/** - * @file navigation_capabilities.h - * - * Definition of navigation capabilities uORB topic. - */ - -#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_ -#define TOPIC_NAVIGATION_CAPABILITIES_H_ - -#include "../uORB.h" -#include - -/** - * @addtogroup topics - * @{ - */ - -/** - * Airspeed - */ -struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ - - /* Landing parameters: see fw_pos_control_l1/landingslope.h */ - float landing_horizontal_slope_displacement; - float landing_slope_angle_rad; - float landing_flare_length; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(navigation_capabilities); - -#endif diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 1303a88356..ef08d367fa 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -58,7 +58,7 @@ public: int init() override; private: - ssize_t read(struct file *filp, char *buffer, size_t buflen); + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void air_pressure_sub_cb(const uavcan::ReceivedDataStructure &msg);