diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 7be449157d..36adc16e8e 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -37,10 +37,11 @@ */ // includes for mathematical manipulation -#include -#include +#include #include +#include #include +#include // clock access #include diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 30cc10b6fe..c6821cd352 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,8 +48,6 @@ #include #include -#include - #include #include @@ -171,15 +169,7 @@ public: /** * @brief Construct and initialize a new Map Projection object */ - MapProjection(double lat_0, double lon_0) - { - initReference(lat_0, lon_0); - } - - /** - * @brief Construct and initialize a new Map Projection object - */ - MapProjection(double lat_0, double lon_0, uint64_t timestamp) + MapProjection(double lat_0, double lon_0, uint64_t timestamp = 0) { initReference(lat_0, lon_0, timestamp); } @@ -192,20 +182,7 @@ public: * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ - void initReference(double lat_0, double lon_0, uint64_t timestamp); - - /** - * Initialize the map transformation - * - * with reference coordinates on the geographic coordinate system - * where the azimuthal equidistant plane's origin is located - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ - inline void initReference(double lat_0, double lon_0) - { - initReference(lat_0, lon_0, hrt_absolute_time()); - } + void initReference(double lat_0, double lon_0, uint64_t timestamp = 0); /** * @return true, if the map reference has been initialized before diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index b2bb5632b7..834f79465f 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -623,7 +623,7 @@ bool FlightTaskAuto::_evaluateGlobalReference() } // init projection - _reference_position.initReference(ref_lat, ref_lon); + _reference_position.initReference(ref_lat, ref_lon, _time_stamp_current); // check if everything is still finite return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon); diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp index c667594b10..1c3e0f0011 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp @@ -231,7 +231,8 @@ void TargetEstimator::measurement_update(follow_target_s follow_target) // Decompose follow_target message into the individual measurements for position and velocity const Vector3f vel_measured{follow_target.vx, follow_target.vy, follow_target.vz}; Vector3f pos_measured{NAN, NAN, -(follow_target.alt - _vehicle_local_position.ref_alt)}; - _reference_position.initReference(_vehicle_local_position.ref_lat, _vehicle_local_position.ref_lon); + _reference_position.initReference(_vehicle_local_position.ref_lat, _vehicle_local_position.ref_lon, + hrt_absolute_time()); _reference_position.project(follow_target.lat, follow_target.lon, pos_measured(0), pos_measured(1)); // Initialize filter if necessary diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index 61486f3f77..336f50daa3 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -68,7 +68,7 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position) { if (!_projection_reference.isInitialized()) { - _projection_reference.initReference(global_position.lat, global_position.lon); + _projection_reference.initReference(global_position.lat, global_position.lon, hrt_absolute_time()); } float x1, y1, x2, y2; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9c53b16f36..ab283812e5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2695,7 +2695,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) const double lon = hil_state.lon * 1e-7; if (!_global_local_proj_ref.isInitialized() || !PX4_ISFINITE(_global_local_alt0)) { - _global_local_proj_ref.initReference(lat, lon); + _global_local_proj_ref.initReference(lat, lon, hrt_absolute_time()); _global_local_alt0 = hil_state.alt / 1000.f; } diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 00a1e3f940..09fa712b20 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -511,7 +511,7 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, } if (!_projection_reference.isInitialized()) { - _projection_reference.initReference(lat, lon); + _projection_reference.initReference(lat, lon, hrt_absolute_time()); } float x1, y1, x2, y2; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 74c81569cf..e75db68629 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -81,7 +81,7 @@ PrecLand::on_activation() vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); if (!_map_ref.isInitialized()) { - _map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon); + _map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon, hrt_absolute_time()); } position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 20eed499b5..c1bdfb6dc1 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -580,7 +580,7 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message double lon = hil_state.lon * 1e-7; if (!_global_local_proj_ref.isInitialized()) { - _global_local_proj_ref.initReference(lat, lon); + _global_local_proj_ref.initReference(lat, lon, timestamp); _global_local_alt0 = hil_state.alt / 1000.f; }