Prearm check to validate backend read rates against scheduler loop rate #28672

bugobliterator · 2024-11-19T00:22:59Z

This PR adds a prearm check that ensures that we are reading gyro data atleast as fast as we are running our main loop, where all controllers run at. We do this for all the INS sensors that are enabled.

This avoids a situation where user might have set SCHED LOOP RATE faster than gyro poll rate or primary gyro with appropriate backend rate but forgot to do so for the rest.

amilcarlucas · 2024-11-19T08:37:27Z

Binary Name      Text [B]        Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  --------------  -----------  -------------  ----------------------------  -------------------------
arducopter-heli  316 (+0.0174%)  0 (0.0000%)  -4 (-0.0015%)  316 (+0.0174%)                                   144800
antennatracker   316 (+0.0234%)  0 (0.0000%)  -4 (-0.0015%)  316 (+0.0234%)                                   614640
arducopter       316 (+0.0174%)  0 (0.0000%)  -4 (-0.0015%)  316 (+0.0173%)                                   144344
ardurover        316 (+0.0190%)  0 (0.0000%)  4 (+0.0015%)   316 (+0.0190%)                                   298392
arduplane        316 (+0.0174%)  0 (0.0000%)  -4 (-0.0015%)  316 (+0.0174%)                                   146824
blimp            316 (+0.0231%)  0 (0.0000%)  4 (+0.0015%)   316 (+0.0230%)                                   592716
ardusub          316 (+0.0196%)  0 (0.0000%)  -4 (-0.0015%)  316 (+0.0196%)                                   354132

andyp1per

Getting there

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

andyp1per · 2024-11-19T17:36:49Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

