From 29b0678d841666000fd67cc955dba5cf3cc980c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 11:56:01 +0100 Subject: [PATCH 01/13] navigator: forbid READY - > RTL transition --- src/modules/navigator/navigator_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..abd7119f78 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -689,7 +689,7 @@ Navigator::task_main() if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -748,7 +748,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -756,9 +756,7 @@ Navigator::task_main() break; case NAV_STATE_LAND: - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); - } + dispatch(EVENT_LAND_REQUESTED); break; From 1d40582cc0301f0af330fe69ee63aa8e3d301214 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 12:42:20 +0100 Subject: [PATCH 02/13] navigator: forbid READY - > RTL transition fix --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index abd7119f78..6f0fec9179 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -965,7 +965,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, From 6631ecf04a0592c816dfb832fa928a95877184d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 11:45:35 +0100 Subject: [PATCH 03/13] commander: reset blink_msg_end when blink message completed to set normal LED status immediately --- src/modules/commander/commander_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88e..04624275f9 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -161,6 +161,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { From 855944fb2eab33074537b5ccfc82e2462b662b5b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 00:24:40 +0100 Subject: [PATCH 04/13] commander: beeps and blinks cleanup --- .../commander/accelerometer_calibration.cpp | 2 +- .../commander/airspeed_calibration.cpp | 2 +- src/modules/commander/commander.cpp | 46 ++++------ src/modules/commander/commander_helper.cpp | 86 +++++++++++-------- src/modules/commander/commander_helper.h | 14 +-- 5 files changed, 76 insertions(+), 74 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 36b75dd58a..1cbdf9bf85 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (double)accel_ref[orient][2]); data_collected[orient] = true; - tune_neutral(); + tune_neutral(true); } close(sensor_combined_sub); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 1809f96888..6039d92a76 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd) } mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(); + tune_neutral(true); close(diff_pres_sub); return OK; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b85737..193a7473eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1101,7 +1101,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; - tune_positive(); + tune_positive(true); } } @@ -1196,8 +1196,9 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine according to mode switches */ res = set_main_state_rc(&status); + /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { - tune_positive(); + tune_positive(armed.armed); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1253,7 +1254,7 @@ int commander_thread_main(int argc, char *argv[]) /* flight termination in manual mode if assisted switch is on easy position */ if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(); + tune_positive(armed.armed); } } @@ -1308,21 +1309,18 @@ int commander_thread_main(int argc, char *argv[]) /* play arming and battery warning tunes */ if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ - if (tune_arm() == OK) - arm_tune_played = true; + set_tune(TONE_ARMING_WARNING_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ - if (tune_low_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ - if (tune_critical_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_FAST_TUNE); } else if (battery_tune_played) { - tune_stop(); + set_tune(TONE_STOP_TUNE); battery_tune_played = false; } @@ -1693,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg) sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - // only buzz if armed, because else we're driving people nuts indoors - // they really need to look at the leds as well. - if (status->arming_state == ARMING_STATE_ARMED) { - tune_negative(); - } else { - - // Always show the led indication - led_negative(); - } + /* only buzz if armed, because else we're driving people nuts indoors + they really need to look at the leds as well. */ + tune_negative(armed.armed); } } @@ -1715,7 +1707,7 @@ print_reject_arm(const char *msg) char s[80]; sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + tune_negative(true); } } @@ -1723,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); - tune_negative(); + tune_negative(true); break; default: @@ -1883,9 +1875,9 @@ void *commander_low_prio_loop(void *arg) } if (calib_ret == OK) - tune_positive(); + tune_positive(true); else - tune_negative(); + tune_negative(true); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 04624275f9..265c0134cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -82,10 +83,21 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) } static int buzzer; -static hrt_abstime blink_msg_end; +static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message +static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence +static int tune_current = 0; // currently playing tune, can be interrupted after tune_end +static int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { + tune_end = 0; + tune_current = 0; + memset(tune_durations, 0, sizeof(tune_durations)); + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { @@ -93,6 +105,8 @@ int buzzer_init() return ERROR; } + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + return OK; } @@ -101,58 +115,60 @@ void buzzer_deinit() close(buzzer); } -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +void set_tune(int tune) { + int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ + if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + /* allow interrupting current non-repeating tune by the same tune */ + if (tune != tune_current || new_tune_duration > 0) { + ioctl(buzzer, TONE_SET_ALARM, tune); + } + tune_current = tune; + if (new_tune_duration > 0) { + tune_end = hrt_absolute_time() + new_tune_duration; + } else { + tune_end = 0; + } + } } -void tune_positive() +/** + * Blink green LED and play positive tune (if use_busser == true). + */ +void tune_positive(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } } -void tune_neutral() +/** + * Blink white LED and play neutral tune (if use_busser == true). + */ +void tune_neutral(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } } -void tune_negative() -{ - led_negative(); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); -} - -void led_negative() +/** + * Blink red LED and play negative tune (if use_busser == true). + */ +void tune_negative(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); -} - -int tune_critical_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); -} - -void tune_stop() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + } } int blink_msg_state() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index af25a5e979..e75f2592f5 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); -int tune_arm(void); -int tune_low_bat(void); -int tune_critical_bat(void); -void tune_stop(void); - -void led_negative(); +void set_tune(int tune); +void tune_positive(bool use_buzzer); +void tune_neutral(bool use_buzzer); +void tune_negative(bool use_buzzer); int blink_msg_state(); From 0613b299c033c21e1ddcbd8faeb9d3430d72386c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 09:13:51 +0100 Subject: [PATCH 05/13] commander: play warning tune (as for low battery) when in failsafe state --- src/modules/commander/commander.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 193a7473eb..20bfca4774 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,14 +1311,14 @@ int commander_thread_main(int argc, char *argv[]) /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* play tune on battery warning */ - set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* play tune on battery warning or failsafe */ + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); + } else if (battery_tune_played) { set_tune(TONE_STOP_TUNE); battery_tune_played = false; @@ -1420,11 +1420,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - rgbled_set_color(RGBLED_COLOR_AMBER); - } - + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { From 0ead560059fff0a31183f40a6b848e82aa2dae80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 18:24:20 +0100 Subject: [PATCH 06/13] commander: tunes cleanup and fixes --- src/modules/commander/commander.cpp | 9 ++---- src/modules/commander/commander_helper.cpp | 32 ++++++++++------------ 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20bfca4774..7284b38c91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -607,7 +607,6 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; - bool battery_tune_played = false; bool arm_tune_played = false; /* set parameters */ @@ -1020,14 +1019,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; if (armed.armed) { arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); @@ -1310,6 +1307,7 @@ int commander_thread_main(int argc, char *argv[]) if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); + arm_tune_played = true; } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ @@ -1319,13 +1317,12 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (battery_tune_played) { + } else { set_tune(TONE_STOP_TUNE); - battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { + if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 265c0134cb..fe6c9bfaa5 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,20 +82,20 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -static int buzzer; +static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence -static int tune_current = 0; // currently playing tune, can be interrupted after tune_end -static int tune_durations[TONE_NUMBER_OF_TUNES]; +static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end +static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { tune_end = 0; tune_current = 0; memset(tune_durations, 0, sizeof(tune_durations)); - tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); @@ -105,8 +105,6 @@ int buzzer_init() return ERROR; } - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - return OK; } @@ -116,15 +114,15 @@ void buzzer_deinit() } void set_tune(int tune) { - int new_tune_duration = tune_durations[tune]; + unsigned int new_tune_duration = tune_durations[tune]; /* don't interrupt currently playing non-repeating tune by repeating */ - if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ - if (tune != tune_current || new_tune_duration > 0) { + if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; - if (new_tune_duration > 0) { + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; } else { tune_end = 0; @@ -133,7 +131,7 @@ void set_tune(int tune) { } /** - * Blink green LED and play positive tune (if use_busser == true). + * Blink green LED and play positive tune (if use_buzzer == true). */ void tune_positive(bool use_buzzer) { @@ -146,7 +144,7 @@ void tune_positive(bool use_buzzer) } /** - * Blink white LED and play neutral tune (if use_busser == true). + * Blink white LED and play neutral tune (if use_buzzer == true). */ void tune_neutral(bool use_buzzer) { @@ -159,7 +157,7 @@ void tune_neutral(bool use_buzzer) } /** - * Blink red LED and play negative tune (if use_busser == true). + * Blink red LED and play negative tune (if use_buzzer == true). */ void tune_negative(bool use_buzzer) { @@ -185,8 +183,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() { From 36bd7a797b6116b2f4b1c3fe358ee56eb7982b1d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 11:46:26 +0100 Subject: [PATCH 07/13] navigator: use bearing to home for RTL --- src/modules/navigator/navigator_main.cpp | 30 +++++++++++++++++++----- 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd2..d7f6fd16ab 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1343,7 +1343,14 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; // don't change altitude - _mission_item.yaw = NAN; // TODO set heading to home + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); + } _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -1409,17 +1416,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); + + } else { + /* else use current position */ + sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); + } + sp->loiter_radius = _parameters.loiter_radius; + sp->loiter_direction = 1; + sp->pitch_min = 0.0f; + } else { sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; } - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; From 8bc0287eccc7871db17c199a3b330e74c92c068a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 22:45:54 +0400 Subject: [PATCH 08/13] commander: disarm system when safety enabled --- src/modules/commander/commander.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6d14472f36..29a930ebb3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,11 +902,13 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); - // XXX this would be the right approach to do it, but do we *WANT* this? - // /* disarm if safety is now on and still armed */ - // if (safety.safety_switch_available && !safety.safety_off) { - // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - // } + /* disarm if safety is now on and still armed */ + if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + } + } } /* update global position estimate */ From a43c7c488eaba5022a18f05e590059cad6f580da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 18:54:58 +0400 Subject: [PATCH 09/13] navigator: "reached" flags reset fixed --- src/modules/navigator/navigator_main.cpp | 39 +++++++++++++++++------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ed12d1062..af1b9aac02 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -321,6 +321,11 @@ private: */ bool onboard_mission_available(unsigned relative_index); + /** + * Reset all "reached" flags. + */ + void reset_reached(); + /** * Check if current mission item has been reached. */ @@ -855,9 +860,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - - /* reset time counter on state changes */ - _time_first_inside_orbit = 0; } perf_end(_loop_perf); @@ -1009,6 +1011,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { void Navigator::start_none() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -1024,6 +1028,8 @@ Navigator::start_none() void Navigator::start_ready() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; @@ -1046,6 +1052,8 @@ Navigator::start_ready() void Navigator::start_loiter() { + reset_reached(); + _do_takeoff = false; /* set loiter position if needed */ @@ -1091,6 +1099,8 @@ Navigator::start_mission() void Navigator::set_mission_item() { + reset_reached(); + /* copy current mission to previous item */ memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); @@ -1104,9 +1114,6 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - /* reset time counter for new item */ - _time_first_inside_orbit = 0; - _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1224,6 +1231,8 @@ Navigator::start_rtl() void Navigator::start_land() { + reset_reached(); + /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ _do_takeoff = false; @@ -1255,6 +1264,8 @@ Navigator::start_land() void Navigator::start_land_home() { + reset_reached(); + /* land to home position, should be called when hovering above home, from RTL state */ _do_takeoff = false; _reset_loiter_pos = true; @@ -1285,8 +1296,7 @@ Navigator::start_land_home() void Navigator::set_rtl_item() { - /*reset time counter for new RTL item */ - _time_first_inside_orbit = 0; + reset_reached(); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -1550,9 +1560,7 @@ Navigator::check_mission_item_reached() /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - _time_first_inside_orbit = 0; - _waypoint_yaw_reached = false; - _waypoint_position_reached = false; + reset_reached(); return true; } } @@ -1561,6 +1569,15 @@ Navigator::check_mission_item_reached() } +void +Navigator::reset_reached() +{ + _time_first_inside_orbit = 0; + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + +} + void Navigator::on_mission_item_reached() { From 7d8f08ad9a733111a9d8b1512a85592a2fb9b045 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 19:05:32 +0400 Subject: [PATCH 10/13] navigator: check if yaw reached only when position reached --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index af1b9aac02..4cd60d8d89 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1533,7 +1533,7 @@ Navigator::check_mission_item_reached() } } - if (!_waypoint_yaw_reached) { + if (_waypoint_position_reached && !_waypoint_yaw_reached) { if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); From eac00a71ea8f0b10d446be752f6fef737f850956 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 11:05:07 +0100 Subject: [PATCH 11/13] navigator: don't enter AUTO_READY for fixedwing because we don't have a land detector yet --- src/modules/navigator/navigator_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 04f87d4caf..8d0710e915 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1386,7 +1386,8 @@ Navigator::set_rtl_item() void Navigator::request_loiter_or_ready() { - if (_vstatus.condition_landed) { + /* XXX workaround: no landing detector for fixedwing yet */ + if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { dispatch(EVENT_READY_REQUESTED); } else { From b264352c179dafd42066ca8ffbdf855c7f111207 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 15:47:09 +0100 Subject: [PATCH 12/13] startup: phantom script was missing the default line --- ROMFS/px4fmu_common/init.d/3031_phantom | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 2e2434bb85..a3004d1e13 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -5,4 +5,6 @@ # Simon Wilks # +sh /etc/init.d/rc.fw_defaults + set MIXER FMU_Q From db157b68c2f3dba4209f8075f6ae7ae9e81a448c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 19:42:10 +0400 Subject: [PATCH 13/13] mc_pos_control: remove some debug logs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) 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 b4bac53d66..78d06ba5b5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -791,7 +791,6 @@ MulticopterPositionControl::task_main() } thrust_int(2) = -i; - mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } } else { @@ -803,7 +802,6 @@ MulticopterPositionControl::task_main() reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; - mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); } } else {