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

fw control: attitude, added pid elements

This commit is contained in:
Thomas Gubler 2012-10-28 11:54:02 +01:00
parent 17772afdaa
commit 62581fe55b
4 changed files with 19 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
apps/fixedwing_att_control/fixedwing_att_control_att.c
View File

@ -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;

12
apps/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -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);
}

6
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
View File

@ -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);

7
apps/mavlink/orb_listener.c
View File

@ -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]);
}
}