Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

SIH: ellipsoidal earth #23974

Draft
wants to merge 15 commits into
base: main
Choose a base branch
from
Draft

SIH: ellipsoidal earth #23974

wants to merge 15 commits into from
+292 −121

Conversation

bresch
Copy link
Member

@bresch bresch commented Nov 19, 2024 •
edited
Loading

requires #24034

Solved Problem

I needed a simulation environment on an ellipsoidal world to test #23854 .

Solution

Run SIH in an ECEF coordinate system with a WGS84 Earth model and convert the output to the usual global (lat/lon/alt) and local tangent plane frame.
The objective is to test the robustness of the EKF by changing the Earth spin rate and radius to exaggerate the non-inertial accelerations measured when navigating on an ellipsoidal environment.

Changelog Entry

New parameter: -
TOTO: mention this in the SIH documentation

Testing:

  • quad short distances
  • quad long distances
  • fixedwing (requires SIH: fix airplane sim #24034)
  • tailsitter -> sim is also broken in main, will fix it after this is merged

@bresch bresch self-assigned this Nov 19, 2024
@bresch bresch marked this pull request as draft November 19, 2024 16:04
@bresch bresch force-pushed the pr_ekf2_ellipsoidal_sih branch 2 times, most recently from 1ecc2d0 to 33c44dc Compare November 20, 2024 10:21
@haumarco
Copy link
Contributor

I don't have anything to complain about. Let's see if all the testing is successful

@bresch bresch force-pushed the pr-ekf2_gpos_rework branch 2 times, most recently from 48a3434 to 66b14f3 Compare November 21, 2024 10:50
@github-actions GitHub Actions
Copy link

github-actions bot commented Nov 21, 2024 •
edited
Loading

FLASH Analysis

px4_fmu-v5x 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.2% +50.5Ki  [ = ]       0    .debug_info
   +26% +45.2Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  [NEW] +28.4Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.0%     +97  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.1%     +91  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  +0.0%      +7  [ = ]       0    ../../src/lib/bezier/BezierN.cpp
  +0.0%      +6  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.0%      +6  [ = ]       0    ../../src/modules/mc_pos_control/PositionControl/ControlMath.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/mag_bias_estimator/MagBiasEstimator.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/vtol_att_control/tailsitter.cpp
  +0.0%      +3  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  [DEL] -23.3Ki  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.6% +34.5Ki  [ = ]       0    .debug_loc
   +32% +29.9Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.3% +4.47Ki  [ = ]       0    [section .debug_loc]
  [NEW] +1.04Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.6%    +162  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  +0.0%     +28  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +27  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  +0.1%     +26  [ = ]       0    ../../src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
  +0.7%     +13  [ = ]       0    ../../src/drivers/camera_capture/camera_capture.cpp
  +0.2%     +13  [ = ]       0    ../../src/drivers/rc/crsf_rc/CrsfRc.cpp
  +0.4%     +13  [ = ]       0    ../../src/drivers/telemetry/frsky_telemetry/frsky_data.cpp
  +0.0%     +13  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  +0.1%     +13  [ = ]       0    ../../src/lib/avoidance/ObstacleAvoidance.cpp
  +0.1%     +13  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  +0.0%     +13  [ = ]       0    ../../src/modules/commander/accelerometer_calibration.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/gimbal/output.cpp
  +0.2%     +13  [ = ]       0    ../../src/modules/gyro_calibration/GyroCalibration.cpp
  +0.0%     +13  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.2%     +13  [ = ]       0    ../../src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  +0.0%     +13  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.1%     -13  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  -1.1%     -13  [ = ]       0    ../../src/drivers/rc/ghst_rc/ghst_telemetry.cpp
  -0.5%     -13  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
  -0.3%     -13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/fw_rate_control/FixedwingRateControl.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/landing_target_estimator/LandingTargetEstimator.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/navigator/rtl_direct.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.cpp
  -0.4%     -13  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
  -0.0%     -14  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  -0.1%     -25  [ = ]       0    ../../src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  -0.1%     -72  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  -1.9%     -84  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
  [DEL] -1.04Ki  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.5% +9.56Ki  +0.5% +9.56Ki    .text
  +2.1% +5.79Ki  +2.1% +5.79Ki    [section .text]
   +18% +3.17Ki   +18% +3.17Ki    ../../src/modules/simulation/simulator_sih/sih.cpp
  [NEW] +1.51Ki  [NEW] +1.51Ki    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  [DEL]    -924  [DEL]    -924    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.2% +8.33Ki  [ = ]       0    .debug_line
   +16% +7.74Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  [NEW] +3.00Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.4%     +52  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.1%     +24  [ = ]       0    ../../src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
  +0.2%     +23  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.3%     +23  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
  +0.4%     +21  [ = ]       0    ../../src/lib/sensor_calibration/Accelerometer.cpp
  +0.1%     +19  [ = ]       0    ../../src/modules/commander/accelerometer_calibration.cpp
  +0.2%     +18  [ = ]       0    ../../src/modules/mc_pos_control/PositionControl/PositionControl.cpp
  +0.1%     +17  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  +0.4%     +16  [ = ]       0    ../../src/lib/system_identification/system_identification.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
  +0.2%     +13  [ = ]       0    ../../src/lib/sensor_calibration/Gyroscope.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.1%     +11  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
  +0.2%     +10  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
  +0.1%     +10  [ = ]       0    ../../src/modules/mc_pos_control/PositionControl/ControlMath.cpp
  +0.1%      +9  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +0.3%      +9  [ = ]       0    ../../src/lib/rate_control/rate_control.cpp
  +0.2%      +8  [ = ]       0    ../../src/lib/rtl/rtl_time_estimator.cpp
  +0.0%      +7  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/sensors/voted_sensors_update.cpp
  +0.1%      +5  [ = ]       0    ../../src/lib/npfg/npfg.cpp
  +0.2%      +5  [ = ]       0    ../../src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp
  +0.1%      +5  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
  +0.1%      +4  [ = ]       0    ../../src/lib/avoidance/ObstacleAvoidance.cpp
  +0.1%      +4  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp
  +0.1%      +4  [ = ]       0    ../../src/modules/gimbal/output_rc.cpp
  +0.1%      +4  [ = ]       0    ../../src/modules/land_detector/LandDetector.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/navigator/precland.cpp
  +0.0%      +3  [ = ]       0    ../../src/lib/battery/battery.cpp
  +0.0%      +3  [ = ]       0    ../../src/lib/bezier/BezierN.cpp
  +0.1%      +3  [ = ]       0    ../../src/lib/weather_vane/WeatherVane.cpp
  +0.1%      +3  [ = ]       0    ../../src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
  +0.1%      +3  [ = ]       0    ../../src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
  +0.1%      +3  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp
  +0.0%      +3  [ = ]       0    ../../src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
  +0.0%      +2  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp
  +0.0%      +2  [ = ]       0    ../../src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
  +0.0%      +1  [ = ]       0    ../../src/drivers/optical_flow/paa3905/PAA3905.cpp
  +0.0%      +1  [ = ]       0    ../../src/drivers/optical_flow/paw3902/PAW3902.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  -0.0%      -1  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/fw_rate_control/FixedwingRateControl.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/mc_rate_control/MulticopterRateControl.cpp
  -0.0%      -2  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
  -0.1%      -3  [ = ]       0    ../../src/lib/wind_estimator/WindEstimator.cpp
  -0.0%      -3  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  -0.1%      -4  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
  -0.1%      -4  [ = ]       0    ../../src/modules/gimbal/input_rc.cpp
  -0.1%      -4  [ = ]       0    ../../src/modules/gimbal/output_mavlink.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/accel.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/baro.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/gyro.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/mag.cpp
  -0.1%      -7  [ = ]       0    ../../src/modules/airspeed_selector/AirspeedValidator.cpp
  -0.1%      -7  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
  -0.1%      -8  [ = ]       0    ../../src/modules/commander/mag_calibration.cpp
  -0.1%      -8  [ = ]       0    ../../src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
  -0.1%      -8  [ = ]       0    ../../src/modules/vtol_att_control/tailsitter.cpp
  -0.0%      -9  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.0%     -10  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
  -0.3%     -11  [ = ]       0    ../../src/drivers/rc_input/crsf_telemetry.cpp
  -0.2%     -11  [ = ]       0    ../../src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp
  -0.2%     -12  [ = ]       0    ../../src/modules/landing_target_estimator/LandingTargetEstimator.cpp
  -0.1%     -12  [ = ]       0    ../../src/modules/vtol_att_control/vtol_type.cpp
  -0.3%     -13  [ = ]       0    ../../src/drivers/osd/msp_osd/uorb_to_msp.cpp
  -0.2%     -13  [ = ]       0    ../../src/drivers/rc/crsf_rc/CrsfRc.cpp
  -0.2%     -13  [ = ]       0    ../../src/drivers/telemetry/bst/bst.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/camera_feedback/CameraFeedback.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/commander/failure_detector/FailureDetector.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/gimbal/output.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/vtol_att_control/tiltrotor.cpp
  -0.1%     -14  [ = ]       0    ../../src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  -0.1%     -14  [ = ]       0    ../../src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
  -0.2%     -14  [ = ]       0    ../../src/modules/vtol_att_control/standard.cpp
  -0.3%     -15  [ = ]       0    ../../platforms/common/uORB/uORB.cpp
  -0.3%     -15  [ = ]       0    ../../src/modules/commander/level_calibration.cpp
  -0.1%     -28  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  -0.3%     -35  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  -0.0%    -108  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  [DEL] -2.33Ki  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.5% +5.74Ki  [ = ]       0    .debug_ranges
   +21% +6.10Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  [NEW]    +328  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  [DEL]    -312  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
  -0.1%    -384  [ = ]       0    [section .debug_ranges]
+0.0% +1.22Ki  [ = ]       0    .debug_str
   +16% +4.57Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  [NEW]    +386  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.0%    +105  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.1%     +45  [ = ]       0    [section .debug_str]
   +13%     +31  [ = ]       0    ../../src/lib/drivers/magnetometer/PX4Magnetometer.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  -6.8%     -10  [ = ]       0    ../../src/lib/conversion/rotation.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -3.5%     -21  [ = ]       0    ../../src/lib/geo/geo.cpp
  [DEL]    -332  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
 -62.0% -3.54Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
+0.1%    +576  [ = ]       0    .debug_frame
+0.1%    +400  [ = ]       0    .symtab
  [NEW]    +400  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +9.6%    +208  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.1%     +96  [ = ]       0    [section .symtab]
  [DEL]    -304  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.0%    +223  [ = ]       0    .debug_abbrev
  +0.0%    +223  [ = ]       0    armv7-m/arm_vectors.c
+0.0%    +180  [ = ]       0    .strtab
  [NEW]    +357  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +5.6%     +88  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.0%     +22  [ = ]       0    [section .strtab]
  [DEL]    -287  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.0%     +32  [ = ]       0    .debug_aranges
-42.3% -9.56Ki  [ = ]       0    [Unmapped]
+0.2%  +101Ki  +0.5% +9.56Ki    TOTAL
px4_fmu-v6x 
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0% +5.32Ki  [ = ]       0    .debug_info
  [NEW] +28.4Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.0%     +97  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.1%     +91  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  +0.0%      +7  [ = ]       0    ../../src/lib/bezier/BezierN.cpp
  +0.0%      +6  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.0%      +6  [ = ]       0    ../../src/modules/mc_pos_control/PositionControl/ControlMath.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/mag_bias_estimator/MagBiasEstimator.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/vtol_att_control/tailsitter.cpp
  +0.0%      +3  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  [DEL] -23.3Ki  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.0% +1.64Ki  [ = ]       0    .debug_loc
  +0.1% +1.55Ki  [ = ]       0    [section .debug_loc]
  [NEW] +1.04Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.1%     +56  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.0%     +52  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +1.0%     +42  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
  +0.0%     +14  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.1%     +13  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  +0.3%     +13  [ = ]       0    ../../src/lib/adsb/AdsbConflict.cpp
  +0.1%     +13  [ = ]       0    ../../src/lib/avoidance/ObstacleAvoidance.cpp
  +0.1%     +13  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +0.2%     +13  [ = ]       0    ../../src/modules/gyro_calibration/GyroCalibration.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/mag_bias_estimator/MagBiasEstimator.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/sensors/voted_sensors_update.cpp
  +0.0%      +3  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  +0.0%      +2  [ = ]       0    ../../src/modules/fw_rate_control/FixedwingRateControl.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  -0.7%     -13  [ = ]       0    ../../src/drivers/camera_capture/camera_capture.cpp
  -0.2%     -13  [ = ]       0    ../../src/drivers/camera_trigger/camera_trigger.cpp
  -1.0%     -13  [ = ]       0    ../../src/modules/commander/esc_calibration.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/commander/mag_calibration.cpp
  -0.5%     -13  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
  -0.3%     -13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/navigator/rtl.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/navigator/rtl_direct_mission_land.cpp
  -0.4%     -13  [ = ]       0    ../../src/modules/navigator/rtl_mission_fast_reverse.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
  -0.1%     -25  [ = ]       0    ../../src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  [DEL] -1.04Ki  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.0%    +639  [ = ]       0    .debug_line
  [NEW] +3.00Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.4%     +52  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.1%     +24  [ = ]       0    ../../src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
  +0.2%     +23  [ = ]       0    ../../src/lib/motion_planning/PositionSmoothing.cpp
  +0.3%     +23  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
  +0.4%     +21  [ = ]       0    ../../src/lib/sensor_calibration/Accelerometer.cpp
  +0.1%     +19  [ = ]       0    ../../src/modules/commander/accelerometer_calibration.cpp
  +0.2%     +18  [ = ]       0    ../../src/modules/mc_pos_control/PositionControl/PositionControl.cpp
  +0.1%     +17  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
  +0.4%     +16  [ = ]       0    ../../src/lib/system_identification/system_identification.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
  +0.2%     +13  [ = ]       0    ../../src/lib/sensor_calibration/Gyroscope.cpp
  +0.1%     +13  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  +0.1%     +11  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
  +0.2%     +10  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
  +0.1%     +10  [ = ]       0    ../../src/modules/mc_pos_control/PositionControl/ControlMath.cpp
  +0.1%      +9  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  +0.3%      +9  [ = ]       0    ../../src/lib/rate_control/rate_control.cpp
  +0.2%      +8  [ = ]       0    ../../src/lib/rtl/rtl_time_estimator.cpp
  +0.0%      +7  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/navigator/mission_block.cpp
  +0.1%      +6  [ = ]       0    ../../src/modules/sensors/voted_sensors_update.cpp
  +0.1%      +5  [ = ]       0    ../../src/lib/npfg/npfg.cpp
  +0.2%      +5  [ = ]       0    ../../src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp
  +0.1%      +5  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
  +0.1%      +4  [ = ]       0    ../../src/lib/avoidance/ObstacleAvoidance.cpp
  +0.1%      +4  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp
  +0.1%      +4  [ = ]       0    ../../src/modules/gimbal/output_rc.cpp
  +0.1%      +4  [ = ]       0    ../../src/modules/land_detector/LandDetector.cpp
  +0.0%      +4  [ = ]       0    ../../src/modules/navigator/precland.cpp
  +0.0%      +3  [ = ]       0    ../../src/lib/battery/battery.cpp
  +0.0%      +3  [ = ]       0    ../../src/lib/bezier/BezierN.cpp
  +0.1%      +3  [ = ]       0    ../../src/lib/weather_vane/WeatherVane.cpp
  +0.1%      +3  [ = ]       0    ../../src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp
  +0.1%      +3  [ = ]       0    ../../src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
  +0.1%      +3  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp
  +0.0%      +3  [ = ]       0    ../../src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
  +0.0%      +2  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp
  +0.0%      +2  [ = ]       0    ../../src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  -0.0%      -1  [ = ]       0    ../../src/lib/tecs/TECS.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/fw_rate_control/FixedwingRateControl.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/mc_rate_control/MulticopterRateControl.cpp
  -0.0%      -2  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
  -0.1%      -3  [ = ]       0    ../../src/lib/wind_estimator/WindEstimator.cpp
  -0.1%      -4  [ = ]       0    ../../src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
  -0.0%      -4  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  -0.1%      -4  [ = ]       0    ../../src/modules/gimbal/input_rc.cpp
  -0.1%      -4  [ = ]       0    ../../src/modules/gimbal/output_mavlink.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/accel.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/baro.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/gyro.cpp
  -0.1%      -6  [ = ]       0    ../../src/modules/temperature_compensation/temperature_calibration/mag.cpp
  -0.1%      -7  [ = ]       0    ../../src/modules/airspeed_selector/AirspeedValidator.cpp
  -0.1%      -7  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
  -0.1%      -8  [ = ]       0    ../../src/modules/commander/mag_calibration.cpp
  -0.1%      -8  [ = ]       0    ../../src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
  -0.1%      -8  [ = ]       0    ../../src/modules/vtol_att_control/tailsitter.cpp
  -0.0%      -9  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.0%     -10  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
  -0.3%     -11  [ = ]       0    ../../src/drivers/rc_input/crsf_telemetry.cpp
  -0.2%     -12  [ = ]       0    ../../src/modules/landing_target_estimator/LandingTargetEstimator.cpp
  -0.1%     -12  [ = ]       0    ../../src/modules/vtol_att_control/vtol_type.cpp
  -0.3%     -13  [ = ]       0    ../../src/drivers/osd/msp_osd/uorb_to_msp.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/camera_feedback/CameraFeedback.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/commander/failure_detector/FailureDetector.cpp
  -0.2%     -13  [ = ]       0    ../../src/modules/gimbal/output.cpp
  -0.1%     -13  [ = ]       0    ../../src/modules/vtol_att_control/tiltrotor.cpp
  -0.1%     -14  [ = ]       0    ../../src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  -0.1%     -14  [ = ]       0    ../../src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
  -0.2%     -14  [ = ]       0    ../../src/modules/vtol_att_control/standard.cpp
  -0.3%     -15  [ = ]       0    ../../platforms/common/uORB/uORB.cpp
  -0.3%     -15  [ = ]       0    ../../src/modules/commander/level_calibration.cpp
  -0.1%     -28  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  -0.3%     -35  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  -0.0%    -108  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  [DEL] -2.33Ki  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.0%    +192  [ = ]       0    .debug_frame
+0.0%    +157  [ = ]       0    .debug_str
  [NEW] +3.92Ki  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.0%    +105  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +12  [ = ]       0    [section .debug_str]
  -0.2%     -13  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  [DEL]    -332  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
 -62.0% -3.54Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
+0.0%    +145  [ = ]       0    .debug_abbrev
  +0.0%    +145  [ = ]       0    armv7-m/arm_vectors.c
+0.0%    +136  [ = ]       0    .debug_ranges
  [NEW]    +328  [ = ]       0    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.0%    +120  [ = ]       0    [section .debug_ranges]
  [DEL]    -312  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
+0.0%     +16  [ = ]       0    .debug_aranges
+0.0%     +16  +0.0%     +16    .text
  [NEW]    +924  [NEW]    +924    ../../src/lib/lat_lon_alt/lat_lon_alt.cpp
  +0.0%     +16  +0.0%     +16    [section .text]
  [DEL]    -924  [DEL]    -924    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
-0.0%     -17  [ = ]       0    [Unmapped]
+0.0% +8.21Ki  +0.0%     +16    TOTAL

Updated: 2024-11-26T17:24:59

@bresch bresch force-pushed the pr-ekf2_gpos_rework branch 3 times, most recently from 94b2686 to c4649fc Compare November 22, 2024 08:25
Base automatically changed from pr-ekf2_gpos_rework to main November 22, 2024 14:13
@github-actions GitHub Actions
Copy link

FLASH Analysis

px4_fmu-v5x 
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.2% +44.9Ki  [ = ]       0    .debug_info
     +26% +44.9Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.6% +32.5Ki  [ = ]       0    .debug_loc
     +31% +29.2Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
    +0.2% +3.38Ki  [ = ]       0    [section .debug_loc]
  +0.5% +9.68Ki  +0.5% +9.68Ki    .text
    +2.1% +5.79Ki  +2.1% +5.79Ki    [section .text]
     +22% +3.89Ki   +22% +3.89Ki    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.2% +7.98Ki  [ = ]       0    .debug_line
     +17% +7.98Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.4% +5.19Ki  [ = ]       0    .debug_ranges
     +18% +5.19Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.0% +1.17Ki  [ = ]       0    .debug_str
     +16% +4.68Ki  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
    +0.1%     +43  [ = ]       0    [section .debug_str]
     +13%     +31  [ = ]       0    ../../src/lib/drivers/magnetometer/PX4Magnetometer.cpp
    +0.0%      +4  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
    -2.4%      -8  [ = ]       0    ../../src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.cpp
    -6.8%     -10  [ = ]       0    ../../src/lib/conversion/rotation.cpp
    -3.5%     -21  [ = ]       0    ../../src/lib/geo/geo.cpp
   -62.0% -3.54Ki  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
  +0.1%    +608  [ = ]       0    .debug_frame
  +0.1%    +384  [ = ]       0    .symtab
     +12%    +272  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
    +0.1%    +112  [ = ]       0    [section .symtab]
  +0.0%    +123  [ = ]       0    .strtab
    +6.4%    +101  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
    +0.0%     +22  [ = ]       0    [section .strtab]
  +0.0%     +92  [ = ]       0    .debug_abbrev
    +0.0%     +92  [ = ]       0    armv7-m/arm_vectors.c
  +0.0%     +32  [ = ]       0    .debug_aranges
 -42.7% -9.68Ki  [ = ]       0    [Unmapped]
  +0.2% +92.9Ki  +0.5% +9.68Ki    TOTAL
px4_fmu-v6x 
    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%      +8  +0.0%      +8    .text
  -0.0%      -8  [ = ]       0    [Unmapped]
  [ = ]       0  +0.0%      +8    TOTAL

@bresch bresch changed the base branch from main to pr-sih_plane_fix November 25, 2024 15:28
@bresch bresch force-pushed the pr_ekf2_ellipsoidal_sih branch 3 times, most recently from d028c81 to da57330 Compare November 26, 2024 11:09
@romain-chiap
Copy link
Contributor

romain-chiap commented Nov 26, 2024 •
edited
Loading

Happy to see that the SIH is being used :)

