diff options
| -rw-r--r-- | src/modules/commander/commander.cpp | 587 | ||||
| -rw-r--r-- | src/modules/commander/commander_params.c | 2 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 30 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.h | 6 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink.c | 6 | ||||
| -rw-r--r-- | src/modules/multirotor_att_control/multirotor_att_control_main.c | 98 | ||||
| -rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 59 | ||||
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 | ||||
| -rw-r--r-- | src/modules/uORB/topics/vehicle_control_mode.h | 4 | ||||
| -rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 13 |
10 files changed, 394 insertions, 413 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d90008633..a548f943e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,8 +147,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* timout until lowlevel failsafe */ -static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; @@ -199,6 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode int commander_thread_main(int argc, char *argv[]); void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); + void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -206,17 +205,20 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); + void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); + int commander_main(int argc, char *argv[]) { @@ -271,7 +273,8 @@ void usage(const char *reason) exit(1); } -void print_status() { +void print_status() +{ warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ @@ -279,33 +282,40 @@ void print_status() { struct vehicle_status_s state; orb_copy(ORB_ID(vehicle_status), state_sub, &state); - const char* armed_str; + const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: - armed_str = "INIT"; - break; - case ARMING_STATE_STANDBY: - armed_str = "STANDBY"; - break; - case ARMING_STATE_ARMED: - armed_str = "ARMED"; - break; - case ARMING_STATE_ARMED_ERROR: - armed_str = "ARMED_ERROR"; - break; - case ARMING_STATE_STANDBY_ERROR: - armed_str = "STANDBY_ERROR"; - break; - case ARMING_STATE_REBOOT: - armed_str = "REBOOT"; - break; - case ARMING_STATE_IN_AIR_RESTORE: - armed_str = "IN_AIR_RESTORE"; - break; - default: - armed_str = "ERR: UNKNOWN STATE"; - break; + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + + default: + armed_str = "ERR: UNKNOWN STATE"; + break; } @@ -326,7 +336,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -415,6 +425,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } else { /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -478,9 +489,6 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); @@ -599,12 +607,6 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; - /* To remember when last notification was sent */ - uint64_t last_print_control_time = 0; - - enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; - bool armed_previous = false; - bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; @@ -665,7 +667,6 @@ int commander_thread_main(int argc, char *argv[]) int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -800,12 +801,15 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + } else { mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } @@ -908,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX should we still allow to arm with critical battery? //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } + status_changed = true; } @@ -943,11 +948,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -963,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) */ if (!home_position_set && gps_position.fix_type >= 3 && - (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate @@ -1013,9 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) } } - status.rc_signal_cutting_off = false; status.rc_signal_lost = false; - status.rc_signal_lost_interval = 0; transition_result_t res; // store all transitions results here @@ -1027,10 +1025,10 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); @@ -1040,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) } else { stick_off_counter++; } + } else { stick_off_counter = 0; } @@ -1087,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { @@ -1097,122 +1096,14 @@ int commander_thread_main(int argc, char *argv[]) } } else { - - /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { - // TODO remove debug code - if (!status.rc_signal_cutting_off) { - warnx("Reason: not rc_signal_cutting_off\n"); - - } else { - warnx("last print time: %llu\n", last_print_control_time); - } - - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (status.rc_signal_lost_interval > 1000000) { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; - status.failsave_lowlevel = true; status_changed = true; } } } - /* END mode switch */ - /* END RC state check */ - - // TODO check this - /* state machine update for offboard control */ - if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? - - // /* decide about attitude control flag, enable in att/pos/vel */ - // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - // /* decide about rate control flag, enable it always XXX (for now) */ - // bool rates_ctrl_enabled = true; - - // /* set up control mode */ - // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - // state_changed = true; - // } - - // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - // current_status.flag_control_rates_enabled = rates_ctrl_enabled; - // state_changed = true; - // } - - // /* handle the case where offboard control signal was regained */ - // if (!current_status.offboard_control_signal_found_once) { - // current_status.offboard_control_signal_found_once = true; - // /* enable offboard control, disable manual input */ - // current_status.flag_control_manual_enabled = false; - // current_status.flag_control_offboard_enabled = true; - // state_changed = true; - // tune_positive(); - - // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - // } else { - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - // state_changed = true; - // tune_positive(); - // } - // } - - status.offboard_control_signal_weak = false; - status.offboard_control_signal_lost = false; - status.offboard_control_signal_lost_interval = 0; - - // XXX check if this is correct - /* arm / disarm on request */ - if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - } - - } else { - - /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { - status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (status.offboard_control_signal_lost_interval > 100000) { - status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_positive(); - - /* kill motors after timeout */ - if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { - status.failsave_lowlevel = true; - status_changed = true; - } - } - } - } - /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); @@ -1230,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { status_changed = false; } @@ -1288,10 +1180,6 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - /* store old modes to detect and act on state transitions */ - battery_warning_previous = status.battery_warning; - armed_previous = armed.armed; - fflush(stdout); counter++; @@ -1343,6 +1231,7 @@ void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (armed->armed) { /* armed, solid */ @@ -1352,11 +1241,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); + } else { /* not ready to arm, blink at 10Hz */ if (leds_counter % 2 == 0) led_toggle(LED_BLUE); } + #endif if (changed) { @@ -1369,50 +1260,60 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - for (i=0; i<3; i++) { + for (i = 0; i < 3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; + pattern.duration[i * 2] = 200; + + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; } + if (status->condition_global_position_valid) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 1000; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 1000; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; + } else { - for (i=3; i<6; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 100; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 100; + for (i = 3; i < 6; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 100; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 100; } - pattern.color[6*2] = RGBLED_COLOR_OFF; - pattern.duration[6*2] = 700; + + pattern.color[6 * 2] = RGBLED_COLOR_OFF; + pattern.duration[6 * 2] = 700; } } else { - for (i=0; i<3; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 200; + for (i = 0; i < 3; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.duration[i * 2] = 200; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 200; } + /* not ready to arm, blink at 10Hz */ } @@ -1423,6 +1324,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->load > 0.95f) { if (leds_counter % 2 == 0) led_toggle(LED_AMBER); + } else { led_off(LED_AMBER); } @@ -1572,70 +1474,124 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) +check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - case MAIN_STATE_AUTO: - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (status->main_state == MAIN_STATE_AUTO) { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status.condition_landed) { + if (local_pos->z > -5.0f || status->condition_landed) { res = TRANSITION_NOT_CHANGED; - break; } } - /* check again, state can be changed */ - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + if (res != TRANSITION_NOT_CHANGED) { + /* check again, state can be changed */ + if (status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + + } else { + /* not landed */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* switch to MISSION in air when no RC control */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } + } + + } else { + /* disarmed, always switch to AUTO_READY */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + } + + } else { + /* manual control modes */ + if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { + /* switch to failsafe mode */ + bool manual_control_old = control_mode->flag_control_manual_enabled; + + if (!status->condition_landed) { + /* in air: try to hold position */ + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + + } else { + /* landed: don't try to hold position but land (if taking off) */ + res = TRANSITION_DENIED; + } + + if (res == TRANSITION_DENIED) { + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + } + + control_mode->flag_control_manual_enabled = false; + + if (res == TRANSITION_NOT_CHANGED && manual_control_old) { + /* mark navigation state as changed to force immediate flag publishing */ + set_navigation_state_changed(); + res = TRANSITION_CHANGED; + } + + if (res == TRANSITION_CHANGED) { + if (control_mode->flag_control_position_enabled) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); } else { - /* switch to mission in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + if (status->condition_landed) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + } } } + } else { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - break; + switch (status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - default: - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + break; + + default: + break; + } + } } return res; @@ -1644,27 +1600,32 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case VEHICLE_CMD_RESULT_ACCEPTED: tune_positive(); - break; - case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - tune_negative(); - break; - default: - break; + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + + default: + break; } } @@ -1720,11 +1681,13 @@ void *commander_low_prio_loop(void *arg) usleep(1000000); /* reboot */ systemreset(false); + } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(1000000); /* reboot to bootloader */ systemreset(true); + } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } @@ -1732,83 +1695,85 @@ void *commander_low_prio_loop(void *arg) } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; + + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - int calib_ret = ERROR; + int calib_ret = ERROR; - /* try to go to INIT/PREFLIGHT arming state */ + /* try to go to INIT/PREFLIGHT arming state */ - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - break; - } + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_gyro_calibration(mavlink_fd); + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_mag_calibration(mavlink_fd); + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_accel_calibration(mavlink_fd); + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_airspeed_calibration(mavlink_fd); - } + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - if (calib_ret == OK) - tune_positive(); - else - tune_negative(); + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } } + + break; } - break; - } default: answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); @@ -1817,7 +1782,7 @@ void *commander_low_prio_loop(void *arg) /* send any requested ACKs */ if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { /* send acknowledge command */ // XXX TODO } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6832fc5ce..0a1703b2e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include <nuttx/config.h> #include <systemlib/param/param.h> -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fe7e8cc53..674f3feda 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * return ret; } -bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) { // System is safe if: // 1) Not armed @@ -276,12 +276,12 @@ check_main_state_changed() } transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) +navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_status->navigation_state) { + if (new_navigation_state == status->navigation_state) { ret = TRANSITION_NOT_CHANGED; } else { @@ -296,6 +296,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_STABILIZE: @@ -307,6 +308,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_ALTHOLD: @@ -318,6 +320,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_VECTOR: @@ -329,6 +332,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_READY: @@ -340,12 +344,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -354,6 +359,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -367,6 +373,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: @@ -378,6 +385,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_RTL: @@ -389,12 +397,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LAND: /* deny transitions from landed state */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -403,6 +412,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -412,8 +422,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } if (ret == TRANSITION_CHANGED) { - current_status->navigation_state = new_navigation_state; - control_mode->auto_state = current_status->navigation_state; + status->navigation_state = new_navigation_state; + control_mode->auto_state = status->navigation_state; navigation_state_changed = true; } } @@ -433,6 +443,12 @@ check_navigation_state_changed() } } +void +set_navigation_state_changed() +{ + navigation_state_changed = true; +} + /** * Transition from one hil state to another */ diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a38c2497e..1641b6f60 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,10 +68,12 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); bool check_navigation_state_changed(); +void set_navigation_state_changed(); + int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 0a506b1a9..d7b0fa9c7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -240,11 +240,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u **/ /* set calibration state */ - if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_INIT + if (v_status.arming_state == ARMING_STATE_INIT || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index b3669d9ff..04582f2a4 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Implementation of multirotor attitude control main loop. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -63,6 +65,7 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> @@ -74,8 +77,6 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. - __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; @@ -102,7 +103,8 @@ mc_thread_main(int argc, char *argv[]) memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -114,6 +116,8 @@ mc_thread_main(int argc, char *argv[]) int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* * Do not rate-limit the loop to prevent aliasing @@ -136,7 +140,6 @@ mc_thread_main(int argc, char *argv[]) orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); @@ -149,12 +152,6 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; - bool failsafe_first_time = true; - - /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); - float failsafe_throttle = 0.0f; - param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +173,6 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -205,6 +201,13 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } + /* get a local copy of status */ + orb_check(status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), status_sub, &status); + } + /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -244,47 +247,18 @@ mc_thread_main(int argc, char *argv[]) /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (control_mode.failsave_highlevel) { - failsafe_first_time = false; - - if (!control_mode.flag_control_velocity_enabled) { - /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - } - - if (!control_mode.flag_control_climb_rate_enabled) { - /* don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - } + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (failsafe_first_time) { + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ reset_yaw_sp = true; } } else { - failsafe_first_time = true; - - /* only move yaw setpoint if manual input is != 0 and not landed */ + /* only move yaw setpoint if manual input is != 0 */ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; @@ -294,18 +268,17 @@ mc_thread_main(int argc, char *argv[]) } else { control_yaw_position = true; } + } - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } - } /* reset yaw setpint to current position if needed */ @@ -324,7 +297,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* publish the controller output */ + /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -342,6 +315,17 @@ mc_thread_main(int argc, char *argv[]) } } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + } + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 385892f04..8227f76c5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,10 +415,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; - } else { - /* non-manual mode, use global setpoint */ + } else if (control_mode.flag_control_auto_enabled) { + /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; @@ -428,21 +429,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); - } - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { - if (reset_auto_pos) { - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; - local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; - att_sp.yaw_body = att.yaw; - reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { @@ -457,6 +450,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (global_pos_sp_reproject) { /* update global setpoint projection */ global_pos_sp_reproject = false; + if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.lat * 1e-7; @@ -470,6 +464,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; @@ -489,6 +484,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } } + reset_auto_pos = true; } @@ -496,9 +492,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_pos_sp_reproject = true; } - /* reset setpoints after non-manual modes */ + /* reset setpoints after AUTO mode */ reset_sp_xy = true; reset_sp_z = true; + + } else { + /* no control, loiter or stay on ground */ + if (local_pos.landed) { + /* landed: move setpoint down */ + /* in air: hold altitude */ + if (local_pos_sp.z < 5.0f) { + /* set altitude setpoint to 5m under ground, + * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ + local_pos_sp.z = 5.0f; + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + } + + reset_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + } + } } /* publish local position setpoint */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ef13ade88..4a4572b09 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e15ddde26..093c6775d 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -80,9 +80,7 @@ struct vehicle_control_mode_s bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ - + bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9f55bb874..43218eac4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -174,8 +174,6 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ -// uint64_t failsave_highlevel_begin; TO BE COMPLETED main_state_t main_state; /**< main state machine */ navigation_state_t navigation_state; /**< navigation state machine */ @@ -207,22 +205,13 @@ struct vehicle_status_s bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception is terminally lost */ - bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ - uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool rc_signal_lost; /**< true if RC reception lost */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; - - bool preflight_calibration; - - bool system_emergency; - /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; |