2012-08-04 19:12:36 -03:00
/****************************************************************************
*
* Copyright ( C ) 2012 PX4 Development Team . All rights reserved .
* 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 :
*
* 1. Redistributions of source code must retain the above copyright
* notice , this list of conditions and the following disclaimer .
* 2. Redistributions in binary form must reproduce the above copyright
* notice , this list of conditions and the following disclaimer in
* the documentation and / or other materials provided with the
* distribution .
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission .
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* " AS IS " AND ANY EXPRESS OR IMPLIED WARRANTIES , INCLUDING , BUT NOT
* LIMITED TO , THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED . IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT , INDIRECT ,
* INCIDENTAL , SPECIAL , EXEMPLARY , OR CONSEQUENTIAL DAMAGES ( INCLUDING ,
* BUT NOT LIMITED TO , PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES ; LOSS
* OF USE , DATA , OR PROFITS ; OR BUSINESS INTERRUPTION ) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY , WHETHER IN CONTRACT , STRICT
* LIABILITY , OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE , EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE .
*
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2012-08-06 18:43:09 -03:00
/**
* @ file state_machine_helper . c
* State machine helper functions implementations
*/
2012-08-04 19:12:36 -03:00
# include <stdio.h>
# include "state_machine_helper.h"
# include <uORB/uORB.h>
# include <uORB/topics/vehicle_status.h>
# include <systemlib/systemlib.h>
# include <arch/board/up_hrt.h>
2012-08-13 13:53:37 -03:00
# include <mavlink/mavlink_log.h>
2012-08-04 19:12:36 -03:00
static const char * system_state_txt [ ] = {
" SYSTEM_STATE_PREFLIGHT " ,
" SYSTEM_STATE_STANDBY " ,
" SYSTEM_STATE_GROUND_READY " ,
" SYSTEM_STATE_MANUAL " ,
" SYSTEM_STATE_STABILIZED " ,
" SYSTEM_STATE_AUTO " ,
" SYSTEM_STATE_MISSION_ABORT " ,
" SYSTEM_STATE_EMCY_LANDING " ,
" SYSTEM_STATE_EMCY_CUTOFF " ,
" SYSTEM_STATE_GROUND_ERROR " ,
" SYSTEM_STATE_REBOOT " ,
} ;
2012-08-13 13:53:37 -03:00
/**
* Transition from one state to another
*/
int do_state_update ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd , commander_state_machine_t new_state )
2012-08-04 19:12:36 -03:00
{
int invalid_state = false ;
2012-08-13 13:53:37 -03:00
int ret = ERROR ;
2012-08-04 19:12:36 -03:00
2012-08-06 18:43:09 -03:00
commander_state_machine_t old_state = current_status - > state_machine ;
2012-08-04 19:12:36 -03:00
switch ( new_state ) {
case SYSTEM_STATE_MISSION_ABORT : {
/* Indoor or outdoor */
uint8_t flight_environment_parameter = ( uint8_t ) ( global_data_parameter_storage - > pm . param_values [ PARAM_FLIGHT_ENV ] ) ;
if ( flight_environment_parameter = = PX4_FLIGHT_ENVIRONMENT_OUTDOOR ) {
2012-08-13 13:53:37 -03:00
ret = do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_EMCY_LANDING ) ;
2012-08-04 19:12:36 -03:00
} else {
2012-08-13 13:53:37 -03:00
ret = do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_EMCY_CUTOFF ) ;
2012-08-04 19:12:36 -03:00
}
return ;
}
break ;
case SYSTEM_STATE_EMCY_LANDING :
/* Tell the controller to land */
//TODO: add emcy landing code here
fprintf ( stderr , " [commander] EMERGENCY LANDING! \n " ) ;
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] EMERGENCY LANDING! " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_EMCY_CUTOFF :
2012-08-13 13:53:37 -03:00
/* Tell the controller to cutoff the motors (thrust = 0) */
2012-08-04 19:12:36 -03:00
fprintf ( stderr , " [commander] EMERGENCY MOTOR CUTOFF! \n " ) ;
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] EMERGENCY MOTOR CUTOFF! " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_GROUND_ERROR :
fprintf ( stderr , " [commander] GROUND ERROR, locking down propulsion system \n " ) ;
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] GROUND ERROR, locking down propulsion system " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_PREFLIGHT :
2012-08-06 18:43:09 -03:00
if ( current_status - > state_machine = = SYSTEM_STATE_STANDBY
| | current_status - > state_machine = = SYSTEM_STATE_PREFLIGHT ) {
invalid_state = false ;
2012-08-13 13:53:37 -03:00
mavlink_log_info ( mavlink_fd , " [commander] Switched to PREFLIGHT state " ) ;
2012-08-06 18:43:09 -03:00
} else {
invalid_state = true ;
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] REFUSED to switch to PREFLIGHT state " ) ;
2012-08-06 18:43:09 -03:00
}
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_REBOOT :
2012-08-13 13:53:37 -03:00
if ( current_status - > state_machine = = SYSTEM_STATE_STANDBY
| | current_status - > state_machine = = SYSTEM_STATE_PREFLIGHT ) {
invalid_state = false ;
mavlink_log_critical ( mavlink_fd , " [commander] REBOOTING SYSTEM " ) ;
usleep ( 500000 ) ;
reboot ( ) ;
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true ;
mavlink_log_critical ( mavlink_fd , " [commander] REFUSED to REBOOT " ) ;
}
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_STANDBY :
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] Switched to STANDBY state " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_GROUND_READY :
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] Switched to GROUND READY state " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_AUTO :
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] Switched to FLYING / AUTO mode " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_STABILIZED :
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] Switched to FLYING / STABILIZED mode " ) ;
2012-08-04 19:12:36 -03:00
break ;
case SYSTEM_STATE_MANUAL :
2012-08-13 13:53:37 -03:00
mavlink_log_critical ( mavlink_fd , " [commander] Switched to FLYING / MANUAL mode " ) ;
2012-08-04 19:12:36 -03:00
break ;
default :
invalid_state = true ;
break ;
}
2012-08-06 18:43:09 -03:00
if ( invalid_state = = false | | old_state ! = new_state ) {
2012-08-04 19:12:36 -03:00
current_status - > state_machine = new_state ;
2012-08-13 13:53:37 -03:00
state_machine_publish ( status_pub , current_status , mavlink_fd ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
void state_machine_publish ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd ) {
2012-08-06 18:43:09 -03:00
/* 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 ] ) ;
}
2012-08-04 19:12:36 -03:00
/*
* Private functions , update the state machine
*/
2012-08-13 13:53:37 -03:00
void state_machine_emergency_always_critical ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
fprintf ( stderr , " [commander] EMERGENCY HANDLER \n " ) ;
/* Depending on the current state go to one of the error states */
if ( current_status - > state_machine = = SYSTEM_STATE_PREFLIGHT | | current_status - > state_machine = = SYSTEM_STATE_STANDBY | | current_status - > state_machine = = SYSTEM_STATE_GROUND_READY ) {
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_GROUND_ERROR ) ;
2012-08-04 19:12:36 -03:00
} else if ( current_status - > state_machine = = SYSTEM_STATE_AUTO | | current_status - > state_machine = = SYSTEM_STATE_MANUAL ) {
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_MISSION_ABORT ) ;
2012-08-04 19:12:36 -03:00
} else {
fprintf ( stderr , " [commander] Unknown system state: #%d \n " , current_status - > state_machine ) ;
}
}
2012-08-13 13:53:37 -03:00
void state_machine_emergency ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd ) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
2012-08-04 19:12:36 -03:00
{
if ( current_status - > state_machine ! = SYSTEM_STATE_MANUAL ) { //if we are in manual: user can react to errors themself
2012-08-13 13:53:37 -03:00
state_machine_emergency_always_critical ( status_pub , current_status , mavlink_fd ) ;
2012-08-04 19:12:36 -03:00
} else {
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
}
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
// */
// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
// *
// * START SUBSYSTEM/EMERGENCY FUNCTIONS
// * */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was removed something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was disabled something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //TODO state machine change (recovering)
// break;
// case SUBSYSTEM_TYPE_ACC:
// //TODO state machine change
// break;
// case SUBSYSTEM_TYPE_MAG:
// //TODO state machine change
// break;
// case SUBSYSTEM_TYPE_GPS:
// //TODO state machine change
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
// if (previosly_healthy) //only throw emergency if previously healthy
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
// if (previosly_healthy) //only throw emergency if previously healthy
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
// if (previosly_healthy) //only throw emergency if previously healthy
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// // //TODO: remove this block
// // break;
// // ///////////////////
// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
// // printf("previosly_healthy = %u\n", previosly_healthy);
// if (previosly_healthy) //only throw emergency if previously healthy
// state_machine_emergency(status_pub, current_status);
// break;
// default:
// break;
// }
// }
/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
2012-08-13 13:53:37 -03:00
void update_state_machine_got_position_fix ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
/* Depending on the current state switch state */
if ( current_status - > state_machine = = SYSTEM_STATE_PREFLIGHT ) {
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_STANDBY ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
void update_state_machine_no_position_fix ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
/* Depending on the current state switch state */
if ( current_status - > state_machine = = SYSTEM_STATE_STANDBY | | current_status - > state_machine = = SYSTEM_STATE_GROUND_READY | | current_status - > state_machine = = SYSTEM_STATE_AUTO ) {
2012-08-13 13:53:37 -03:00
state_machine_emergency ( status_pub , current_status , mavlink_fd ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
void update_state_machine_arm ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
2012-08-08 13:47:46 -03:00
// XXX CHANGE BACK
2012-08-04 19:12:36 -03:00
if ( current_status - > state_machine = = SYSTEM_STATE_STANDBY ) {
printf ( " [commander] arming \n " ) ;
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_GROUND_READY ) ;
2012-08-04 19:12:36 -03:00
} /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
printf ( " [commander] landing \n " ) ;
do_state_update ( status_pub , current_status , ( commander_state_machine_t ) SYSTEM_STATE_GROUND_READY ) ;
} */
}
2012-08-13 13:53:37 -03:00
void update_state_machine_disarm ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
if ( current_status - > state_machine = = SYSTEM_STATE_GROUND_READY | | current_status - > state_machine = = SYSTEM_STATE_MANUAL | | current_status - > state_machine = = SYSTEM_STATE_PREFLIGHT ) {
printf ( " [commander] going standby \n " ) ;
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_STANDBY ) ;
2012-08-04 19:12:36 -03:00
} else if ( current_status - > state_machine = = SYSTEM_STATE_STABILIZED | | current_status - > state_machine = = SYSTEM_STATE_AUTO ) {
printf ( " [commander] MISSION ABORT! \n " ) ;
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_STANDBY ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
void update_state_machine_mode_manual ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
2012-08-13 13:53:37 -03:00
int old_mode = current_status - > flight_mode ;
current_status - > flight_mode = VEHICLE_FLIGHT_MODE_MANUAL ;
current_status - > control_manual_enabled = true ;
if ( old_mode ! = current_status - > flight_mode ) state_machine_publish ( status_pub , current_status , mavlink_fd ) ;
2012-08-04 19:12:36 -03:00
if ( current_status - > state_machine = = SYSTEM_STATE_GROUND_READY | | current_status - > state_machine = = SYSTEM_STATE_STABILIZED | | current_status - > state_machine = = SYSTEM_STATE_AUTO ) {
printf ( " [commander] manual mode \n " ) ;
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_MANUAL ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
void update_state_machine_mode_stabilized ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
2012-08-13 13:53:37 -03:00
int old_mode = current_status - > flight_mode ;
current_status - > flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED ;
current_status - > control_manual_enabled = true ;
if ( old_mode ! = current_status - > flight_mode ) state_machine_publish ( status_pub , current_status , mavlink_fd ) ;
2012-08-04 19:12:36 -03:00
if ( current_status - > state_machine = = SYSTEM_STATE_GROUND_READY | | current_status - > state_machine = = SYSTEM_STATE_MANUAL | | current_status - > state_machine = = SYSTEM_STATE_AUTO ) {
printf ( " [commander] stabilized mode \n " ) ;
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_STABILIZED ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
void update_state_machine_mode_auto ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd )
2012-08-04 19:12:36 -03:00
{
2012-08-13 13:53:37 -03:00
int old_mode = current_status - > flight_mode ;
current_status - > flight_mode = VEHICLE_FLIGHT_MODE_AUTO ;
current_status - > control_manual_enabled = true ;
if ( old_mode ! = current_status - > flight_mode ) state_machine_publish ( status_pub , current_status , mavlink_fd ) ;
2012-08-04 19:12:36 -03:00
if ( current_status - > state_machine = = SYSTEM_STATE_GROUND_READY | | current_status - > state_machine = = SYSTEM_STATE_MANUAL | | current_status - > state_machine = = SYSTEM_STATE_STABILIZED ) {
printf ( " [commander] auto mode \n " ) ;
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_AUTO ) ;
2012-08-04 19:12:36 -03:00
}
}
2012-08-13 13:53:37 -03:00
uint8_t update_state_machine_mode_request ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd , uint8_t mode )
2012-08-04 19:12:36 -03:00
{
printf ( " in update state request \n " ) ;
uint8_t ret = 1 ;
2012-08-13 13:53:37 -03:00
current_status - > mode | = VEHICLE_MODE_FLAG_SAFETY_ARMED ;
2012-08-08 13:47:46 -03:00
/* Set manual input enabled flag */
2012-08-13 13:53:37 -03:00
current_status - > mode | = VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED ;
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_GROUND_READY ) ;
2012-08-04 19:12:36 -03:00
2012-08-08 13:47:46 -03:00
/* vehicle is disarmed, mode requests arming */
if ( ! ( current_status - > mode & VEHICLE_MODE_FLAG_SAFETY_ARMED ) & & ( mode & VEHICLE_MODE_FLAG_SAFETY_ARMED ) ) {
/* only arm in standby state */
// XXX REMOVE
if ( current_status - > state_machine = = SYSTEM_STATE_STANDBY | | current_status - > state_machine = = SYSTEM_STATE_PREFLIGHT ) {
2012-08-04 19:12:36 -03:00
/* Set armed flag */
2012-08-13 13:53:37 -03:00
current_status - > mode | = VEHICLE_MODE_FLAG_SAFETY_ARMED ;
2012-08-04 19:12:36 -03:00
/* Set manual input enabled flag */
2012-08-13 13:53:37 -03:00
current_status - > mode | = VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED ;
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_GROUND_READY ) ;
2012-08-04 19:12:36 -03:00
ret = OK ;
2012-08-08 13:47:46 -03:00
printf ( " [commander] arming due to command request \n " ) ;
2012-08-04 19:12:36 -03:00
}
2012-08-08 13:47:46 -03:00
}
2012-08-04 19:12:36 -03:00
2012-08-08 13:47:46 -03:00
/* vehicle is armed, mode requests disarming */
//if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
/* only disarm in ground ready */
//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
2012-08-04 19:12:36 -03:00
/* Clear armed flag, leave manual input enabled */
2012-08-13 13:53:37 -03:00
// current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED;
2012-08-08 13:47:46 -03:00
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[commander] disarming due to command request\n");
//}
//}
2012-08-04 19:12:36 -03:00
2012-08-08 13:47:46 -03:00
/* Switch on HIL if in standby */
2012-08-13 13:53:37 -03:00
if ( ( current_status - > state_machine = = SYSTEM_STATE_STANDBY ) & & ( mode & VEHICLE_MODE_FLAG_HIL_ENABLED ) ) {
2012-08-08 13:47:46 -03:00
/* Enable HIL on request */
2012-08-13 13:53:37 -03:00
current_status - > mode | = VEHICLE_MODE_FLAG_HIL_ENABLED ;
2012-08-08 13:47:46 -03:00
ret = OK ;
2012-08-13 13:53:37 -03:00
state_machine_publish ( status_pub , current_status , mavlink_fd ) ;
2012-08-08 13:47:46 -03:00
printf ( " [commander] Enabling HIL \n " ) ;
2012-08-04 19:12:36 -03:00
}
2012-08-08 13:47:46 -03:00
/* NEVER actually switch off HIL without reboot */
2012-08-13 13:53:37 -03:00
if ( ( current_status - > mode & VEHICLE_MODE_FLAG_HIL_ENABLED ) & & ! ( mode & VEHICLE_MODE_FLAG_HIL_ENABLED ) ) {
2012-08-08 13:47:46 -03:00
fprintf ( stderr , " [commander] DENYING request to switch of HIL. Please power cycle (safety reasons) \n " ) ;
ret = ERROR ;
}
2012-08-04 19:12:36 -03:00
return ret ;
}
2012-08-13 13:53:37 -03:00
uint8_t update_state_machine_custom_mode_request ( int status_pub , struct vehicle_status_s * current_status , const int mavlink_fd , uint8_t custom_mode ) //TODO: add more checks to avoid state switching in critical situations
2012-08-04 19:12:36 -03:00
{
commander_state_machine_t current_system_state = current_status - > state_machine ;
uint8_t ret = 1 ;
switch ( custom_mode ) {
case SYSTEM_STATE_GROUND_READY :
break ;
case SYSTEM_STATE_STANDBY :
break ;
case SYSTEM_STATE_REBOOT :
printf ( " try to reboot \n " ) ;
if ( current_system_state = = SYSTEM_STATE_STANDBY | | current_system_state = = SYSTEM_STATE_PREFLIGHT ) {
printf ( " system will reboot \n " ) ;
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_REBOOT ) ;
2012-08-04 19:12:36 -03:00
ret = 0 ;
}
break ;
case SYSTEM_STATE_AUTO :
printf ( " try to switch to auto/takeoff \n " ) ;
if ( current_system_state = = SYSTEM_STATE_GROUND_READY | | current_system_state = = SYSTEM_STATE_MANUAL ) {
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_AUTO ) ;
2012-08-04 19:12:36 -03:00
printf ( " state: auto \n " ) ;
ret = 0 ;
}
break ;
case SYSTEM_STATE_MANUAL :
printf ( " try to switch to manual \n " ) ;
if ( current_system_state = = SYSTEM_STATE_GROUND_READY | | current_system_state = = SYSTEM_STATE_AUTO ) {
2012-08-13 13:53:37 -03:00
do_state_update ( status_pub , current_status , mavlink_fd , ( commander_state_machine_t ) SYSTEM_STATE_MANUAL ) ;
2012-08-04 19:12:36 -03:00
printf ( " state: manual \n " ) ;
ret = 0 ;
}
break ;
default :
break ;
}
return ret ;
}