From c543f89ec17048c1b5264623a885a9247a05304c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 23:36:49 +0200 Subject: commander, multirotor_pos_control, multirotor_att_control: bugfixes --- src/modules/commander/commander.cpp | 132 ++++++++++++--------- src/modules/commander/state_machine_helper.cpp | 89 +++++++------- .../multirotor_att_control_main.c | 8 +- .../multirotor_pos_control.c | 22 ++-- src/modules/uORB/topics/vehicle_control_mode.h | 3 +- src/modules/uORB/topics/vehicle_local_position.h | 1 + 6 files changed, 134 insertions(+), 121 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6181dafab..872939d6d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -122,7 +122,7 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + case VEHICLE_CMD_NAV_TAKEOFF: { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } break; + } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { + if (is_safe(status, safety, armed)) { - if (((int)(cmd->param1)) == 1) { - /* reboot */ - up_systemreset(); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value /* armed topic */ struct actuator_armed_s armed; @@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[]) last_diff_pres_time = diff_pres.timestamp; } + /* Check for valid airspeed/differential pressure measurements */ + if (t - last_diff_pres_time < 2000000 && t > 2000000) { + status.condition_airspeed_valid = true; + + } else { + status.condition_airspeed_valid = false; + } + orb_check(cmd_sub, &updated); if (updated) { @@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* set the condition to valid if there has recently been a global position estimate */ + if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { + if (!status.condition_global_position_valid) { + status.condition_global_position_valid = true; + status_changed = true; + } + + } else { + if (status.condition_global_position_valid) { + status.condition_global_position_valid = false; + status_changed = true; + } + } + /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { if (!status.condition_local_position_valid) { status.condition_local_position_valid = true; status_changed = true; @@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; - - } else { - status.condition_global_position_valid = false; - } - - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; - - } else { - status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } - orb_check(gps_sub, &updated); if (updated) { @@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { /* DENIED here indicates safety switch not pressed */ @@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + } else { 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); @@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; } else { - /* if not landed: act depending on switches */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* 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); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2..f313adce4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ - { + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s // 3) Safety switch is present AND engaged -> actuators locked if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; + } else { return false; } @@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; @@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; @@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; case NAVIGATION_STATE_AUTO_READY: ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } break; case NAVIGATION_STATE_AUTO_LOITER: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_RTL: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_LAND: @@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } 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 65b19c8e9..1aa24a4fc 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -250,12 +250,12 @@ mc_thread_main(int argc, char *argv[]) /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { - if (!control_mode.flag_control_position_enabled) { + 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_altitude_enabled) { + 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 @@ -309,12 +309,12 @@ mc_thread_main(int argc, char *argv[]) control_yaw_position = true; } - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0120fa61c..1cb470240 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -221,10 +221,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime home_alt_t = 0; uint64_t local_home_timestamp = 0; - static PID_t xy_pos_pids[2]; - static PID_t xy_vel_pids[2]; - static PID_t z_pos_pid; - static thrust_pid_t z_vel_pid; + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; thread_running = true; @@ -238,7 +238,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -247,9 +247,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz*/ - if (--paramcheck_counter <= 0) { - paramcheck_counter = 50; + /* check parameters at 1 Hz */ + if (++paramcheck_counter >= 50) { + paramcheck_counter = 0; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -441,14 +441,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subcrive to velocity setpoint if altitude/position control disabled - if (control_mode.flag_control_velocity_enabled) { + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_altitude_enabled) { + if (control_mode.flag_control_climb_rate_enabled) { thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } - if (control_mode.flag_control_position_enabled) { + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index d83eb45d9..fe169c6e6 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -75,9 +75,10 @@ struct vehicle_control_mode_s //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool 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 */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd..9d3b4468c 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -75,6 +75,7 @@ struct vehicle_local_position_s float home_alt; /**< Altitude in meters LOGME */ float home_hdg; /**< Compass heading in radians -PI..+PI. */ + bool landed; /**< true if vehicle is landed */ }; /** -- cgit v1.2.3