Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: fixes for use of longitude_scale()

This commit is contained in:
Andrew Tridgell 2021-07-01 09:19:05 +10:00 committed by Randy Mackay
parent d3163c8f48
commit 3304730d7c
4 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Sub.cpp
View File

@ -27,7 +27,6 @@ Sub::Sub()
: logger(g.log_bitmask), : logger(g.log_bitmask),
control_mode(MANUAL), control_mode(MANUAL),
motors(MAIN_LOOP_RATE), motors(MAIN_LOOP_RATE),
scaleLongDown(1),
auto_mode(Auto_WP), auto_mode(Auto_WP),
guided_mode(Guided_WP), guided_mode(Guided_WP),
auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),

4
ArduSub/Sub.h
View File

@ -255,10 +255,6 @@ private:
AP_Motors6DOF motors; AP_Motors6DOF motors;
// GPS variables
// Sometimes we need to remove the scaling for distance calcs
float scaleLongDown;
// Auto // Auto
AutoMode auto_mode; // controls which auto controller is run AutoMode auto_mode; // controls which auto controller is run

2
ArduSub/commands.cpp
View File

@ -70,8 +70,6 @@ bool Sub::set_home(const Location& loc, bool lock)
// init inav and compass declination // init inav and compass declination
if (!home_was_set) { if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = loc.longitude_scale();
// record home is set // record home is set
AP::logger().Write_Event(LogEvent::SET_HOME); AP::logger().Write_Event(LogEvent::SET_HOME);

5
ArduSub/position_vector.cpp
View File

@ -15,7 +15,10 @@ Vector3f Sub::pv_location_to_vector(const Location& loc)
origin.zero(); origin.zero();
} }
float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin
return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, Location::diff_longitude(loc.lng,origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin); Vector3f vec = origin.get_distance_NED(loc);
vec.xy() *= 100;
vec.z = alt_above_origin;
return vec;
} }
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin // pv_alt_above_origin - convert altitude above home to altitude above EKF origin