dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixing mavlink waypoint handling

This commit is contained in:
Thomas Gubler 2012-11-05 21:51:39 +01:00
parent be9b58e1b9
commit 59725ccd3a
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
apps/mavlink/waypoints.c
View File

@ -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);
// }
}