Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 14:17:20 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

added parameters

Browse Source
This commit is contained in:
Thomas Gubler
2012-10-22 18:52:25 +02:00
parent 836c55e122
commit 69185643c0
2 changed files with 82 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_att_control/fixedwing_att_control_main.c
+28
View File
@@ -65,6 +65,34 @@
#include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_rate.h>
#include <fixedwing_att_control_att.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 */ /* Prototypes */
/** /**
* Deamon management function. * Deamon management function.
apps/fixedwing_att_control/fixedwing_att_control_rate.c
+54 -36
View File
@@ -62,41 +62,60 @@
struct fw_rate_control_params { 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; struct fw_rate_control_param_handles {
param_t attrate_i; float rollrate_p;
param_t attrate_d; float rollrate_i;
param_t attrate_awu; float rollrate_awu;
param_t attrate_lim; 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 */ /* Internal Prototypes */
static int parameters_init(struct fw_rate_control_params *h); static int parameters_init(struct fw_rate_control_param_handles *h);
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);
static int parameters_init(struct fw_rate_control_params *h) static int parameters_init(struct fw_rate_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->yawrate_p = param_find("MC_YAWRATE_P"); //TODO define rate params for fixed wing h->rollrate_p = param_find("FW_ROLLRATE_P"); //TODO define rate params for fixed wing
h->yawrate_i = param_find("MC_YAWRATE_I"); h->rollrate_i = param_find("FW_ROLLRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D"); h->rollrate_awu = param_find("FW_ROLLRATE_AWU");
h->yawrate_awu = param_find("MC_YAWRATE_AWU"); h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
h->yawrate_lim = param_find("MC_YAWRATE_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");
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");
// if(h->attrate_i == PARAM_INVALID) // if(h->attrate_i == PARAM_INVALID)
// printf("FATAL MC_ATTRATE_I does not exist\n"); // 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; 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_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i)); 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_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim)); 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; return OK;
} }
@@ -131,7 +149,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static bool initialized = false; static bool initialized = false;
static struct fw_rate_control_params p; 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; 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_init(&h);
parameters_update(&h, &p); 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; initialized = true;
} }
/* load new parameters with lower rate */ /* load new parameters with lower rate */
if (counter % 2500 == 0) { if (counter % 2500 == 0) {
/* update parameters from storage */ /* 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); parameters_update(&h, &p);
} }