fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 08:53:34 +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:51:41 +01:00
parent 9d2c27bbd0
commit 0eea4bfb4e
1 changed files with 5 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+5 -11
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); orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
if(global_sp_updated) { 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; 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);
@@ -252,12 +252,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
/* Simple Horizontal Control */ /* Simple Horizontal Control */
if(global_sp_updated_set_once) if(global_sp_updated_set_once)
{ {
if (counter % 100 == 0) // 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); // 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);
/* calculate crosstrack error */ /* calculate crosstrack error */
// Only the case of a straight line track following handled so far // 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_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw; float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */ /* shift error to prevent wrapping issues */
psi_e = _wrapPI(psi_e); psi_e = _wrapPI(psi_e);
/* calculate roll setpoint, do this artificially around zero */ /* 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); attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
if (counter % 100 == 0) // if (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 (counter % 100 == 0)