From 62581fe55b0fb138f88bd0dcd89dffdbf99496ef Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Oct 2012 11:54:02 +0100 Subject: fw control: attitude, added pid elements --- apps/fixedwing_att_control/fixedwing_att_control_att.c | 18 ++++++++++-------- .../fixedwing_att_control/fixedwing_att_control_rate.c | 12 ++++-------- .../fixedwing_pos_control/fixedwing_pos_control_main.c | 6 +++--- apps/mavlink/orb_listener.c | 7 ++----- 4 files changed, 19 insertions(+), 24 deletions(-) (limited to 'apps') diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index c691c6938..711a1cc31 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -113,10 +113,15 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att static struct fw_att_control_params p; static struct fw_att_control_params_handles h; + static PID_t roll_controller; + static PID_t pitch_controller; + if(!initialized) { parameters_init(&h); parameters_update(&h, &p); + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -124,22 +129,19 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim); } /* Roll (P) */ - float roll_error = att_sp->roll_tait_bryan - att->roll; - //TODO convert to body frame - rates_sp->roll = p.roll_p * roll_error; //TODO enabled for testing only + rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0); /* Pitch (P) */ - float pitch_error = att_sp->pitch_tait_bryan - att->pitch; - //TODO convert to body frame + //rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0); - /* Yaw (from coordinated turn constraint) */ + /* Yaw (from coordinated turn constraint or lateral force) */ //TODO - //TODO Limits - counter++; return 0; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index 24dce199f..4b7e2c2ba 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -116,10 +116,6 @@ static int parameters_init(struct fw_rate_control_param_handles *h) h->yawrate_awu = param_find("FW_YAWRATE_AWU"); h->yawrate_lim = param_find("FW_YAWRATE_LIM"); - -// if(h->attrate_i == PARAM_INVALID) -// printf("FATAL MC_ATTRATE_I does not exist\n"); - return OK; } @@ -163,19 +159,19 @@ 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 + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); // 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_NONE); // 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_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } /* load new parameters with lower rate */ if (counter % 100 == 0) { /* update parameters from storage */ + parameters_update(&h, &p); 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); } diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 63ea1e5b4..c08017b8d 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -105,9 +105,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* publish attitude setpoint */ - attitude_setpoint.roll_body = 0.0f; - attitude_setpoint.pitch_body = 0.0f; - attitude_setpoint.yaw_body = 0.0f; + attitude_setpoint.roll_tait_bryan = 0.0f; + attitude_setpoint.pitch_tait_bryan = 0.0f; + attitude_setpoint.yaw_tait_bryan = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 8a604d6c1..d8b9dc3db 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -547,12 +547,9 @@ uorb_receive_thread(void *arg) mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); } else { - static bool updated = false; for (unsigned i = 0; i < n_listeners; i++) { - orb_check(*(listeners[i].subp), &updated); -// printf("revents: %d:%d", i, fds[i].revents); -// if (fds[i].revents & POLLIN) - if(updated) + bool updated = false; + if(OK == orb_check(*(listeners[i].subp), &updated) && updated) listeners[i].callback(&listeners[i]); } } -- cgit v1.2.3