diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 22:36:09 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 22:51:41 +0100 |
| commit | 0eea4bfb4e6c7e076f9dbe9d93641884ea20de6f (patch) | |
| tree | 7a69a1727fc0b329e37eedf29b4a1581c064c467 | |
| parent | 9d2c27bbd02469e59ecf6aad774257f8b45cab0c (diff) | |
| download | px4-firmware-0eea4bfb4e6c7e076f9dbe9d93641884ea20de6f.tar.gz px4-firmware-0eea4bfb4e6c7e076f9dbe9d93641884ea20de6f.tar.bz2 px4-firmware-0eea4bfb4e6c7e076f9dbe9d93641884ea20de6f.zip | |
fw control: minor cleanup (work in progress)
| -rw-r--r-- | apps/fixedwing_pos_control/fixedwing_pos_control_main.c | 16 |
1 files changed, 5 insertions, 11 deletions
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 5f5b70090..38f5db9e4 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -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) |