diff options
author | Anton Babushkin <rk3dov@gmail.com> | 2013-04-20 12:33:02 +0400 |
---|---|---|
committer | Anton Babushkin <rk3dov@gmail.com> | 2013-04-20 12:33:02 +0400 |
commit | 57cca9dbb41a1803c3cdb61151947e99625c59cb (patch) | |
tree | 879888272d94efd8918fddfaefa1e2e377e38d60 | |
parent | 5abae2c78d0f7f41e1340fe5e396936da2c7a580 (diff) | |
download | px4-firmware-57cca9dbb41a1803c3cdb61151947e99625c59cb.tar.gz px4-firmware-57cca9dbb41a1803c3cdb61151947e99625c59cb.tar.bz2 px4-firmware-57cca9dbb41a1803c3cdb61151947e99625c59cb.zip |
Abs yaw and other bugs fixed
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 199 | ||||
-rw-r--r-- | apps/multirotor_pos_control/multirotor_pos_control.c | 152 |
2 files changed, 194 insertions, 157 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 09710f0fc..258299828 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -241,123 +241,140 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { + /* Control attitude, update attitude setpoint depending on SAS mode: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS: roll, pitch, yaw, thrust + * VEHICLE_MANUAL_SAS_MODE_ALTITUDE: roll, pitch, yaw + * VEHICLE_MANUAL_SAS_MODE_SIMPLE: yaw + * */ + + /* 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_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { - /* direct manual input */ - if (state.flag_control_attitude_enabled) { - - /* 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_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; + 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) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* Don't reset attitude setpoint in SIMPLE SAS mode, it's handled by position controller. */ 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; + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* Don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * 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) { + /* 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.thrust = failsafe_throttle; - } else { - att_sp.thrust = 0.0f; + } 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; + /* 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 = false; + rc_loss_first_time = false; - } else { - rc_loss_first_time = true; + } else { + rc_loss_first_time = true; - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + /* control yaw in all SAS modes */ + /* set yaw if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; - } + /* 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) { - /* 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; - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + } 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) && ( + state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS || + manual.throttle > 0.3f)) { rates_sp.yaw = manual.yaw; control_yaw_position = false; + first_time_after_yaw_speed_control = true; } 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; + 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 (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(); + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* don't update attitude setpoint in SIMPLE SAS mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* don't set throttle in alt hold modes */ + att_sp.thrust = manual.throttle; + } } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + att_sp.timestamp = hrt_absolute_time(); + } - } 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(); - } + 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 controller output */ + 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(); } } } diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index d2b6685ac..7757a78c1 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -162,6 +162,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -186,18 +187,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int paramcheck_counter = 0; while (!thread_should_exit) { - /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &status); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (status.state_machine == SYSTEM_STATE_AUTO) { - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - } /* check parameters at 1 Hz*/ paramcheck_counter++; @@ -210,26 +200,58 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } - if (status.state_machine == SYSTEM_STATE_MANUAL) { - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - hrt_abstime t = hrt_absolute_time(); - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); - } + /* Check if controller should act */ + bool act = status.flag_system_armed && ( + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); + + if (act) { + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + } - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - /* move altitude setpoint by manual controls */ + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } + + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + reset_sp_pos = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + char str[80]; + sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, str); + } + + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + + if (status.flag_control_manual_enabled) { + /* move altitude setpoint with manual controls */ float alt_ctl = manual.throttle - 0.5f; if (fabs(alt_ctl) < alt_ctl_dz) { alt_ctl = 0.0f; @@ -245,48 +267,46 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { local_pos_sp.z = local_pos.z - err_linear_limit; } - /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } - /* PID for altitude rate */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; - } else { - /* integrate only in linear area (with 20% gap) and not on min/max throttle */ - if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO add position controller - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; - } else { - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; + // TODO move position setpoint with manual controls } - att_sp.thrust = thrust_ctl; - att_sp.timestamp = hrt_absolute_time(); - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; + } + + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* P and D components */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + // TODO add position controller } else { - reset_sp_alt = true; + reset_sp_pos = true; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { + /* publish local position setpoint in manual mode */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; } else { reset_sp_alt = true; + reset_sp_pos = true; } /* run at approximately 50 Hz */ |