dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Change parameter names in FW control to fit within MAVLink parameter name size limit

This commit is contained in:
Doug Weibel 2012-10-08 14:14:43 -06:00
parent 4fea0a3fc1
commit 77e6375920
1 changed files with 81 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
apps/fixedwing_control/fixedwing_control.c
View File

@ -88,53 +88,53 @@ static void usage(const char *reason);
* *
*/ */
// Roll control parameters // Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLL_RATE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
// Need to add functionality to suppress integrator windup while on the ground // Need to add functionality to suppress integrator windup while on the ground
// Suggested value of FW_ROLL_RATE_I is 0.0 till this is in place // Suggested value of FW_ROLLRATE_I is 0.0 till this is in place
PARAM_DEFINE_FLOAT(FW_ROLL_RATE_I, 0.0f); PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLL_RATE_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLL_RATE_LIMIT, 0.7f); // Roll rate limit in radians/sec PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLL_ANGLE_LIMIT, 0.7f); // Roll angle limit in radians PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
//Pitch control parameters //Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCH_RATE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
// Need to add functionality to suppress integrator windup while on the ground // Need to add functionality to suppress integrator windup while on the ground
// Suggested value of FW_PITCH_RATE_I is 0.0 till this is in place // Suggested value of FW_PITCHRATE_I is 0.0 till this is in place
PARAM_DEFINE_FLOAT(FW_PITCH_RATE_I, 0.0f); PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RATE_AWU, 0.0f); PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RATE_LIMIT, 0.35f); // Pitch rate limit in radians/sec PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_P, 0.3f); PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_PITCH_ANGLE_LIMIT, 0.35f); // Pitch angle limit in radians PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
struct fw_att_control_params { struct fw_att_control_params {
float roll_rate_p; float rollrate_p;
float roll_rate_i; float rollrate_i;
float roll_rate_awu; float rollrate_awu;
float roll_rate_limit; float rollrate_lim;
float roll_angle_p; float roll_p;
float roll_angle_limit; float roll_lim;
float pitch_rate_p; float pitchrate_p;
float pitch_rate_i; float pitchrate_i;
float pitch_rate_awu; float pitchrate_awu;
float pitch_rate_limit; float pitchrate_lim;
float pitch_angle_p; float pitch_p;
float pitch_angle_limit; float pitch_lim;
}; };
struct fw_att_control_param_handles { struct fw_att_control_param_handles {
param_t roll_rate_p; param_t rollrate_p;
param_t roll_rate_i; param_t rollrate_i;
param_t roll_rate_awu; param_t rollrate_awu;
param_t roll_rate_limit; param_t rollrate_lim;
param_t roll_angle_p; param_t roll_p;
param_t roll_angle_limit; param_t roll_lim;
param_t pitch_rate_p; param_t pitchrate_p;
param_t pitch_rate_i; param_t pitchrate_i;
param_t pitch_rate_awu; param_t pitchrate_awu;
param_t pitch_rate_limit; param_t pitchrate_lim;
param_t pitch_angle_p; param_t pitch_p;
param_t pitch_angle_limit; param_t pitch_lim;
}; };
@ -260,18 +260,18 @@ int fixedwing_control_thread_main(int argc, char *argv[])
// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere // TO_DO Fix output limit functionallity of PID controller or add that function elsewhere
PID_t roll_rate_controller; PID_t roll_rate_controller;
pid_init(&roll_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, p.roll_rate_awu, pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu,
p.roll_rate_limit,PID_MODE_DERIVATIV_NONE); p.rollrate_lim,PID_MODE_DERIVATIV_NONE);
PID_t roll_angle_controller; PID_t roll_angle_controller;
pid_init(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, 0.0f, pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f,
p.roll_angle_limit,PID_MODE_DERIVATIV_NONE); p.roll_lim,PID_MODE_DERIVATIV_NONE);
PID_t pitch_rate_controller; PID_t pitch_rate_controller;
pid_init(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, p.pitch_rate_awu, pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu,
p.pitch_rate_limit,PID_MODE_DERIVATIV_NONE); p.pitchrate_lim,PID_MODE_DERIVATIV_NONE);
PID_t pitch_angle_controller; PID_t pitch_angle_controller;
pid_init(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, 0.0f, pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f,
p.pitch_angle_limit,PID_MODE_DERIVATIV_NONE); p.pitch_lim,PID_MODE_DERIVATIV_NONE);
PID_t heading_controller; PID_t heading_controller;
pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
@ -298,18 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[])
} else { } else {
// FIXME SUBSCRIBE // FIXME SUBSCRIBE
if (loopcounter % 20 == 0) { if (loopcounter % 100 == 0) {
att_parameters_update(&h, &p); att_parameters_update(&h, &p);
pos_parameters_update(&hpos, &ppos); pos_parameters_update(&hpos, &ppos);
pid_set_parameters(&roll_rate_controller, p.roll_rate_p, p.roll_rate_i, 0.0f, pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f,
p.roll_rate_awu, p.roll_rate_limit); p.rollrate_awu, p.rollrate_lim);
pid_set_parameters(&roll_angle_controller, p.roll_angle_p, 0.0f, 0.0f, pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f,
0.0f, p.roll_angle_limit); 0.0f, p.roll_lim);
pid_set_parameters(&pitch_rate_controller, p.pitch_rate_p, p.pitch_rate_i, 0.0f, pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f,
p.pitch_rate_awu, p.pitch_rate_limit); p.pitchrate_awu, p.pitchrate_lim);
pid_set_parameters(&pitch_angle_controller, p.pitch_angle_p, 0.0f, 0.0f, pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f,
0.0f, p.pitch_angle_limit); 0.0f, p.pitch_lim);
pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f);
//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n",
//p.rollrate_p, p.rollrate_i, p.rollrate_lim);
} }
/* if position updated, run position control loop */ /* if position updated, run position control loop */
@ -512,36 +514,36 @@ static int att_parameters_init(struct fw_att_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->roll_rate_p = param_find("FW_ROLL_RATE_P"); h->rollrate_p = param_find("FW_ROLLRATE_P");
h->roll_rate_i = param_find(";FW_ROLL_RATE_I"); h->rollrate_i = param_find("FW_ROLLRATE_I");
h->roll_rate_awu = param_find("FW_ROLL_RATE_AWU"); h->rollrate_awu = param_find("FW_ROLLRATE_AWU");
h->roll_rate_limit = param_find("FW_ROLL_RATE_LIMIT"); h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
h->roll_angle_p = param_find("FW_ROLL_ANGLE_P"); h->roll_p = param_find("FW_ROLL_P");
h->roll_angle_limit = param_find("FW_ROLL_ANGLE_LIMIT"); h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_rate_p = param_find("FW_PITCH_RATE_P"); h->pitchrate_p = param_find("FW_PITCHRATE_P");
h->pitch_rate_i = param_find("FW_PITCH_RATE_I"); h->pitchrate_i = param_find("FW_PITCHRATE_I");
h->pitch_rate_awu = param_find("FW_PITCH_RATE_AWU"); h->pitchrate_awu = param_find("FW_PITCHRATE_AWU");
h->pitch_rate_limit = param_find("FW_PITCH_RATE_LIMIT"); h->pitchrate_lim = param_find("FW_PITCHRATE_LIM");
h->pitch_angle_p = param_find("FW_PITCH_ANGLE_P"); h->pitch_p = param_find("FW_PITCH_P");
h->pitch_angle_p = param_find("FW_PITCH_ANGLE_LIMIT"); h->pitch_lim = param_find("FW_PITCH_LIM");
return OK; return OK;
} }
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p)
{ {
param_get(h->roll_rate_p, &(p->roll_rate_p)); param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->roll_rate_i, &(p->roll_rate_i)); param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->roll_rate_awu, &(p->roll_rate_awu)); param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->roll_rate_limit, &(p->roll_rate_limit)); param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->roll_angle_p, &(p->roll_angle_p)); param_get(h->roll_p, &(p->roll_p));
param_get(h->roll_angle_limit, &(p->roll_angle_limit)); param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitch_rate_p, &(p->pitch_rate_p)); param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitch_rate_i, &(p->pitch_rate_i)); param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitch_rate_awu, &(p->pitch_rate_awu)); param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->pitch_rate_limit, &(p->pitch_rate_limit)); param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->pitch_angle_p, &(p->pitch_angle_p)); param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitch_angle_limit, &(p->pitch_angle_limit)); param_get(h->pitch_lim, &(p->pitch_lim));
return OK; return OK;
} }