diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index 5854511a91..db18899cb1 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -5,10 +5,10 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -int32 lat # Latitude in 1E-7 degrees -int32 lon # Longitude in 1E-7 degrees -int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) -int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) +float64 latitude_deg # Latitude in degrees, allows centimeter level RTK precision +float64 longitude_deg # Longitude in degrees, allows centimeter level RTK precision +float64 altitude_msl_m # Altitude above MSL, meters +float64 altitude_ellipsoid_m # Altitude above Ellipsoid, meters float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) float32 c_variance_rad # GPS course accuracy estimate, (radians) diff --git a/src/drivers/cyphal/Publishers/udral/Gnss.hpp b/src/drivers/cyphal/Publishers/udral/Gnss.hpp index 168b1e437f..ef592c7855 100644 --- a/src/drivers/cyphal/Publishers/udral/Gnss.hpp +++ b/src/drivers/cyphal/Publishers/udral/Gnss.hpp @@ -66,9 +66,9 @@ public: size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; reg_udral_physics_kinematics_geodetic_Point_0_1 geo {}; - geo.latitude = gps.lat; - geo.longitude = gps.lon; - geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast(gps.alt) }; + geo.latitude = (int64_t)(gps.latitude_deg / 1e7); + geo.longitude = (int64_t)(gps.longitude_deg / 1e7); + geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = gps.altitude_msl_m }; uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 6eabe40e58..261480cb78 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 6eabe40e58f94b997a2347aff34b7aa0bc093dd5 +Subproject commit 261480cb78a3ba60cdda2dcc95b874ebc2c9312c diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 8b3bc95145..eae3dc5486 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -320,10 +320,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.fix_type = gpsFix; - sensor_gps.lat = positionGpsLla.c[0] * 1e7; - sensor_gps.lon = positionGpsLla.c[1] * 1e7; - sensor_gps.alt = positionGpsLla.c[2] * 1e3; - sensor_gps.alt_ellipsoid = sensor_gps.alt; + sensor_gps.latitude_deg = positionGpsLla.c[0]; + sensor_gps.longitude_deg = positionGpsLla.c[1]; + sensor_gps.altitude_msl_m = positionGpsLla.c[2]; + sensor_gps.altitude_ellipsoid_m = sensor_gps.altitude_msl_m; sensor_gps.vel_ned_valid = true; sensor_gps.vel_n_m_s = velocityGpsNed.c[0]; diff --git a/src/drivers/osd/msp_osd/msp_defines.h b/src/drivers/osd/msp_osd/msp_defines.h index fd1c3e5aec..eab7bebe52 100644 --- a/src/drivers/osd/msp_osd/msp_defines.h +++ b/src/drivers/osd/msp_osd/msp_defines.h @@ -386,9 +386,9 @@ struct msp_raw_gps_t { uint8_t numSat; int32_t lat; // 1 / 10000000 deg int32_t lon; // 1 / 10000000 deg - int16_t alt; // meters + int16_t alt; // centimeters since MSP API 1.39, meters before int16_t groundSpeed; // cm/s - int16_t groundCourse; // unit: degree x 10 + int16_t groundCourse; // unit: degree x 100, centidegrees uint16_t hdop; } __attribute__((packed)); diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index f30960ddc5..1a78179855 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -314,9 +314,9 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, msp_raw_gps_t raw_gps {0}; if (vehicle_gps_position.fix_type >= 2) { - raw_gps.lat = vehicle_gps_position.lat; - raw_gps.lon = vehicle_gps_position.lon; - raw_gps.alt = vehicle_gps_position.alt / 10; + raw_gps.lat = static_cast(vehicle_gps_position.latitude_deg * 1e7); + raw_gps.lon = static_cast(vehicle_gps_position.longitude_deg * 1e7); + raw_gps.alt = static_cast(vehicle_gps_position.altitude_msl_m * 100.0); float course = math::degrees(vehicle_gps_position.cog_rad); @@ -432,14 +432,14 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, msp_altitude_t altitude {0}; if (vehicle_gps_position.fix_type >= 2) { - altitude.estimatedActualPosition = vehicle_gps_position.alt / 10; + altitude.estimatedActualPosition = static_cast(vehicle_gps_position.altitude_msl_m * 100.0); // cm } else { altitude.estimatedActualPosition = 0; } if (estimator_status.solution_status_flags & (1 << 5)) { - altitude.estimatedActualVelocity = -vehicle_local_position.vz * 10; //m/s to cm/s + altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s } else { altitude.estimatedActualVelocity = 0; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 9ba3bc939e..dc30620e98 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -241,11 +241,11 @@ void CrsfRc::Run() sensor_gps_s sensor_gps; if (_vehicle_gps_position_sub.update(&sensor_gps)) { - int32_t latitude = sensor_gps.lat; - int32_t longitude = sensor_gps.lon; + int32_t latitude = static_cast(round(sensor_gps.latitude_deg * 1e7)); + int32_t longitude = static_cast(round(sensor_gps.longitude_deg * 1e7)); uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f; uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f; - uint16_t altitude = sensor_gps.alt + 1000; + uint16_t altitude = static_cast(sensor_gps.altitude_msl_m * 1e3) + 1000; uint8_t num_satellites = sensor_gps.satellites_used; this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites); } diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index e11687b5d8..720437c823 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -96,11 +96,11 @@ bool CRSFTelemetry::send_gps() return false; } - int32_t latitude = vehicle_gps_position.lat; - int32_t longitude = vehicle_gps_position.lon; + int32_t latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); + int32_t longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f; uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f; - uint16_t altitude = vehicle_gps_position.alt + 1000; + uint16_t altitude = static_cast(round(vehicle_gps_position.altitude_msl_m + 1.0)); uint8_t num_satellites = vehicle_gps_position.satellites_used; return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed, diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp index 2b253bbc3f..6edd0de598 100644 --- a/src/drivers/rc_input/ghst_telemetry.cpp +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -110,9 +110,9 @@ bool GHSTTelemetry::send_gps1_status() return false; } - int32_t latitude = vehicle_gps_position.lat; // 1e-7 degrees - int32_t longitude = vehicle_gps_position.lon; // 1e-7 degrees - uint16_t altitude = vehicle_gps_position.alt / 1000; // mm -> m + int32_t latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); // 1e-7 degrees + int32_t longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); // 1e-7 degrees + uint16_t altitude = static_cast(round(vehicle_gps_position.altitude_msl_m)); // meters return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude); } diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 5eaa83d011..7d7013651b 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -268,9 +268,9 @@ void BST::RunImpl() if (gps.fix_type >= 3 && gps.eph < 50.0f) { BSTPacket bst_gps = {}; bst_gps.type = 0x02; - bst_gps.payload.lat = swap_int32(gps.lat); - bst_gps.payload.lon = swap_int32(gps.lon); - bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000); + bst_gps.payload.lat = swap_int32(static_cast(round(gps.latitude_deg * 1e7))); + bst_gps.payload.lon = swap_int32(static_cast(round(gps.longitude_deg * 1e7))); + bst_gps.payload.alt = swap_int16(static_cast(round(gps.altitude_msl_m)) + 1000); bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f); bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F); bst_gps.payload.sats = gps.satellites_used; diff --git a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp index 238f8949c0..cac0b1c719 100644 --- a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp @@ -233,11 +233,11 @@ void sPort_send_GPS_LON(int uart) /* send longitude */ /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ /* precision is approximately 0.1m */ - uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lon); + uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg * 1e7); iLon |= (1 << 31); - if (s_port_subscription_data->vehicle_gps_position_sub.get().lon < 0) { iLon |= (1 << 30); } + if (s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg < 0) { iLon |= (1 << 30); } sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon); } @@ -246,9 +246,9 @@ void sPort_send_GPS_LAT(int uart) { /* send latitude */ /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */ - uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lat); + uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg * 1e7); - if (s_port_subscription_data->vehicle_gps_position_sub.get().lat < 0) { iLat |= (1 << 30); } + if (s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg < 0) { iLat |= (1 << 30); } sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat); } @@ -256,7 +256,7 @@ void sPort_send_GPS_LAT(int uart) void sPort_send_GPS_ALT(int uart) { /* send altitude */ - uint32_t iAlt = s_port_subscription_data->vehicle_gps_position_sub.get().alt / 10; + uint32_t iAlt = static_cast(s_port_subscription_data->vehicle_gps_position_sub.get().altitude_msl_m * 1e2); sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt); } diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 96c2dd3386..150bc21fe9 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -242,7 +242,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat)) * 1e-7d; + double lat = gps.latitude_deg; /* Set the N or S specifier */ msg.latitude_ns = 0; @@ -265,7 +265,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon)) * 1e-7d; + double lon = gps.longitude_deg; /* Set the E or W specifier */ msg.longitude_ew = 0; @@ -285,7 +285,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f); + uint16_t alt = (uint16_t)(round(gps.altitude_msl_m) + 500.0); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp index cd0f5b11e5..379f651be9 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -455,7 +455,7 @@ void SagetechMXS::determine_furthest_aircraft() continue; } - const float distance = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE, + const float distance = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg, vehicle_list[index].lat, vehicle_list[index].lon); @@ -492,8 +492,8 @@ void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle) // needs to handle updating the vehicle list, keeping track of which vehicles to drop // and which to keep, allocating new vehicles, and publishing to the transponder_report topic uint16_t index = list_size_allocated + 1; // Make invalid to start with. - const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0); - const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE, + const bool my_loc_is_zero = (fabs(_gps.latitude_deg) < DBL_EPSILON) && (fabs(_gps.longitude_deg) < DBL_EPSILON); + const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg, vehicle.lat, vehicle.lon); const bool is_tracked_in_list = find_index(vehicle, &index); // const bool is_special = is_special_vehicle(vehicle.icao_address); @@ -745,7 +745,8 @@ void SagetechMXS::send_operating_msg() mxs_state.op.altRes25 = !mxs_state.inst.altRes100; // Host Altitude Resolution from install - mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + mxs_state.op.altitude = static_cast(_gps.altitude_msl_m * + SAGETECH_SCALE_M_TO_FT); // Height above sealevel in feet mxs_state.op.identOn = _adsb_ident.get(); @@ -806,8 +807,8 @@ void SagetechMXS::send_gps_msg() } // Get Vehicle Longitude and Latitude and Convert to string - const int32_t longitude = _gps.lon; - const int32_t latitude = _gps.lat; + const int32_t longitude = static_cast(_gps.longitude_deg * 1e7); + const int32_t latitude = static_cast(_gps.latitude_deg * 1e7); const double lon_deg = longitude * 1.0E-7 * (longitude < 0 ? -1 : 1); const double lon_minutes = (lon_deg - int(lon_deg)) * 60; snprintf((char *)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, @@ -836,7 +837,7 @@ void SagetechMXS::send_gps_msg() snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6); - gps.height = _gps.alt_ellipsoid * 1E-3; + gps.height = (float)_gps.altitude_ellipsoid_m; // checkGPSInputs(&gps); last.msg.type = SG_MSG_TYPE_HOST_GPS; @@ -1284,7 +1285,8 @@ void SagetechMXS::auto_config_operating() mxs_state.op.altHostAvlbl = false; mxs_state.op.altRes25 = true; // Host Altitude Resolution from install - mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + mxs_state.op.altitude = static_cast(_gps.altitude_msl_m * + SAGETECH_SCALE_M_TO_FT); // Height above sealevel in feet mxs_state.op.identOn = false; diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp index f63661b629..ad4314d130 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp @@ -142,7 +142,7 @@ private: static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F}; static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F}; static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F}; - static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F}; + static constexpr double SAGETECH_SCALE_M_TO_FT{3.28084}; static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F}; static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0}; static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1}; @@ -156,7 +156,6 @@ private: static constexpr uint16_t INVALID_SQUAWK{7777}; static constexpr unsigned BAUD_460800{0010004}; // B460800 not defined in MacOS termios static constexpr unsigned BAUD_921600{0010007}; // B921600 not defined in MacOS termios - static constexpr double GPS_SCALE{1.0E-7}; // Stored variables uint64_t _loop_count; diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 3c263b500e..ddbc7907e6 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -337,10 +337,10 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure */ report.timestamp = hrt_absolute_time(); - report.lat = msg.latitude_deg_1e8 / 10; - report.lon = msg.longitude_deg_1e8 / 10; - report.alt = msg.height_msl_mm; - report.alt_ellipsoid = msg.height_ellipsoid_mm; + report.latitude_deg = msg.latitude_deg_1e8 / 1e8; + report.longitude_deg = msg.longitude_deg_1e8 / 1e8; + report.altitude_msl_m = msg.height_msl_mm / 1e3; + report.altitude_ellipsoid_m = msg.height_ellipsoid_mm / 1e3; if (valid_pos_cov) { // Horizontal position uncertainty diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp index e7453e04f9..5cd977a1bf 100644 --- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -81,10 +81,10 @@ public: fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC; fix2.gnss_timestamp.usec = gps.time_utc_usec; - fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10; - fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10; - fix2.height_msl_mm = gps.alt; - fix2.height_ellipsoid_mm = gps.alt_ellipsoid; + fix2.latitude_deg_1e8 = (int64_t)(gps.latitude_deg * 1e8); + fix2.longitude_deg_1e8 = (int64_t)(gps.longitude_deg * 1e8); + fix2.height_msl_mm = (int32_t)(gps.altitude_msl_m * 1e3); + fix2.height_ellipsoid_mm = (int32_t)(gps.altitude_ellipsoid_m * 1e3); fix2.status = gps.fix_type; fix2.ned_velocity[0] = gps.vel_n_m_s; fix2.ned_velocity[1] = gps.vel_e_m_s; diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index 6b08b58666..e2f966e023 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -35,12 +35,12 @@ using namespace time_literals; -FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) : +FakeGps::FakeGps(double latitude_deg, double longitude_deg, double altitude_m) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _latitude(latitude_deg * 10e6), - _longitude(longitude_deg * 10e6), - _altitude(altitude_m * 10e2f) + _latitude(latitude_deg), + _longitude(longitude_deg), + _altitude(altitude_m) { } @@ -60,10 +60,10 @@ void FakeGps::Run() sensor_gps_s sensor_gps{}; sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954; - sensor_gps.lat = _latitude; - sensor_gps.lon = _longitude; - sensor_gps.alt = _altitude; - sensor_gps.alt_ellipsoid = _altitude; + sensor_gps.latitude_deg = _latitude; + sensor_gps.longitude_deg = _longitude; + sensor_gps.altitude_msl_m = _altitude; + sensor_gps.altitude_ellipsoid_m = _altitude; sensor_gps.s_variance_m_s = 0.3740f; sensor_gps.c_variance_rad = 0.6737f; sensor_gps.eph = 2.1060f; diff --git a/src/examples/fake_gps/FakeGps.hpp b/src/examples/fake_gps/FakeGps.hpp index ef5a9197eb..eae3b47512 100644 --- a/src/examples/fake_gps/FakeGps.hpp +++ b/src/examples/fake_gps/FakeGps.hpp @@ -45,7 +45,7 @@ class FakeGps : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f); + FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, double altitude_m = 30.1); ~FakeGps() override = default; @@ -67,7 +67,7 @@ private: uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - int32_t _latitude{296603018}; // Latitude in 1e-7 degrees - int32_t _longitude{-823160500}; // Longitude in 1e-7 degrees - int32_t _altitude{30100}; // Altitude in 1e-3 meters above MSL, (millimetres) + double _latitude{29.6603018}; // Latitude in degrees + double _longitude{-82.3160500}; // Longitude in degrees + double _altitude{30.1}; // Altitude in meters above MSL, (millimetres) }; diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 488ed0d407..425c09f3a7 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -66,8 +66,8 @@ void FakeMagnetometer::Run() if (_vehicle_gps_position_sub.copy(&gps)) { if (gps.eph < 1000) { - const double lat = gps.lat / 1.e7; - const double lon = gps.lon / 1.e7; + const double lat = gps.latitude_deg; + const double lon = gps.longitude_deg; // magnetic field data returned by the geo library using the current GPS position const float mag_declination_gps = get_mag_declination_radians(lat, lon); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 89ff104279..b4da68f722 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -219,7 +219,8 @@ void AttitudeEstimatorQ::update_gps_position() if (_vehicle_gps_position_sub.update(&gps)) { if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) { // set magnetic declination automatically - update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon)); + update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg, + (float)gps.longitude_deg)); } } } diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 0f9766bebb..b53b7d6c30 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -152,10 +152,10 @@ void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, flo void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) { - fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt); + fillGlobalHomePos(home, gpos.lat, gpos.lon, (double)gpos.alt); } -void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) +void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt) { home.lat = lat; home.lon = lon; @@ -189,7 +189,7 @@ void HomePosition::setInAirHomePosition() ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon); const float home_alt = gpos.alt + home.z; - fillGlobalHomePos(home, home_lat, home_lon, home_alt); + fillGlobalHomePos(home, home_lat, home_lon, (double)home_alt); setHomePosValid(); home.timestamp = hrt_absolute_time(); @@ -206,8 +206,8 @@ void HomePosition::setInAirHomePosition() double home_lon; ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon); - const float home_alt = _gps_alt + home.z; - fillGlobalHomePos(home, home_lat, home_lon, home_alt); + const double home_alt = _gps_alt + (double)home.z; + fillGlobalHomePos(home, home_lat, home_lon, (double)home_alt); setHomePosValid(); home.timestamp = hrt_absolute_time(); @@ -304,9 +304,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) sensor_gps_s vehicle_gps_position; _vehicle_gps_position_sub.copy(&vehicle_gps_position); - _gps_lat = static_cast(vehicle_gps_position.lat) * 1e-7; - _gps_lon = static_cast(vehicle_gps_position.lon) * 1e-7; - _gps_alt = static_cast(vehicle_gps_position.alt) * 1e-3f; + _gps_lat = vehicle_gps_position.latitude_deg; + _gps_lon = vehicle_gps_position.longitude_deg; + _gps_alt = vehicle_gps_position.altitude_msl_m; _gps_eph = vehicle_gps_position.eph; _gps_epv = vehicle_gps_position.epv; diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 8456ddb6b8..6346dac42e 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -68,7 +68,7 @@ private: static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos); static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading); static void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos); - static void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt); + static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt); uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; @@ -83,7 +83,7 @@ private: bool _gps_position_for_home_valid{false}; double _gps_lat{0}; double _gps_lon{0}; - float _gps_alt{0.f}; + double _gps_alt{0}; float _gps_eph{0.f}; float _gps_epv{0.f}; }; diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 4d69f37311..2a027920a2 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -136,7 +136,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub) if ((hrt_elapsed_time(&sensor_gps.timestamp) < 1_s) && (sensor_gps.fix_type >= 2) && (sensor_gps.epv < 100)) { - float alt = sensor_gps.alt * 0.001f; + float alt = (float)sensor_gps.altitude_msl_m; if (PX4_ISFINITE(gps_altitude_sum)) { gps_altitude_sum += alt; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index fcb35ca782..184e10f876 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -237,8 +237,8 @@ static float get_sphere_radius() if (gps_sub.copy(&gps)) { if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { - const double lat = gps.lat / 1.e7; - const double lon = gps.lon / 1.e7; + const double lat = gps.latitude_deg; + const double lon = gps.longitude_deg; // magnetic field data returned by the geo library using the current GPS position return get_mag_strength_gauss(lat, lon); @@ -960,8 +960,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian if (vehicle_gps_position_sub.copy(&gps)) { if ((gps.timestamp != 0) && (gps.eph < 1000)) { - latitude = gps.lat / 1.e7f; - longitude = gps.lon / 1.e7f; + latitude = (float)gps.latitude_deg; + longitude = (float)gps.longitude_deg; mag_earth_available = true; } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9ce6f31e75..f5180c59a1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2245,9 +2245,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) gpsMessage gps_msg{ .time_usec = vehicle_gps_position.timestamp, - .lat = vehicle_gps_position.lat, - .lon = vehicle_gps_position.lon, - .alt = vehicle_gps_position.alt, + .lat = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)), + .lon = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)), + .alt = static_cast(round(vehicle_gps_position.altitude_msl_m * 1e3)), .yaw = vehicle_gps_position.heading, .yaw_offset = vehicle_gps_position.heading_offset, .yaw_accuracy = vehicle_gps_position.heading_accuracy, @@ -2269,7 +2269,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.setGpsData(gps_msg); _gps_time_usec = gps_msg.time_usec; - _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid; + _gps_alttitude_ellipsoid = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)); } } diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index acfef88097..d3306e8d69 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -92,9 +92,9 @@ int BlockLocalPositionEstimator::gpsMeasure(Vector &y) { // gps measurement y.setZero(); - y(0) = _sub_gps.get().lat * 1e-7; - y(1) = _sub_gps.get().lon * 1e-7; - y(2) = _sub_gps.get().alt * 1e-3; + y(0) = _sub_gps.get().latitude_deg; + y(1) = _sub_gps.get().longitude_deg; + y(2) = _sub_gps.get().altitude_msl_m; y(3) = (double)_sub_gps.get().vel_n_m_s; y(4) = (double)_sub_gps.get().vel_e_m_s; y(5) = (double)_sub_gps.get().vel_d_m_s; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 1068d511bd..247c0b9e84 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -2103,7 +2103,8 @@ void Logger::write_version(LogType type) // data versioning: increase this on every larger data change (format/semantic) // 1: switch to FIFO drivers (disabled on-chip DLPF) - write_info(type, "ver_data_format", static_cast(1)); + // 2: changed lat/lon/alt* to double to accommodate RTK GPS centimeter level precision + write_info(type, "ver_data_format", static_cast(2)); #ifndef BOARD_HAS_NO_UUID diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 26103f656a..d0a0791d12 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2351,10 +2351,10 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) gps.device_id = device_id.devid; - gps.lat = hil_gps.lat; - gps.lon = hil_gps.lon; - gps.alt = hil_gps.alt; - gps.alt_ellipsoid = hil_gps.alt; + gps.latitude_deg = hil_gps.lat * 1e-7; + gps.longitude_deg = hil_gps.lon * 1e-7; + gps.altitude_msl_m = hil_gps.alt * 1e-3; + gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3; gps.s_variance_m_s = 0.25f; gps.c_variance_rad = 0.5f; diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index eb77c8c319..ef91b95f27 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -71,9 +71,9 @@ private: msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; - msg.lat = gps.lat; - msg.lon = gps.lon; - msg.alt = gps.alt; + msg.lat = static_cast(round(gps.latitude_deg * 1e7)); + msg.lon = static_cast(round(gps.longitude_deg * 1e7)); + msg.alt = static_cast(round(gps.altitude_msl_m * 1e3)); // convert [m] to [mm] msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless) msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless) diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index f6887c73c9..ca2077d5a7 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -70,9 +70,9 @@ private: if (_sensor_gps_sub.update(&gps)) { msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; - msg.lat = gps.lat; - msg.lon = gps.lon; - msg.alt = gps.alt; + msg.lat = static_cast(round(gps.latitude_deg * 1e7)); + msg.lon = static_cast(round(gps.longitude_deg * 1e7)); + msg.alt = static_cast(round(gps.altitude_msl_m * 1e3)); // convert [m] to [mm] msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless) msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless) @@ -85,7 +85,7 @@ private: msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f; msg.satellites_visible = gps.satellites_used; - msg.alt_ellipsoid = gps.alt_ellipsoid; + msg.alt_ellipsoid = static_cast(round(gps.altitude_ellipsoid_m * 1e3)); // convert [m] to [mm] msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm msg.v_acc = gps.epv * 1e3f; // altitude uncertainty in mm msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp index f3586ae126..3a519c5e90 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -165,12 +165,12 @@ private: } if (vehicle_gps_position.fix_type >= 2) { - msg.latitude = vehicle_gps_position.lat; - msg.longitude = vehicle_gps_position.lon; + msg.latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); + msg.longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); // altitude_geodetic if (vehicle_gps_position.fix_type >= 3) { - msg.altitude_geodetic = vehicle_gps_position.alt * 1e-3f; + msg.altitude_geodetic = static_cast(round(vehicle_gps_position.altitude_msl_m)); // [m] } // horizontal_accuracy diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp index fdc8ae1ecc..b75c1d7a60 100644 --- a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp @@ -86,9 +86,9 @@ private: // Required update for dynamic message is 5 [Hz] mavlink_uavionix_adsb_out_dynamic_t dynamic_msg = { .utcTime = static_cast(vehicle_gps_position.time_utc_usec / 1000000ULL), - .gpsLat = vehicle_gps_position.lat, - .gpsLon = vehicle_gps_position.lon, - .gpsAlt = vehicle_gps_position.alt_ellipsoid, + .gpsLat = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)), + .gpsLon = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)), + .gpsAlt = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)), // convert [m] to [mm] .baroAltMSL = static_cast(vehicle_air_data.baro_pressure_pa / 100.0f), // convert [Pa] to [mBar] .accuracyHor = static_cast(vehicle_gps_position.eph * 1000.0f), // convert [m] to [mm] .accuracyVert = static_cast(vehicle_gps_position.epv * 100.0f), // convert [m] to [cm] diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f131130b90..5ccf78b291 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -220,7 +220,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const sen return checkAll(global_position); } else { - return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3); + return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, gps_position.altitude_msl_m); } } else { @@ -232,7 +232,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const sen return checkAll(global_position, baro_altitude_amsl); } else { - return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, baro_altitude_amsl); + return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, baro_altitude_amsl); } } } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 0ecdc0a4e4..3673ca14bb 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -70,7 +70,7 @@ void GpsBlending::update(uint64_t hrt_now_us) // Check for new data on selected GPS, and clear blend offsets for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { _NE_pos_offset_m[i].zero(); - _hgt_offset_mm[i] = 0.0f; + _hgt_offset_m[i] = 0.0; } // Only use a secondary instance if the fallback is allowed @@ -443,38 +443,38 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS // Convert each GPS position to a local NEU offset relative to the reference position Vector2f blended_NE_offset_m{0, 0}; - float blended_alt_offset_mm = 0.0f; + double blended_alt_offset_m = 0.0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { if ((blend_weights[i] > 0.0f) && (i != gps_best_index)) { // calculate the horizontal offset Vector2f horiz_offset{}; - get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), - (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, + _gps_state[i].latitude_deg, _gps_state[i].longitude_deg, &horiz_offset(0), &horiz_offset(1)); // sum weighted offsets blended_NE_offset_m += horiz_offset * blend_weights[i]; - // calculate vertical offset - float vert_offset = (float)(_gps_state[i].alt - gps_blended_state.alt); + // calculate vertical offset, meters + double vert_offset_m = _gps_state[i].altitude_msl_m - gps_blended_state.altitude_msl_m; // sum weighted offsets - blended_alt_offset_mm += vert_offset * blend_weights[i]; + blended_alt_offset_m += vert_offset_m * (double)blend_weights[i]; } } // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7; - const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7; + const double lat_deg_now = gps_blended_state.latitude_deg; + const double lon_deg_now = gps_blended_state.longitude_deg; double lat_deg_res = 0; double lon_deg_res = 0; add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, &lon_deg_res); - gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); - gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); - gps_blended_state.alt += (int32_t)blended_alt_offset_mm; + gps_blended_state.latitude_deg = lat_deg_res; + gps_blended_state.longitude_deg = lon_deg_res; + gps_blended_state.altitude_msl_m += blended_alt_offset_m; // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value int8_t gps_best_yaw_index = -1; @@ -516,30 +516,30 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) // Calculate a filtered position delta for each GPS relative to the blended solution state for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { Vector2f offset; - get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - (gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg, + gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, &offset(0), &offset(1)); _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); - _hgt_offset_mm[i] = (float)(gps_blended_state.alt - _gps_state[i].alt) * alpha[i] + - _hgt_offset_mm[i] * (1.0f - alpha[i]); + _hgt_offset_m[i] = (gps_blended_state.altitude_msl_m - _gps_state[i].altitude_msl_m) * (double)alpha[i] + + _hgt_offset_m[i] * (1.0 - (double)alpha[i]); } // calculate offset limits from the largest difference between receivers Vector2f max_ne_offset{}; - float max_alt_offset = 0; + double max_alt_offset = 0.0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { for (uint8_t j = i; j < GPS_MAX_RECEIVERS_BLEND; j++) { if (i != j) { Vector2f offset; - get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), + get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg, + _gps_state[j].latitude_deg, _gps_state[j].longitude_deg, &offset(0), &offset(1)); - max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); - max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); - max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); + max_ne_offset(0) = fmax(max_ne_offset(0), fabsf(offset(0))); + max_ne_offset(1) = fmax(max_ne_offset(1), fabsf(offset(1))); + max_alt_offset = fmax(max_alt_offset, fabs(_gps_state[i].altitude_msl_m - _gps_state[j].altitude_msl_m)); } } } @@ -548,7 +548,7 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); - _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset); + _hgt_offset_m[i] = constrain(_hgt_offset_m[i], -max_alt_offset, max_alt_offset); } } @@ -558,26 +558,26 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, // Convert each GPS position to a local NEU offset relative to the reference position // which is defined as the positon of the blended solution calculated from non offset corrected data Vector2f blended_NE_offset_m{0, 0}; - float blended_alt_offset_mm = 0.0f; + double blended_alt_offset_m = 0.0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { if (blend_weights[i] > 0.0f) { // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_orig = (double)_gps_state[i].lat * 1.0e-7; - const double lon_deg_orig = (double)_gps_state[i].lon * 1.0e-7; + const double lat_deg_orig = _gps_state[i].latitude_deg; + const double lon_deg_orig = _gps_state[i].longitude_deg; double lat_deg_offset_res = 0; double lon_deg_offset_res = 0; add_vector_to_global_position(lat_deg_orig, lon_deg_orig, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_offset_res, &lon_deg_offset_res); - float alt_offset = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i]; + double alt_offset_m = _gps_state[i].altitude_msl_m + _hgt_offset_m[i]; // calculate the horizontal offset Vector2f horiz_offset{}; - get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, lat_deg_offset_res, lon_deg_offset_res, &horiz_offset(0), &horiz_offset(1)); @@ -585,23 +585,23 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, blended_NE_offset_m += horiz_offset * blend_weights[i]; // calculate vertical offset - float vert_offset = alt_offset - gps_blended_state.alt; + double vert_offset_m = alt_offset_m - gps_blended_state.altitude_msl_m; // sum weighted offsets - blended_alt_offset_mm += vert_offset * blend_weights[i]; + blended_alt_offset_m += vert_offset_m * (double)blend_weights[i]; } } // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7; - const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7; + const double lat_deg_now = gps_blended_state.latitude_deg; + const double lon_deg_now = gps_blended_state.longitude_deg; double lat_deg_res = 0; double lon_deg_res = 0; add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, &lon_deg_res); - gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); - gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); - gps_blended_state.alt = gps_blended_state.alt + (int32_t)blended_alt_offset_mm; + gps_blended_state.latitude_deg = lat_deg_res; + gps_blended_state.longitude_deg = lon_deg_res; + gps_blended_state.altitude_msl_m = gps_blended_state.altitude_msl_m + blended_alt_offset_m; } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index fa52806632..6d33a643cd 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -131,7 +131,7 @@ private: bool _is_new_output_data_available{false}; matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) - float _hgt_offset_mm[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) + double _hgt_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (meters) uint64_t _time_prev_us[GPS_MAX_RECEIVERS_BLEND] {}; ///< the previous value of time_us for that GPS instance - used to detect new data. uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index 1baaf1da42..af5b1feab7 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -60,10 +60,10 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData() sensor_gps_s gps_data{}; gps_data.timestamp = _time_now_us - 10e3; gps_data.time_utc_usec = 0; - gps_data.lat = 47e7; - gps_data.lon = 9e7; - gps_data.alt = 800e3; - gps_data.alt_ellipsoid = 800e3; + gps_data.latitude_deg = 47.0; + gps_data.longitude_deg = 9.0; + gps_data.altitude_msl_m = 800.0; + gps_data.altitude_ellipsoid_m = 800.0; gps_data.s_variance_m_s = 0.2f; gps_data.c_variance_rad = 0.5f; gps_data.eph = 0.7f; @@ -213,9 +213,9 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than EXPECT_EQ(gps_blending.getOutputGpsData().timestamp, gps_data0.timestamp); EXPECT_EQ(gps_blending.getOutputGpsData().timestamp_sample, gps_data0.timestamp_sample); - EXPECT_EQ(gps_blending.getOutputGpsData().lat, gps_data0.lat); - EXPECT_EQ(gps_blending.getOutputGpsData().lon, gps_data0.lon); - EXPECT_EQ(gps_blending.getOutputGpsData().alt, gps_data0.alt); + EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg); + EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg); + EXPECT_EQ(gps_blending.getOutputGpsData().altitude_msl_m, gps_data0.altitude_msl_m); } TEST_F(GpsBlendingTest, dualReceiverFailover) diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 1e1b5be22c..283f71d684 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -116,7 +116,7 @@ void SensorGpsSim::Run() double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - float altitude = gpos.alt + (generate_wgn() * 0.5f); + double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); @@ -153,10 +153,10 @@ void SensorGpsSim::Run() sensor_gps.timestamp_sample = gpos.timestamp_sample; sensor_gps.time_utc_usec = 0; sensor_gps.device_id = device_id.devid; - sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees - sensor_gps.lon = roundf(longitude * 1e7); // Longitude in 1E-7 degrees - sensor_gps.alt = roundf(altitude * 1000.f); // Altitude in 1E-3 meters above MSL, (millimetres) - sensor_gps.alt_ellipsoid = sensor_gps.alt; + sensor_gps.latitude_deg = latitude; // Latitude in degrees + sensor_gps.longitude_deg = longitude; // Longitude in degrees + sensor_gps.altitude_msl_m = altitude; // Altitude in meters above MSL + sensor_gps.altitude_ellipsoid_m = altitude; sensor_gps.noise_per_ms = 0; sensor_gps.jamming_indicator = 0; sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index e3bc981805..d50d1dcd1f 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -402,10 +402,10 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) if (!_gps_blocked) { sensor_gps_s gps{}; - gps.lat = hil_gps.lat; - gps.lon = hil_gps.lon; - gps.alt = hil_gps.alt; - gps.alt_ellipsoid = hil_gps.alt; + gps.latitude_deg = hil_gps.lat / 1e7; + gps.longitude_deg = hil_gps.lon / 1e7; + gps.altitude_msl_m = hil_gps.alt / 1e3; + gps.altitude_ellipsoid_m = hil_gps.alt / 1e3; gps.s_variance_m_s = 0.25f; gps.c_variance_rad = 0.5f;