zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Added feedforward throttle to pitch compensation, heading from position controller still broken

sbg
Lorenz Meier 12 years ago
parent
cc1e0ef235
commit
54d624f7c7
4 changed files with 62 additions and 61 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      apps/fixedwing_att_control/fixedwing_att_control_att.c
  2. 25
      apps/fixedwing_att_control/fixedwing_att_control_main.c
  3. 55
      apps/fixedwing_att_control/fixedwing_att_control_rate.c
  4. 38
      apps/fixedwing_pos_control/fixedwing_pos_control_main.c

5
apps/fixedwing_att_control/fixedwing_att_control_att.c
Unescape View File

@ -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 */ /* 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)); float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
/* set pitch plus feedforward roll compensation */ /* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, rates_sp->pitch = pid_calculate(&pitch_controller,
att->pitch, 0, 0); att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */ /* 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)) 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))

25
apps/fixedwing_att_control/fixedwing_att_control_main.c
Unescape View File

@ -68,31 +68,6 @@
#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_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 */ /* Prototypes */
/** /**
* Deamon management function. * Deamon management function.

55
apps/fixedwing_att_control/fixedwing_att_control_rate.c
Unescape View File

@ -61,9 +61,33 @@
#include <systemlib/geo/geo.h> #include <systemlib/geo/geo.h>
#include <systemlib/systemlib.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 { struct fw_rate_control_params {
float rollrate_p; float rollrate_p;
@ -75,7 +99,7 @@ struct fw_rate_control_params {
float yawrate_p; float yawrate_p;
float yawrate_i; float yawrate_i;
float yawrate_awu; float yawrate_awu;
float pitch_thr_ff;
}; };
struct fw_rate_control_param_handles { struct fw_rate_control_param_handles {
@ -88,7 +112,7 @@ struct fw_rate_control_param_handles {
param_t yawrate_p; param_t yawrate_p;
param_t yawrate_i; param_t yawrate_i;
param_t yawrate_awu; 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) static int parameters_init(struct fw_rate_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing 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_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU"); h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->pitchrate_p = param_find("FW_PITCHR_P"); h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I"); h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHR_AWU"); h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->yawrate_p = param_find("FW_YAWR_P"); h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I"); h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU"); h->yawrate_awu = param_find("FW_YAWR_AWU");
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
return OK; 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_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_awu, &(p->yawrate_awu)); param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
return OK; 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); actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
/* pitch rate (PI) */ /* pitch rate (PI) */
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); 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) */ /* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);

38
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
Unescape View File

@ -75,8 +75,7 @@ PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
struct fw_pos_control_params { struct fw_pos_control_params {
float heading_p; float heading_p;
@ -98,7 +97,6 @@ struct fw_pos_control_param_handles {
param_t altitude_p; param_t altitude_p;
param_t roll_lim; param_t roll_lim;
param_t pitch_lim; param_t pitch_lim;
}; };
@ -147,15 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */
static int parameters_init(struct fw_pos_control_param_handles *h) static int parameters_init(struct fw_pos_control_param_handles *h)
{ {
/* PID parameters */ /* PID parameters */
h->heading_p = param_find("FW_HEAD_P"); h->heading_p = param_find("FW_HEAD_P");
h->headingr_p = param_find("FW_HEADR_P"); h->headingr_p = param_find("FW_HEADR_P");
h->headingr_i = param_find("FW_HEADR_I"); h->headingr_i = param_find("FW_HEADR_I");
h->headingr_lim = param_find("FW_HEADR_LIM"); h->headingr_lim = param_find("FW_HEADR_LIM");
h->xtrack_p = param_find("FW_XTRACK_P"); h->xtrack_p = param_find("FW_XTRACK_P");
h->altitude_p = param_find("FW_ALT_P"); h->altitude_p = param_find("FW_ALT_P");
h->roll_lim = param_find("FW_ROLL_LIM"); h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM"); h->pitch_lim = param_find("FW_PITCH_LIM");
return OK; return OK;
} }
@ -325,7 +322,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
if(!(distance_res != OK || xtrack_err.past_end)) { // XXX what is xtrack_err.past_end?
if(distance_res == OK /*&& !xtrack_err.past_end*/) {
// printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); // printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
@ -360,19 +358,17 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// printf("psi_rate_c %.4f ", (double)psi_rate_c); // printf("psi_rate_c %.4f ", (double)psi_rate_c);
float psi_rate_e = psi_rate_c - att.yawspeed; float psi_rate_e = psi_rate_c - att.yawspeed;
float psi_rate_e_scaled = psi_rate_e * sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) / 9.81f; //* V_gr / g
float psi_rate_e_scaled = psi_rate_e * sqrtf(powf(global_pos.vx, 2.0f) + powf(global_pos.vy, 2.0f)) / 9.81f; //* V_gr / g
// printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); // printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);\ attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
// printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); // printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
// if (counter % 100 == 0) if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
} } else {
else {
if (counter % 100 == 0) if (counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
} }
@ -395,6 +391,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
/* measure in what intervals the controller runs */ /* measure in what intervals the controller runs */
perf_count(fw_interval_perf); perf_count(fw_interval_perf);
counter++;
} else { } else {
// XXX no setpoint, decent default needed (loiter?) // XXX no setpoint, decent default needed (loiter?)
} }

Write Preview
Loading…
Cancel
Save