diff options
| -rw-r--r-- | apps/commander/commander.c | 28 | ||||
| -rw-r--r-- | apps/px4io/safety.c | 2 |
2 files changed, 27 insertions, 3 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f61fd053c..5dfdf83ad 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1476,21 +1476,45 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * 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 = current_status.flag_global_position_valid; bool local_pos_valid = current_status.flag_local_position_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_vector_flight_mode_ok = true; current_status.flag_global_position_valid = true; // XXX check for controller status and home position as well + } else { + current_status.flag_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_vector_flight_mode_ok = true; current_status.flag_local_position_valid = true; // XXX check for controller status and home position as well + } else { + current_status.flag_local_position_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + } else { + current_status.flag_vector_flight_mode_ok = false; } /* consolidate state change, flag as changed if required */ diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 780a4331a..3314ef513 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -64,7 +64,7 @@ static unsigned counter = 0; * Define the various LED flash sequences for each system state. */ #define LED_PATTERN_SAFE 0xffff /**< always on */ -#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0x3000 /**< always on with short break */ +#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ #define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ #define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ #define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ |