diff options
Diffstat (limited to 'apps/multirotor_att_control')
3 files changed, 25 insertions, 26 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 91439d36d..10d155ffc 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -252,7 +252,7 @@ mc_thread_main(int argc, char *argv[]) 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_tait_bryan = att.yaw; + att_sp.yaw_body = att.yaw; } static bool rc_loss_first_time = true; @@ -283,29 +283,28 @@ mc_thread_main(int argc, char *argv[]) att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + /* only move setpoint if manual input is != 0 */ if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { - // XXX turn into param - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; - } else if (manual.throttle <= 0.3f) { - att_sp.yaw_body = att.yaw; - } - control_yaw_position = true; - } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { 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 { - rates_sp.yaw = 0.0f; - if(first_time_after_yaw_speed_control) { - att_sp.yaw_tait_bryan = att.yaw; + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; first_time_after_yaw_speed_control = false; } 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; @@ -330,9 +329,7 @@ mc_thread_main(int argc, char *argv[]) /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL, control_yaw_position); - - + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 87eeaced5..e94d7c55d 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -201,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* update parameters from storage */ parameters_update(&h, &p); - //printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); - /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); @@ -220,15 +218,19 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if(control_yaw_position) { /* control yaw rate */ - //rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed); - yaw_error = att->yaw - att_sp->yaw_tait_bryan; - if ((double)yaw_error > M_PI) { - yaw_error -= M_PI; - } else if ((double)yaw_error < -M_PI) { - yaw_error += M_PI; + + /* positive error: rotate to right, negative error, rotate to left (NED frame) */ + // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); + + yaw_error = att_sp->yaw_body - att->yaw; + + if (yaw_error > M_PI_F) { + yaw_error -= M_TWOPI_F; + } else if (yaw_error < -M_PI_F) { + yaw_error += M_TWOPI_F; } - rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); + rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); } rates_sp->thrust = att_sp->thrust; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index f9003e9b4..abc94d617 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -52,6 +52,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ |