diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index d27f50b437..957036b415 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 8773db9b6c..8ca87ab53f 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 #include -/* - * 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 e8277f3de3..f3e36c0ec3 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 #include - - - +/* + * 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); diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 5bb794cad5..1d7e9ccd2f 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -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_ROLL_LIM, 0.7f); // Roll angle limit in radians 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 { float heading_p; @@ -98,7 +97,6 @@ struct fw_pos_control_param_handles { param_t altitude_p; param_t roll_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) { /* PID parameters */ - h->heading_p = param_find("FW_HEAD_P"); - h->headingr_p = param_find("FW_HEADR_P"); - h->headingr_i = param_find("FW_HEADR_I"); + h->heading_p = param_find("FW_HEAD_P"); + h->headingr_p = param_find("FW_HEADR_P"); + h->headingr_i = param_find("FW_HEADR_I"); h->headingr_lim = param_find("FW_HEADR_LIM"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - + h->xtrack_p = param_find("FW_XTRACK_P"); + h->altitude_p = param_find("FW_ALT_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); 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)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); @@ -360,19 +358,17 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // printf("psi_rate_c %.4f ", (double)psi_rate_c); float psi_rate_e = psi_rate_c - att.yawspeed; - - 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 + 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 // 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) - // printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); - } - else { + if (counter % 100 == 0) + printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); + } else { if (counter % 100 == 0) 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 */ perf_count(fw_interval_perf); + counter++; + } else { // XXX no setpoint, decent default needed (loiter?) }