fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 07:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moving nav state from commander to navigator, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-29 12:16:49 +04:00
parent 72d9c80ce9
commit 7e29028429
11 changed files with 249 additions and 692 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/state_machine_helper.cpp
+7 -182
View File
@@ -46,7 +46,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -64,12 +63,10 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool navigation_state_changed = true;
static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
const struct vehicle_control_mode_s *control_mode,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
/*
@@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
} else {
/* enforce lockdown in HIL */
if (control_mode->flag_system_hil_enabled) {
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
@@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED
|| control_mode->flag_system_hil_enabled) {
|| status->hil_state == HIL_STATE_ON) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
@@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
&& (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
&& (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
@@ -289,169 +286,6 @@ check_main_state_changed()
}
}
transition_result_t
navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == status->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_STABILIZE:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_ALTHOLD:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_VECTOR:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_AUTO_READY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_LOITER:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_RTL:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed state */
if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
}
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
status->navigation_state = new_navigation_state;
control_mode->auto_state = status->navigation_state;
navigation_state_changed = true;
}
}
return ret;
}
bool
check_navigation_state_changed()
{
if (navigation_state_changed) {
navigation_state_changed = false;
return true;
} else {
return false;
}
}
bool
check_flighttermination_state_changed()
{
@@ -464,16 +298,10 @@ check_flighttermination_state_changed()
}
}
void
set_navigation_state_changed()
{
navigation_state_changed = true;
}
/**
* Transition from one hil state to another
*/
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
@@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
@@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
current_control_mode->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
// XXX also set lockdown here
ret = OK;
@@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/**
* Transition from one flightermination state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}