fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 14:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix for ground speed minimum, untested

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-28 15:02:24 +01:00
parent 54d624f7c7
commit 520f335b55
1 changed files with 37 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+37 -29
View File
@@ -176,13 +176,13 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
int fixedwing_pos_control_thread_main(int argc, char *argv[]) int fixedwing_pos_control_thread_main(int argc, char *argv[])
{ {
/* read arguments */ /* read arguments */
// bool verbose = false; bool verbose = false;
// for (int i = 1; i < argc; i++) { for (int i = 1; i < argc; i++) {
// if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
// verbose = true; verbose = true;
// } }
// } }
/* welcome user */ /* welcome user */
printf("[fixedwing pos control] started\n"); printf("[fixedwing pos control] started\n");
@@ -307,7 +307,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
global_sp_updated_set_once = true; global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_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);
printf("psi_track: %0.4f\n", (double)psi_track);
if (verbose)
printf("psi_track: %0.4f\n", (double)psi_track);
} }
/* Simple Horizontal Control */ /* Simple Horizontal Control */
@@ -325,21 +327,18 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// XXX what is xtrack_err.past_end? // XXX what is xtrack_err.past_end?
if(distance_res == OK /*&& !xtrack_err.past_end*/) { if(distance_res == OK /*&& !xtrack_err.past_end*/) {
// printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
// printf("delta_psi_c %.4f ", (double)delta_psi_c);
float psi_c = psi_track + delta_psi_c; float psi_c = psi_track + delta_psi_c;
// printf("psi_c %.4f ", (double)psi_c);
// printf("att.yaw %.4f ", (double)att.yaw);
float psi_e = psi_c - att.yaw; float psi_e = psi_c - att.yaw;
// printf("psi_e %.4f ", (double)psi_e); if (verbose) {
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
printf("delta_psi_c %.4f ", (double)delta_psi_c);
printf("psi_c %.4f ", (double)psi_c);
printf("att.yaw %.4f ", (double)att.yaw);
printf("psi_e %.4f ", (double)psi_e);
}
/* shift error to prevent wrapping issues */ /* shift error to prevent wrapping issues */
psi_e = _wrap_pi(psi_e); psi_e = _wrap_pi(psi_e);
@@ -349,27 +348,36 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track; float psi_rate_c = delta_psi_rate_c + psi_rate_track;
//limit turn rate /* limit turn rate */
if(psi_rate_c > p.headingr_lim) if(psi_rate_c > p.headingr_lim) {
psi_rate_c = p.headingr_lim; psi_rate_c = p.headingr_lim;
else if(psi_rate_c < -p.headingr_lim) } else if(psi_rate_c < -p.headingr_lim) {
psi_rate_c = - p.headingr_lim; psi_rate_c = -p.headingr_lim;
}
// 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
// printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); // XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
if (ground_speed < 10.0f) {
ground_speed = 10.0f;
}
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
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); if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
}
if (counter % 100 == 0) if (verbose && 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 (verbose && 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);
} }