diff options
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 120 |
1 files changed, 33 insertions, 87 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index b4eb3469b..2a57f93f0 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -56,18 +56,21 @@ // PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; + float yawrate_d; float yawrate_i; float yawrate_awu; float yawrate_lim; float attrate_p; + float attrate_d; float attrate_i; float attrate_awu; float attrate_lim; @@ -79,11 +82,13 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; + param_t yawrate_d; param_t yawrate_awu; param_t yawrate_lim; param_t attrate_p; param_t attrate_i; + param_t attrate_d; param_t attrate_awu; param_t attrate_lim; }; @@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h) /* PID parameters */ 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->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); + h->attrate_d = param_find("MC_ATTRATE_D"); h->attrate_awu = param_find("MC_ATTRATE_AWU"); h->attrate_lim = param_find("MC_ATTRATE_LIM"); @@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru { 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->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); + param_get(h->attrate_d, &(p->attrate_d)); param_get(h->attrate_awu, &(p->attrate_awu)); param_get(h->attrate_lim, &(p->attrate_lim)); @@ -151,106 +160,43 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - - pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - - initialized = true; - } /* load new parameters with lower rate */ - if (motor_skip_counter % 250 == 0) { + + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); /* apply parameters */ + + + pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu); pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); } /* calculate current control outputs */ - - /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, - rates[1], 0.0f, deltaT); - /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll, - rates[0], 0.0f, deltaT); - /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - /* - * compensate the vertical loss of thrust - * when thrust plane has an angle. - * start with a factor of 1.0 (no change) - */ - float zcompensation = 1.0f; - - // if (fabsf(att->roll) > 0.3f) { - // zcompensation *= 1.04675160154f; - - // } else { - // zcompensation *= 1.0f / cosf(att->roll); - // } - - // if (fabsf(att->pitch) > 0.3f) { - // zcompensation *= 1.04675160154f; - - // } else { - // zcompensation *= 1.0f / cosf(att->pitch); - // } - - float motor_thrust = 0.0f; - - motor_thrust = rate_sp->thrust; - - /* compensate thrust vector for roll / pitch contributions */ - motor_thrust *= zcompensation; - - /* limit yaw rate output */ - if (yaw_rate_control > p.yawrate_lim) { - yaw_rate_control = p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } - - if (yaw_rate_control < -p.yawrate_lim) { - yaw_rate_control = -p.yawrate_lim; - yaw_speed_controller.saturated = 1; - } - - if (pitch_control > p.attrate_lim) { - pitch_control = p.attrate_lim; - pitch_controller.saturated = 1; - } - - if (pitch_control < -p.attrate_lim) { - pitch_control = -p.attrate_lim; - pitch_controller.saturated = 1; - } + float setpointXrate; + float setpointYrate; + float setpointZrate; + float setRollRate=rate_sp->roll; + float setPitchRate=rate_sp->pitch; + float setYawRate=rate_sp->yaw; + + //x-axis + setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]); + //Y-axis + setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]); + //Z-axis + setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]); - if (roll_control > p.attrate_lim) { - roll_control = p.attrate_lim; - roll_controller.saturated = 1; - } - if (roll_control < -p.attrate_lim) { - roll_control = -p.attrate_lim; - roll_controller.saturated = 1; - } - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; + actuators->control[0] = setpointXrate; //roll + actuators->control[1] = setpointYrate; //pitch + actuators->control[2] = setpointZrate; //yaw + actuators->control[3] = rate_sp->thrust; motor_skip_counter++; } |