SITL: added SIM_INIT_LAT_OFS and SIM_INIT_LON_OFS

these give an initial lat/lon offset to the GPS in SITL so we can end
up with an origin a long way from the final position
This commit is contained in:
Andrew Tridgell 2021-06-18 11:03:56 +10:00
parent e191c48d9f
commit f73564f593
2 changed files with 9 additions and 0 deletions

View File

@ -342,6 +342,10 @@ const AP_Param::GroupInfo SITL::var_gps[] = {
AP_GROUPINFO("GPS2_ACC", 43, SITL, gps_accuracy[1], 0.3),
AP_GROUPINFO("GPS2_VERR", 44, SITL, gps_vel_err[1], 0),
AP_GROUPINFO("INIT_LAT_OFS", 45, SITL, gps_init_lat_ofs, 0),
AP_GROUPINFO("INIT_LON_OFS", 46, SITL, gps_init_lon_ofs, 0),
AP_GROUPINFO("INIT_ALT_OFS", 47, SITL, gps_init_alt_ofs, 0),
AP_GROUPEND
};

View File

@ -211,6 +211,11 @@ public:
AP_Float gps_accuracy[2];
AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D)
// initial offset on GPS lat/lon, used to shift origin
AP_Float gps_init_lat_ofs;
AP_Float gps_init_lon_ofs;
AP_Float gps_init_alt_ofs;
AP_Float batt_voltage; // battery voltage base
AP_Float batt_capacity_ah; // battery capacity in Ah
AP_Int8 rc_fail; // fail RC input