From 29b0678d841666000fd67cc955dba5cf3cc980c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 11:56:01 +0100 Subject: [PATCH 01/23] 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/23] 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/23] 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/23] 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/23] 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/23] 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/23] 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/23] 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/23] 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/23] 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 ff8f92af31fe1eadb52dbe1e612fd1f31b00ff82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:24:03 -0800 Subject: [PATCH 11/23] Remove Ubuntu code style script - people should use the reference one --- Tools/fix_code_style_ubuntu.sh | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100755 Tools/fix_code_style_ubuntu.sh diff --git a/Tools/fix_code_style_ubuntu.sh b/Tools/fix_code_style_ubuntu.sh deleted file mode 100755 index 90ab57b895..0000000000 --- a/Tools/fix_code_style_ubuntu.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh -astyle \ - --style=linux \ - --indent=force-tab=8 \ - --indent-cases \ - --indent-preprocessor \ - --break-blocks=all \ - --pad-oper \ - --pad-header \ - --unpad-paren \ - --keep-one-line-blocks \ - --keep-one-line-statements \ - --align-pointer=name \ - --suffix=none \ - --lineend=linux \ - $* - #--ignore-exclude-errors-x \ - #--exclude=EASTL \ - #--align-reference=name \ From 99b376089f6a35c977174ae8e01bc4686b405cc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:26:23 -0800 Subject: [PATCH 12/23] Replace single if lines with brackets in astyle --- Tools/fix_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index 832ee79da6..0b6743013f 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -16,4 +16,5 @@ astyle \ --ignore-exclude-errors-x \ --lineend=linux \ --exclude=EASTL \ + --add-brackets \ $* From 20618a611b5cd719cab803823b2f3a08d9a498f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:44:05 -0800 Subject: [PATCH 13/23] Fixed a number of compile warnings in mount tests. --- src/systemcmds/tests/test_mount.c | 38 ++++++++++++++----------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 44e34d9ef3..4b6303cfb1 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -141,8 +141,8 @@ test_mount(int argc, char *argv[]) /* announce mode switch */ if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); } @@ -162,7 +162,7 @@ test_mount(int argc, char *argv[]) } char buf[64]; - int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + (void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); lseek(cmd_fd, 0, SEEK_SET); write(cmd_fd, buf, strlen(buf) + 1); fsync(cmd_fd); @@ -174,8 +174,8 @@ test_mount(int argc, char *argv[]) printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); for (unsigned a = 0; a < alignments; a++) { @@ -185,22 +185,20 @@ test_mount(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - hrt_abstime start, end; int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); - if (wret != chunk_sizes[c]) { + if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) @@ -214,8 +212,8 @@ test_mount(int argc, char *argv[]) fsync(fd); } else { printf("#"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); } } @@ -224,12 +222,10 @@ test_mount(int argc, char *argv[]) } printf("."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(200000); - end = hrt_absolute_time(); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -237,7 +233,7 @@ test_mount(int argc, char *argv[]) for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, chunk_sizes[c]); - if (rret != chunk_sizes[c]) { + if (rret != (int)chunk_sizes[c]) { warnx("READ ERROR!"); return 1; } @@ -245,7 +241,7 @@ test_mount(int argc, char *argv[]) /* compare value */ bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { + for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j + a]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); compare_ok = false; @@ -271,16 +267,16 @@ test_mount(int argc, char *argv[]) } } - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); /* we always reboot for the next test if we get here */ warnx("Iteration done, rebooting.."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); systemreset(false); From e1f60e770948d28ec1152f66b9c3ad57eae0c2e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:53:20 -0800 Subject: [PATCH 14/23] Code style fixes (authors should be in the doxygen section), using lseek to attempt to work around log corruption --- src/modules/sdlog2/logbuffer.c | 3 +-- src/modules/sdlog2/sdlog2.c | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index b3243f7b5a..6a29d7e5c9 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c4fafb5a66..16c37459b5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -95,6 +93,7 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ + } else { \ log_msgs_skipped++; \ } @@ -450,6 +449,7 @@ static void *logwriter_thread(void *arg) n = available; } + lseek(log_fd, 0, SEEK_CUR); n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; From ba0bf456b9b0cf6cdc58ce2fd80192da3c1744d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:37:35 +0100 Subject: [PATCH 15/23] gitignore: ignore pydev project file created in eclipse --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 3e94cf6205..71326517f2 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/ /Documentation/doxygen*objdb*tmp .tags .tags_sorted_by_file +.pydevproject From 5ed5e04cb24c261c9a8b40857bd1d733238f847e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:43:23 +0100 Subject: [PATCH 16/23] sdlog2: code style fixes broke compilation --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 16c37459b5..41e2248bbe 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -93,7 +93,6 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ - } else { \ log_msgs_skipped++; \ } From a79eef05bcb23c9be386a2980437a67151cfa3ea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Feb 2014 23:48:00 +0400 Subject: [PATCH 17/23] perf_counter: added include --- src/modules/systemlib/perf_counter.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index e1e3cbe95e..d8f69fdbf7 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -39,6 +39,8 @@ #ifndef _SYSTEMLIB_PERF_COUNTER_H #define _SYSTEMLIB_PERF_COUNTER_H value +#include + /** * Counter types. */ From f8187650083b361f61c07ceb712c21a25d1c80c4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:32:55 +0400 Subject: [PATCH 18/23] top: CPU % indication bug fixed --- src/systemcmds/top/top.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 1ca3fc9281..37e913040d 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -233,8 +233,8 @@ top_main(void) system_load.tasks[i].tcb->pid, CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100), - (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + (int)(curr_loads[i] * 100.0f), + (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), stack_size - stack_free, stack_size, system_load.tasks[i].tcb->sched_priority, From eac00a71ea8f0b10d446be752f6fef737f850956 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 11:05:07 +0100 Subject: [PATCH 19/23] 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 20/23] 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 e06263f76f9e38ea9624f1f1ab21af8731a3cd7f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:43:56 +0400 Subject: [PATCH 21/23] position_estimator_inav: failsafe against NaN estimate --- .../position_estimator_inav_main.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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 ad363efe06..fb68dd31c0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -708,6 +708,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); + float x_est_prev[3], y_est_prev[3]; + + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -715,7 +720,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); - thread_should_exit = true; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ @@ -739,7 +745,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); - thread_should_exit = true; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_flow, 0, sizeof(corr_flow)); } } From d11235585cf1014c5391b63e394151c171ed286d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:56:02 +0400 Subject: [PATCH 22/23] position_estimator_inav: log writing on NaN estimate fixed --- .../position_estimator_inav_main.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 fb68dd31c0..d6d03367b6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); - fputs(f, s); - snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); - fputs(f, s); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fwrite(s, 1, n, f); + n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); } From db157b68c2f3dba4209f8075f6ae7ae9e81a448c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 19:42:10 +0400 Subject: [PATCH 23/23] 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 {