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..d689b8058fa 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2886,6 +2886,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..120a7f34b2f 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -497,7 +497,8 @@ "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" }, "BlackboxState": { "_source": "inav/src/main/blackbox/blackbox.h", @@ -1298,59 +1299,68 @@ "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_NEVER": "68", "FLIGHT_LOG_FIELD_CONDITION_FIRST": "FLIGHT_LOG_FIELD_CONDITION_ALWAYS", "FLIGHT_LOG_FIELD_CONDITION_LAST": "FLIGHT_LOG_FIELD_CONDITION_NEVER" }, @@ -2543,6 +2553,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..34053135e0b 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -195,6 +195,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 +1061,7 @@ | `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 | | --- ## `BlackboxState` @@ -2190,59 +2192,65 @@ | `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_NEVER` | 68 | | | `FLIGHT_LOG_FIELD_CONDITION_FIRST` | FLIGHT_LOG_FIELD_CONDITION_ALWAYS | | | `FLIGHT_LOG_FIELD_CONDITION_LAST` | FLIGHT_LOG_FIELD_CONDITION_NEVER | | @@ -3804,6 +3812,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..74370622d29 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -505,6 +505,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..bc08f2b57c4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -79,6 +79,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 +119,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 +398,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)}, @@ -486,57 +491,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 }; @@ -657,6 +670,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 +695,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 +756,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 +898,11 @@ 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_NEVER: return false; @@ -889,21 +913,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) @@ -1480,6 +1500,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 +1613,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 } /** @@ -2147,6 +2184,12 @@ 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 @@ -2415,7 +2458,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..c164c43e15d 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -36,6 +36,7 @@ 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, } blackboxFeatureMask_e; typedef enum BlackboxState { diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 8dc8f5defdd..4becb648bf1 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,8 @@ 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_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..42097377563 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -200,6 +200,7 @@ static const char * const blackboxIncludeFlagNames[] = { "PEAKS_P", "PEAKS_Y", "SERVOS", + "MAG_LEARN", 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/settings.yaml b/src/main/fc/settings.yaml index 142f5f06c9b..58acd0a9880 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 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/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..f07af1c2aa9 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -173,6 +173,11 @@ 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 + // 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..11a5b002f3b 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) 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); +} + +}