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

fw control: minor cleanup (work in progress)

Browse Source
This commit is contained in:
Thomas Gubler 2012-11-05 22:36:09 +01:00
parent 9d2c27bbd0
commit 0eea4bfb4e
1 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
View File

@ -239,7 +239,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
if(global_sp_updated) {
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint)
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
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,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
@ -252,12 +252,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
/* Simple Horizontal Control */
if(global_sp_updated_set_once)
{
if (counter % 100 == 0)
printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate bearing error */
// float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d,
// global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
@ -277,18 +273,16 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrapPI(psi_e);
/* calculate roll setpoint, do this artificially around zero */
attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
if (counter % 100 == 0)
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
// 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)