+        }
+        // if gyro backend rate hz is 0, that means we are polling sensor at the sample rate or higher
+        if (_backends[i]->get_gyro_backend_rate_hz() == 0) {
+            if (_gyro_raw_sample_rates[i] < _loop_rate) {


Don't you have to cater for oversampling here? See get_gyro_rate_hz()

Didn't we agree that the rate needs to be at least 2x the loop rate?

Don't you have to cater for oversampling here? See get_gyro_rate_hz()

No, oversampling is only set by drivers that use FIFO, and hence will be setting backend_rate_hz and we will be using that for comparision

Didn't we agree that the rate needs to be at least 2x the loop rate?

Done.

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

libraries/AP_InertialSensor/AP_InertialSensor.cpp

rmackay9

It looks good to me but I'll leave it to Andy and Sid to sort out naming

tridge

looks good to me, thanks @bugobliterator !

tridge · 2024-11-20T07:46:28Z

@andyp1per I'd like to get your comments on this if possible

peterbarker · 2024-11-20T07:46:54Z

@andyp1per looking for your approval on this one. Seems like a sensible sanity check.

andyp1per

Looks good now - thanks @bugobliterator !

peterbarker · 2024-11-25T22:45:05Z

I've rebased this on top of CI fixes and marking it for MergeOnCIPass.

peterbarker · 2024-11-26T04:40:11Z

.... has to pass CI:

2024-11-26T03:16:37.4288781Z AT-0007.5: Received TIMESYNC response after 0.000000s
2024-11-26T03:16:37.4289428Z AT-0008.8: AP: PreArm: Gyro 1 backend rate 760Hz < sched loop rat
2024-11-26T03:16:37.4289977Z AT-0008.8: AP: e 400Hz
2024-11-26T03:16:37.4290385Z AT-0010.3: AP: PreArm: Gyro 1 backend rate 760Hz < sched loop rat
2024-11-26T03:16:37.4290930Z AT-0010.3: AP: e 400Hz
2024-11-26T03:16:37.4291424Z AT-0011.8: AP: PreArm: Gyro 1 backend rate 760Hz < sched loop rat
2024-11-26T03:16:37.4291859Z AT-0011.8: AP: e 400Hz
2024-11-26T03:16:37.4292372Z AT-0013.3: AP: PreArm: Gyro 1 backend rate 760Hz < sched loop rat
2024-11-26T03:16:37.4292827Z AT-0013.3: AP: e 400Hz
2024-11-26T03:16:37.4293426Z AT-0013.4: Prearm bit never went true.  Attempting arm to elicit reason from autopilot
2024-11-26T03:16:37.4294321Z AT-0013.4: Arm motors with MAVLink cmd
2024-11-26T03:16:37.4296075Z AT-0013.4: Sending COMMAND_LONG to (1,1) (MAV_CMD_COMPONENT_ARM_DISARM=400) (p1=1.000000 p2=0.000000 p3=0.000000 p4=0.000000 p5=0.000000 p6=0.000000  p7=0.000000)
2024-11-26T03:16:37.4297415Z AT-0013.4: ACK received: COMMAND_ACK {command : 400, result : 4, progress : 0, result_param2 : 0, target_system : 250, target_component : 250} (0.000000s)

bugobliterator · 2024-12-11T09:08:59Z

Seems like SITL was failing the prearm because the second IMU gyro rate is 760 < 2*400.

Pixhawk1 and other boards using LSM9DS0 will have the same issue. @tridge

libraries/AP_InertialSensor/AP_InertialSensor.cpp

rmackay9 · 2025-02-18T00:20:10Z

@bugobliterator,

This PR is linked from our 4.6.0 issues list so I wonder if we want to try again to get this in?

…t scheduler loop rate

…ler loop rate

fixed

tridge · 2025-03-12T08:47:08Z

@bugobliterator I updated the fn to avoid the multiple fn calls, and shorted the msg so it fits in the message buffer, tested on a Pixhawk1, behaves as expected

tridge · 2025-03-14T22:05:48Z

@bugobliterator VectorNavEAHRS test is failing - it produces very low gyro rates. It is flyable on fixed wing but cannot be used for notch filtering. I think we will need an exception for external AHRS IMUs

peterbarker · 2025-03-16T05:01:14Z

OK, new real failure on EAHRS systems; we can consume gyro data from these systems and we're failing the new test:

2025-03-14T20:38:45.0515324Z AT-0057.2: AP: EKF2 IMU1 is using GPS
2025-03-14T20:38:45.0515815Z AT-0057.7: AP: PreArm: Gyro 0 rate 48Hz < loop ratex1.8 90Hz
2025-03-14T20:38:45.0516412Z AT-0058.6: AP: PreArm: Gyro 0 rate 48Hz < loop ratex1.8 90Hz
2025-03-14T20:38:45.0517000Z AT-0059.4: AP: PreArm: Gyro 0 rate 48Hz < loop ratex1.8 90Hz
2025-03-14T20:38:45.0517717Z AT-0059.7: Prearm bit never went true.  Attempting arm to elicit reason from autopilot
2025-03-14T20:38:45.0518384Z AT-0059.7: Arm motors with MAVLink cmd
2025-03-14T20:38:45.0519301Z AT-0059.7: Sending COMMAND_LONG to (1,1) (MAV_CMD_COMPONENT_ARM_DISARM=400) (p1=1.000000 p2=0.000000 p3=0.000000 p4=0.000000 p5=0.000000 p6=0.000000  p7=0.000000)
2025-03-14T20:38:45.0520955Z AT-0059.7: ACK received: COMMAND_ACK {command : 400, result : 4, progress : 0, result_param2 : 0, target_system : 250, target_component : 250} (0.000000s)
2025-03-14T20:38:45.0521977Z AT-0059.7: AP: Arm: Gyro 0 rate 48Hz < loop ratex1.8 90Hz
2025-03-14T20:38:45.0522457Z AT-0059.7: Delaying 5.000000 seconds
2025-03-14T20:38:45.0522936Z AT-0059.9: Exception caught: Prearm bit never went true
2025-03-14T20:38:45.0523436Z Traceback (most recent call last):
2025-03-14T20:38:45.0524243Z   File "/__w/ardupilot/ardupilot/Tools/autotest/vehicle_test_suite.py", line 8915, in run_one_test_attempt
2025-03-14T20:38:45.0525067Z     test_function(**test_kwargs)
2025-03-14T20:38:45.0525748Z   File "/__w/ardupilot/ardupilot/Tools/autotest/arduplane.py", line 3074, in VectorNavEAHRS
2025-03-14T20:38:45.0526547Z     self.fly_external_AHRS("VectorNav", 1, "ap1.txt")
2025-03-14T20:38:45.0527335Z   File "/__w/ardupilot/ardupilot/Tools/autotest/arduplane.py", line 2984, in fly_external_AHRS
2025-03-14T20:38:45.0528319Z     self.wait_ready_to_arm()
2025-03-14T20:38:45.0529082Z   File "/__w/ardupilot/ardupilot/Tools/autotest/vehicle_test_suite.py", line 8454, in wait_ready_to_arm
2025-03-14T20:38:45.0530136Z     self.wait_prearm_sys_status_healthy(timeout=timeout)
2025-03-14T20:38:45.0531075Z   File "/__w/ardupilot/ardupilot/Tools/autotest/vehicle_test_suite.py", line 8336, in wait_prearm_sys_status_healthy
2025-03-14T20:38:45.0532084Z     raise AutoTestTimeoutException("Prearm bit never went true")
2025-03-14T20:38:45.0532684Z vehicle_test_suite.AutoTestTimeoutException: Prearm bit never went true

peterbarker · 2025-03-16T05:02:15Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

+        }
+        const auto rate_hz = _backends[i]->get_gyro_backend_rate_hz();
+        if (rate_hz < threshold) {
+            hal.util->snprintf(fail_msg, fail_msg_len, "Gyro %d rate %dHz < loop ratex1.8 %dHz",


Suggested change

hal.util->snprintf(fail_msg, fail_msg_len, "Gyro %d rate %dHz < loop ratex1.8 %dHz",

hal.util->snprintf(fail_msg, fail_msg_len, "Gyro%d-rate=%dHz < loop rate*1.8=%dHz",

tridge · 2025-03-24T23:45:06Z

@bugobliterator ping on the test fail with vectornav? see above comments

…(DEVTYPE_SERIAL)

rmackay9 · 2025-04-25T11:52:24Z

this is included in 4.6.0-beta6

bugobliterator requested a review from andyp1per November 19, 2024 00:23

bugobliterator force-pushed the pr-sched-rate-prearm-check branch 3 times, most recently from fafeac5 to 6628454 Compare November 19, 2024 01:55

andyp1per requested changes Nov 19, 2024

View reviewed changes

bugobliterator force-pushed the pr-sched-rate-prearm-check branch from 6628454 to 023a3ad Compare November 20, 2024 06:27

bugobliterator requested a review from andyp1per November 20, 2024 06:30

bugobliterator added the DevCallEU label Nov 20, 2024

rmackay9 reviewed Nov 20, 2024

View reviewed changes

libraries/AP_InertialSensor/AP_InertialSensor.cpp Outdated Show resolved Hide resolved

rmackay9 approved these changes Nov 20, 2024

View reviewed changes

tridge approved these changes Nov 20, 2024

View reviewed changes

peterbarker removed the DevCallEU label Nov 20, 2024

rmackay9 added the WikiNeeded needs wiki update label Nov 20, 2024

tridge mentioned this pull request Nov 20, 2024

ArduPilot 4.6 issues list #28612

Open

94 tasks

bugobliterator mentioned this pull request Nov 20, 2024

Do not Read FIFO faster than requested rate for ICM45686 #27573

Merged

bugobliterator force-pushed the pr-sched-rate-prearm-check branch from 023a3ad to ff88246 Compare November 25, 2024 04:12

andyp1per approved these changes Nov 25, 2024

View reviewed changes

peterbarker force-pushed the pr-sched-rate-prearm-check branch from ff88246 to 94c4ebc Compare November 25, 2024 22:44

peterbarker added the MergeOnCIPass label Nov 25, 2024

peterbarker removed the MergeOnCIPass label Nov 26, 2024

tridge force-pushed the pr-sched-rate-prearm-check branch from 94c4ebc to d30d6f7 Compare December 11, 2024 03:40

bugobliterator force-pushed the pr-sched-rate-prearm-check branch from d30d6f7 to 70db719 Compare December 11, 2024 08:47

bugobliterator added the DevCallEU label Dec 18, 2024

tridge previously requested changes Dec 18, 2024

View reviewed changes

libraries/AP_InertialSensor/AP_InertialSensor.cpp Outdated Show resolved Hide resolved

peterbarker reviewed Dec 18, 2024

View reviewed changes

libraries/AP_InertialSensor/AP_InertialSensor.cpp Outdated Show resolved Hide resolved

tridge removed the DevCallEU label Dec 18, 2024

bugobliterator force-pushed the pr-sched-rate-prearm-check branch from 5939782 to dd644b3 Compare March 4, 2025 03:13

rmackay9 added the DevCallEU label Mar 4, 2025

tridge force-pushed the pr-sched-rate-prearm-check branch from dd644b3 to 7d16d55 Compare March 12, 2025 08:42

bugobliterator added 2 commits March 12, 2025 19:43

AP_InertialSensor: prearm check to validate backend read rates agains…

271be24

…t scheduler loop rate

AP_Arming: prearm check to validate backend read rates against schedu…

75ca7e5

…ler loop rate

tridge force-pushed the pr-sched-rate-prearm-check branch from 7d16d55 to 75ca7e5 Compare March 12, 2025 08:43

tridge approved these changes Mar 12, 2025

View reviewed changes

peterbarker approved these changes Mar 12, 2025

View reviewed changes

tridge added the MergeOnCIPass label Mar 12, 2025

peterbarker removed the DevCallEU label Mar 12, 2025

peterbarker reviewed Mar 16, 2025

View reviewed changes

peterbarker removed the MergeOnCIPass label Mar 18, 2025

rmackay9 added the DevCallTopic label Mar 24, 2025

CraigElder removed the DevCallTopic label Mar 24, 2025

AP_InertialSensor: don't do gyro rate prearm check for external ahrs …

bec18df

…(DEVTYPE_SERIAL)

bugobliterator force-pushed the pr-sched-rate-prearm-check branch from 3223ad4 to bec18df Compare April 2, 2025 00:22

rmackay9 added the DevCallEU label Apr 2, 2025

tridge merged commit e0c3234 into ArduPilot:master Apr 2, 2025
102 checks passed

github-project-automation bot moved this to Pending in 4.6 Backports Apr 2, 2025

IamPete1 removed the DevCallEU label Apr 5, 2025

rmackay9 moved this from Pending to 4.6.0-beta6 in 4.6 Backports Apr 25, 2025

	hal.util->snprintf(fail_msg, fail_msg_len, "Gyro %d rate %dHz < loop ratex1.8 %dHz",
	hal.util->snprintf(fail_msg, fail_msg_len, "Gyro%d-rate=%dHz < loop rate*1.8=%dHz",

Prearm check to validate backend read rates against scheduler loop rate #28672

Prearm check to validate backend read rates against scheduler loop rate #28672

Uh oh!

Conversation

bugobliterator commented Nov 19, 2024

Uh oh!

amilcarlucas commented Nov 19, 2024

Uh oh!

andyp1per left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

andyp1per Nov 19, 2024

Choose a reason for hiding this comment

Uh oh!

andyp1per Nov 19, 2024

Choose a reason for hiding this comment

Uh oh!

bugobliterator Nov 20, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

rmackay9 left a comment

Choose a reason for hiding this comment

Uh oh!

tridge left a comment

Choose a reason for hiding this comment

Uh oh!

tridge commented Nov 20, 2024

Uh oh!

peterbarker commented Nov 20, 2024

Uh oh!

andyp1per left a comment

Choose a reason for hiding this comment

Uh oh!

peterbarker commented Nov 25, 2024

Uh oh!

peterbarker commented Nov 26, 2024

Uh oh!

bugobliterator commented Dec 11, 2024

Uh oh!

Uh oh!

Uh oh!

rmackay9 commented Feb 18, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tridge commented Mar 12, 2025

Uh oh!

tridge commented Mar 14, 2025

Uh oh!

peterbarker commented Mar 16, 2025

Uh oh!

peterbarker Mar 16, 2025

Choose a reason for hiding this comment

Uh oh!

tridge commented Mar 24, 2025

Uh oh!

Uh oh!

rmackay9 commented Apr 25, 2025

Uh oh!

Uh oh!

bugobliterator Nov 20, 2024 •

edited

Loading

rmackay9 commented Feb 18, 2025 •

edited

Loading