I tried SITL

  • make px4_sitl sihsim_quadx, I get a Preflight Fail: height estimate not stable, then it's working
  • make px4_sitl sihsim_airplane, the sih outputs nan when I arm the vehicle

I also tried HITL

  • on fmu-v5 (Pixhawk 4) and SYS_AUTOSTART=1100 (quadx), I get a few Preflight: GPS Horizontal Pos Drift too high. I can arm in Altitude mode, most of the time it doesn't allow me to arm in Position mode.
  • on fmu-v5 and SYS_AUTOSTART=1101 (airplane), the airplane is unstable when trying to takeoff in altitude or stabilized mode.

@haumarco @bresch
SIH: ellipsoidal earth model
35d3c17 
SIH: use projection functions and constants from geo lib

SIH: remove unnecessary member variable

SIH: clarify names of rotation matrices and frames

SIH: do not store DCM corresponding to quaternion attitude

Using DCM is more efficient when more than 1 rotation needs to be done,
which is not the case here.

SIH: don't store local variable as member

SIH: use Wgs84 constants everywhere

SIH: do not store delta_quaternion

Converting an AxisAngle to a Quaternion uses the exponenial

SIH: organise ECEF member variables

SIH: add earth spin rate to gyro data
@bresch
Copy link
Member Author

bresch commented Nov 26, 2024 •
edited
Loading

Thanks for the tests @romain-chiap . FW sim failed because I forgot to commit some fixes in the other PR (76e9cf8). I added it and rebased this one on top; could you give it a try again please?

@romain-chiap
Copy link
Contributor

Thanks, so I tried again SITL quadx and airplane: they both work fine.

I was able to reach 230km with the airplane, then I got

ERROR [vehicle_magnetometer] MAG #0 failed:  TIMEOUT!	
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: No valid data from Compass 0

image

I still have the same problems on HITL: quadx can fly in altitude but not in position, FW is unstable at takeoff.

Base automatically changed from pr-sih_plane_fix to main November 27, 2024 16:14
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@romain-chiap romain-chiap Awaiting requested review from romain-chiap

@haumarco haumarco Awaiting requested review from haumarco

At least 1 approving review is required to merge this pull request.

Assignees

@bresch bresch

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@bresch @haumarco @romain-chiap