diff --git a/cmake/pg_struct_sizes.reference.db b/cmake/pg_struct_sizes.reference.db index e5dfaf51a51..dfd9037673d 100644 --- a/cmake/pg_struct_sizes.reference.db +++ b/cmake/pg_struct_sizes.reference.db @@ -18,7 +18,7 @@ gimbalSerialConfig_t 1 0 gpsConfig_t 11 5 gyroConfig_t 64 12 headTrackerConfig_t 16 1 -imuConfig_t 16 2 +imuConfig_t 18 2 ledPinConfig_t 1 0 logConfig_t 8 0 modeActivationOperatorConfig_t 1 0 diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 55b0f19c103..4e9b91799e6 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -167,6 +167,7 @@ The CLI command `blackbox` allows setting which Blackbox fields are recorded to * `PEAKS_P` - Pitch axis noise peak * `PEAKS_Y` - Yaw axis noise peak * `SERVOS` - Servo outputs (for planes, tris, etc.) +* `MAG_LEARN` - In-flight compass offset learning (`mag_learn`), in the slow frame; logged only while the setting is ON On a board with two IMUs, `gyro_secondary_enabled` adds `gyroRaw2[0..2]`, the second sensor's rates in deg/s, in the same body frame as `gyroRaw` and with that sensor's diff --git a/docs/Sensors.md b/docs/Sensors.md index e8966bfdb4f..fba9df57f85 100644 --- a/docs/Sensors.md +++ b/docs/Sensors.md @@ -9,9 +9,19 @@ Best calibrated by checking barometer altitude in the Configurator Sensor tab, p ### Nav Acceleromter Z Axis Calibration The Nav accelerometer Z axis is calibrated in a similar way to the Barometer except it isn't so easy to check while calibrating given the lack of direct feedback. The acc Z value in the Configurator Sensors tab isn't the corrected value so can't be used. Instead set `debug_mode` to `ALTITUDE` and check the behaviour of `Debug 3` in the Configurator Sensors Tab for Debug output. It will drift during calibration then show a sudden change when calibration is finished. After saving and rebooting check `Debug 3` in the Configurator Sensors tab, it should show much less drift from zero if the calibration was successful. Successful calibration can also be checked by making sure `acc_temp_correction` shows a value other than 0 or 51. +## Compass offset learning in flight +With `mag_learn` ON, the compass offsets (`magzero_x/y/z`) are refitted from what the compass reads during each flight and saved after disarm. It refines a calibration that already exists: it needs a calibrated compass, leaves `maggain_x/y/z` as they are, and is not available on targets with 512 KB of flash or less. +While armed and flying, the flight controller keeps one reading for each direction the field has taken in the aircraft's frame (12 headings by 6 elevation bands). After disarm it fits a sphere to those readings, as the ground calibration does, and saves the new offsets only if all of these hold: +* the flight turned through at least 270 degrees of heading (9 of 12 heading sectors) and filled at least 12 sectors; +* the readings lie on a sphere: their distance from the fitted centre varies by no more than 5%. A field that changes with throttle (current in a battery lead next to the compass) or a nearby iron object fails this check; +* the sphere's radius is within 30% of what `maggain` implies, so the stored scale still holds; +* no offset moves by more than 30% of the radius in one flight. +A circle flown at one bank angle shows the field on a single tilted plane, and the offset across that plane stays unobserved: the fit keeps that part, and the vertical in level flight, near the stored value. Turns both ways, at a fair bank, measure it; otherwise it closes over a few flights. The steeper the local inclination of the field, the smaller the part of the error one flight removes. The learned offsets include the average field of the motor current during flight, which is what the heading needs in cruise. +Samples are taken only while the flight controller considers the aircraft flying, so an airplane needs a GPS; rovers and boats never collect, as INAV's flight detection does not run for them. The save waits out the 5 s in which INAV accepts an emergency rearm after a disarm, so that a rearm finds the same calibration and no write to the flash, and then for the aircraft to be still (it may be carried away right after landing). If it still moves a minute after the disarm, nothing is saved. Powering off before the save loses the flight's result. +With the `MAG_LEARN` field group selected (`blackbox MAG_LEARN`; on by default in a new configuration, while one kept from an earlier version needs it set, or the box in the Configurator's Blackbox tab), the blackbox logs the estimate, refitted about once a second, in the slow frame: `magBias[0..2]` (the change a save would write, in raw counts), `magBiasFlags` (bit 0 collecting, 1 paused, 4 save due, 5 too few sectors, 6 too few headings, 7 not a sphere, 8 off the stored scale, 9 step too big, 11 disarmed while flying), `magBiasSectors` and `magBiasSpread` (0.1% of the radius). The header records `mag_learn` and the `mag_zero` the flight started from. With the default `blackbox_arm_control` the log closes at disarm, before the save, and the next log's `mag_zero` shows what was written. `MSP2_INAV_MAG_LEARN` reports the same state, with bit 10 when the last disarm saved. diff --git a/docs/Settings.md b/docs/Settings.md index a0bdc187935..2f690bcbf6a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -286,6 +286,16 @@ Wind compensation in heading estimation from gps groundcourse(fixed wing only) --- +### ahrs_gsf_yaw + +Airplanes only. Uses the heading the GSF estimator learns in flight from GPS velocity and the IMU, without the compass, once it has settled: it replaces the GPS course as the AHRS's GPS heading (sharing with the compass as set by ahrs_gps_yaw_weight), the compass is left out while it disagrees with it, and a heading far from it with no compass to hold it is reset to it, at most twice a flight. Off, the estimator still runs and the blackbox logs it (GSF). Not on targets with 512 KB of flash or less. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### ahrs_inertia_comp_method Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop @@ -2886,6 +2896,16 @@ Selection of mag hardware. See Wiki Sensor auto detect and hardware failure dete --- +### mag_learn + +Refits the compass offsets (magzero; maggain is kept) from the readings of each flight and saves them after disarm, when the flight turned through at least 270 degrees and the fit agrees with the stored calibration. Needs a calibrated compass; airplanes and multirotors only. The blackbox logs the estimate as magBias. Not on targets with 512 KB of flash or less. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### mag_to_use Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index ec6054fb51b..f21bcb2d51c 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -474,6 +474,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: [8754 - MSP2_INAV_MAG_UNALIGNED](#msp2_inav_mag_unaligned) [8755 - MSP2_INAV_ESC_SRXL2_STATUS](#msp2_inav_esc_srxl2_status) [8756 - MSP2_INAV_ESC_SRXL2_CALIBRATE](#msp2_inav_esc_srxl2_calibrate) +[8757 - MSP2_INAV_MAG_LEARN](#msp2_inav_mag_learn) [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) [12289 - MSP2_RX_BIND](#msp2_rx_bind) @@ -5045,6 +5046,22 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Notes:** Requires USE_MOTOR_SRXL2 and at least one request byte. Accepted commands: 0 abort, 1 automatic start, 4 manual high, 5 manual low. Driver safety checks can reject start requests; read MSP2_INAV_ESC_SRXL2_STATUS for the reason. Other command values return an MSP error. Remove propellers before calibration. +## `MSP2_INAV_MAG_LEARN (8757 / 0x2235)` +**Description:** Reads the state of the in-flight compass offset learning for the current flight, or for the last one once disarmed. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `flags` | `uint16_t` | 2 | Bitmask | Bitmask (magLearnFlags_e): 0 collecting, 1 paused (armed, not flying), 4 save due at disarm, 5 too few sectors, 6 too few headings, 7 not a sphere, 8 off the stored scale, 9 step too big, 10 saved at the last disarm, 11 still moving a minute after the disarm (nothing saved). | +| `sectors` | `uint8_t` | 1 | - | Direction sectors filled, of 72 (12 headings x 6 elevation bands). | +| `headings` | `uint8_t` | 1 | - | Heading sectors filled, of 12. | +| `spread` | `uint16_t` | 2 | 0.1 % | Spread of the field magnitude around the fitted radius. | +| `delta` | `int16_t[3]` | 6 | Raw ADC | Fitted offset minus the stored magZero, X, Y, Z. After a save it is the change written. | + +**Notes:** Requires USE_MAG_LEARN (USE_MAG on targets with more than 512 KB of flash); without it the command is not supported. All zero until the first armed flight with mag_learn ON. The save is decided once the 5 s emergency rearm window is over and the aircraft is still, at most a minute after the disarm. Kept in RAM only: lost at power off. + ## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)` **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 7dd35b0a171..476c5105846 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -497,7 +497,9 @@ "BLACKBOX_FEATURE_GYRO_PEAKS_ROLL": "1 << 10", "BLACKBOX_FEATURE_GYRO_PEAKS_PITCH": "1 << 11", "BLACKBOX_FEATURE_GYRO_PEAKS_YAW": "1 << 12", - "BLACKBOX_FEATURE_SERVOS": "1 << 13" + "BLACKBOX_FEATURE_SERVOS": "1 << 13", + "BLACKBOX_FEATURE_MAG_LEARN": "1 << 14", + "BLACKBOX_FEATURE_GSF": "1 << 15" }, "BlackboxState": { "_source": "inav/src/main/blackbox/blackbox.h", @@ -1298,59 +1300,69 @@ "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6": "7", "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7": "8", "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8": "9", - "FLIGHT_LOG_FIELD_CONDITION_SERVOS": "10", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1": "11", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2": "12", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3": "13", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4": "14", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5": "15", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6": "16", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7": "17", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8": "18", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9": "19", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10": "20", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11": "21", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12": "22", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13": "23", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14": "24", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15": "25", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16": "26", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17": "27", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18": "28", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19": "29", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20": "30", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21": "31", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22": "32", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23": "33", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24": "34", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25": "35", - "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26": "36", - "FLIGHT_LOG_FIELD_CONDITION_MAG": "37", - "FLIGHT_LOG_FIELD_CONDITION_BARO": "38", - "FLIGHT_LOG_FIELD_CONDITION_PITOT": "39", - "FLIGHT_LOG_FIELD_CONDITION_VBAT": "40", - "FLIGHT_LOG_FIELD_CONDITION_AMPERAGE": "41", - "FLIGHT_LOG_FIELD_CONDITION_SURFACE": "42", - "FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV": "43", - "FLIGHT_LOG_FIELD_CONDITION_MC_NAV": "44", - "FLIGHT_LOG_FIELD_CONDITION_RSSI": "45", - "FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0": "46", - "FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1": "47", - "FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2": "48", - "FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME": "49", - "FLIGHT_LOG_FIELD_CONDITION_DEBUG": "50", - "FLIGHT_LOG_FIELD_CONDITION_NAV_ACC": "51", - "FLIGHT_LOG_FIELD_CONDITION_NAV_POS": "52", - "FLIGHT_LOG_FIELD_CONDITION_NAV_PID": "53", - "FLIGHT_LOG_FIELD_CONDITION_ACC": "54", - "FLIGHT_LOG_FIELD_CONDITION_ATTITUDE": "55", - "FLIGHT_LOG_FIELD_CONDITION_RC_DATA": "56", - "FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND": "57", - "FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW": "58", - "FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL": "59", - "FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH": "60", - "FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW": "61", - "FLIGHT_LOG_FIELD_CONDITION_NEVER": "62", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_9": "10", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_10": "11", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_11": "12", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_12": "13", + "FLIGHT_LOG_FIELD_CONDITION_SERVOS": "14", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1": "15", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2": "16", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3": "17", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4": "18", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5": "19", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6": "20", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7": "21", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8": "22", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9": "23", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10": "24", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11": "25", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12": "26", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13": "27", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14": "28", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15": "29", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16": "30", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17": "31", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18": "32", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19": "33", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20": "34", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21": "35", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22": "36", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23": "37", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24": "38", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25": "39", + "FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26": "40", + "FLIGHT_LOG_FIELD_CONDITION_MAG": "41", + "FLIGHT_LOG_FIELD_CONDITION_BARO": "42", + "FLIGHT_LOG_FIELD_CONDITION_PITOT": "43", + "FLIGHT_LOG_FIELD_CONDITION_VBAT": "44", + "FLIGHT_LOG_FIELD_CONDITION_AMPERAGE": "45", + "FLIGHT_LOG_FIELD_CONDITION_SURFACE": "46", + "FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV": "47", + "FLIGHT_LOG_FIELD_CONDITION_MC_NAV": "48", + "FLIGHT_LOG_FIELD_CONDITION_RSSI": "49", + "FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0": "50", + "FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1": "51", + "FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2": "52", + "FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME": "53", + "FLIGHT_LOG_FIELD_CONDITION_DEBUG": "54", + "FLIGHT_LOG_FIELD_CONDITION_NAV_ACC": "55", + "FLIGHT_LOG_FIELD_CONDITION_NAV_POS": "56", + "FLIGHT_LOG_FIELD_CONDITION_NAV_PID": "57", + "FLIGHT_LOG_FIELD_CONDITION_ACC": "58", + "FLIGHT_LOG_FIELD_CONDITION_ATTITUDE": "59", + "FLIGHT_LOG_FIELD_CONDITION_RC_DATA": "60", + "FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND": "61", + "FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW": "62", + "FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY": [ + "(63)", + "USE_DUAL_GYRO" + ], + "FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL": "64", + "FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH": "65", + "FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW": "66", + "FLIGHT_LOG_FIELD_CONDITION_MAG_LEARN": "67", + "FLIGHT_LOG_FIELD_CONDITION_GSF": "68", + "FLIGHT_LOG_FIELD_CONDITION_NEVER": "69", "FLIGHT_LOG_FIELD_CONDITION_FIRST": "FLIGHT_LOG_FIELD_CONDITION_ALWAYS", "FLIGHT_LOG_FIELD_CONDITION_LAST": "FLIGHT_LOG_FIELD_CONDITION_NEVER" }, @@ -1721,6 +1733,17 @@ "GYRO_HARDWARE_LPF_EXPERIMENTAL": "3", "GYRO_HARDWARE_LPF_COUNT": "4" }, + "gsfYawFlags_e": { + "_source": "inav/src/main/flight/gsf_yaw.h", + "GSF_YAW_FUSING": "1 << 0", + "GSF_YAW_USABLE": "1 << 1", + "GSF_YAW_INCONSISTENT": "1 << 2", + "GSF_YAW_DELAY_MEASURED": "1 << 3", + "GSF_YAW_MAG_COMPARED": "1 << 4", + "GSF_YAW_MAG_DISAGREES": "1 << 5", + "GSF_YAW_IN_AHRS": "1 << 6", + "GSF_YAW_RESET": "1 << 7" + }, "gyroSensor_e": { "_source": "inav/src/main/sensors/gyro.h", "GYRO_NONE": "0", @@ -2543,6 +2566,19 @@ "LTM_RATE_MEDIUM": "1", "LTM_RATE_SLOW": "2" }, + "magLearnFlags_e": { + "_source": "inav/src/main/sensors/compass_learn.h", + "MAG_LEARN_COLLECTING": "1 << 0", + "MAG_LEARN_PAUSED": "1 << 1", + "MAG_LEARN_SAVE_DUE": "1 << 4", + "MAG_LEARN_FEW_SECTORS": "1 << 5", + "MAG_LEARN_FEW_HEADINGS": "1 << 6", + "MAG_LEARN_NOT_SPHERE": "1 << 7", + "MAG_LEARN_OFF_SCALE": "1 << 8", + "MAG_LEARN_STEP_TOO_BIG": "1 << 9", + "MAG_LEARN_SAVED": "1 << 10", + "MAG_LEARN_DISARMED_FLYING": "1 << 11" + }, "magSensor_e": { "_source": "inav/src/main/sensors/compass.h", "MAG_NONE": "0", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 471655cd695..7adea12987c 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -153,6 +153,7 @@ - [gyro_fsr_e](#enum-gyro_fsr_e) - [gyroFilterMode_e](#enum-gyrofiltermode_e) - [gyroHardwareLpf_e](#enum-gyrohardwarelpf_e) +- [gsfYawFlags_e](#enum-gsfyawflags_e) - [gyroSensor_e](#enum-gyrosensor_e) - [HardwareMotorTypes_e](#enum-hardwaremotortypes_e) - [hardwareSensorStatus_e](#enum-hardwaresensorstatus_e) @@ -195,6 +196,7 @@ - [ltm_frame_e](#enum-ltm_frame_e) - [ltm_modes_e](#enum-ltm_modes_e) - [ltmUpdateRate_e](#enum-ltmupdaterate_e) +- [magLearnFlags_e](#enum-maglearnflags_e) - [magSensor_e](#enum-magsensor_e) - [mavlinkAutopilotType_e](#enum-mavlinkautopilottype_e) - [mavlinkFcDispatchResult_e](#enum-mavlinkfcdispatchresult_e) @@ -1060,6 +1062,8 @@ | `BLACKBOX_FEATURE_GYRO_PEAKS_PITCH` | 1 << 11 | | | `BLACKBOX_FEATURE_GYRO_PEAKS_YAW` | 1 << 12 | | | `BLACKBOX_FEATURE_SERVOS` | 1 << 13 | | +| `BLACKBOX_FEATURE_MAG_LEARN` | 1 << 14 | | +| `BLACKBOX_FEATURE_GSF` | 1 << 15 | | --- ## `BlackboxState` @@ -2190,59 +2194,66 @@ | `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6` | 7 | | | `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7` | 8 | | | `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8` | 9 | | -| `FLIGHT_LOG_FIELD_CONDITION_SERVOS` | 10 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1` | 11 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2` | 12 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3` | 13 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4` | 14 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5` | 15 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6` | 16 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7` | 17 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8` | 18 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9` | 19 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10` | 20 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11` | 21 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12` | 22 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13` | 23 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14` | 24 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15` | 25 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16` | 26 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17` | 27 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18` | 28 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19` | 29 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20` | 30 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21` | 31 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22` | 32 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23` | 33 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24` | 34 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25` | 35 | | -| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26` | 36 | | -| `FLIGHT_LOG_FIELD_CONDITION_MAG` | 37 | | -| `FLIGHT_LOG_FIELD_CONDITION_BARO` | 38 | | -| `FLIGHT_LOG_FIELD_CONDITION_PITOT` | 39 | | -| `FLIGHT_LOG_FIELD_CONDITION_VBAT` | 40 | | -| `FLIGHT_LOG_FIELD_CONDITION_AMPERAGE` | 41 | | -| `FLIGHT_LOG_FIELD_CONDITION_SURFACE` | 42 | | -| `FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV` | 43 | | -| `FLIGHT_LOG_FIELD_CONDITION_MC_NAV` | 44 | | -| `FLIGHT_LOG_FIELD_CONDITION_RSSI` | 45 | | -| `FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0` | 46 | | -| `FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1` | 47 | | -| `FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2` | 48 | | -| `FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME` | 49 | | -| `FLIGHT_LOG_FIELD_CONDITION_DEBUG` | 50 | | -| `FLIGHT_LOG_FIELD_CONDITION_NAV_ACC` | 51 | | -| `FLIGHT_LOG_FIELD_CONDITION_NAV_POS` | 52 | | -| `FLIGHT_LOG_FIELD_CONDITION_NAV_PID` | 53 | | -| `FLIGHT_LOG_FIELD_CONDITION_ACC` | 54 | | -| `FLIGHT_LOG_FIELD_CONDITION_ATTITUDE` | 55 | | -| `FLIGHT_LOG_FIELD_CONDITION_RC_DATA` | 56 | | -| `FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND` | 57 | | -| `FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW` | 58 | | -| `FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL` | 59 | | -| `FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH` | 60 | | -| `FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW` | 61 | | -| `FLIGHT_LOG_FIELD_CONDITION_NEVER` | 62 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_9` | 10 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_10` | 11 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_11` | 12 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_12` | 13 | | +| `FLIGHT_LOG_FIELD_CONDITION_SERVOS` | 14 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1` | 15 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_2` | 16 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_3` | 17 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_4` | 18 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_5` | 19 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_6` | 20 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_7` | 21 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_8` | 22 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_9` | 23 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_10` | 24 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_11` | 25 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_12` | 26 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_13` | 27 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_14` | 28 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_15` | 29 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16` | 30 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17` | 31 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18` | 32 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19` | 33 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20` | 34 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21` | 35 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22` | 36 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23` | 37 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24` | 38 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25` | 39 | | +| `FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26` | 40 | | +| `FLIGHT_LOG_FIELD_CONDITION_MAG` | 41 | | +| `FLIGHT_LOG_FIELD_CONDITION_BARO` | 42 | | +| `FLIGHT_LOG_FIELD_CONDITION_PITOT` | 43 | | +| `FLIGHT_LOG_FIELD_CONDITION_VBAT` | 44 | | +| `FLIGHT_LOG_FIELD_CONDITION_AMPERAGE` | 45 | | +| `FLIGHT_LOG_FIELD_CONDITION_SURFACE` | 46 | | +| `FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV` | 47 | | +| `FLIGHT_LOG_FIELD_CONDITION_MC_NAV` | 48 | | +| `FLIGHT_LOG_FIELD_CONDITION_RSSI` | 49 | | +| `FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0` | 50 | | +| `FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1` | 51 | | +| `FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2` | 52 | | +| `FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME` | 53 | | +| `FLIGHT_LOG_FIELD_CONDITION_DEBUG` | 54 | | +| `FLIGHT_LOG_FIELD_CONDITION_NAV_ACC` | 55 | | +| `FLIGHT_LOG_FIELD_CONDITION_NAV_POS` | 56 | | +| `FLIGHT_LOG_FIELD_CONDITION_NAV_PID` | 57 | | +| `FLIGHT_LOG_FIELD_CONDITION_ACC` | 58 | | +| `FLIGHT_LOG_FIELD_CONDITION_ATTITUDE` | 59 | | +| `FLIGHT_LOG_FIELD_CONDITION_RC_DATA` | 60 | | +| `FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND` | 61 | | +| `FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW` | 62 | | +| `FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY` | (63) | USE_DUAL_GYRO | +| `FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL` | 64 | | +| `FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH` | 65 | | +| `FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW` | 66 | | +| `FLIGHT_LOG_FIELD_CONDITION_MAG_LEARN` | 67 | | +| `FLIGHT_LOG_FIELD_CONDITION_GSF` | 68 | | +| `FLIGHT_LOG_FIELD_CONDITION_NEVER` | 69 | | | `FLIGHT_LOG_FIELD_CONDITION_FIRST` | FLIGHT_LOG_FIELD_CONDITION_ALWAYS | | | `FLIGHT_LOG_FIELD_CONDITION_LAST` | FLIGHT_LOG_FIELD_CONDITION_NEVER | | @@ -2824,6 +2835,21 @@ | `GYRO_HARDWARE_LPF_COUNT` | 4 | | --- +## `gsfYawFlags_e` + +> Source: src/main/flight/gsf_yaw.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GSF_YAW_FUSING` | 1 << 0 | | +| `GSF_YAW_USABLE` | 1 << 1 | | +| `GSF_YAW_INCONSISTENT` | 1 << 2 | | +| `GSF_YAW_DELAY_MEASURED` | 1 << 3 | | +| `GSF_YAW_MAG_COMPARED` | 1 << 4 | | +| `GSF_YAW_MAG_DISAGREES` | 1 << 5 | | +| `GSF_YAW_IN_AHRS` | 1 << 6 | | +| `GSF_YAW_RESET` | 1 << 7 | | + ## `gyroSensor_e` > Source: src/main/sensors/gyro.h @@ -3804,6 +3830,24 @@ | `LTM_RATE_MEDIUM` | 1 | | | `LTM_RATE_SLOW` | 2 | | +--- +## `magLearnFlags_e` + +> Source: src/main/sensors/compass_learn.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAG_LEARN_COLLECTING` | 1 << 0 | | +| `MAG_LEARN_PAUSED` | 1 << 1 | | +| `MAG_LEARN_SAVE_DUE` | 1 << 4 | | +| `MAG_LEARN_FEW_SECTORS` | 1 << 5 | | +| `MAG_LEARN_FEW_HEADINGS` | 1 << 6 | | +| `MAG_LEARN_NOT_SPHERE` | 1 << 7 | | +| `MAG_LEARN_OFF_SCALE` | 1 << 8 | | +| `MAG_LEARN_STEP_TOO_BIG` | 1 << 9 | | +| `MAG_LEARN_SAVED` | 1 << 10 | | +| `MAG_LEARN_DISARMED_FLYING` | 1 << 11 | | + --- ## `magSensor_e` diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 50d6d0b27d0..7eee7aba117 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -2,7 +2,7 @@ "version": { "major": 2, "minor": 1, - "patch": 2 + "patch": 3 }, "messages": { "MSP_API_VERSION": { @@ -12023,6 +12023,50 @@ "reply": null, "notes": "Requires USE_MOTOR_SRXL2 and at least one request byte. Accepted commands: 0 abort, 1 automatic start, 4 manual high, 5 manual low. Driver safety checks can reject start requests; read MSP2_INAV_ESC_SRXL2_STATUS for the reason. Other command values return an MSP error. Remove propellers before calibration.", "description": "Controls SRXL2 ESC throttle-range calibration." + }, + "MSP2_INAV_MAG_LEARN": { + "code": 8757, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "flags", + "ctype": "uint16_t", + "desc": "Bitmask (magLearnFlags_e): 0 collecting, 1 paused (armed, not flying), 4 save due at disarm, 5 too few sectors, 6 too few headings, 7 not a sphere, 8 off the stored scale, 9 step too big, 10 saved at the last disarm, 11 still moving a minute after the disarm (nothing saved).", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "sectors", + "ctype": "uint8_t", + "desc": "Direction sectors filled, of 72 (12 headings x 6 elevation bands).", + "units": "" + }, + { + "name": "headings", + "ctype": "uint8_t", + "desc": "Heading sectors filled, of 12.", + "units": "" + }, + { + "name": "spread", + "ctype": "uint16_t", + "desc": "Spread of the field magnitude around the fitted radius.", + "units": "0.1 %" + }, + { + "name": "delta", + "ctype": "int16_t", + "desc": "Fitted offset minus the stored magZero, X, Y, Z. After a save it is the change written.", + "units": "Raw ADC", + "array": true, + "array_size": 3 + } + ] + }, + "notes": "Requires USE_MAG_LEARN (USE_MAG on targets with more than 512 KB of flash); without it the command is not supported. All zero until the first armed flight with mag_learn ON. The save is decided once the 5 s emergency rearm window is over and the aircraft is still, at most a minute after the disarm. Kept in RAM only: lost at power off.", + "description": "Reads the state of the in-flight compass offset learning for the current flight, or for the last one once disarmed." } } } diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 1cfd028c3f9..409884ec183 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -347,6 +347,8 @@ main_sources(COMMON_SRC flight/failsafe.c flight/failsafe.h + flight/gsf_yaw.c + flight/gsf_yaw.h flight/imu.c flight/imu.h flight/kalman.c @@ -505,6 +507,8 @@ main_sources(COMMON_SRC sensors/boardalignment.h sensors/compass.c sensors/compass.h + sensors/compass_learn.c + sensors/compass_learn.h sensors/compass_orientation.c sensors/compass_orientation.h sensors/diagnostics.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fc6b8c11d3b..0bc03cca799 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -57,6 +57,7 @@ #include "fc/rc_smoothing.h" #include "flight/failsafe.h" +#include "flight/gsf_yaw.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" @@ -79,6 +80,7 @@ #include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/compass.h" +#include "sensors/compass_learn.h" #include "sensors/gyro.h" #include "sensors/pitotmeter.h" #include "sensors/rangefinder.h" @@ -118,7 +120,7 @@ PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, .includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS | BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE | BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | - BLACKBOX_FEATURE_MOTORS | BLACKBOX_FEATURE_SERVOS, + BLACKBOX_FEATURE_MOTORS | BLACKBOX_FEATURE_SERVOS | BLACKBOX_FEATURE_MAG_LEARN, ); void blackboxIncludeFlagSet(uint32_t mask) @@ -397,6 +399,10 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"motor", 5, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)}, {"motor", 6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)}, {"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)}, + {"motor", 8, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_9)}, + {"motor", 9, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_10)}, + {"motor", 10, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_11)}, + {"motor", 11, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_12)}, /* servos */ {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_1)}, @@ -458,6 +464,15 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC}, {"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC}, {"navAcc", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC}, +#ifdef USE_GSF_YAW + {"gsfYaw", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, + {"gsfYawStd", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, + {"gsfExcitation", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, + {"gsfNis", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, + {"gsfFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, + {"gsfGpsDelay", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, + {"gsfMagErr", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(GSF)}, +#endif }; #ifdef USE_GPS @@ -486,57 +501,65 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = { }; #endif -// Rarely-updated fields -static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { +// Rarely-updated fields; one without a condition is always logged +static const blackboxConditionalFieldDefinition_t blackboxSlowFields[] = { /* "flightModeFlags" renamed internally to more correct ref of rcModeFlags, since it logs rc boxmode selections, * but name kept for external compatibility reasons. * "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes. * 'active' should at least distinguish it from the existing "flightModeFlags" */ - {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - - {"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, - {"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, - {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, - {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)}, - - {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"wind", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"wind", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + + {"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, + {"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, + {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32), CONDITION(ALWAYS)}, + {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + + {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"wind", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"wind", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"wind", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - {"mspOverrideFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"mspOverrideFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, #endif - {"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #ifdef USE_BARO - {"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #endif #ifdef USE_TEMPERATURE_SENSOR - {"sens0Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens1Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens2Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens3Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens4Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens5Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens6Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"sens7Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"sens0Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens1Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens2Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens3Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens4Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens5Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens6Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"sens7Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #endif #ifdef USE_ESC_SENSOR - {"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)}, + {"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, + {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #endif #ifdef USE_DRONECAN - {"droneCANBusOffCount", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"droneCANBusOffCount", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, #endif #ifdef USE_TERRAIN - {"terrainAGL", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, - {"terrainAMSL", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"terrainAGL", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"terrainAMSL", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, +#endif +#ifdef USE_MAG_LEARN + {"magBias", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(MAG_LEARN)}, + {"magBias", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(MAG_LEARN)}, + {"magBias", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(MAG_LEARN)}, + {"magBiasFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(MAG_LEARN)}, + {"magBiasSectors", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(MAG_LEARN)}, + {"magBiasSpread", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(MAG_LEARN)}, #endif }; @@ -608,6 +631,15 @@ typedef struct blackboxMainState_s { int32_t navPos[XYZ_AXIS_COUNT]; int16_t navRealVel[XYZ_AXIS_COUNT]; int16_t navAccNEU[XYZ_AXIS_COUNT]; +#ifdef USE_GSF_YAW + uint16_t gsfYaw; // decidegrees + uint16_t gsfYawStd; // decidegrees + uint16_t gsfExcitation; // m/s + uint16_t gsfNis; // x100 + uint8_t gsfFlags; + uint16_t gsfGpsDelay; // ms + int16_t gsfMagErr; // decidegrees, compass heading minus the estimate +#endif int16_t navTargetVel[XYZ_AXIS_COUNT]; int32_t navTargetPos[XYZ_AXIS_COUNT]; int16_t navHeading; @@ -657,6 +689,9 @@ typedef struct blackboxSlowState_s { #ifdef USE_DRONECAN uint32_t droneCANBusOffCount; #endif +#ifdef USE_MAG_LEARN + magLearnStatus_t magLearn; +#endif } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -679,12 +714,11 @@ static struct { } u; } xmitState; -// Cache for FLIGHT_LOG_FIELD_CONDITION_* test results: -static uint64_t blackboxConditionCache; +// Cache for FLIGHT_LOG_FIELD_CONDITION_* test results, a bit for every condition up to and including LAST +static uint32_t blackboxConditionCache[FLIGHT_LOG_FIELD_CONDITION_LAST / 32 + 1]; -// The cache holds a bit for every condition up to and including LAST, which is NEVER, so -// it needs LAST + 1 bits. A dual-gyro target takes the 64th -STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) > FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions); +// The field definitions above declare 12 motors, and the writers log getMotorCount() of them +STATIC_ASSERT(MAX_SUPPORTED_MOTORS <= 12, blackbox_declares_up_to_12_motors); static uint32_t blackboxIFrameInterval; static uint32_t blackboxIteration; @@ -741,6 +775,10 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_9: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_10: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_11: + case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_12: return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS); case FLIGHT_LOG_FIELD_CONDITION_SERVOS: @@ -879,6 +917,18 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW: return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_PEAKS_YAW); +#ifdef USE_MAG_LEARN + case FLIGHT_LOG_FIELD_CONDITION_MAG_LEARN: + return sensors(SENSOR_MAG) && compassConfig()->magLearn && blackboxIncludeFlag(BLACKBOX_FEATURE_MAG_LEARN); +#endif + + case FLIGHT_LOG_FIELD_CONDITION_GSF: +#ifdef USE_GSF_YAW + return feature(FEATURE_GPS) && blackboxIncludeFlag(BLACKBOX_FEATURE_GSF); +#else + return false; +#endif + case FLIGHT_LOG_FIELD_CONDITION_NEVER: return false; @@ -889,21 +939,17 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) static void blackboxBuildConditionCache(void) { - blackboxConditionCache = 0; + memset(blackboxConditionCache, 0, sizeof(blackboxConditionCache)); for (uint8_t cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) { - - const uint64_t position = ((uint64_t)1) << cond; - if (testBlackboxConditionUncached(cond)) { - blackboxConditionCache |= position; + blackboxConditionCache[cond / 32] |= 1U << (cond % 32); } } } static bool testBlackboxCondition(FlightLogFieldCondition condition) { - const uint64_t position = ((uint64_t)1) << condition; - return (blackboxConditionCache & position) != 0; + return (blackboxConditionCache[condition / 32] >> (condition % 32)) & 1U; } static void blackboxSetState(BlackboxState newState) @@ -1149,6 +1195,18 @@ static void writeIntraframe(void) } } +#ifdef USE_GSF_YAW + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GSF)) { + blackboxWriteUnsignedVB(blackboxCurrent->gsfYaw); + blackboxWriteUnsignedVB(blackboxCurrent->gsfYawStd); + blackboxWriteUnsignedVB(blackboxCurrent->gsfExcitation); + blackboxWriteUnsignedVB(blackboxCurrent->gsfNis); + blackboxWriteUnsignedVB(blackboxCurrent->gsfFlags); + blackboxWriteUnsignedVB(blackboxCurrent->gsfGpsDelay); + blackboxWriteSignedVB(blackboxCurrent->gsfMagErr); + } +#endif + //Rotate our history buffers: //The current state becomes the new "before" state @@ -1414,6 +1472,18 @@ static void writeInterframe(void) } } +#ifdef USE_GSF_YAW + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GSF)) { + blackboxWriteSignedVB(blackboxCurrent->gsfYaw - blackboxLast->gsfYaw); + blackboxWriteSignedVB(blackboxCurrent->gsfYawStd - blackboxLast->gsfYawStd); + blackboxWriteSignedVB(blackboxCurrent->gsfExcitation - blackboxLast->gsfExcitation); + blackboxWriteSignedVB(blackboxCurrent->gsfNis - blackboxLast->gsfNis); + blackboxWriteSignedVB(blackboxCurrent->gsfFlags - blackboxLast->gsfFlags); + blackboxWriteSignedVB(blackboxCurrent->gsfGpsDelay - blackboxLast->gsfGpsDelay); + blackboxWriteSignedVB(blackboxCurrent->gsfMagErr - blackboxLast->gsfMagErr); + } +#endif + //Rotate our history buffers blackboxHistory[2] = blackboxHistory[1]; blackboxHistory[1] = blackboxHistory[0]; @@ -1480,6 +1550,15 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.droneCANBusOffCount); #endif +#ifdef USE_MAG_LEARN + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG_LEARN)) { + blackboxWriteSigned16VBArray(slowHistory.magLearn.delta, XYZ_AXIS_COUNT); + blackboxWriteUnsignedVB(slowHistory.magLearn.flags); + blackboxWriteUnsignedVB(slowHistory.magLearn.sectors); + blackboxWriteUnsignedVB(slowHistory.magLearn.spread); + } +#endif + blackboxSlowFrameIterationTimer = 0; } @@ -1584,6 +1663,14 @@ static void loadSlowState(blackboxSlowState_t *slow) slow->terrainAGL = terrainGetLastDistanceCm(); slow->terrainAMSL = terrainGetLastAMSL(); #endif +#ifdef USE_MAG_LEARN + // Zero when not logged, or the state changing underneath would still trigger slow frames + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG_LEARN)) { + slow->magLearn = magLearnStatus; + } else { + memset(&slow->magLearn, 0, sizeof(slow->magLearn)); + } +#endif } /** @@ -1904,6 +1991,17 @@ static void loadMainState(timeUs_t currentTimeUs) } blackboxCurrent->navTargetHeading = navDesiredHeading; blackboxCurrent->navSurface = navActualSurface; + +#ifdef USE_GSF_YAW + const gsfYawStatus_t *gsf = gsfYawGetStatus(); + blackboxCurrent->gsfYaw = constrain(lrintf(RADIANS_TO_DECIDEGREES(gsf->yaw)), 0, 3599); + blackboxCurrent->gsfYawStd = constrain(lrintf(RADIANS_TO_DECIDEGREES(gsf->yawSigma)), 0, 3600); + blackboxCurrent->gsfExcitation = constrain(lrintf(gsf->excitation), 0, UINT16_MAX); + blackboxCurrent->gsfNis = constrain(lrintf(gsf->nis * 100.0f), 0, UINT16_MAX); + blackboxCurrent->gsfFlags = gsf->flags; + blackboxCurrent->gsfGpsDelay = gsf->gpsDelayUs / 1000; + blackboxCurrent->gsfMagErr = (gsf->flags & GSF_YAW_MAG_COMPARED) ? lrintf(RADIANS_TO_DECIDEGREES(gsf->magError)) : 0; +#endif } /** @@ -2147,8 +2245,17 @@ static bool blackboxWriteSysinfo(void) #endif #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); +#ifdef USE_MAG_LEARN + BLACKBOX_PRINT_HEADER_LINE("mag_learn", "%d", compassConfig()->magLearn); + BLACKBOX_PRINT_HEADER_LINE("mag_zero", "%d,%d,%d", compassConfig()->magZero.raw[X], + compassConfig()->magZero.raw[Y], + compassConfig()->magZero.raw[Z]); +#endif #else BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", MAG_NONE); +#endif +#ifdef USE_GSF_YAW + BLACKBOX_PRINT_HEADER_LINE("ahrs_gsf_yaw", "%d", imuConfig()->gsf_yaw); #endif BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol); @@ -2415,7 +2522,7 @@ void blackboxUpdate(timeUs_t currentTimeUs) case BLACKBOX_STATE_SEND_SLOW_HEADER: //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAYLEN(blackboxSlowFields), - NULL, NULL)) { + &blackboxSlowFields[0].condition, &blackboxSlowFields[1].condition)) { blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO); } break; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 97829df9cca..28e9fe5f3db 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -36,6 +36,8 @@ typedef enum { BLACKBOX_FEATURE_GYRO_PEAKS_PITCH = 1 << 11, BLACKBOX_FEATURE_GYRO_PEAKS_YAW = 1 << 12, BLACKBOX_FEATURE_SERVOS = 1 << 13, + BLACKBOX_FEATURE_MAG_LEARN = 1 << 14, + BLACKBOX_FEATURE_GSF = 1 << 15, } blackboxFeatureMask_e; typedef enum BlackboxState { diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 8dc8f5defdd..24feafd5c34 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -33,6 +33,10 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_9, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_10, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_11, + FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_12, FLIGHT_LOG_FIELD_CONDITION_SERVOS, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1, @@ -106,6 +110,9 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH, FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW, + FLIGHT_LOG_FIELD_CONDITION_MAG_LEARN, + FLIGHT_LOG_FIELD_CONDITION_GSF, + FLIGHT_LOG_FIELD_CONDITION_NEVER, FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS, diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 15339c4f39e..46711f8567a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -200,6 +200,8 @@ static const char * const blackboxIncludeFlagNames[] = { "PEAKS_P", "PEAKS_Y", "SERVOS", + "MAG_LEARN", + "GSF", NULL }; #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 778dce2e511..4b2e009186e 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -114,7 +114,6 @@ enum { #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10 -#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000 timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index e5983f5f940..afa895dea40 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -33,6 +33,8 @@ typedef enum { DISARM_REASON_COUNT } disarmReason_e; +#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000 + void handleInflightCalibrationStickPosition(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3bf520b977a..e3f937689e9 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -135,6 +135,7 @@ #include "sensors/barometer.h" #include "sensors/pitotmeter.h" #include "sensors/compass.h" +#include "sensors/compass_learn.h" #include "sensors/gyro.h" #include "sensors/opflow.h" #include "sensors/temperature.h" @@ -1762,6 +1763,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; +#ifdef USE_MAG_LEARN + case MSP2_INAV_MAG_LEARN: + sbufWriteU16(dst, magLearnStatus.flags); + sbufWriteU8(dst, magLearnStatus.sectors); + sbufWriteU8(dst, magLearnStatus.headings); + sbufWriteU16(dst, magLearnStatus.spread); + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + sbufWriteU16(dst, magLearnStatus.delta[i]); + } + break; +#endif + case MSP2_INAV_MIXER: sbufWriteU8(dst, mixerConfig()->motorDirectionInverted); sbufWriteU8(dst, 0); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 31143e92706..800e416ed5c 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -47,6 +47,7 @@ #include "fc/runtime_config.h" #include "flight/dynamic_lpf.h" +#include "flight/gsf_yaw.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -404,6 +405,9 @@ void fcTasksInit(void) #ifdef USE_GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif +#ifdef USE_GSF_YAW + setTaskEnabled(TASK_GSF_YAW, feature(FEATURE_GPS) && sensors(SENSOR_ACC)); +#endif #ifdef USE_TERRAIN setTaskEnabled(TASK_TERRAIN, terrainConfig()->terrainEnabled); setTaskEnabled(TASK_TERRAIN_IO, terrainConfig()->terrainEnabled); @@ -600,6 +604,15 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#ifdef USE_GSF_YAW + [TASK_GSF_YAW] = { + .taskName = "GSF_YAW", + .taskFunc = taskGsfYaw, + .desiredPeriod = TASK_PERIOD_HZ(50), // the filter steps on 20 ms windows of IMU data + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + #ifdef USE_ADSB [TASK_ADSB] = { .taskName = "ADSB", diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 142f5f06c9b..4c83f293fef 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -664,6 +664,12 @@ groups: field: yawDeciDegrees min: -1800 max: 3600 + - name: mag_learn + description: "Refits the compass offsets (magzero; maggain is kept) from the readings of each flight and saves them after disarm, when the flight turned through at least 270 degrees and the fit agrees with the stored calibration. Needs a calibrated compass; airplanes and multirotors only. The blackbox logs the estimate as magBias. Not on targets with 512 KB of flash or less." + condition: USE_MAG_LEARN + default_value: OFF + field: magLearn + type: bool - name: PG_BAROMETER_CONFIG type: barometerConfig_t @@ -1687,6 +1693,12 @@ groups: field: gps_yaw_weight min: 0 max: 500 + - name: ahrs_gsf_yaw + description: "Airplanes only. Uses the heading the GSF estimator learns in flight from GPS velocity and the IMU, without the compass, once it has settled: it replaces the GPS course as the AHRS's GPS heading (sharing with the compass as set by ahrs_gps_yaw_weight), the compass is left out while it disagrees with it, and a heading far from it with no compass to hold it is reset to it, at most twice a flight. Off, the estimator still runs and the blackbox logs it (GSF). Not on targets with 512 KB of flash or less." + condition: USE_GSF_YAW + default_value: OFF + field: gsf_yaw + type: bool - name: PG_ARMING_CONFIG type: armingConfig_t diff --git a/src/main/flight/gsf_yaw.c b/src/main/flight/gsf_yaw.c new file mode 100644 index 00000000000..1da6ba43449 --- /dev/null +++ b/src/main/flight/gsf_yaw.c @@ -0,0 +1,904 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +/* + * Yaw from GPS velocity and the IMU, without the compass: a bank of 3-state EKFs (velocity north, east and yaw), each + * started from a different yaw, combined as a Gaussian sum filter. After Paul Riseborough's EKF-GSF yaw estimator + * (ArduPilot AP_NavEKF/EKFGSF_yaw.cpp, PX4 ekf2 yaw_estimator); the covariance and Kalman equations are his. Unlike + * those, each model takes roll and pitch from INAV's AHRS instead of running its own: what reaches it is the + * acceleration in the horizontal plane and the rate of the nose's heading. The AHRS heading cancels out of both, so + * the compass reaches the estimate only through the tilt (the GPS inertia compensation uses the full orientation). + */ + +#include +#include + +#include "platform.h" + +#include "common/maths.h" + +#include "flight/gsf_yaw.h" + +#define GSF_ACCEL_NOISE 2.0f // horizontal acceleration noise (m/s^2) +#define GSF_GYRO_NOISE 0.1f // yaw rate noise (rad/s) +#define GSF_VEL_ACC_MIN 0.5f // floor on the GPS velocity accuracy (m/s), for stability +#define GSF_WEIGHT_MIN 1e-5f +#define GSF_VAR_MIN 1e-6f + +// Bounded whatever the input: a loop would never end once 2 pi no longer changes the float +static float wrapPi(float a) +{ + return a - 2.0f * M_PIf * floorf((a + M_PIf) / (2.0f * M_PIf)); +} + +float gsfYawRate(const float up[3], const float rate[3]) +{ + // Euler yaw rate: (q sin roll + r cos roll) / cos pitch, which a roll about the nose leaves at zero + const float cosPitchSq = 1.0f - sq(up[0]); + if (cosPitchSq < 0.01f) { + // Within 6 deg of vertical the heading is undefined; the rate about the vertical stands in + return -(up[0] * rate[0] + up[1] * rate[1] + up[2] * rate[2]); + } + return -(up[1] * rate[1] + up[2] * rate[2]) / cosPitchSq; +} + +static void forceSymmetry(gsfModel_t *m) +{ + const float p01 = 0.5f * (m->P[0][1] + m->P[1][0]); + const float p02 = 0.5f * (m->P[0][2] + m->P[2][0]); + const float p12 = 0.5f * (m->P[1][2] + m->P[2][1]); + m->P[0][1] = m->P[1][0] = p01; + m->P[0][2] = m->P[2][0] = p02; + m->P[1][2] = m->P[2][1] = p12; +} + +void gsfReset(gsfState_t *gsf) +{ + memset(gsf, 0, sizeof(*gsf)); + + const float yawIncrement = 2.0f * M_PIf / GSF_MODEL_COUNT; + // Half the spacing: the farthest the best model can be from the truth + gsf->yawVariance = sq(0.5f * yawIncrement); + + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + gsf->model[i].X[2] = -M_PIf + 0.5f * yawIncrement + i * yawIncrement; + gsf->model[i].P[2][2] = gsf->yawVariance; + gsf->model[i].weight = 1.0f / GSF_MODEL_COUNT; + } +} + +// dvLevel: velocity change over dt forward and right of a reference yaw, which dYaw then moves on from +void gsfPredict(gsfState_t *gsf, const float dvLevel[2], float dYaw, float dt) +{ + const float dvxVar = sq(GSF_ACCEL_NOISE * dt); + const float dvyVar = dvxVar; + const float dazVar = sq(GSF_GYRO_NOISE * dt); + const float dvx = dvLevel[0]; + const float dvy = dvLevel[1]; + + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + gsfModel_t *m = &gsf->model[i]; + + if (gsf->fusing) { + const float t2 = sin_approx(m->X[2]); + const float t3 = cos_approx(m->X[2]); + + m->X[0] += dvx * t3 - dvy * t2; + m->X[1] += dvx * t2 + dvy * t3; + + const float P00 = m->P[0][0], P01 = m->P[0][1], P02 = m->P[0][2]; + const float P10 = m->P[1][0], P11 = m->P[1][1], P12 = m->P[1][2]; + const float P20 = m->P[2][0], P21 = m->P[2][1], P22 = m->P[2][2]; + + const float t4 = dvy * t3; + const float t5 = dvx * t2; + const float t6 = t4 + t5; + const float t8 = P22 * t6; + const float t7 = P02 - t8; + const float t9 = dvx * t3; + const float t11 = dvy * t2; + const float t10 = t9 - t11; + const float t12 = dvxVar * t2 * t3; + const float t13 = t2 * t2; + const float t14 = t3 * t3; + const float t15 = P22 * t10; + const float t16 = P12 + t15; + + m->P[0][0] = MAX(P00 - P20 * t6 + dvxVar * t14 + dvyVar * t13 - t6 * t7, GSF_VAR_MIN); + m->P[0][1] = P01 + t12 - P21 * t6 + t7 * t10 - dvyVar * t2 * t3; + m->P[0][2] = t7; + m->P[1][0] = P10 + t12 + P20 * t10 - t6 * t16 - dvyVar * t2 * t3; + m->P[1][1] = MAX(P11 + P21 * t10 + dvxVar * t13 + dvyVar * t14 + t10 * t16, GSF_VAR_MIN); + m->P[1][2] = t16; + m->P[2][0] = P20 - t8; + m->P[2][1] = P21 + t15; + m->P[2][2] = MAX(P22 + dazVar, GSF_VAR_MIN); + forceSymmetry(m); + } + + m->X[2] = wrapPi(m->X[2] + dYaw); + } +} + +static bool correctModel(gsfModel_t *m, float velNorth, float velEast, float velObsVar) +{ + m->innov[0] = m->X[0] - velNorth; + m->innov[1] = m->X[1] - velEast; + + const float P00 = m->P[0][0], P01 = m->P[0][1], P02 = m->P[0][2]; + const float P10 = m->P[1][0], P11 = m->P[1][1], P12 = m->P[1][2]; + const float P20 = m->P[2][0], P21 = m->P[2][1], P22 = m->P[2][2]; + + m->S[0][0] = P00 + velObsVar; + m->S[1][1] = P11 + velObsVar; + m->S[0][1] = P01; + m->S[1][0] = P10; + + // Innovations past 5 sigma are scaled down to it, against GPS spikes + float detS = m->S[0][0] * m->S[1][1] - m->S[0][1] * m->S[1][0]; + if (fabsf(detS) <= 1e-6f) { + return false; + } + detS = 1.0f / detS; + const float iNN = m->S[1][1] * detS; + const float iEE = m->S[0][0] * detS; + const float iNE = -m->S[0][1] * detS; + const float testRatio = m->innov[0] * (m->innov[0] * iNN + m->innov[1] * iNE) + m->innov[1] * (m->innov[0] * iNE + m->innov[1] * iEE); + const float innovScale = testRatio > 25.0f ? fast_fsqrtf(25.0f / testRatio) : 1.0f; + + const float t2 = P00 * velObsVar; + const float t3 = P11 * velObsVar; + const float t4 = velObsVar * velObsVar; + const float t5 = P00 * P11; + const float t9 = P01 * P10; + const float t6 = t2 + t3 + t4 + t5 - t9; + if (fabsf(t6) <= 1e-6f) { + return false; + } + const float t7 = 1.0f / t6; + const float t8 = P11 + velObsVar; + const float t10 = P00 + velObsVar; + + const float K[3][2] = { + { -P01 * P10 * t7 + P00 * t7 * t8, -P00 * P01 * t7 + P01 * t7 * t10 }, + { -P10 * P11 * t7 + P10 * t7 * t8, -P01 * P10 * t7 + P11 * t7 * t10 }, + { -P10 * P21 * t7 + P20 * t7 * t8, -P01 * P20 * t7 + P21 * t7 * t10 }, + }; + + const float t11 = P00 * P01 * t7; + const float t15 = P01 * t7 * t10; + const float t12 = t11 - t15; + const float t13 = P01 * P10 * t7; + const float t16 = P00 * t7 * t8; + const float t14 = t13 - t16; + const float t19 = t8 * t12 + P01 * t14; + const float t22 = t10 * t14 + P10 * t12; + const float t27 = P11 * t7 * t10; + const float t23 = t13 - t27; + const float t24 = P10 * P11 * t7; + const float t26 = P10 * t7 * t8; + const float t25 = t24 - t26; + const float t30 = t8 * t23 + P01 * t25; + const float t33 = t10 * t25 + P10 * t23; + const float t34 = P01 * P20 * t7; + const float t38 = P21 * t7 * t10; + const float t35 = t34 - t38; + const float t36 = P10 * P21 * t7; + const float t39 = P20 * t7 * t8; + const float t37 = t36 - t39; + const float t42 = t8 * t35 + P01 * t37; + const float t45 = t10 * t37 + P10 * t35; + + m->P[0][0] = MAX(P00 - t12 * t19 - t14 * t22, GSF_VAR_MIN); + m->P[0][1] = P01 - t19 * t23 - t22 * t25; + m->P[0][2] = P02 - t19 * t35 - t22 * t37; + m->P[1][0] = P10 - t12 * t30 - t14 * t33; + m->P[1][1] = MAX(P11 - t23 * t30 - t25 * t33, GSF_VAR_MIN); + m->P[1][2] = P12 - t30 * t35 - t33 * t37; + m->P[2][0] = P20 - t12 * t42 - t14 * t45; + m->P[2][1] = P21 - t23 * t42 - t25 * t45; + m->P[2][2] = MAX(P22 - t35 * t42 - t37 * t45, GSF_VAR_MIN); + forceSymmetry(m); + + for (int obs = 0; obs < 2; obs++) { + for (int row = 0; row < 3; row++) { + m->X[row] -= K[row][obs] * m->innov[obs] * innovScale; + } + } + m->X[2] = wrapPi(m->X[2]); + + return true; +} + +static float gaussianDensity(const gsfModel_t *m) +{ + const float det = m->S[0][0] * m->S[1][1] - m->S[0][1] * m->S[1][0]; + const float detInv = 1.0f / MAX(det, 1e-12f); + const float inv00 = detInv * m->S[1][1]; + const float inv11 = detInv * m->S[0][0]; + const float inv01 = -detInv * m->S[0][1]; + const float inv10 = -detInv * m->S[1][0]; + const float tv0 = inv00 * m->innov[0] + inv01 * m->innov[1]; + const float tv1 = inv10 * m->innov[0] + inv11 * m->innov[1]; + const float normDist = tv0 * m->innov[0] + tv1 * m->innov[1]; + + return expf(-0.5f * normDist) * fast_fsqrtf(detInv) / (2.0f * M_PIf); +} + +// Returns false when the update was skipped +bool gsfFuseVelocity(gsfState_t *gsf, float velNorth, float velEast, float velAccuracy) +{ + const float velObsVar = sq(MAX(velAccuracy, GSF_VEL_ACC_MIN)); + + if (!gsf->fusing) { + // In-flight alignment: velocities from the first sample, yaws spread over the circle + gsfReset(gsf); + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + gsf->model[i].X[0] = velNorth; + gsf->model[i].X[1] = velEast; + gsf->model[i].P[0][0] = velObsVar; + gsf->model[i].P[1][1] = velObsVar; + } + gsf->fusing = true; + return true; + } + + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + if (!correctModel(&gsf->model[i], velNorth, velEast, velObsVar)) { + return false; + } + } + + float newWeight[GSF_MODEL_COUNT]; + float totalWeight = 0.0f; + gsf->clipped = 0; + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + newWeight[i] = gaussianDensity(&gsf->model[i]) * gsf->model[i].weight; + if (newWeight[i] < GSF_WEIGHT_MIN) { + gsf->clipped++; + newWeight[i] = GSF_WEIGHT_MIN; + } + totalWeight += newWeight[i]; + } + + // Every model so far off that its weight underflowed: start again + if (gsf->clipped == GSF_MODEL_COUNT) { + gsfReset(gsf); + return false; + } + + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + gsf->model[i].weight = newWeight[i] / totalWeight; + } + + return true; +} + +void gsfUpdateComposite(gsfState_t *gsf) +{ + float sinSum = 0.0f; + float cosSum = 0.0f; + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + sinSum += gsf->model[i].weight * sin_approx(gsf->model[i].X[2]); + cosSum += gsf->model[i].weight * cos_approx(gsf->model[i].X[2]); + } + gsf->yaw = atan2_approx(sinSum, cosSum); + + float variance = 0.0f; + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + variance += gsf->model[i].weight * (gsf->model[i].P[2][2] + sq(wrapPi(gsf->model[i].X[2] - gsf->yaw))); + } + gsf->yawVariance = variance; +} + +#define GSF_DELAY_FORGET 0.998f // per GPS sample: about 50 s at 10 Hz +#define GSF_DELAY_BASELINE_US 400000 // long enough for a turn to swing the velocity change visibly +#define GSF_DELAY_MIN_SAMPLES 100 +#define GSF_DELAY_SIDE_STEPS 3 // the cost's curvature is taken 60 ms either side +#define GSF_DELAY_OVERLAP 4.0f // successive samples share most of a baseline: about 1 in 4 is independent +#define GSF_DELAY_MAX_SIGMA 1.25f // steps: 25 ms + +void gsfHistoryAdd(gsfImuHistory_t *h, uint32_t time, const float vel[2]) +{ + h->time[h->head] = time; + h->vel[h->head][0] = vel[0]; + h->vel[h->head][1] = vel[1]; + h->head = (h->head + 1) % GSF_HISTORY_COUNT; + if (h->count < GSF_HISTORY_COUNT) { + h->count++; + } +} + +// Interpolated between the windows around the time; false outside the history +static bool historyVelAt(const gsfImuHistory_t *h, uint32_t time, float vel[2]) +{ + int newer = -1; + for (int n = 0; n < h->count; n++) { + const int i = (h->head + GSF_HISTORY_COUNT - 1 - n) % GSF_HISTORY_COUNT; + const int32_t after = (int32_t)(time - h->time[i]); + if (after >= 0) { + if (newer < 0) { + if (after > 0) { + return false; + } + vel[0] = h->vel[i][0]; + vel[1] = h->vel[i][1]; + return true; + } + const float f = (float)after / (float)(int32_t)(h->time[newer] - h->time[i]); + vel[0] = h->vel[i][0] + f * (h->vel[newer][0] - h->vel[i][0]); + vel[1] = h->vel[i][1] + f * (h->vel[newer][1] - h->vel[i][1]); + return true; + } + newer = i; + } + return false; +} + +void gsfDelayAddGps(gsfDelayEstimator_t *e, const gsfImuHistory_t *h, uint32_t arrival, const float vel[2]) +{ + // The earlier sample nearest one baseline back + int32_t bestErr = GSF_DELAY_BASELINE_US / 2; + float prevVel[2] = { 0.0f, 0.0f }; + uint32_t prevArrival = 0; + bool found = false; + for (int n = 0; n < e->gpsCount; n++) { + const int i = (e->gpsHead + GSF_GPS_HISTORY - 1 - n) % GSF_GPS_HISTORY; + const int32_t err = ABS((int32_t)(arrival - e->gpsTime[i]) - GSF_DELAY_BASELINE_US); + if (err < bestErr) { + bestErr = err; + prevVel[0] = e->gpsVel[i][0]; + prevVel[1] = e->gpsVel[i][1]; + prevArrival = e->gpsTime[i]; + found = true; + } + } + + e->gpsTime[e->gpsHead] = arrival; + e->gpsVel[e->gpsHead][0] = vel[0]; + e->gpsVel[e->gpsHead][1] = vel[1]; + e->gpsHead = (e->gpsHead + 1) % GSF_GPS_HISTORY; + if (e->gpsCount < GSF_GPS_HISTORY) { + e->gpsCount++; + } + + float from[2], to[2]; + const uint32_t longest = (GSF_DELAY_STEPS - 1) * GSF_DELAY_STEP_US; + if (!found || !historyVelAt(h, arrival, to) || !historyVelAt(h, prevArrival - longest, from)) { + return; + } + + const float gps[2] = { vel[0] - prevVel[0], vel[1] - prevVel[1] }; + e->sgg = e->sgg * GSF_DELAY_FORGET + sq(gps[0]) + sq(gps[1]); + for (int j = 0; j < GSF_DELAY_STEPS; j++) { + const uint32_t delay = j * GSF_DELAY_STEP_US; + historyVelAt(h, arrival - delay, to); + historyVelAt(h, prevArrival - delay, from); + const float imu[2] = { to[0] - from[0], to[1] - from[1] }; + e->sii[j] = e->sii[j] * GSF_DELAY_FORGET + sq(imu[0]) + sq(imu[1]); + e->sdot[j] = e->sdot[j] * GSF_DELAY_FORGET + imu[0] * gps[0] + imu[1] * gps[1]; + e->scross[j] = e->scross[j] * GSF_DELAY_FORGET + imu[0] * gps[1] - imu[1] * gps[0]; + } + if (e->samples < UINT16_MAX) { + e->samples++; + } +} + +bool gsfDelayEstimate(const gsfDelayEstimator_t *e, uint32_t *delayUs) +{ + if (e->samples < GSF_DELAY_MIN_SAMPLES) { + return false; + } + + // Residual after the best rotation, for each delay + float cost[GSF_DELAY_STEPS]; + int best = 0; + for (int j = 0; j < GSF_DELAY_STEPS; j++) { + cost[j] = MAX(e->sii[j] + e->sgg - 2.0f * fast_fsqrtf(sq(e->sdot[j]) + sq(e->scross[j])), 0.0f); + if (cost[j] < cost[best]) { + best = j; + } + } + // At the end of the range the delay may be longer still + if (best == GSF_DELAY_STEPS - 1) { + return false; + } + + // As in least squares, the delay's variance is the residual per independent sample over the cost's curvature: it + // shrinks with every turn, however noisy the GPS + const int side = GSF_DELAY_SIDE_STEPS; + float curvature; // per step squared + if (best >= side && best + side < GSF_DELAY_STEPS) { + curvature = (cost[best - side] + cost[best + side] - 2.0f * cost[best]) / (2.0f * side * side); + } else if (best + side < GSF_DELAY_STEPS) { + curvature = (cost[best + side] - cost[best]) / (side * side); + } else { + curvature = (cost[best - side] - cost[best]) / (side * side); + } + const float independent = MIN(e->samples, 1.0f / (1.0f - GSF_DELAY_FORGET)) / GSF_DELAY_OVERLAP; + if (curvature <= 0.0f || cost[best] > sq(GSF_DELAY_MAX_SIGMA) * independent * curvature) { + return false; + } + + float offset = 0.0f; + if (best > 0) { + const float curvature = cost[best - 1] - 2.0f * cost[best] + cost[best + 1]; + if (curvature > 0.0f) { + offset = constrainf(0.5f * (cost[best - 1] - cost[best + 1]) / curvature, -0.5f, 0.5f); + } + } + *delayUs = lrintf((best + offset) * GSF_DELAY_STEP_US); + return true; +} + +#if defined(USE_GSF_YAW) + +#include "build/build_config.h" + +#include "common/time.h" + +#include "drivers/time.h" + +#include "fc/runtime_config.h" + +#include "flight/imu.h" + +#include "io/gps.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "sensors/compass.h" + +#define GSF_WINDOW_COUNT 16 // 320 ms of 20 ms IMU windows, room for the GPS delay +#define GSF_DELAY_MAX_US 280000u // what the windows can hold back +#define GSF_DELAY_DEFAULT_US 100000 // before the receiver's rate is known +#define GSF_DELAY_PROCESSING_US 20000 // on top of one GPS interval: 120 ms at 10 Hz and 220 ms at 5 Hz, ArduPilot's u-blox lags +#define GSF_VEL_ACCURACY 0.5f // m/s +#define GSF_MIN_SATS 6 +#define GSF_MAX_EPH_CM 500 +#define GSF_STEADY_RATE DEGREES_TO_RADIANS(90.0f) // faster rotation leaves the AHRS tilt less sure +#define GSF_EXCITATION_BLOCK_S 1.0f // velocity change counted per second and only past 1 m/s, so vibration and +#define GSF_EXCITATION_MIN 1.0f // tilt errors do not add up to manoeuvres in straight flight +#define GSF_USABLE_EXCITATION 25.0f // m/s: a 90 deg turn at 16 m/s +#define GSF_USABLE_SIGMA DEGREES_TO_RADIANS(15.0f) +#define GSF_USABLE_SETTLE_US 3000000 +#define GSF_NIS_FILTER 0.1f +#define GSF_NIS_INCONSISTENT 6.0f // chi-square with 2 degrees of freedom, 95 % +#define GSF_NIS_CONSISTENT 3.0f +#define GSF_INCONSISTENT_US 10000000 // then it learns again from scratch +#define GSF_WINDOW_US 20000 // the task period: a GPS sample is fused within one window of its time +#define GSF_GPS_STALE_US 1000000 // no GPS velocity for this long: the heading is the gyro's alone +#define GSF_FUSION_MIN_US 80000 // at most ~10 fusions a second, whatever the receiver's rate +#define GSF_MAG_ERROR_FILTER 0.02f // about 1 s at 50 Hz +#define GSF_MAG_ERROR_MIN DEGREES_TO_RADIANS(25.0f) +#define GSF_MAG_DISAGREE_US 3000000 +#define GSF_MAG_AGREE_US 10000000 +#define GSF_AHRS_FULL_SIGMA DEGREES_TO_RADIANS(5.0f) +#define GSF_RESET_ERROR DEGREES_TO_RADIANS(45.0f) // the gentle correction is slow from here, and nil from 180 +#define GSF_RESET_AFTER_US 1000000 +#define GSF_RESETS_MAX 2 // per flight, as ArduPilot's EK3_GSF_RST_MAX +#define GSF_IMU_DT_MAX 0.1f // s: a longer step is a stalled loop or a stale timestamp, not motion + +typedef struct gsfWindow_s { + float dv[2]; // m/s, forward and right of the yaw at the window's start + float dYaw; // rad + float dt; // s + float maxRateSq; // (rad/s)^2 + timeUs_t end; +} gsfWindow_t; + +STATIC_FASTRAM gsfState_t gsfState; +STATIC_FASTRAM gsfWindow_t gsfAccumulating; +STATIC_FASTRAM gsfWindow_t gsfWindows[GSF_WINDOW_COUNT]; +STATIC_FASTRAM uint8_t gsfWindowHead; // where the next window goes +STATIC_FASTRAM uint8_t gsfWindowCount; +STATIC_FASTRAM gsfImuHistory_t gsfHistory; +STATIC_FASTRAM gsfDelayEstimator_t gsfDelay; +STATIC_FASTRAM float gsfHistoryVel[2]; // m/s, velocity change summed since boot, in a frame the gyro turns +STATIC_FASTRAM float gsfHistoryYaw; // rad, that frame's yaw +STATIC_FASTRAM float gsfBlockDv[2]; +STATIC_FASTRAM float gsfBlockDt; +STATIC_FASTRAM gsfYawStatus_t gsfStatus; +STATIC_FASTRAM float gsfGpsVel[2]; // m/s, north and east +STATIC_FASTRAM timeUs_t gsfGpsArrival; +STATIC_FASTRAM timeUs_t gsfGpsTakenAt; +STATIC_FASTRAM timeUs_t gsfGpsLastArrival; +STATIC_FASTRAM float gsfGpsIntervalUs; +STATIC_FASTRAM bool gsfGpsPending; +STATIC_FASTRAM bool gsfGpsPendingDelay; +STATIC_FASTRAM timeUs_t gsfSettledSince; +STATIC_FASTRAM timeUs_t gsfLastFusion; +STATIC_FASTRAM timeUs_t gsfInconsistentSince; +STATIC_FASTRAM timeUs_t gsfMagDisagreeSince; +STATIC_FASTRAM timeUs_t gsfMagAgreeSince; +STATIC_FASTRAM timeUs_t gsfFarSince; +STATIC_FASTRAM bool gsfMagDisagrees; +STATIC_FASTRAM bool gsfResetRequested; +STATIC_FASTRAM bool gsfAirborne; +STATIC_FASTRAM bool gsfWasArmed; +STATIC_FASTRAM bool gsfWasTailsitter; +STATIC_FASTRAM bool gsfActive; // the task runs: without it the accumulator would grow for ever +STATIC_FASTRAM bool gsfDelayMeasured; +STATIC_FASTRAM uint32_t gsfDelayMeasuredUs; + +// FASTRAM is not zeroed at boot on every MCU (AT32's startup clears .bss only) +void gsfYawInit(void) +{ + memset(&gsfState, 0, sizeof(gsfState)); + gsfReset(&gsfState); + memset(&gsfAccumulating, 0, sizeof(gsfAccumulating)); + memset(gsfWindows, 0, sizeof(gsfWindows)); + gsfWindowHead = gsfWindowCount = 0; + memset(&gsfHistory, 0, sizeof(gsfHistory)); + memset(&gsfDelay, 0, sizeof(gsfDelay)); + gsfHistoryVel[0] = gsfHistoryVel[1] = gsfHistoryYaw = 0.0f; + gsfBlockDv[0] = gsfBlockDv[1] = gsfBlockDt = 0.0f; + memset(&gsfStatus, 0, sizeof(gsfStatus)); + gsfStatus.gpsDelayUs = GSF_DELAY_DEFAULT_US; + gsfGpsVel[0] = gsfGpsVel[1] = gsfGpsIntervalUs = 0.0f; + gsfGpsArrival = gsfGpsTakenAt = gsfGpsLastArrival = 0; + gsfSettledSince = gsfLastFusion = gsfInconsistentSince = 0; + gsfMagDisagreeSince = gsfMagAgreeSince = gsfFarSince = 0; + gsfGpsPending = gsfGpsPendingDelay = gsfMagDisagrees = gsfResetRequested = false; + gsfAirborne = gsfWasArmed = gsfWasTailsitter = gsfActive = gsfDelayMeasured = false; + gsfDelayMeasuredUs = 0; +} + +// Every AHRS update: out of line, since the AHRS is FAST_CODE on some targets (RP2350) and this is not time critical +NOINLINE void gsfYawAccumulateImu(float dT) +{ + if (!gsfActive || dT <= 0.0f || dT > GSF_IMU_DT_MAX) { + return; + } + const fpVector3_t *acc = &imuMeasuredAccelBF; // cm/s^2 + const fpVector3_t *rate = &imuMeasuredRotationBF; // rad/s + + // rMat's last row is earth-up in body axes: roll and pitch only, no heading in it + const float yawRate = gsfYawRate(rMat[2], rate->v); + + // Acceleration forward and right of the nose in the horizontal plane; the AHRS heading cancels between the two + const float noseN = rMat[0][0]; + const float noseW = rMat[1][0]; + const float noseLen = fast_fsqrtf(sq(noseN) + sq(noseW)); + if (noseLen > 0.1f) { + const float accN = rMat[0][0] * acc->x + rMat[0][1] * acc->y + rMat[0][2] * acc->z; + const float accW = rMat[1][0] * acc->x + rMat[1][1] * acc->y + rMat[1][2] * acc->z; + const float forward = (accN * noseN + accW * noseW) / noseLen; + const float right = (accN * noseW - accW * noseN) / noseLen; + + // Into the frame of the window's start, which the yaw has turned from since + const float c = cos_approx(gsfAccumulating.dYaw); + const float s = sin_approx(gsfAccumulating.dYaw); + gsfAccumulating.dv[0] += (forward * c - right * s) * dT * 0.01f; + gsfAccumulating.dv[1] += (forward * s + right * c) * dT * 0.01f; + } + + gsfAccumulating.dYaw += yawRate * dT; + gsfAccumulating.dt += dT; + gsfAccumulating.maxRateSq = MAX(gsfAccumulating.maxRateSq, vectorNormSquared(rate)); +} + +void gsfYawOnNewGpsData(void) +{ + if (!STATE(GPS_FIX) || !gpsSol.flags.validVelNE) { + return; + } + + // The receiver's rate gives the delay until one is measured + const timeUs_t now = micros(); + if (gsfGpsLastArrival) { + const timeDelta_t interval = cmpTimeUs(now, gsfGpsLastArrival); + if (interval > 0 && interval < 1000000) { + gsfGpsIntervalUs += (gsfGpsIntervalUs > 0.0f ? 0.1f : 1.0f) * (interval - gsfGpsIntervalUs); + } + } + gsfGpsLastArrival = now; + + if (gpsSol.numSat < GSF_MIN_SATS || gpsSol.eph > GSF_MAX_EPH_CM) { + return; + } + + gsfGpsVel[0] = gpsSol.velNED[X] * 0.01f; + gsfGpsVel[1] = gpsSol.velNED[Y] * 0.01f; + gsfGpsArrival = now; + gsfGpsTakenAt = now - gsfStatus.gpsDelayUs; + gsfGpsPending = true; + gsfGpsPendingDelay = true; +} + +static void updateNis(void) +{ + int lead = 0; + for (int i = 1; i < GSF_MODEL_COUNT; i++) { + if (gsfState.model[i].weight > gsfState.model[lead].weight) { + lead = i; + } + } + + const gsfModel_t *m = &gsfState.model[lead]; + const float det = m->S[0][0] * m->S[1][1] - m->S[0][1] * m->S[1][0]; + if (det > 1e-9f) { + const float nis = (m->innov[0] * (m->innov[0] * m->S[1][1] - m->innov[1] * m->S[0][1]) + + m->innov[1] * (m->innov[1] * m->S[0][0] - m->innov[0] * m->S[1][0])) / det; + gsfStatus.nis += GSF_NIS_FILTER * (nis - gsfStatus.nis); + } +} + +// A timer that runs while cond holds: true once it has held for us. 64-bit: cmpTimeUs would wrap after 35 min +static bool heldFor(timeUs_t *since, bool cond, timeUs_t now, timeUs_t us) +{ + if (!cond) { + *since = 0; + return false; + } + if (*since == 0) { + *since = now ? now : 1; + } + return now - *since >= us; +} + +static void closeWindow(timeUs_t now) +{ + gsfAccumulating.end = now; + gsfWindows[gsfWindowHead] = gsfAccumulating; + gsfWindowHead = (gsfWindowHead + 1) % GSF_WINDOW_COUNT; + if (gsfWindowCount < GSF_WINDOW_COUNT) { + gsfWindowCount++; + } + + // The same change in the gyro-turned frame, for the delay and the manoeuvres + const float c = cos_approx(gsfHistoryYaw); + const float s = sin_approx(gsfHistoryYaw); + const float dvN = gsfAccumulating.dv[0] * c - gsfAccumulating.dv[1] * s; + const float dvE = gsfAccumulating.dv[0] * s + gsfAccumulating.dv[1] * c; + gsfHistoryVel[0] += dvN; + gsfHistoryVel[1] += dvE; + gsfHistoryYaw = wrapPi(gsfHistoryYaw + gsfAccumulating.dYaw); + gsfHistoryAdd(&gsfHistory, now, gsfHistoryVel); + + gsfBlockDv[0] += dvN; + gsfBlockDv[1] += dvE; + gsfBlockDt += gsfAccumulating.dt; + if (gsfBlockDt >= GSF_EXCITATION_BLOCK_S) { + const float dv = fast_fsqrtf(sq(gsfBlockDv[0]) + sq(gsfBlockDv[1])); + if (gsfState.fusing && dv > GSF_EXCITATION_MIN) { + gsfStatus.excitation += dv; + } + gsfBlockDv[0] = gsfBlockDv[1] = gsfBlockDt = 0.0f; + } + + memset(&gsfAccumulating, 0, sizeof(gsfAccumulating)); +} + +// A measured delay is kept once it has passed the confidence test, so the value does not toggle with the manoeuvres +static void updateGpsDelay(bool inFlight) +{ + if (gsfGpsPendingDelay) { + gsfGpsPendingDelay = false; + if (inFlight) { + gsfDelayAddGps(&gsfDelay, &gsfHistory, gsfGpsArrival, gsfGpsVel); + uint32_t delay; + if (gsfDelayEstimate(&gsfDelay, &delay)) { + gsfDelayMeasuredUs = delay; + gsfDelayMeasured = true; + } + } + } + + uint32_t delay; + if (gsfDelayMeasured) { + gsfStatus.flags |= GSF_YAW_DELAY_MEASURED; + delay = gsfDelayMeasuredUs; + } else { + delay = gsfGpsIntervalUs > 0.0f ? lrintf(gsfGpsIntervalUs) + GSF_DELAY_PROCESSING_US : GSF_DELAY_DEFAULT_US; + } + gsfStatus.gpsDelayUs = MIN(delay, GSF_DELAY_MAX_US); +} + +// The compass against the estimate: dropped after staying off it, back after staying close for longer. False when +// there was nothing to compare +static bool checkCompass(timeUs_t now, bool usable, bool wasCompared) +{ + float compassHeading; + if (!usable || !imuGetCompassHeading(&compassHeading)) { + gsfMagDisagreeSince = gsfMagAgreeSince = 0; + return false; + } + + const float error = wrapPi(compassHeading - gsfStatus.yaw); + gsfStatus.magError = wasCompared ? wrapPi(gsfStatus.magError + GSF_MAG_ERROR_FILTER * wrapPi(error - gsfStatus.magError)) : error; + + const float limit = MAX(GSF_MAG_ERROR_MIN, 3.0f * gsfStatus.yawSigma); + const float size = fabsf(gsfStatus.magError); + if (heldFor(&gsfMagDisagreeSince, size > limit, now, GSF_MAG_DISAGREE_US)) { + gsfMagDisagrees = true; + } + if (heldFor(&gsfMagAgreeSince, size < 0.5f * limit, now, GSF_MAG_AGREE_US)) { + gsfMagDisagrees = false; + } + return true; +} + +// An AHRS heading far from a settled estimate, with no compass to hold it, is reset to it +static void checkAhrsHeading(timeUs_t now, bool usable) +{ +#ifdef USE_MAG + const bool compassHeld = sensors(SENSOR_MAG) && compassIsHealthy() && !gsfMagDisagrees; +#else + const bool compassHeld = false; +#endif + bool far = false; + if (imuConfig()->gsf_yaw && usable && !compassHeld && gsfStatus.resets < GSF_RESETS_MAX) { + const float error = wrapPi(DECIDEGREES_TO_RADIANS(attitude.values.yaw) - gsfStatus.yaw); + far = fabsf(error) > GSF_RESET_ERROR; + } + if (heldFor(&gsfFarSince, far, now, GSF_RESET_AFTER_US)) { + gsfResetRequested = true; + gsfStatus.resets++; + gsfFarSince = 0; + } +} + +void taskGsfYaw(timeUs_t currentTimeUs) +{ + gsfActive = true; + + // A tailsitter's transition turns the nose by 90 deg with no gyro yaw: the window and the models start over + const bool tailsitter = STATE(TAILSITTER); + if (tailsitter != gsfWasTailsitter) { + gsfWasTailsitter = tailsitter; + memset(&gsfAccumulating, 0, sizeof(gsfAccumulating)); + gsfReset(&gsfState); + } + + if (gsfAccumulating.dt > 0.0f) { + closeWindow(currentTimeUs); + } + + // Disarmed, the compass is the only heading there is; the next flight starts trusting it, as INAV always has + const bool armed = ARMING_FLAG(ARMED); + if (!armed) { + gsfMagDisagrees = false; + gsfMagDisagreeSince = gsfMagAgreeSince = 0; + } + if (armed && !gsfWasArmed) { + gsfStatus.resets = 0; + } + gsfWasArmed = armed; + + // Airplanes only, in flight (the wheels and the takeoff acceleration mislead it), from the first flight detection + // to the landing detection: isFixedWingFlying alone drops with the throttle under cruise + if (!armed || STATE(LANDING_DETECTED) || !STATE(AIRPLANE)) { + gsfAirborne = false; + } else if (isFixedWingFlying()) { + gsfAirborne = true; + } + const bool inFlight = gsfAirborne; + if (!inFlight && gsfState.fusing) { + gsfState.fusing = false; + } + + const uint8_t previous = gsfStatus.flags; + gsfStatus.flags = 0; + + // Step the filter up to the GPS delay behind the present + const gsfWindow_t *last = NULL; + while (gsfWindowCount > 0) { + const gsfWindow_t *w = &gsfWindows[(gsfWindowHead + GSF_WINDOW_COUNT - gsfWindowCount) % GSF_WINDOW_COUNT]; + if (cmpTimeUs(currentTimeUs, w->end) < (timeDelta_t)gsfStatus.gpsDelayUs) { + break; + } + gsfPredict(&gsfState, w->dv, w->dYaw, w->dt); + last = w; + gsfWindowCount--; + } + + // The latest GPS sample, fused once the filter has reached the time it was taken. One the filter is already well + // past, after the delay grew, is dropped rather than fused against the wrong motion + const timeDelta_t pastSample = last ? cmpTimeUs(last->end, gsfGpsTakenAt) : 0; + if (gsfGpsPending && last && pastSample >= -GSF_WINDOW_US) { + gsfGpsPending = false; + const bool spaced = !gsfState.fusing || gsfGpsTakenAt - gsfLastFusion >= GSF_FUSION_MIN_US; + if (inFlight && last->maxRateSq < sq(GSF_STEADY_RATE) && spaced && pastSample <= 2 * GSF_WINDOW_US) { + if (!gsfState.fusing) { + gsfStatus.excitation = 0.0f; + gsfStatus.nis = 2.0f; + } + if (gsfFuseVelocity(&gsfState, gsfGpsVel[0], gsfGpsVel[1], GSF_VEL_ACCURACY)) { + updateNis(); + } + gsfLastFusion = gsfGpsTakenAt; + } + } + + // A new delay applies from the next sample: the pending one was stamped and stepped to with the old one + updateGpsDelay(inFlight); + + // A leading model the GPS keeps disagreeing with: past the limit for long, start over, and the compass it may + // have excluded gets its trust back + bool inconsistent = gsfState.fusing && gsfStatus.nis > ((previous & GSF_YAW_INCONSISTENT) ? GSF_NIS_CONSISTENT : GSF_NIS_INCONSISTENT); + if (heldFor(&gsfInconsistentSince, inconsistent, currentTimeUs, GSF_INCONSISTENT_US)) { + gsfReset(&gsfState); + gsfInconsistentSince = 0; + gsfMagDisagrees = false; + inconsistent = false; + } + + gsfUpdateComposite(&gsfState); + + // Carry the delayed yaw to the present with the gyro + float yaw = gsfState.yaw + gsfAccumulating.dYaw; + for (int n = 0; n < gsfWindowCount; n++) { + yaw += gsfWindows[(gsfWindowHead + GSF_WINDOW_COUNT - 1 - n) % GSF_WINDOW_COUNT].dYaw; + } + yaw = wrapPi(yaw); + gsfStatus.yaw = yaw < 0.0f ? yaw + 2.0f * M_PIf : yaw; + gsfStatus.yawSigma = fast_fsqrtf(gsfState.yawVariance); + + // Its own variance is not proof on its own: it also has to have seen manoeuvres, and stay settled a while + const bool gpsFresh = gsfGpsArrival && currentTimeUs - gsfGpsArrival < GSF_GPS_STALE_US; + const bool settled = gsfState.fusing && !inconsistent && gpsFresh && gsfStatus.excitation >= GSF_USABLE_EXCITATION && + gsfStatus.yawSigma < GSF_USABLE_SIGMA; + const bool usable = heldFor(&gsfSettledSince, settled, currentTimeUs, GSF_USABLE_SETTLE_US); + + if (checkCompass(currentTimeUs, usable, previous & GSF_YAW_MAG_COMPARED)) { + gsfStatus.flags |= GSF_YAW_MAG_COMPARED; + } + checkAhrsHeading(currentTimeUs, usable); + + if (gsfState.fusing) { + gsfStatus.flags |= GSF_YAW_FUSING; + } + if (usable) { + gsfStatus.flags |= GSF_YAW_USABLE; + } + if (inconsistent) { + gsfStatus.flags |= GSF_YAW_INCONSISTENT; + } + if (gsfMagDisagrees) { + gsfStatus.flags |= GSF_YAW_MAG_DISAGREES; + } + if (imuConfig()->gsf_yaw && usable) { + gsfStatus.flags |= GSF_YAW_IN_AHRS; + } + if (gsfStatus.resets) { + gsfStatus.flags |= GSF_YAW_RESET; + } +} + +const gsfYawStatus_t *gsfYawGetStatus(void) +{ + return &gsfStatus; +} + +// Every AHRS update with ahrs_gsf_yaw on: out of line, as the accumulation +NOINLINE void gsfYawGetAhrsInput(gsfYawAhrsInput_t *in) +{ + const bool enabled = imuConfig()->gsf_yaw; + in->yaw = wrapPi(gsfStatus.yaw + gsfAccumulating.dYaw); + in->weight = 0.0f; + if (enabled && (gsfStatus.flags & GSF_YAW_USABLE)) { + in->weight = constrainf(scaleRangef(gsfStatus.yawSigma, GSF_AHRS_FULL_SIGMA, GSF_USABLE_SIGMA, 1.0f, 0.0f), 0.0f, 1.0f); + } + // Only while there is an estimate to take the compass's place + in->ignoreMag = enabled && gsfMagDisagrees && (gsfStatus.flags & GSF_YAW_USABLE); + in->resetYaw = gsfResetRequested; + gsfResetRequested = false; +} + +#endif diff --git a/src/main/flight/gsf_yaw.h b/src/main/flight/gsf_yaw.h new file mode 100644 index 00000000000..20276955643 --- /dev/null +++ b/src/main/flight/gsf_yaw.h @@ -0,0 +1,116 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include +#include + +#define GSF_MODEL_COUNT 5 + +typedef struct gsfModel_s { + float X[3]; // velocity north and east (m/s), yaw (rad, clockwise from north) + float P[3][3]; + float S[2][2]; // velocity innovation variance (m/s)^2 + float innov[2]; // velocity innovation (m/s) + float weight; +} gsfModel_t; + +typedef struct gsfState_s { + gsfModel_t model[GSF_MODEL_COUNT]; + float yaw; // composite yaw (rad) + float yawVariance; // rad^2 + bool fusing; // velocity fusion has started + uint8_t clipped; // models whose weight underflowed at the last fusion +} gsfState_t; + +void gsfReset(gsfState_t *gsf); +void gsfPredict(gsfState_t *gsf, const float dvLevel[2], float dYaw, float dt); +bool gsfFuseVelocity(gsfState_t *gsf, float velNorth, float velEast, float velAccuracy); +void gsfUpdateComposite(gsfState_t *gsf); + +// Rate of the nose's heading, clockwise, from the body rates and earth-up in body axes (rMat's last row) +float gsfYawRate(const float up[3], const float rate[3]); + +// GPS delay from how well the IMU's velocity change over 400 ms matches the GPS one, turned by the best single angle +#define GSF_HISTORY_COUNT 40 // 800 ms of 20 ms IMU windows: the longest delay plus the baseline +#define GSF_GPS_HISTORY 8 +#define GSF_DELAY_STEP_US 20000 +#define GSF_DELAY_STEPS 16 // 0 to 300 ms + +typedef struct gsfImuHistory_s { + uint32_t time[GSF_HISTORY_COUNT]; // us, end of each window + float vel[GSF_HISTORY_COUNT][2]; // m/s, velocity change summed since boot, in a frame the yaw rate turns with + uint8_t head; // where the next one goes + uint8_t count; +} gsfImuHistory_t; + +typedef struct gsfDelayEstimator_s { + float sii[GSF_DELAY_STEPS]; // sums over the GPS samples, forgetting the old ones: IMU change squared, + float sdot[GSF_DELAY_STEPS]; // its dot and cross product with the GPS change, + float scross[GSF_DELAY_STEPS]; + float sgg; // and the GPS change squared + uint32_t gpsTime[GSF_GPS_HISTORY]; + float gpsVel[GSF_GPS_HISTORY][2]; + uint8_t gpsHead; + uint8_t gpsCount; + uint16_t samples; +} gsfDelayEstimator_t; + +void gsfHistoryAdd(gsfImuHistory_t *h, uint32_t time, const float vel[2]); +void gsfDelayAddGps(gsfDelayEstimator_t *e, const gsfImuHistory_t *h, uint32_t arrival, const float vel[2]); +bool gsfDelayEstimate(const gsfDelayEstimator_t *e, uint32_t *delayUs); + +#ifdef USE_GSF_YAW +#include "common/time.h" + +typedef enum { + GSF_YAW_FUSING = 1 << 0, // in flight with GPS: the models are learning + GSF_YAW_USABLE = 1 << 1, // enough manoeuvring seen and the estimate has settled + GSF_YAW_INCONSISTENT = 1 << 2, // the GPS velocity keeps disagreeing with the leading model + GSF_YAW_DELAY_MEASURED = 1 << 3, // the GPS delay comes from this flight, not from the receiver's rate + GSF_YAW_MAG_COMPARED = 1 << 4, // magError holds the compass against the estimate + GSF_YAW_MAG_DISAGREES = 1 << 5, // the compass has stayed off the estimate: the AHRS ignores it when ahrs_gsf_yaw is on + GSF_YAW_IN_AHRS = 1 << 6, // the AHRS heading is following the estimate + GSF_YAW_RESET = 1 << 7, // the AHRS heading was reset to the estimate during this flight +} gsfYawFlags_e; + +typedef struct gsfYawStatus_s { + float yaw; // rad, clockwise from north, carried to the present with the gyro + float yawSigma; // rad + float excitation; // velocity change seen since fusion started (m/s), only from sustained accelerations + float nis; // normalised innovation squared of the leading model, filtered (about 2 when consistent) + float magError; // rad, compass heading minus the estimate, filtered + uint32_t gpsDelayUs; // the delay in use + uint8_t flags; + uint8_t resets; // AHRS heading resets this flight +} gsfYawStatus_t; + +typedef struct gsfYawAhrsInput_s { + float yaw; // rad, clockwise from north, at the present + float weight; // 0: not usable as a heading reference + bool ignoreMag; + bool resetYaw; // once: set the AHRS heading to yaw +} gsfYawAhrsInput_t; + +void gsfYawInit(void); +void gsfYawAccumulateImu(float dT); +void gsfYawOnNewGpsData(void); +void taskGsfYaw(timeUs_t currentTimeUs); +const gsfYawStatus_t *gsfYawGetStatus(void); +void gsfYawGetAhrsInput(gsfYawAhrsInput_t *in); +#endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 1682815a596..0c31b8a0612 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -47,6 +47,7 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "flight/gsf_yaw.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" @@ -135,7 +136,10 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT, .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT, .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT, - .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT + .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT, +#ifdef USE_GSF_YAW + .gsf_yaw = SETTING_AHRS_GSF_YAW_DEFAULT, +#endif ); STATIC_UNIT_TESTED RP2350_FAST_CODE void imuComputeRotationMatrix(void) @@ -189,6 +193,9 @@ void imuInit(void) // Explicitly initialize FASTRAM statics isAccelUpdatedAtLeastOnce = false; gpsHeadingInitialized = false; +#ifdef USE_GSF_YAW + gsfYawInit(); +#endif // Create magnetic declination matrix #ifdef USE_MAG @@ -369,7 +376,25 @@ static float imuCalculateMcCogAccWeight(void) return wCoGAcc; } -static RP2350_FAST_CODE void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) +#ifdef USE_GSF_YAW +// vGsf: the GSF heading as the GPS course is passed, scaled by its weight; a heading, so always against the nose +NOINLINE static void imuAddGsfHeadingError(fpVector3_t *vErr, const fpVector3_t *vGsf) +{ + const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } }; + fpVector3_t vHeadingEF; + quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); + vHeadingEF.z = 0.0f; + if (vectorNormSquared(&vHeadingEF) > 0.01f) { + vectorNormalize(&vHeadingEF, &vHeadingEF); + fpVector3_t vGsfErr; + vectorCrossProduct(&vGsfErr, vGsf, &vHeadingEF); + quaternionRotateVector(&vGsfErr, &vGsfErr, &orientation); + vectorAdd(vErr, vErr, &vGsfErr); + } +} +#endif + +static RP2350_FAST_CODE void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, const fpVector3_t * vGsf, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; @@ -392,7 +417,7 @@ static RP2350_FAST_CODE void imuMahonyAHRSupdate(float dt, const fpVector3_t * g /* Step 1: Yaw correction */ // Use measured magnetic field vector - if (magBF || vCOG || vCOGAcc) { + if (magBF || vCOG || vCOGAcc || vGsf) { float wMag = 1.0f; float wCoG = 1.0f; if (magBF) { wCoG *= imuConfig()->gps_yaw_weight / 100.0f; } @@ -496,6 +521,11 @@ static RP2350_FAST_CODE void imuMahonyAHRSupdate(float dt, const fpVector3_t * g vectorScale(&vMagErr, &vMagErr, wMag); vectorScale(&vCoGErr, &vCoGErr, wCoG); vectorAdd(&vErr, &vMagErr, &vCoGErr); +#ifdef USE_GSF_YAW + if (vGsf) { + imuAddGsfHeadingError(&vErr, vGsf); + } +#endif // Compute and apply integral feedback if enabled if (imuRuntimeConfig.dcm_ki_mag > 0.0f) { // Stop integrating if spinning beyond the certain limit @@ -784,6 +814,60 @@ void imuUpdateTailSitter(void) lastTailSitter = STATE(TAILSITTER); } +#ifdef USE_GSF_YAW +// The heading the compass alone gives: the AHRS heading plus the angle from the expected to the measured north +NOINLINE bool imuGetCompassHeading(float *heading) +{ +#ifdef USE_MAG + if (!sensors(SENSOR_MAG) || !compassIsHealthy() || compassIsCalibrating()) { + return false; + } + const fpVector3_t magBF = { .v = { mag.magADC[X], mag.magADC[Y], mag.magADC[Z] } }; + fpVector3_t magEF; + quaternionRotateVectorInv(&magEF, &magBF, &orientation); + const float cross = vCorrectedMagNorth.x * magEF.y - vCorrectedMagNorth.y * magEF.x; + const float dot = vCorrectedMagNorth.x * magEF.x + vCorrectedMagNorth.y * magEF.y; + if (sq(cross) + sq(dot) < 1.0f) { + return false; + } + *heading = atan2_approx(cross, dot) - atan2_approx(rMat[1][0], rMat[0][0]); + return true; +#else + UNUSED(heading); + return false; +#endif +} + +// The GSF stands in for the GPS course, the compass is dropped while it disagrees with it, a far heading is reset to it +NOINLINE static bool imuGsfHeadingInput(fpVector3_t *vGsf, bool *useMag) +{ + gsfYawAhrsInput_t in; + gsfYawGetAhrsInput(&in); + + if (in.resetYaw) { + int16_t yaw = lrintf(RADIANS_TO_DECIDEGREES(in.yaw)); + if (yaw < 0) { + yaw += 3600; + } + imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, yaw); + resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(yaw)); + } + if (in.ignoreMag) { + *useMag = false; + } + if (in.weight <= 0.0f) { + return false; + } + + // Shares the AHRS with the compass as the GPS course does + const float weight = *useMag ? in.weight * imuConfig()->gps_yaw_weight / 100.0f : in.weight; + vGsf->x = -cos_approx(in.yaw) * weight; + vGsf->y = sin_approx(in.yaw) * weight; + vGsf->z = 0.0f; + return true; +} +#endif + static RP2350_FAST_CODE void imuCalculateEstimatedAttitude(float dT) { #if defined(USE_MAG) @@ -897,6 +981,16 @@ static RP2350_FAST_CODE void imuCalculateEstimatedAttitude(float dT) accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler); const bool useAcc = (accWeight > 0.001f); + const fpVector3_t *vGsfRef = NULL; +#ifdef USE_GSF_YAW + static fpVector3_t vGsf; + if (imuConfig()->gsf_yaw && imuGsfHeadingInput(&vGsf, &useMag)) { + vGsfRef = &vGsf; + useCOG = false; + useCOGAcc = false; + } +#endif + const float magWeight = imuGetPGainScaleFactor() * 1.0f; fpVector3_t measuredMagBF = {.v = {mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}}; imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF, @@ -904,6 +998,7 @@ static RP2350_FAST_CODE void imuCalculateEstimatedAttitude(float dT) useMag ? &measuredMagBF : NULL, useCOG ? &vCOG : NULL, useCOGAcc ? &vCOGAcc : NULL, + vGsfRef, accWeight, magWeight); imuUpdateTailSitter(); @@ -944,6 +1039,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s imuCheckVibrationLevels(); imuCalculateEstimatedAttitude(dT); // Update attitude estimate +#ifdef USE_GSF_YAW + gsfYawAccumulateImu(dT); +#endif } else { acc.accADCf[X] = 0.0f; acc.accADCf[Y] = 0.0f; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 600a10afb24..5424c1cbad8 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -53,6 +53,9 @@ typedef struct imuConfig_s { uint8_t gps_yaw_windcomp; uint8_t inertia_comp_method; uint16_t gps_yaw_weight; +#ifdef USE_GSF_YAW + uint8_t gsf_yaw; // heading from the GSF yaw estimator once it has settled +#endif } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); @@ -90,6 +93,10 @@ bool isYawZeroResetAllowed(void); void imuTransformVectorBodyToEarth(fpVector3_t * v); void imuTransformVectorEarthToBody(fpVector3_t * v); +#ifdef USE_GSF_YAW +bool imuGetCompassHeading(float *heading); +#endif + void imuInit(void); #if defined(SITL_BUILD) diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index eadd294a0ab..5df28f70100 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -165,3 +165,5 @@ // command keeps its number, an unmerged one has no claim on it. #define MSP2_INAV_ESC_SRXL2_STATUS 0x2233 #define MSP2_INAV_ESC_SRXL2_CALIBRATE 0x2234 + +#define MSP2_INAV_MAG_LEARN 0x2235 //out message in-flight compass offset learning: state of the current or last flight diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 02ea92e58d5..43a9e4117df 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -38,6 +38,7 @@ #include "fc/settings.h" #include "fc/rc_modes.h" +#include "flight/gsf_yaw.h" #include "flight/imu.h" #include "io/gps.h" @@ -184,6 +185,10 @@ void onNewGPSData(void) gpsLocation_t newLLH; const timeUs_t currentTimeUs = micros(); +#ifdef USE_GSF_YAW + gsfYawOnNewGpsData(); +#endif + newLLH.lat = gpsSol.llh.lat; newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index b1d5b8713aa..4fbe95bae66 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -67,6 +67,9 @@ typedef enum { #ifdef USE_MAG TASK_COMPASS, #endif +#ifdef USE_GSF_YAW + TASK_GSF_YAW, +#endif #ifdef USE_BARO TASK_BARO, #endif diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 79bcf167bbc..a9fae55aa3a 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -51,6 +51,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/runtime_config.h" #include "fc/settings.h" @@ -59,8 +60,11 @@ #include "flight/imu.h" +#include "navigation/navigation.h" + #include "sensors/boardalignment.h" #include "sensors/compass.h" +#include "sensors/compass_learn.h" #include "sensors/compass_orientation.h" #include "sensors/gyro.h" #include "sensors/sensors.h" @@ -83,6 +87,9 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig, .pitchDeciDegrees = SETTING_ALIGN_MAG_PITCH_DEFAULT, .yawDeciDegrees = SETTING_ALIGN_MAG_YAW_DEFAULT, .magGain = {SETTING_MAGGAIN_X_DEFAULT, SETTING_MAGGAIN_Y_DEFAULT, SETTING_MAGGAIN_Z_DEFAULT}, +#ifdef USE_MAG_LEARN + .magLearn = SETTING_MAG_LEARN_DEFAULT, +#endif ); static bool magUpdatedAtLeastOnce = false; @@ -430,6 +437,69 @@ static void compassApplyDetectedOrientation(int16_t rollDD, int16_t pitchDD, int compassRefreshAlignment(); } +#ifdef USE_MAG_LEARN +static void compassLearnUpdate(timeUs_t currentTimeUs) +{ + static bool learnArmed; + static bool learnPending; + static timeUs_t lastFitAt; + + // A calibration, or mag_learn turned off, while armed spoils what the flight collected up to then + const bool canLearn = compassConfig()->magLearn && calStartedAt == 0 && STATE(COMPASS_CALIBRATED); + + if (ARMING_FLAG(ARMED)) { + if (!learnArmed || !canLearn) { + magLearnReset(); + learnArmed = true; + learnPending = false; + lastFitAt = currentTimeUs; + } + if (!canLearn) { + return; + } + if (isProbablyStillFlying() && !STATE(LANDING_DETECTED)) { + magLearnStatus.flags = (magLearnStatus.flags & ~MAG_LEARN_PAUSED) | MAG_LEARN_COLLECTING; + magLearnAddSample(mag.magADC, mag.dev.magADCRaw); + } else { + magLearnStatus.flags = (magLearnStatus.flags & ~MAG_LEARN_COLLECTING) | MAG_LEARN_PAUSED; + } + // Refit about once a second so the blackbox shows the estimate and the checks evolving + if (cmpTimeUs(currentTimeUs, lastFitAt) >= 1000000) { + lastFitAt = currentTimeUs; + magLearnEvaluate(compassConfig()->magZero.raw, compassConfig()->magGain); + } + } else if (learnArmed) { + learnArmed = false; + learnPending = canLearn; + magLearnStatus.flags &= ~(MAG_LEARN_COLLECTING | MAG_LEARN_PAUSED); + } else if (learnPending) { + // No heading step or flash write while an emergency rearm may follow, nor while the aircraft is carried away + const timeUs_t sinceDisarm = currentTimeUs - getLastDisarmTimeUs(); + if (canLearn && (sinceDisarm < MS2US(EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS) || + (isProbablyStillFlying() && sinceDisarm < MS2US(MAG_LEARN_SAVE_TIMEOUT_MS)))) { + return; + } + learnPending = false; + if (!canLearn) { + magLearnStatus.flags &= ~MAG_LEARN_SAVE_DUE; + return; + } + if (isProbablyStillFlying()) { + magLearnStatus.flags = (magLearnStatus.flags & ~MAG_LEARN_SAVE_DUE) | MAG_LEARN_DISARMED_FLYING; + return; + } + magLearnEvaluate(compassConfig()->magZero.raw, compassConfig()->magGain); + if (magLearnStatus.flags & MAG_LEARN_SAVE_DUE) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + compassConfigMutable()->magZero.raw[axis] = constrain(compassConfig()->magZero.raw[axis] + magLearnStatus.delta[axis], INT16_MIN, INT16_MAX); + } + magLearnStatus.flags |= MAG_LEARN_SAVED; + saveConfigAndNotify(); + } + } +} +#endif + void compassUpdate(timeUs_t currentTimeUs) { #ifdef USE_SIMULATOR @@ -589,6 +659,10 @@ void compassUpdate(timeUs_t currentTimeUs) applyBoardAlignment(mag.magADC); } +#ifdef USE_MAG_LEARN + compassLearnUpdate(currentTimeUs); +#endif + magUpdatedAtLeastOnce = true; } diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index b277f6b919a..01fecd93047 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -73,6 +73,9 @@ typedef struct compassConfig_s { int16_t rollDeciDegrees; // Alignment for external mag on the roll (X) axis (0.1deg) int16_t pitchDeciDegrees; // Alignment for external mag on the pitch (Y) axis (0.1deg) int16_t yawDeciDegrees; // Alignment for external mag on the yaw (Z) axis (0.1deg) +#ifdef USE_MAG_LEARN + uint8_t magLearn; // Refit magZero from the samples of each flight, saved at disarm +#endif } compassConfig_t; PG_DECLARE(compassConfig_t, compassConfig); diff --git a/src/main/sensors/compass_learn.c b/src/main/sensors/compass_learn.c new file mode 100644 index 00000000000..6635dfce263 --- /dev/null +++ b/src/main/sensors/compass_learn.c @@ -0,0 +1,153 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_MAG_LEARN + +#include "build/build_config.h" + +#include "common/bitarray.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "sensors/compass_learn.h" + +magLearnStatus_t magLearnStatus; + +// Read only where magLearnFilled is set: AT32 startup leaves FASTRAM uncleared +STATIC_FASTRAM int16_t magLearnSample[MAG_LEARN_BINS][XYZ_AXIS_COUNT]; +static BITARRAY_DECLARE(magLearnFilled, MAG_LEARN_BINS); +static uint16_t magLearnHeadingsSeen; + +void magLearnReset(void) +{ + BITARRAY_CLR_ALL(magLearnFilled); + magLearnHeadingsSeen = 0; + memset(&magLearnStatus, 0, sizeof(magLearnStatus)); +} + +void magLearnAddSample(const float bodyField[XYZ_AXIS_COUNT], const int16_t raw[XYZ_AXIS_COUNT]) +{ + const float norm = calc_length_pythagorean_3D(bodyField[X], bodyField[Y], bodyField[Z]); + if (norm < 1.0f) { + return; + } + + // Bands of equal z/|B| have equal area on the sphere + const int elevation = constrain((bodyField[Z] / norm + 1.0f) * (MAG_LEARN_ELEVATION_BINS / 2.0f), 0, MAG_LEARN_ELEVATION_BINS - 1); + const int azimuth = constrain((atan2_approx(bodyField[Y], bodyField[X]) + M_PIf) * (MAG_LEARN_AZIMUTH_BINS / (2.0f * M_PIf)), 0, MAG_LEARN_AZIMUTH_BINS - 1); + const int bin = elevation * MAG_LEARN_AZIMUTH_BINS + azimuth; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + magLearnSample[bin][axis] = raw[axis]; + } + if (!bitArrayGet(magLearnFilled, bin)) { + bitArraySet(magLearnFilled, bin); + magLearnStatus.sectors++; + } + if (!(magLearnHeadingsSeen & BIT(azimuth))) { + magLearnHeadingsSeen |= BIT(azimuth); + magLearnStatus.headings++; + } +} + +// Scaled by maggain like magADC, so the axes weigh alike and the sphere has radius 1024 while the gains hold +static void magLearnPass(const float centre[XYZ_AXIS_COUNT], const int16_t gain[XYZ_AXIS_COUNT], + sensorCalibrationState_t *fit, float *radius, float *variance) +{ + float sum = 0.0f; + float sumSq = 0.0f; + for (int bin = 0; bin < MAG_LEARN_BINS; bin++) { + if (bitArrayGet(magLearnFilled, bin)) { + float v[XYZ_AXIS_COUNT]; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + v[axis] = (magLearnSample[bin][axis] - centre[axis]) * 1024 / gain[axis]; + } + if (fit) { + sensorCalibrationPushSampleForOffsetCalculation(fit, v); + } + const float r = calc_length_pythagorean_3D(v[X], v[Y], v[Z]); + sum += r; + sumSq += r * r; + } + } + *radius = sum / magLearnStatus.sectors; + *variance = sumSq / magLearnStatus.sectors - sq(*radius); +} + +void magLearnEvaluate(const int16_t zero[XYZ_AXIS_COUNT], const int16_t gain[XYZ_AXIS_COUNT]) +{ + uint16_t flags = magLearnStatus.flags & ~MAG_LEARN_CHECK_FLAGS; + if (magLearnStatus.sectors < MAG_LEARN_MIN_SECTORS) { + flags |= MAG_LEARN_FEW_SECTORS; + } + if (magLearnStatus.headings < MAG_LEARN_MIN_HEADINGS) { + flags |= MAG_LEARN_FEW_HEADINGS; + } + if (flags & (MAG_LEARN_FEW_SECTORS | MAG_LEARN_FEW_HEADINGS)) { + magLearnStatus.flags = flags; + return; + } + + float centre[XYZ_AXIS_COUNT]; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + centre[axis] = zero[axis]; + } + sensorCalibrationState_t fit; + sensorCalibrationResetState(&fit); + float radius, variance; + magLearnPass(centre, gain, &fit, &radius, &variance); + + // Pulls the fit toward the stored offset: a direction the flight never swept (the vertical, in level turns) keeps it + const float ridge = (fit.XtX[X][X] + fit.XtX[Y][Y] + fit.XtX[Z][Z]) / 64.0f; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + fit.XtX[axis][axis] += ridge; + } + float step[XYZ_AXIS_COUNT]; + sensorCalibrationSolveForOffset(&fit, step); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + centre[axis] += step[axis] * gain[axis] / 1024; + } + magLearnPass(centre, gain, NULL, &radius, &variance); + + // Every check is written to fail on NaN, so a degenerate fit is never saved + const float spread = sqrtf(fabsf(variance)) / radius * 1000; + magLearnStatus.spread = spread < 1000.0f ? lrintf(spread) : 1000; + if (!(spread <= MAG_LEARN_MAX_SPREAD)) { + flags |= MAG_LEARN_NOT_SPHERE; + } + if (!(fabsf(radius - 1024) <= MAG_LEARN_MAX_SCALE_ERROR * 1024)) { + flags |= MAG_LEARN_OFF_SCALE; + } + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + if (!(fabsf(step[axis]) <= MAG_LEARN_MAX_STEP * radius)) { + flags |= MAG_LEARN_STEP_TOO_BIG; + } + const float delta = centre[axis] - zero[axis]; + magLearnStatus.delta[axis] = fabsf(delta) < INT16_MAX ? lrintf(delta) : 0; + } + if (!(flags & MAG_LEARN_CHECK_FLAGS)) { + flags |= MAG_LEARN_SAVE_DUE; + } + magLearnStatus.flags = flags; +} + +#endif diff --git a/src/main/sensors/compass_learn.h b/src/main/sensors/compass_learn.h new file mode 100644 index 00000000000..698a4c95fea --- /dev/null +++ b/src/main/sensors/compass_learn.h @@ -0,0 +1,69 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include + +#include "common/axis.h" + +#ifdef USE_MAG_LEARN + +// In-flight refit of the compass offsets: samples binned by field direction, sphere fit on them, checks before a save + +#define MAG_LEARN_AZIMUTH_BINS 12 +#define MAG_LEARN_ELEVATION_BINS 6 +#define MAG_LEARN_BINS (MAG_LEARN_AZIMUTH_BINS * MAG_LEARN_ELEVATION_BINS) +#define MAG_LEARN_MIN_SECTORS 12 +#define MAG_LEARN_MIN_HEADINGS 9 +#define MAG_LEARN_MAX_SPREAD 50 // 5%, in 0.1% of the radius +#define MAG_LEARN_MAX_SCALE_ERROR 0.3f // of the radius maggain implies +#define MAG_LEARN_MAX_STEP 0.3f // of the radius, per axis and flight +#define MAG_LEARN_SAVE_TIMEOUT_MS 60000 // after the disarm, for the aircraft to be still + +// magBiasFlags in the blackbox log, flags in MSP2_INAV_MAG_LEARN +typedef enum { + MAG_LEARN_COLLECTING = 1 << 0, + MAG_LEARN_PAUSED = 1 << 1, + MAG_LEARN_SAVE_DUE = 1 << 4, + MAG_LEARN_FEW_SECTORS = 1 << 5, + MAG_LEARN_FEW_HEADINGS = 1 << 6, + MAG_LEARN_NOT_SPHERE = 1 << 7, + MAG_LEARN_OFF_SCALE = 1 << 8, + MAG_LEARN_STEP_TOO_BIG = 1 << 9, + MAG_LEARN_SAVED = 1 << 10, + MAG_LEARN_DISARMED_FLYING = 1 << 11, +} magLearnFlags_e; + +#define MAG_LEARN_CHECK_FLAGS (MAG_LEARN_SAVE_DUE | MAG_LEARN_FEW_SECTORS | MAG_LEARN_FEW_HEADINGS | \ + MAG_LEARN_NOT_SPHERE | MAG_LEARN_OFF_SCALE | MAG_LEARN_STEP_TOO_BIG) + +typedef struct magLearnStatus_s { + int16_t delta[XYZ_AXIS_COUNT]; // estimated magzero minus the stored one, raw counts + uint16_t flags; // magLearnFlags_e + uint8_t sectors; + uint8_t headings; + uint16_t spread; // of the radius, 0.1% +} magLearnStatus_t; + +extern magLearnStatus_t magLearnStatus; + +void magLearnReset(void); +void magLearnAddSample(const float bodyField[XYZ_AXIS_COUNT], const int16_t raw[XYZ_AXIS_COUNT]); +void magLearnEvaluate(const int16_t zero[XYZ_AXIS_COUNT], const int16_t gain[XYZ_AXIS_COUNT]); + +#endif diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 183b44a3908..7a53acaa790 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -173,6 +173,16 @@ extern uint8_t __config_end; #define USE_MAG_CALIBRATION_ORIENTATION #endif +// In-flight compass offset learning, ~2 KB of flash: kept off 512 KB targets like the larger features in common.h +#if defined(USE_MAG) && !defined(USE_MAG_LEARN) && (MCU_FLASH_SIZE > 512) +#define USE_MAG_LEARN +#endif + +// Yaw from GPS velocity, without the compass: same rule +#if defined(USE_GPS) && !defined(USE_GSF_YAW) && (MCU_FLASH_SIZE > 512) +#define USE_GSF_YAW +#endif + // CRSF sensor input on a dedicated UART #if defined(USE_SERIALRX_CRSF) #define USE_CRSF_SENSOR_INPUT diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index de97812ddcc..b6f63aa5324 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -21,6 +21,10 @@ set_property(SOURCE battery_ina226_unittest.cc PROPERTY definitions USE_INA226 U set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") +set_property(SOURCE compass_learn_unittest.cc PROPERTY depends + "common/bitarray.c" "common/maths.c" "sensors/compass_learn.c") +set_property(SOURCE compass_learn_unittest.cc PROPERTY definitions USE_MAG USE_MAG_LEARN) + set_property(SOURCE compass_orientation_unittest.cc PROPERTY depends "common/maths.c" "sensors/compass_orientation.c") set_property(SOURCE compass_orientation_unittest.cc PROPERTY definitions USE_MAG USE_MAG_CALIBRATION_ORIENTATION) @@ -30,6 +34,8 @@ set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" "sensors/gyro.c") +set_property(SOURCE gsf_yaw_unittest.cc PROPERTY depends "flight/gsf_yaw.c" "common/maths.c") + set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") set_property(SOURCE navigation_fixedwing_turn_math_unittest.cc PROPERTY depends diff --git a/src/test/unit/compass_learn_unittest.cc b/src/test/unit/compass_learn_unittest.cc new file mode 100644 index 00000000000..caad0a771d6 --- /dev/null +++ b/src/test/unit/compass_learn_unittest.cc @@ -0,0 +1,310 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +extern "C" { + #include "common/maths.h" + #include "sensors/compass_learn.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +#include + +namespace { + +const float kInclination = 58.0f * M_PIf / 180.0f; +const int16_t kTrueZero[3] = {120, -310, 75}; + +struct Sim { + float radius = 450.0f; // field magnitude in raw counts + float axisScale[3] = {1.0f, 1.0f, 1.0f}; + int16_t storedZero[3]; + int16_t gain[3]; + std::function interference; + uint32_t seed = 12345; + int sample = 0; + + explicit Sim(int16_t ex = 60, int16_t ey = -40, int16_t ez = 80) + { + storedZero[0] = kTrueZero[0] + ex; + storedZero[1] = kTrueZero[1] + ey; + storedZero[2] = kTrueZero[2] + ez; + setGainFromScale(); + } + + void setGainFromScale(float wrong = 1.0f) + { + for (int i = 0; i < 3; i++) { + gain[i] = lrintf(radius * axisScale[i] * wrong); + } + } + + float noise() + { + seed = seed * 1103515245u + 12345u; + return ((seed >> 16) & 0x7FFF) / 32767.0f * 6.0f - 3.0f; + } + + // Earth field (NED) seen by the body at roll phi, pitch theta, yaw psi; the sensor frame is the body frame + void push(float phi, float theta, float psi) + { + const float b[3] = {radius * cosf(kInclination), 0.0f, radius * sinf(kInclination)}; + const float cf = cosf(phi), sf = sinf(phi), ct = cosf(theta), st = sinf(theta), cp = cosf(psi), sp = sinf(psi); + const float m[3][3] = { + {ct * cp, ct * sp, -st}, + {sf * st * cp - cf * sp, sf * st * sp + cf * cp, sf * ct}, + {cf * st * cp + sf * sp, cf * st * sp - sf * cp, cf * ct}, + }; + float extra[3] = {0.0f, 0.0f, 0.0f}; + if (interference) { + interference(sample, extra); + } + sample++; + + int16_t raw[3]; + float body[3]; + for (int i = 0; i < 3; i++) { + const float field = m[i][0] * b[0] + m[i][1] * b[1] + m[i][2] * b[2]; + raw[i] = lrintf(axisScale[i] * field + kTrueZero[i] + extra[i] + noise()); + body[i] = (raw[i] - storedZero[i]) * 1024.0f / gain[i]; + } + magLearnAddSample(body, raw); + } + + // Two full turns, one each way, with straight legs between them + void flyCircuits(float bankDeg) + { + const float bank = bankDeg * M_PIf / 180.0f; + for (int i = 0; i < 40; i++) { + push(0.0f, 0.05f, 0.0f); + } + for (int deg = 0; deg < 360; deg += 3) { + push(bank, 0.05f, deg * M_PIf / 180.0f); + } + for (int i = 0; i < 40; i++) { + push(0.0f, 0.05f, 0.0f); + } + for (int deg = 360; deg > 0; deg -= 3) { + push(-bank, 0.05f, deg * M_PIf / 180.0f); + } + } + + void evaluate() + { + magLearnEvaluate(storedZero, gain); + } + + // Each flight starts from what the previous one saved, as at disarm + void flyFlightsAndSave(int flights) + { + for (int flight = 0; flight < flights; flight++) { + magLearnReset(); + flyCircuits(30); + evaluate(); + ASSERT_TRUE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); + for (int axis = 0; axis < 3; axis++) { + storedZero[axis] += magLearnStatus.delta[axis]; + } + } + } + + int error(int axis) const + { + return storedZero[axis] + magLearnStatus.delta[axis] - kTrueZero[axis]; + } +}; + +class CompassLearnTest : public ::testing::Test { +protected: + void SetUp() override + { + magLearnReset(); + } +}; + +TEST_F(CompassLearnTest, ResetClearsTheStatus) +{ + Sim sim; + sim.flyCircuits(30); + sim.evaluate(); + magLearnReset(); + + EXPECT_EQ(0, magLearnStatus.flags); + EXPECT_EQ(0, magLearnStatus.sectors); + EXPECT_EQ(0, magLearnStatus.headings); + EXPECT_EQ(0, magLearnStatus.spread); + for (int axis = 0; axis < 3; axis++) { + EXPECT_EQ(0, magLearnStatus.delta[axis]); + } +} + +TEST_F(CompassLearnTest, TurnsRecoverTheHorizontalOffset) +{ + Sim sim; + sim.flyCircuits(30); + sim.evaluate(); + + EXPECT_EQ(MAG_LEARN_SAVE_DUE, magLearnStatus.flags); + EXPECT_EQ(12, magLearnStatus.headings); + EXPECT_LE(abs(sim.error(X)), 6); + EXPECT_LE(abs(sim.error(Y)), 6); + // The vertical shows only through the bank and the ridge holds it near the stored value: it closes over flights + EXPECT_LE(abs(sim.error(Z)), 30); +} + +TEST_F(CompassLearnTest, LevelTurnsKeepTheStoredVertical) +{ + Sim sim; + sim.flyCircuits(2); + sim.evaluate(); + + EXPECT_EQ(MAG_LEARN_SAVE_DUE, magLearnStatus.flags); + EXPECT_LE(abs(sim.error(X)), 6); + EXPECT_LE(abs(sim.error(Y)), 6); + EXPECT_LE(abs(magLearnStatus.delta[Z]), 15); +} + +TEST_F(CompassLearnTest, RepeatedFlightsCloseTheVertical) +{ + Sim sim; + sim.flyFlightsAndSave(3); + + for (int axis = 0; axis < 3; axis++) { + EXPECT_LE(abs(sim.storedZero[axis] - kTrueZero[axis]), 5); + } +} + +// Fitting raw samples instead leaves the vertical about 70 counts off however many flights follow +TEST_F(CompassLearnTest, UnequalAxisGainsAreScaledOut) +{ + Sim sim; + sim.axisScale[1] = 0.95f; + sim.axisScale[2] = 1.06f; + sim.setGainFromScale(); + sim.flyFlightsAndSave(3); + + for (int axis = 0; axis < 3; axis++) { + EXPECT_LE(abs(sim.storedZero[axis] - kTrueZero[axis]), 5); + } +} + +TEST_F(CompassLearnTest, StraightLegIsNotEnough) +{ + Sim sim; + for (int i = 0; i < 500; i++) { + sim.push(0.0f, 0.05f, 0.3f); + } + sim.evaluate(); + + EXPECT_TRUE(magLearnStatus.flags & MAG_LEARN_FEW_SECTORS); + EXPECT_TRUE(magLearnStatus.flags & MAG_LEARN_FEW_HEADINGS); + EXPECT_FALSE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); + for (int axis = 0; axis < 3; axis++) { + EXPECT_EQ(0, magLearnStatus.delta[axis]); + } +} + +TEST_F(CompassLearnTest, HalfTurnIsNotEnough) +{ + Sim sim; + for (int deg = 0; deg < 180; deg += 3) { + sim.push(0.5f, 0.05f, deg * M_PIf / 180.0f); + } + sim.evaluate(); + + EXPECT_TRUE(magLearnStatus.flags & MAG_LEARN_FEW_HEADINGS); + EXPECT_FALSE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); +} + +TEST_F(CompassLearnTest, StrongMotorFieldIsNotASphere) +{ + Sim sim; + // A battery lead next to the compass, its current swinging with the throttle + sim.interference = [](int n, float extra[3]) { + const float throttle = 0.5f + 0.5f * sinf(n * 0.37f); + extra[0] = 140.0f * throttle; + extra[2] = -140.0f * throttle; + }; + sim.flyCircuits(30); + sim.evaluate(); + + EXPECT_TRUE(magLearnStatus.flags & MAG_LEARN_NOT_SPHERE); + EXPECT_GT(magLearnStatus.spread, MAG_LEARN_MAX_SPREAD); + EXPECT_FALSE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); +} + +TEST_F(CompassLearnTest, GainThatNoLongerHoldsIsOffScale) +{ + Sim sim; + sim.setGainFromScale(1.6f); + sim.flyCircuits(30); + sim.evaluate(); + + EXPECT_TRUE(magLearnStatus.flags & MAG_LEARN_OFF_SCALE); + EXPECT_FALSE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); +} + +TEST_F(CompassLearnTest, LargeCorrectionIsRefused) +{ + Sim sim(200, 0, 0); + sim.flyCircuits(30); + sim.evaluate(); + + EXPECT_TRUE(magLearnStatus.flags & MAG_LEARN_STEP_TOO_BIG); + EXPECT_FALSE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); +} + +// maggain accepts any int16 from the CLI: a zero turns the scaled samples into inf and NaN +TEST_F(CompassLearnTest, ZeroGainIsNeverSaved) +{ + Sim sim; + sim.gain[1] = 0; + sim.flyCircuits(30); + sim.evaluate(); + + EXPECT_FALSE(magLearnStatus.flags & MAG_LEARN_SAVE_DUE); + EXPECT_EQ(0, magLearnStatus.delta[Y]); +} + +TEST_F(CompassLearnTest, NegativeGainLearnsLikeAPositiveOne) +{ + Sim sim; + sim.gain[2] = -sim.gain[2]; + sim.flyCircuits(30); + sim.evaluate(); + + EXPECT_EQ(MAG_LEARN_SAVE_DUE, magLearnStatus.flags); + EXPECT_LE(abs(sim.error(X)), 6); + EXPECT_LE(abs(sim.error(Y)), 6); + EXPECT_LE(abs(sim.error(Z)), 30); +} + +TEST_F(CompassLearnTest, FlightStateBitsAreKept) +{ + Sim sim; + sim.flyCircuits(30); + magLearnStatus.flags |= MAG_LEARN_COLLECTING; + sim.evaluate(); + + EXPECT_EQ(MAG_LEARN_COLLECTING | MAG_LEARN_SAVE_DUE, magLearnStatus.flags); +} + +} diff --git a/src/test/unit/gsf_yaw_unittest.cc b/src/test/unit/gsf_yaw_unittest.cc new file mode 100644 index 00000000000..35ec5d959e4 --- /dev/null +++ b/src/test/unit/gsf_yaw_unittest.cc @@ -0,0 +1,332 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include +#include + +#include +#include +#include + +extern "C" { + #include "common/maths.h" + #include "flight/gsf_yaw.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +// A flight with no wind and no sideslip, so the heading is the track. The IMU's level frame turns with the heading, +// as INAV's does: dvLevel is the velocity change forward and right of the heading at the start of each step. +class GsfFlight { +public: + gsfState_t gsf; + float heading; // rad, clockwise from north + float speed; // m/s + float velNoise; // GPS velocity noise (m/s), uniform + int step = 0; + uint32_t rng = 12345; + + GsfFlight(float headingDeg, float speedMs, float noise = 0.0f) : heading(headingDeg * RAD), speed(speedMs), velNoise(noise) + { + gsfReset(&gsf); + } + + float noise() + { + rng = rng * 1664525u + 1013904223u; + return velNoise * (2.0f * (rng >> 8) / 16777216.0f - 1.0f); + } + + // turnRate in deg/s, accel in m/s^2; 50 Hz steps, GPS at 10 Hz + void fly(float seconds, float turnRateDeg, float accel) + { + const float dt = 0.02f; + for (int n = 0; n < (int)(seconds / dt); n++) { + const float vN0 = speed * cosf(heading); + const float vE0 = speed * sinf(heading); + const float headingStart = heading; + heading += turnRateDeg * RAD * dt; + speed += accel * dt; + const float dN = speed * cosf(heading) - vN0; + const float dE = speed * sinf(heading) - vE0; + const float dvLevel[2] = { + dN * cosf(headingStart) + dE * sinf(headingStart), + -dN * sinf(headingStart) + dE * cosf(headingStart), + }; + gsfPredict(&gsf, dvLevel, turnRateDeg * RAD * dt, dt); + if (++step % 5 == 0) { + gsfFuseVelocity(&gsf, speed * cosf(heading) + noise(), speed * sinf(heading) + noise(), 0.5f); + } + gsfUpdateComposite(&gsf); + } + } + + float errorDeg() const + { + float e = (gsf.yaw - heading) / RAD; + while (e > 180.0f) e -= 360.0f; + while (e < -180.0f) e += 360.0f; + return fabsf(e); + } + + float sigmaDeg() const + { + return sqrtf(gsf.yawVariance) / RAD; + } +}; + +TEST(GsfYawTest, ConvergesFromAnyHeadingAfterATurn) +{ + for (int start = 0; start < 360; start += 30) { + GsfFlight f(start + 7.0f, 18.0f); + f.fly(5.0f, 0.0f, 0.0f); + f.fly(3.0f, 0.0f, 1.0f); + f.fly(18.0f, 20.0f, 0.0f); // a full turn to the right + EXPECT_LT(f.errorDeg(), 2.0f) << "start heading " << start; + EXPECT_LT(f.sigmaDeg(), 5.0f) << "start heading " << start; + f.fly(10.0f, 0.0f, 0.0f); + f.fly(18.0f, -20.0f, 0.0f); // and one to the left + EXPECT_LT(f.errorDeg(), 2.0f) << "start heading " << start; + } +} + +TEST(GsfYawTest, DoesNotClaimAHeadingWithoutManoeuvres) +{ + // Straight and level at constant speed: nothing tells the heading apart + GsfFlight f(123.0f, 18.0f); + f.fly(30.0f, 0.0f, 0.0f); + EXPECT_GT(f.sigmaDeg(), 30.0f); +} + +TEST(GsfYawTest, ConvergesWithNoisyGpsVelocity) +{ + for (int start = 0; start < 360; start += 45) { + GsfFlight f(start + 3.0f, 16.0f, 0.4f); + f.fly(5.0f, 0.0f, 0.0f); + f.fly(36.0f, 20.0f, 0.0f); // two full turns + EXPECT_LT(f.errorDeg(), 5.0f) << "start heading " << start; + } +} + +TEST(GsfYawTest, WrapsAcrossSouth) +{ + // Heading near +-180 deg, where the composite yaw changes sign + GsfFlight f(178.0f, 18.0f); + f.fly(5.0f, 0.0f, 0.0f); + f.fly(18.0f, 20.0f, 0.0f); + f.fly(1.0f, 4.0f, 0.0f); + EXPECT_LT(f.errorDeg(), 2.0f); +} + +// The GPS reports the velocity it measured delayUs before the message arrives. The IMU history is the velocity change +// in a frame turned by an unknown angle from north, with an accelerometer bias and noise +class DelayFlight { +public: + gsfImuHistory_t history; + gsfDelayEstimator_t estimator; + uint32_t delayUs; + uint32_t gpsPeriodUs; + bool turning; + float gpsNoise; // m/s + float frameDrift; // rad/s, the gyro's yaw bias turning the history frame + uint32_t rng = 777; + + DelayFlight(uint32_t delay, uint32_t gpsPeriod, bool turns, float noise = 0.05f, float drift = 0.0f) + : delayUs(delay), gpsPeriodUs(gpsPeriod), turning(turns), gpsNoise(noise), frameDrift(drift) + { + memset(&history, 0, sizeof(history)); + memset(&estimator, 0, sizeof(estimator)); + } + + float uniform() + { + rng = rng * 1664525u + 1013904223u; + return 2.0f * (rng >> 8) / 16777216.0f - 1.0f; + } + + std::string dump() const + { + std::string s = " samples " + std::to_string(estimator.samples) + " sgg " + std::to_string(estimator.sgg) + " cost"; + for (int j = 0; j < GSF_DELAY_STEPS; j++) { + const float c = estimator.sii[j] + estimator.sgg - 2.0f * sqrtf(estimator.sdot[j] * estimator.sdot[j] + estimator.scross[j] * estimator.scross[j]); + s += " " + std::to_string(c); + } + return s; + } + + // Turn rate in rad/s: straight 5 s, right 6 s, straight 4 s, left 6 s, with a 0.4 s roll in and out + float turnRate(float t) const + { + if (!turning) { + return 0.0f; + } + const float phase = fmodf(t, 21.0f); + const float rate = 20.0f * RAD; + auto ramp = [](float x) { return x <= 0.0f ? 0.0f : (x >= 0.4f ? 1.0f : x / 0.4f); }; + if (phase < 5.0f) { + return 0.0f; + } else if (phase < 11.0f) { + return rate * ramp(phase - 5.0f) * ramp(11.0f - phase); + } else if (phase < 15.0f) { + return 0.0f; + } + return -rate * ramp(phase - 15.0f) * ramp(21.0f - phase); + } + + void fly(float seconds) + { + const float speed = 18.0f; + float frame = 77.0f * RAD; + const float bias[2] = { 0.15f, -0.1f }; // m/s^2 in the history frame + const uint32_t start = 1000000; + float heading = 0.3f; + std::vector> truth; // every 20 ms + uint32_t nextGps = start + gpsPeriodUs; + const int steps = (int)(seconds / 0.002f); + const float v0[2] = { speed * cosf(heading), speed * sinf(heading) }; + + for (int n = 1; n <= steps; n++) { + const float t = n * 0.002f; + heading += turnRate(t) * 0.002f; + const uint32_t now = start + n * 2000; + + frame += frameDrift * 0.002f; + if (n % 10 == 0) { + const float vN = speed * cosf(heading), vE = speed * sinf(heading); + truth.push_back({ vN, vE }); + + // 20 ms window: the true change turned into the history frame, plus bias and noise + const float dN = vN - v0[0], dE = vE - v0[1]; + const float imuVel[2] = { + cosf(frame) * dN + sinf(frame) * dE + bias[0] * t + 0.01f * uniform(), + -sinf(frame) * dN + cosf(frame) * dE + bias[1] * t + 0.01f * uniform(), + }; + gsfHistoryAdd(&history, now, imuVel); + + // Measured at nextGps, arriving delayUs later with up to 10 ms of jitter + while (nextGps + delayUs <= now) { + const size_t idx = (nextGps - start) / 20000 - 1; + const float vel[2] = { truth[idx][0] + gpsNoise * uniform(), truth[idx][1] + gpsNoise * uniform() }; + const uint32_t arrival = nextGps + delayUs + (uint32_t)(5000.0f * (1.0f + uniform())); + gsfDelayAddGps(&estimator, &history, MIN(arrival, now), vel); + nextGps += gpsPeriodUs; + } + } + } + } +}; + +TEST(GsfYawTest, MeasuresTheGpsDelayInTurns) +{ + const uint32_t delays[] = { 0, 60000, 120000, 200000, 260000 }; + for (uint32_t delay : delays) { + DelayFlight f(delay, 100000, true); + f.fly(80.0f); + uint32_t estimate = 0; + ASSERT_TRUE(gsfDelayEstimate(&f.estimator, &estimate)) << "delay " << delay << f.dump(); + EXPECT_NEAR((float)estimate, (float)delay, 15000.0f) << "delay " << delay; + } +} + +TEST(GsfYawTest, MeasuresTheGpsDelayAtFiveHertz) +{ + DelayFlight f(220000, 200000, true); + f.fly(120.0f); + uint32_t estimate = 0; + ASSERT_TRUE(gsfDelayEstimate(&f.estimator, &estimate)); + EXPECT_NEAR((float)estimate, 220000.0f, 20000.0f); +} + +TEST(GsfYawTest, MeasuresTheGpsDelayWithANoisyGpsAndGyroDrift) +{ + // Three times the velocity noise, and the history frame drifting 0.1 deg/s against north + DelayFlight f(120000, 100000, true, 0.15f, 0.1f * RAD); + f.fly(120.0f); + uint32_t estimate = 0; + ASSERT_TRUE(gsfDelayEstimate(&f.estimator, &estimate)) << f.dump(); + EXPECT_NEAR((float)estimate, 120000.0f, 25000.0f); +} + +TEST(GsfYawTest, NoGpsDelayWithoutManoeuvres) +{ + DelayFlight f(120000, 100000, false); + f.fly(80.0f); + uint32_t estimate = 0; + EXPECT_FALSE(gsfDelayEstimate(&f.estimator, &estimate)); +} + +// Earth-up in body axes for a roll and a pitch, as rMat's last row: (-sin pitch, sin roll cos pitch, cos roll cos pitch) +static void earthUp(float rollDeg, float pitchDeg, float up[3]) +{ + up[0] = -sinf(pitchDeg * RAD); + up[1] = sinf(rollDeg * RAD) * cosf(pitchDeg * RAD); + up[2] = cosf(rollDeg * RAD) * cosf(pitchDeg * RAD); +} + +TEST(GsfYawTest, YawRateIsTheNoseHeadingRate) +{ + float up[3]; + + // A roll about the nose at 20 deg pitch leaves the heading alone (the rate about the vertical would say 0.34 rad/s) + earthUp(0.0f, 20.0f, up); + const float roll[3] = { 1.0f, 0.0f, 0.0f }; + EXPECT_NEAR(gsfYawRate(up, roll), 0.0f, 1e-4f); + + // Level, a rotation about the body's up axis (counterclockwise seen from above) lowers the heading + earthUp(0.0f, 0.0f, up); + const float yawLeft[3] = { 0.0f, 0.0f, 0.5f }; + EXPECT_NEAR(gsfYawRate(up, yawLeft), -0.5f, 1e-4f); + + // A clockwise turn about the vertical at 0.3 rad/s, banked 45 deg and 10 deg nose up: the body rates are the + // vertical rate spread over the body axes, and the heading rate is the whole 0.3 + earthUp(45.0f, 10.0f, up); + const float turn[3] = { -0.3f * up[0], -0.3f * up[1], -0.3f * up[2] }; + EXPECT_NEAR(gsfYawRate(up, turn), 0.3f, 1e-4f); +} + +TEST(GsfYawTest, ClipsASpikeAlongCorrelatedVelocityErrors) +{ + gsfState_t gsf; + gsfReset(&gsf); + gsf.fusing = true; + const float velN = 10.0f, velE = -4.0f; + for (int i = 0; i < GSF_MODEL_COUNT; i++) { + gsf.model[i].X[0] = velN; + gsf.model[i].X[1] = velE; + } + + // North and east errors that move together: an innovation of opposite signs is 6.7 sigma away (test ratio 45), + // though each axis alone looks like 3.4 sigma + gsfModel_t *m = &gsf.model[0]; + memset(m->P, 0, sizeof(m->P)); + m->P[0][0] = m->P[1][1] = 0.75f; + m->P[0][1] = m->P[1][0] = 0.6f; + m->P[2][2] = 0.1f; + m->X[0] = velN + 3.0f; + m->X[1] = velE - 3.0f; + m->X[2] = 0.0f; + + ASSERT_TRUE(gsfFuseVelocity(&gsf, velN, velE, 0.5f)); + + // Gain 0.15 / (0.15 + 0.25) along the innovation, scaled down to 5 sigma + const float step = 0.375f * 3.0f * sqrtf(25.0f / 45.0f); + EXPECT_NEAR(m->X[0], velN + 3.0f - step, 1e-3f); + EXPECT_NEAR(m->X[1], velE - 3.0f + step, 1e-3f); +}