diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 21:51:39 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 21:51:39 +0100 |
| commit | 808d897d2817dd1958a0fad75ac2bfa3925cfd8d (patch) | |
| tree | ced8cbed11db34cea9274a1e93f06a86f3140b73 | |
| parent | 5344e891671f75f2c38477f07643052bf6a3a3d3 (diff) | |
| download | px4-firmware-808d897d2817dd1958a0fad75ac2bfa3925cfd8d.tar.gz px4-firmware-808d897d2817dd1958a0fad75ac2bfa3925cfd8d.tar.bz2 px4-firmware-808d897d2817dd1958a0fad75ac2bfa3925cfd8d.zip | |
fixing mavlink waypoint handling
| -rw-r--r-- | apps/mavlink/waypoints.c | 7 |
1 files changed, 3 insertions, 4 deletions
diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 3d9e5750a..16759237e 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); // } } |