diff options
author | Julian Oes <julian@oes.ch> | 2013-06-24 17:24:49 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-06-24 17:24:49 +0200 |
commit | 794a2248f02e014bc81e977da5a4eee7d71902b5 (patch) | |
tree | 190d1522d9924c939cd1c26bb148a1905022d835 | |
parent | d563960267ab1145d5100f9dcfad6035205e4021 (diff) | |
download | px4-firmware-794a2248f02e014bc81e977da5a4eee7d71902b5.tar.gz px4-firmware-794a2248f02e014bc81e977da5a4eee7d71902b5.tar.bz2 px4-firmware-794a2248f02e014bc81e977da5a4eee7d71902b5.zip |
Only some small cleanup to att control
-rw-r--r-- | src/modules/multirotor_att_control/multirotor_att_control_main.c | 100 |
1 files changed, 23 insertions, 77 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 4467d2d82..25447aaba 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -256,7 +256,7 @@ mc_thread_main(int argc, char *argv[]) } - } else if (control_mode.flag_control_manual_enabled) { + } else if (control_mode.flag_control_manual_enabled) { if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || @@ -268,90 +268,34 @@ 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 */ -/* XXX fix this */ -#if 0 - 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; - att_sp.pitch_body = 0.0f; - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; + rc_loss_first_time = true; - rc_loss_first_time = false; + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - } else { -#endif - rc_loss_first_time = true; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && safety.armed) { + att_sp.yaw_body = att.yaw; + } - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; - /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + } else { + if (first_time_after_yaw_speed_control) { att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ -#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.1f) { - 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(); -#if 0 + control_yaw_position = true; } -#endif + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -369,7 +313,9 @@ mc_thread_main(int argc, char *argv[]) } } else { -#warning fix this + + //XXX TODO add acro mode here + /* manual rate inputs, from RC control or joystick */ // if (state.flag_control_rates_enabled && // state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { |