fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:23: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/multirotor_att_control/multirotor_att_control_main.c
+50 -35
View File
@@ -232,19 +232,9 @@ mc_thread_main(int argc, char *argv[])
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* decide wether we want rate or position input */
}
else if (state.flag_control_manual_enabled) {
/* manual inputs, from RC control or joystick */
if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
} else if (state.flag_control_manual_enabled) {
if (state.flag_control_attitude_enabled) {
@@ -258,7 +248,7 @@ mc_thread_main(int argc, char *argv[])
static bool rc_loss_first_time = true;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(state.rc_signal_lost) {
if (state.rc_signal_lost) {
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.0f;
@@ -285,41 +275,66 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = att.yaw;
}
/* only move setpoint if manual input is != 0 */
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) {
if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
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;
/*
* This mode SHOULD be the default mode, which is:
* VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
*
* However, we fall back to this setting for all other (nonsense)
* 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;
}
control_yaw_position = true;
}
} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
}
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
}
}
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
/* 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;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* manual rate inputs, from RC control or joystick */
if (state.flag_control_rates_enabled &&
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
rates_sp.roll = manual.roll;
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
}
}