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

Checkpoint: implement new state machine, compiling, WIP

Browse Source
This commit is contained in:
Julian Oes
2013-02-16 13:40:09 -08:00
parent 2d1009a897
commit 0e2db0beb9
14 changed files with 920 additions and 936 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/multirotor_att_control/multirotor_att_control_main.c
+43 -41
View File
@@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[])
}
/* 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 (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
rates_sp.yaw = manual.yaw;
control_yaw_position = false;
} else {
/*
* 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;
}
}
}
#warning fix this
// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
//
// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
// rates_sp.yaw = manual.yaw;
// control_yaw_position = false;
//
// } else {
// /*
// * 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;
// }
// }
// }
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
@@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[])
}
} else {
#warning fix this
/* 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();
}
// 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();
// }
}
}