diff options
4 files changed, 45 insertions, 23 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 7d648ccfa..b13d564d6 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -35,7 +35,7 @@ * @file fixedwing_att_control_rate.c * Implementation of a fixed wing attitude controller. */ -#include <fixedwing_att_control_rate.h> +#include <fixedwing_att_control_att.h> #include <nuttx/config.h> #include <stdio.h> @@ -63,30 +63,42 @@ struct fw_att_control_params { - param_t roll_p; + float roll_p; + float roll_lim; + float pitch_p; + float pitch_lim; +}; + +struct fw_att_control_params_handles { + float roll_p; + float roll_lim; + float pitch_p; + float pitch_lim; }; /* Internal Prototypes */ -static int parameters_init(struct fw_att_control_params *h); -static int parameters_update(const struct fw_att_control_params *h, struct fw_att_control_params *p); +static int parameters_init(struct fw_att_control_params_handles *h); +static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p); -static int parameters_init(struct fw_att_control_params *h) +static int parameters_init(struct fw_att_control_params_handles *h) { /* PID parameters */ - h->roll_p = param_find("FW_ROLL_POS_P"); //TODO define rate params for fixed wing - - -// if(h->attrate_i == PARAM_INVALID) -// printf("FATAL MC_ATTRATE_I does not exist\n"); + h->roll_p = param_find("FW_ROLL_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_p = param_find("FW_PITCH_P"); + h->pitch_lim = param_find("FW_PITCH_LIM"); return OK; } -static int parameters_update(const struct fw_att_control_params *h, struct fw_att_control_params *p) +static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p) { param_get(h->roll_p, &(p->roll_p)); + param_get(h->roll_lim, &(p->roll_lim)); + param_get(h->pitch_p, &(p->pitch_p)); + param_get(h->pitch_lim, &(p->pitch_lim)); return OK; } @@ -99,11 +111,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att static bool initialized = false; static struct fw_att_control_params p; - static struct fw_att_control_params h; - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + static struct fw_att_control_params_handles h; if(!initialized) { @@ -120,7 +128,17 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* Roll (P) */ float roll_error = att_sp->roll_tait_bryan - att->roll; - rates_sp->roll = p.roll_p * roll_error; + //TODO convert to body frame + rates_sp->roll = p.roll_p * roll_error; //TODO enabled for testing only + + /* Pitch (P) */ + float pitch_error = att_sp->pitch_tait_bryan - att->pitch; + //TODO convert to body frame + + /* Yaw (from coordinated turn constraint) */ + //TODO + + //TODO Limits counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index a6dd1c9fb..0766a3d40 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -90,8 +90,6 @@ 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 */ /** diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index e6f5c6857..959628be9 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -152,6 +152,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static struct fw_rate_control_param_handles h; static PID_t roll_rate_controller; + static PID_t pitch_rate_controller; + static PID_t yaw_rate_controller; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -162,6 +164,8 @@ 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.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 + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim, PID_MODE_DERIVATIV_SET); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim, PID_MODE_DERIVATIV_SET); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -169,15 +173,17 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 2500 == 0) { /* update parameters from storage */ pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim); parameters_update(&h, &p); } /* Roll Rate (PI) */ actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); - - actuators->control[1] = 0; - actuators->control[2] = 0; + //XXX TODO disabled for now + actuators->control[1] = 0;//pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); + actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); counter++; diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 892da5f09..278931e4a 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -112,7 +112,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) -// /* subscribe to attitude (for attitude rate) and rate septoint */ +// /* subscribe */ // /* Setup of loop */ |