fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Checkpoint: More state machine fixes, commited to wrong branch and now copied over

Browse Source
This commit is contained in:
Julian Oes
2013-03-11 10:39:57 -07:00
parent f0d8ce009e
commit 0fe5aeb02c
7 changed files with 86 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+24 -8
View File
@@ -60,13 +60,9 @@
#include <debug.h>
#include <sys/prctl.h>
#include <string.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include "state_machine_helper.h"
#include "systemlib/systemlib.h"
#include <math.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/battery_status.h>
@@ -80,11 +76,19 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <mavlink/mavlink_log.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include "state_machine_helper.h"
/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
#include <drivers/drv_accel.h>
@@ -101,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
#include <systemlib/cpuload.h>
extern struct system_load_s system_load;
/* Decouple update interval and hysteris counters, all depends on intervals */
@@ -120,6 +124,8 @@ extern struct system_load_s system_load;
#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
/* File descriptors */
static int leds;
static int buzzer;
@@ -1324,9 +1330,12 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.navigation_state = NAVIGATION_STATE_INIT;
current_status.arming_state = ARMING_STATE_INIT;
current_status.hil_state = HIL_STATE_OFF;
current_status.flag_hil_enabled = false;
current_status.flag_fmu_armed = false;
current_status.flag_io_armed = false; // XXX read this from somewhere
@@ -1542,6 +1551,13 @@ int commander_thread_main(int argc, char *argv[])
last_local_position_time = local_position.timestamp;
}
/* set the condition to valid if there has recently been a local position estimate */
if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) {
current_status.condition_local_position_valid = true;
} else {
current_status.condition_local_position_valid = false;
}
/* update battery status */
orb_check(battery_sub, &new_data);
if (new_data) {
@@ -1565,7 +1581,7 @@ int commander_thread_main(int argc, char *argv[])
// current_status.state_machine == SYSTEM_STATE_AUTO ||
// current_status.state_machine == SYSTEM_STATE_MANUAL)) {
// /* armed */
// led_toggle(LED_BLUE);
led_toggle(LED_BLUE);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* not armed */
apps/commander/state_machine_helper.c
+24 -10
View File
@@ -40,19 +40,20 @@
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) {
int ret = ERROR;
@@ -68,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* allow going back from INIT for calibration */
if (current_state->arming_state == ARMING_STATE_STANDBY) {
ret = OK;
current_state->flag_system_armed = false;
current_state->flag_fmu_armed = false;
}
break;
case ARMING_STATE_STANDBY:
@@ -80,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* sensors need to be initialized for STANDBY state */
if (current_state->condition_system_sensors_initialized) {
ret = OK;
current_state->flag_system_armed = false;
current_state->flag_fmu_armed = false;
} else {
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
}
@@ -94,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* XXX conditions for arming? */
ret = OK;
current_state->flag_system_armed = true;
current_state->flag_fmu_armed = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
@@ -104,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* XXX conditions for an error state? */
ret = OK;
current_state->flag_system_armed = true;
current_state->flag_fmu_armed = true;
}
break;
case ARMING_STATE_STANDBY_ERROR:
@@ -113,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|| current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = OK;
current_state->flag_system_armed = false;
current_state->flag_fmu_armed = false;
}
break;
default:
@@ -157,6 +158,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = false;
current_state->flag_control_velocity_enabled = false;
current_state->flag_control_position_enabled = false;
current_state->flag_control_manual_enabled = false;
}
break;
@@ -177,6 +179,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = false;
current_state->flag_control_position_enabled = false;
current_state->flag_control_manual_enabled = true;
}
}
break;
@@ -192,6 +195,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = false;
current_state->flag_control_position_enabled = false;
current_state->flag_control_manual_enabled = true;
}
break;
@@ -215,6 +219,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = false;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -243,6 +248,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = false;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -270,6 +276,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = false;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -295,6 +302,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -316,6 +324,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -330,6 +339,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
break;
@@ -353,6 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -375,6 +386,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -399,6 +411,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -420,6 +433,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -530,7 +544,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
//void publish_armed_status(const struct vehicle_status_s *current_status)
//{
// struct actuator_armed_s armed;
// armed.armed = current_status->flag_system_armed;
// armed.armed = current_status->flag_fmu_armed;
// /* 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);
@@ -669,7 +683,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
////
//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
//// current_status->flag_system_armed) {
//// current_status->flag_fmu_armed) {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
////
@@ -694,7 +708,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
// }
//
// /* vehicle is disarmed, mode requests arming */
// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// if (!(current_status->flag_fmu_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) {
@@ -705,7 +719,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
// }
//
// /* vehicle is armed, mode requests disarming */
// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only disarm in ground ready */
// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);