fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rate controller update

Browse Source
This commit is contained in:
tnaegeli 2012-10-03 15:05:50 +02:00
parent b5d2ec3d92
commit f3cb2cf8a3
2 changed files with 43 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -104,14 +104,14 @@ static void *rate_control_thread_main(void *arg)
while (!thread_should_exit) {
/* rate control at maximum rate */
/* wait for a sensor update, check for exit condition every 1000 ms */
int ret = poll(&fds, 1, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
} else {
// int ret = poll(&fds, 1, 1000);
//
// if (ret < 0) {
// /* XXX this is seriously bad - should be an emergency */
// } else if (ret == 0) {
// /* XXX this means no sensor data - should be critical or emergency */
// printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
// } else {
/* get data */
orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
bool rates_sp_valid = false;
@ -133,7 +133,8 @@ static void *rate_control_thread_main(void *arg)
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
// }
usleep(2000);
}
return NULL;

120
apps/multirotor_att_control/multirotor_rate_control.c
View File

@ -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 */
float setpointXrate;
float setpointYrate;
float setpointZrate;
float setRollRate=rate_sp->roll;
float setPitchRate=rate_sp->pitch;
float setYawRate=rate_sp->yaw;
/* 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;
}
//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++;
}