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

Cleaned up control mode state machine / flight mode / navigation state machine still needs a bit cleaning up

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-27 18:27:08 +01:00
parent 61d7e1d285
commit f5bad08bd0
10 changed files with 625 additions and 300 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_att_control/fixedwing_att_control_main.c
+51 -34
View File
@@ -195,44 +195,61 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* set flaps to zero */
actuators.control[4] = 0.0f;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = 0.5f;
} else {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// XXX disable yaw control, loiter
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = 0.5f;
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
att_sp.timestamp = hrt_absolute_time();
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
att_sp.timestamp = hrt_absolute_time();
}
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
}
/* publish rates */