fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 08:20:35 +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/ardrone_interface/ardrone_interface.c
+3 -1
View File
@@ -331,7 +331,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !armed.lockdown) {
//XXX fix this
//if (armed.armed && !armed.lockdown) {
if (state.flag_fmu_armed) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
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);
apps/mavlink/mavlink.c
+6 -4
View File
@@ -230,10 +230,12 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
//
// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
// }
if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
//
// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
//
apps/mavlink_onboard/mavlink.c
+6
View File
@@ -296,6 +296,12 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
if (v_status->flag_control_velocity_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
}
// switch (v_status->state_machine) {
// case SYSTEM_STATE_PREFLIGHT:
// if (v_status->flag_preflight_gyro_calibration ||
apps/mavlink_onboard/mavlink_receiver.c
+1 -1
View File
@@ -328,4 +328,4 @@ receive_start(int uart)
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
return thread;
}
}
apps/multirotor_att_control/multirotor_att_control_main.c
+22 -19
View File
@@ -150,7 +150,9 @@ mc_thread_main(int argc, char *argv[])
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool flag_system_armed = false;
bool flag_fmu_armed = false;
bool flag_control_position_enabled = false;
bool flag_control_velocity_enabled = false;
/* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
@@ -162,7 +164,6 @@ mc_thread_main(int argc, char *argv[])
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@@ -247,7 +248,7 @@ mc_thread_main(int argc, char *argv[])
/* initialize to current yaw if switching to manual or att control */
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
state.flag_fmu_armed != flag_system_armed) {
state.flag_fmu_armed != flag_fmu_armed) {
att_sp.yaw_body = att.yaw;
}
@@ -313,20 +314,20 @@ mc_thread_main(int argc, char *argv[])
// * settings as well.
// */
//
// /* only move setpoint if manual input is != 0 */
// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
// rates_sp.yaw = manual.yaw;
// control_yaw_position = false;
// first_time_after_yaw_speed_control = true;
//
// } else {
// if (first_time_after_yaw_speed_control) {
// att_sp.yaw_body = att.yaw;
// first_time_after_yaw_speed_control = false;
// }
//
// control_yaw_position = true;
// }
/* only move setpoint if manual input is != 0 */
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
first_time_after_yaw_speed_control = true;
} else {
if (first_time_after_yaw_speed_control) {
att_sp.yaw_body = att.yaw;
first_time_after_yaw_speed_control = false;
}
control_yaw_position = true;
}
// }
// }
@@ -337,6 +338,7 @@ mc_thread_main(int argc, char *argv[])
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
@@ -395,8 +397,9 @@ mc_thread_main(int argc, char *argv[])
/* update state */
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
flag_control_manual_enabled = state.flag_control_manual_enabled;
flag_system_armed = state.flag_fmu_armed;
// XXX add some logic to this
flag_control_position_enabled = state.flag_control_position_enabled;
flag_control_velocity_enabled = state.flag_control_velocity_enabled;
flag_fmu_armed = state.flag_fmu_armed;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */