diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2012-10-22 18:52:25 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-10-22 18:52:25 +0200 |
commit | 69185643c0bb96cae1daa08ceb052e884a2c2ed1 (patch) | |
tree | c4d78be61a9c0f33cb75af7b375822a4ca39fdc9 /apps/fixedwing_att_control | |
parent | 836c55e1225b378e7e0e4fb4841024b6b09e7243 (diff) | |
download | px4-firmware-69185643c0bb96cae1daa08ceb052e884a2c2ed1.tar.gz px4-firmware-69185643c0bb96cae1daa08ceb052e884a2c2ed1.tar.bz2 px4-firmware-69185643c0bb96cae1daa08ceb052e884a2c2ed1.zip |
added parameters
Diffstat (limited to 'apps/fixedwing_att_control')
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 28 | ||||
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_rate.c | 94 |
2 files changed, 84 insertions, 38 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 9bf984d1d..a6dd1c9fb 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -65,6 +65,34 @@ #include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_att.h> +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWRATE_LIM, 0.35f); // Yaw rate limit in radians/sec +PARAM_DEFINE_FLOAT(FW_YAW_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAW_LIM, 0.35f); // Yaw angle limit in radians + /* 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 48892e4aa..e6f5c6857 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -62,41 +62,60 @@ + struct fw_rate_control_params { + float rollrate_p; + float rollrate_i; + float rollrate_awu; + float rollrate_lim; + float pitchrate_p; + float pitchrate_i; + float pitchrate_awu; + float pitchrate_lim; + float yawrate_p; + float yawrate_i; + float yawrate_awu; + float yawrate_lim; + +}; - 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; +struct fw_rate_control_param_handles { + float rollrate_p; + float rollrate_i; + float rollrate_awu; + float rollrate_lim; + float pitchrate_p; + float pitchrate_i; + float pitchrate_awu; + float pitchrate_lim; + float yawrate_p; + float yawrate_i; + float yawrate_awu; + float yawrate_lim; }; /* Internal Prototypes */ -static int parameters_init(struct fw_rate_control_params *h); -static int parameters_update(const struct fw_rate_control_params *h, struct fw_rate_control_params *p); +static int parameters_init(struct fw_rate_control_param_handles *h); +static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p); -static int parameters_init(struct fw_rate_control_params *h) +static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->yawrate_p = param_find("MC_YAWRATE_P"); //TODO define rate params for fixed wing - 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"); + h->rollrate_p = param_find("FW_ROLLRATE_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLRATE_I"); + h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); + h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); + h->pitchrate_p = param_find("FW_PITCHRATE_P"); + h->pitchrate_i = param_find("FW_PITCHRATE_I"); + h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); + h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); + h->yawrate_p = param_find("FW_YAWRATE_P"); + h->yawrate_i = param_find("FW_YAWRATE_I"); + h->yawrate_awu = param_find("FW_YAWRATE_AWU"); + h->yawrate_lim = param_find("FW_YAWRATE_LIM"); + // if(h->attrate_i == PARAM_INVALID) // printf("FATAL MC_ATTRATE_I does not exist\n"); @@ -104,22 +123,21 @@ static int parameters_init(struct fw_rate_control_params *h) return OK; } -static int parameters_update(const struct fw_rate_control_params *h, struct fw_rate_control_params *p) +static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p) { + param_get(h->rollrate_p, &(p->rollrate_p)); + param_get(h->rollrate_i, &(p->rollrate_i)); + param_get(h->rollrate_awu, &(p->rollrate_awu)); + param_get(h->rollrate_lim, &(p->rollrate_lim)); + param_get(h->pitchrate_p, &(p->pitchrate_p)); + param_get(h->pitchrate_i, &(p->pitchrate_i)); + param_get(h->pitchrate_awu, &(p->pitchrate_awu)); + param_get(h->pitchrate_lim, &(p->pitchrate_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->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)); - - p->attrate_i = 0.01f; //TODO: as long as the parameter is not implemented - return OK; } @@ -131,7 +149,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; static struct fw_rate_control_params p; - static struct fw_rate_control_params h; + static struct fw_rate_control_param_handles h; static PID_t roll_rate_controller; @@ -143,14 +161,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, PID_MODE_DERIVATIV_SET); //D part set to 0 because the controller layout is with a PI rate controller + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim, PID_MODE_DERIVATIV_SET); // set D part to 0 because the controller layout is with a PI rate controller initialized = true; } /* load new parameters with lower rate */ if (counter % 2500 == 0) { /* update parameters from storage */ - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim); parameters_update(&h, &p); } |