diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-27 18:11:48 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-27 18:11:48 +0100 |
commit | 54d624f7c7b5e57d6ec7747056ba2a795ef47dd0 (patch) | |
tree | 373c08df65da4f564697bdc6bb263e77a5652a2c /apps/fixedwing_att_control | |
parent | cc1e0ef2356fd3fe24726386add7e6f7106574b6 (diff) | |
download | px4-firmware-54d624f7c7b5e57d6ec7747056ba2a795ef47dd0.tar.gz px4-firmware-54d624f7c7b5e57d6ec7747056ba2a795ef47dd0.tar.bz2 px4-firmware-54d624f7c7b5e57d6ec7747056ba2a795ef47dd0.zip |
Added feedforward throttle to pitch compensation, heading from position controller still broken
Diffstat (limited to 'apps/fixedwing_att_control')
3 files changed, 44 insertions, 41 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index d27f50b43..957036b41 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -152,8 +152,9 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); /* set pitch plus feedforward roll compensation */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + rates_sp->pitch = pid_calculate(&pitch_controller, + att_sp->pitch_body + pitch_sp_rollcompensation, + att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 8773db9b6..8ca87ab53 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -68,31 +68,6 @@ #include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_att.h> -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - /* Prototypes */ /** * Deamon management function. diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index e8277f3de..f3e36c0ec 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -61,9 +61,33 @@ #include <systemlib/geo/geo.h> #include <systemlib/systemlib.h> - - - +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller +PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller +PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec + +/* feedforward compensation */ +PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ struct fw_rate_control_params { float rollrate_p; @@ -75,7 +99,7 @@ struct fw_rate_control_params { float yawrate_p; float yawrate_i; float yawrate_awu; - + float pitch_thr_ff; }; struct fw_rate_control_param_handles { @@ -88,7 +112,7 @@ struct fw_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_awu; - + param_t pitch_thr_ff; }; @@ -100,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); return OK; } @@ -126,7 +151,7 @@ static int parameters_update(const struct fw_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_awu, &(p->yawrate_awu)); - + param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); return OK; } @@ -173,6 +198,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); /* pitch rate (PI) */ actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + /* set pitch minus feedforward throttle compensation (nose pitches up from throttle */ + actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust; /* yaw rate (PI) */ actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); |