fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 21:23: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/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 */