geo: remove dependency on drv_hrt time driver

This commit is contained in:
Matthias Grob
2024-10-04 14:45:57 +02:00
committed by Mathieu Bresciani
parent f971c0e617
commit fd04ece6d4
9 changed files with 13 additions and 34 deletions
@@ -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);
@@ -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
+1 -1
View File
@@ -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;
+1 -1
View File
@@ -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;
}
+1 -1
View File
@@ -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;
+1 -1
View File
@@ -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();
@@ -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;
}