diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-06 23:43:09 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-06 23:43:09 +0200 |
| commit | 7f2a63eb964b384a6b76e7004f1250d705f35fb0 (patch) | |
| tree | 2af4ef8e8057838ce74d29371c48fc3090c8dc2a /apps/commander/state_machine_helper.c | |
| parent | f88bba0cec9fa98037a966e2e3bcac8ad10b68f0 (diff) | |
| download | px4-firmware-7f2a63eb964b384a6b76e7004f1250d705f35fb0.tar.gz px4-firmware-7f2a63eb964b384a6b76e7004f1250d705f35fb0.tar.bz2 px4-firmware-7f2a63eb964b384a6b76e7004f1250d705f35fb0.zip | |
Completed calibration state machine, calibration state now propagating to sensor, scale calibration soon
Diffstat (limited to 'apps/commander/state_machine_helper.c')
| -rw-r--r-- | apps/commander/state_machine_helper.c | 30 |
1 files changed, 22 insertions, 8 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 2171ccedf..91eea2117 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -4,7 +4,6 @@ * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * - * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: @@ -34,7 +33,10 @@ * ****************************************************************************/ -/* @file State machine helper functions implementations */ +/** + * @file state_machine_helper.c + * State machine helper functions implementations + */ #include <stdio.h> #include "state_machine_helper.h" @@ -64,6 +66,8 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co { int invalid_state = false; + commander_state_machine_t old_state = current_status->state_machine; + switch (new_state) { case SYSTEM_STATE_MISSION_ABORT: { /* Indoor or outdoor */ @@ -103,6 +107,12 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co case SYSTEM_STATE_PREFLIGHT: //global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO); + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + invalid_state = false; + } else { + invalid_state = true; + } break; case SYSTEM_STATE_REBOOT: @@ -143,16 +153,20 @@ void do_state_update(int status_pub, struct vehicle_status_s *current_status, co break; } - if (invalid_state == false) { - //publish the new state + if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[new_state]); + state_machine_publish(status_pub, current_status); } } +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status) { + /* publish the new state */ + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); +} + /* * Private functions, update the state machine |