From be9b58e1b9fff8c1b5b6ada3ab05a20c478c64a4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Nov 2012 20:22:00 +0100 Subject: [PATCH 1/2] re-adding pid limitation --- apps/systemlib/pid/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index dd8b6adc66..0358caa250 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - //if (output > pid->limit) output = pid->limit; + if (output > pid->limit) output = pid->limit; - //if (output < -pid->limit) output = -pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { pid->last_output = output; From 59725ccd3a80fd32ed8ef08025daeb4372adb1f1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 Nov 2012 21:51:39 +0100 Subject: [PATCH 2/2] fixing mavlink waypoint handling --- apps/mavlink/waypoints.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 3d9e5750aa..16759237e9 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -360,7 +360,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ float dist = -1.0f; if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); @@ -376,14 +376,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw wpm->pos_reached = true; - if (counter % 10 == 0) + if (counter % 100 == 0) printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); } - // else // { // if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f\n",dist,orbit); +// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); // } }