diff options
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 263 |
1 files changed, 167 insertions, 96 deletions
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 886ce304e..2fb9ae544 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -54,23 +54,138 @@ #include <systemlib/param/param.h> #include <arch/board/up_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_P, 0.3f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.1f); +PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.3f); + +PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); + +struct mc_att_control_params { + float yaw_p; + float yaw_i; + float yaw_d; + float yaw_awu; + float yaw_lim; + + float yawrate_p; + float yawrate_i; + float yawrate_d; + float yawrate_awu; + float yawrate_lim; + + float att_p; + float att_i; + float att_d; + float att_awu; + float att_lim; + + float att_xoff; + float att_yoff; +}; + +struct mc_att_control_param_handles { + param_t yaw_p; + param_t yaw_i; + param_t yaw_d; + param_t yaw_awu; + param_t yaw_lim; + + param_t yawrate_p; + param_t yawrate_i; + param_t yawrate_d; + param_t yawrate_awu; + param_t yawrate_lim; + + param_t att_p; + param_t att_i; + param_t att_d; + param_t att_awu; + param_t att_lim; + + param_t att_xoff; + param_t att_yoff; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int parameters_init(struct mc_att_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); + + +static int parameters_init(struct mc_att_control_param_handles *h) +{ + /* PID parameters */ + h->yaw_p = param_find("MC_YAWPOS_P"); + h->yaw_i = param_find("MC_YAWPOS_I"); + h->yaw_d = param_find("MC_YAWPOS_D"); + h->yaw_awu = param_find("MC_YAWPOS_AWU"); + h->yaw_lim = param_find("MC_YAWPOS_LIM"); + + h->yawrate_p = param_find("MC_YAWRATE_P"); + h->yawrate_i = param_find("MC_YAWRATE_I"); + h->yawrate_d = param_find("MC_YAWRATE_D"); + h->yawrate_awu = param_find("MC_YAWRATE_AWU"); + h->yawrate_lim = param_find("MC_YAWRATE_LIM"); + + h->att_p = param_find("MC_ATT_P"); + h->att_i = param_find("MC_ATT_I"); + h->att_d = param_find("MC_ATT_D"); + h->att_awu = param_find("MC_ATT_AWU"); + h->att_lim = param_find("MC_ATT_LIM"); + + h->att_xoff = param_find("MC_ATT_XOFF"); + h->att_yoff = param_find("MC_ATT_YOFF"); + + return OK; +} + +static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) +{ + param_get(h->yaw_p, &(p->yaw_p)); + param_get(h->yaw_i, &(p->yaw_i)); + param_get(h->yaw_d, &(p->yaw_d)); + param_get(h->yaw_awu, &(p->yaw_awu)); + param_get(h->yaw_lim, &(p->yaw_lim)); + + param_get(h->yawrate_p, &(p->yawrate_p)); + param_get(h->yawrate_i, &(p->yawrate_i)); + param_get(h->yawrate_d, &(p->yawrate_d)); + param_get(h->yawrate_awu, &(p->yawrate_awu)); + param_get(h->yawrate_lim, &(p->yawrate_lim)); + + param_get(h->att_p, &(p->att_p)); + param_get(h->att_i, &(p->att_i)); + param_get(h->att_d, &(p->att_d)); + param_get(h->att_awu, &(p->att_awu)); + param_get(h->att_lim, &(p->att_lim)); + + param_get(h->att_xoff, &(p->att_xoff)); + param_get(h->att_yoff, &(p->att_yoff)); + + return OK; +} void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, @@ -87,90 +202,49 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s static PID_t pitch_controller; static PID_t roll_controller; - static float pid_yawpos_lim; - static float pid_yawspeed_lim; - static float pid_att_lim; + static struct mc_att_control_params p; + static struct mc_att_control_param_handles h; static bool initialized = false; /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { - - pid_init(&yaw_pos_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU], - PID_MODE_DERIVATIV_CALC, 154); - - pid_init(&yaw_speed_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU], - PID_MODE_DERIVATIV_CALC, 155); - - pid_init(&pitch_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], - PID_MODE_DERIVATIV_SET, 156); - - pid_init(&roll_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], - PID_MODE_DERIVATIV_SET, 157); - - pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; - pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; - pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; + parameters_init(&h); + parameters_update(&h, &p); + + pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, + PID_MODE_DERIVATIV_CALC, 154); + pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu, + PID_MODE_DERIVATIV_CALC, 155); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, + PID_MODE_DERIVATIV_SET, 156); + pid_init(&roll_controller, p.att_d, p.att_i, p.att_d, p.att_awu, + PID_MODE_DERIVATIV_SET, 157); initialized = true; } /* load new parameters with lower rate */ if (motor_skip_counter % 50 == 0) { - pid_set_parameters(&yaw_pos_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); - - pid_set_parameters(&yaw_speed_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]); - - pid_set_parameters(&pitch_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); - - pid_set_parameters(&roll_controller, - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], - global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); - - pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; - pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; - pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; + /* update parameters from storage */ + parameters_update(&h, &p); + /* apply parameters */ + pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu); + pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); } - /*Calculate Controllers*/ - //control Nick - float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET], + /* calculate current control outputs */ + + /* control pitch (forward) output */ + float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, att->pitch, att->pitchspeed, deltaT); - //control Roll - float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET], + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, att->roll, att->rollspeed, deltaT); - //control Yaw Speed - float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); //attitude_setpoint_bodyframe.z is yaw speed! + /* control yaw rate */ + float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); /* * compensate the vertical loss of thrust @@ -195,43 +269,40 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s float motor_thrust = 0.0f; - // FLYING MODES motor_thrust = att_sp->thrust; - //printf("mot0: %3.1f\n", motor_thrust); - /* compensate thrust vector for roll / pitch contributions */ motor_thrust *= zcompensation; /* limit yaw rate output */ - if (yaw_rate_control > pid_yawspeed_lim) { - yaw_rate_control = pid_yawspeed_lim; + if (yaw_rate_control > p.yawrate_lim) { + yaw_rate_control = p.yawrate_lim; yaw_speed_controller.saturated = 1; } - if (yaw_rate_control < -pid_yawspeed_lim) { - yaw_rate_control = -pid_yawspeed_lim; + if (yaw_rate_control < -p.yawrate_lim) { + yaw_rate_control = -p.yawrate_lim; yaw_speed_controller.saturated = 1; } - if (pitch_control > pid_att_lim) { - pitch_control = pid_att_lim; + if (pitch_control > p.att_lim) { + pitch_control = p.att_lim; pitch_controller.saturated = 1; } - if (pitch_control < -pid_att_lim) { - pitch_control = -pid_att_lim; + if (pitch_control < -p.att_lim) { + pitch_control = -p.att_lim; pitch_controller.saturated = 1; } - if (roll_control > pid_att_lim) { - roll_control = pid_att_lim; + if (roll_control > p.att_lim) { + roll_control = p.att_lim; roll_controller.saturated = 1; } - if (roll_control < -pid_att_lim) { - roll_control = -pid_att_lim; + if (roll_control < -p.att_lim) { + roll_control = -p.att_lim; roll_controller.saturated = 1; } |