diff options
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/commander.c | 30 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.c | 5 |
2 files changed, 29 insertions, 6 deletions
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 01ab9e3d9..aab8f3e04 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1503,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[]) if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO || current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); + /* armed, solid */ + led_on(LED_AMBER); } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not armed */ - led_toggle(LED_BLUE); + led_toggle(LED_AMBER); + } + + if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { + + /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ + if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + /* GPS lock */ + led_on(LED_BLUE); + + } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* no GPS lock, but GPS module is aquiring lock */ + led_toggle(LED_BLUE); + } + + } else { + /* no GPS info, don't light the blue led */ + led_off(LED_BLUE); } - /* toggle error led at 5 Hz in HIL mode */ + /* toggle GPS led at 5 Hz in HIL mode */ if (current_status.flag_hil_enabled) { /* hil enabled */ - led_toggle(LED_AMBER); + led_toggle(LED_BLUE); } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (red) at 5 Hz on low battery or error */ + /* toggle arming (red) at 5 Hz on low battery or error */ led_toggle(LED_AMBER); } else { diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index bea388a10..ab728c7bb 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { struct actuator_armed_s armed; armed.armed = current_status->flag_system_armed; + + /* XXX allow arming by external components on multicopters only if not yet armed by RC */ + /* XXX allow arming only if core sensors are ok */ + armed.ready_to_arm = true; + /* lock down actuators if required, only in HIL */ armed.lockdown = (current_status->flag_hil_enabled) ? true : false; orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); |