Copter: switch to NED for WPNav

[jason4short]

// wpinav_set_origin_and_destination - set origin and destination using lat/lon coordinates
public function wpinav_set_origin_and_destination(origin:Location, destination:Location):void
{
    //trace("wpinav_set_origin_and_destination", destination);
    wpinav_origin.x         = (origin.lat-home.lat) * LATLON_TO_CM;
    wpinav_origin.y         = (origin.lng-home.lng) * LATLON_TO_CM * scaleLongDown;
    wpinav_destination.x    = (destination.lat-home.lat) * LATLON_TO_CM;
    wpinav_destination.y    = (destination.lng-home.lng) * LATLON_TO_CM * scaleLongDown;
    wpinav_pos_delta.x      = wpinav_destination.x - wpinav_origin.x;
    wpinav_pos_delta.y      = wpinav_destination.y - wpinav_origin.y;
    wpinav_track_length     = wpinav_pos_delta.length;
    wpinav_track_desired    = 0;

    // set next_WP, prev_WP for reporting purposes
    // To-Do: move calcs below to a function
    set_next_WP_latlon(
        home.lat + wpinav_destination.x / LATLON_TO_CM,
        home.lng + wpinav_destination.y / LATLON_TO_CM * scaleLongUp);
}

Throughout the code, the lat is assigned to the X axis. Latitude is North/South so it should be the Y axis!

Jason

[rmackay9]

Yes, I agree it's backwards. I've sent an email to the group re switching to NED...this would also mean switching the altitude to be negative at least for the internal calculations in the navigation library although we wouldn't want to expose that to the user...we can leave them dealing with lat-lon-alt which is more understandable to humans.

[squilter]

@rmackay9 I think this has been fixed.

[rmackay9]

@squilter, the inertial nav part has been fixed because the EKF uses NED. The waypoint controller is still all NEU though. I'll update the subject but we probably need to leave this open until we convert the navigation code too.

[Naterater]

Does the waypoint controller still use NEU?

[rmackay9]

yes it does I'm afraid