diff --git a/docs/Multirotor Chirp.md b/docs/Multirotor Chirp.md new file mode 100644 index 00000000000..be563a21012 --- /dev/null +++ b/docs/Multirotor Chirp.md @@ -0,0 +1,143 @@ +# Experimental multirotor chirp measurement + +Chirp generates a repeatable, single-axis rate excitation for Blackbox analysis. +It is a measurement tool: it does **not** select or change PID gains, filters, +rates or EZ-Tune settings. The initial implementation is intended for developer +validation; automated recommendations and a Configurator workflow are separate +work. It has not been validated in real flight. + +## Relationship to existing work + +[Q-Tune #8546](https://github.com/iNavFlight/inav/pull/8546) detects oscillations +from flight samples. This feature supplies a known excitation signal for +identification experiments instead. It neither copies nor replaces Q-Tune's +detector. Both relate to [the multirotor autotuning request +#10433](https://github.com/iNavFlight/inav/issues/10433); this measurement feature +alone does not resolve that request. + +## Scope and prerequisites + +- Conventional multirotor mixer and multirotor PID controller; no tricopters, + helicopters, VTOL transitions, fixed wings or reversible motors. +- Manual-throttle ANGLE flight only. Navigation modes, heading hold, horizon, + failsafe and programming overrides inhibit the test. +- A working, already flyable tune. This is not a way to make an unstable aircraft + flyable. Maintain visual control and space to recover from the excitation. +- Blackbox must actually be recording with `debug_mode = CHIRP`, at least + 600 samples/second. The PID loop must also run at least at 600 Hz. For example, + a 1 kHz PID loop with `blackbox_rate_num = 1` and `blackbox_rate_denom = 1` + meets this requirement. A 500 Hz log does not. +- A dedicated receiver switch channel between **5 and 12** (5 = AUX1), with no + other function assigned to it. Channels 13-32 can be overlaid by MSP, so are + deliberately excluded. The selected channel must exist on the receiver. + +The feature is built when `USE_CHIRP` and `USE_BLACKBOX` are available, and is +disabled by default (`chirp_axis = OFF`). It uses a receiver channel directly, +without adding or reusing a flight-mode ID or changing MSP status payloads. + +## Configuration + +Save a `diff all` backup first. Set up and verify normal Blackbox recording before +enabling the test. For example, to prepare a roll measurement using CH8 (AUX4): + +```text +set chirp_axis = ROLL +set chirp_trigger_channel = 8 +set chirp_amplitude = 10 +set debug_mode = CHIRP +save +``` + +`chirp_amplitude` is the peak rate perturbation in degrees/second (1-30, default +10). It describes the commanded excitation, not the maximum aircraft response. +Use small excitation appropriate to the airframe. Change the axis to `PITCH` or +`YAW` while disarmed to measure the other axes. Do not change controller settings +or profiles during a run. + +## Measurement sequence + +1. Leave the trigger low, take off normally, and establish a level hover in + ANGLE with manual throttle. This implementation does not hold position or + altitude for the pilot. +2. With roll/pitch/yaw sticks centred and recording running, the test observes + the low trigger (900-1300). Switching high (1700-2100) starts a two-second + settling period followed by one 20-second logarithmic sweep from 2 to 60 Hz. + The amplitude fades in and out over one second. +3. Keep the aircraft under control. Moving a stick, moving the trigger out of + its high range, or changing to an incompatible mode stops excitation. After + an abort the switch must be observed low again under valid conditions before + another high transition can start a test. A held-high switch never repeats + a completed run and cannot start a run at arming or after receiver recovery. +4. Switch low, land and inspect the log. Repeat for another axis if needed. +5. Disable the experiment afterwards with `set chirp_axis = OFF` and `save`. + +During settling and excitation, any of the following stops the test: + +- Disarm, loss of valid receiver data, failsafe, wrong controller/platform/mode, + landing detection, mixer transition, MSP RC override or programming override. +- Roll/pitch/yaw channel deviation above 50 from centre, processed stick command + above 50, processed throttle outside (1200, 1800), or throttle change greater + than 100 from its value at test start. +- Roll or pitch exceeding 20 degrees, measured gyro rate above 200 degrees/sec + on any axis, non-finite gyro data or motor mix range at/above 0.9. +- Blackbox stopping/pausing, inadequate logging rate, missing CHIRP debug mode, + changed chirp configuration/control profile/mixer profile, or a PID update gap + exceeding 10 ms. + +These limits are conservative experimental checks, not a guarantee of safe +flight. Receiver values are processed by INAV's normal RX path. Mixer saturation +feedback is from the preceding mixer update, and storage-full/write-error +detection follows the normal Blackbox state machine. A short output clipping +event or dropped log record can still occur; inspect the recorded output and +timestamps and discard invalid runs. There is no automatic airborne detection +beyond the normal landing flag: deliberately operating the switch while armed +on the ground can start the test if the other conditions are met. + +## Blackbox contract + +The new `CHIRP` debug enum is appended; existing debug IDs are unchanged. All +eight debug channels are populated on each PID iteration. Phase values are: +0 idle/disabled, 1 ready, 2 settling, 3 running, 4 complete, 5 aborted. + +| Field | Meaning | Scale | +| --- | --- | --- | +| `debug[0]` | Phase | enum above | +| `debug[1]` | Selected axis | 0 off, 1 roll, 2 pitch, 3 yaw | +| `debug[2]` | Injected rate perturbation | degrees/sec × 100 | +| `debug[3]` | Instantaneous sweep frequency | Hz × 100 | +| `debug[4]` | Inhibit/abort bitmask | below | +| `debug[5]` | Actual selected-axis rate-controller setpoint, including chirp | degrees/sec × 100 | +| `debug[6]` | Selected-axis gyro feedback used by the controller | degrees/sec × 100 | +| `debug[7]` | Selected-axis limited PID sum before mixing | PID output × 100 | + +Inhibit bits: 1 flight conditions, 2 pilot/override input, 4 logging, 8 motion, +16 mixer saturation, 32 configuration/profile, 64 timing, 128 trigger state. +Several bits may be present. Aborted runs retain their last reason until another +inhibit occurs, a valid low trigger resets them or the feature is disabled. + +Excitation is added **after** angle control, rate acceleration limiting and +programming overrides, immediately before the multicopter rate controller. The +normal P, I, D and CD paths all receive the perturbed setpoint. The standard +Blackbox setpoint is updated accordingly. `debug[6]` is the actual controller +feedback; it can differ from the standard `gyroADC` field when the Smith predictor +is enabled. No changes are made to the predictor or filters for this test. + +Use `debug[2]` as the known external excitation, and retain the actual setpoint, +feedback and actuator output. ANGLE feedback and the CD path remain active, so +the gyro/excitation ratio alone is **not** the open-loop plant or the isolated +rate-controller response. Do not derive PID recommendations from that ratio +without modelling the closed loop and validating the estimate. A completed +sweep is not a quality or stability certificate. + +## Developer validation + +`chirp_unittest` exercises the production generator, single-shot state machine, +inhibits, switch reset, amplitude limits, sweep frequency, timing gaps and the +32-bit microsecond wrap. `chirp_flight_unittest` links the production firmware +adapter with stubbed flight inputs and exercises axis selection, logging, +controller readiness, invalid channels and the flight abort paths. + +Before release, validate the full control loop in a flight-dynamics simulator, +measure CPU cost and flash/RAM use on supported boards, verify Blackbox storage +throughput, and perform staged flight tests. Unit tests and a successful SITL +build do not substitute for those checks. diff --git a/docs/Settings.md b/docs/Settings.md index 6d7169b7c5b..d2c744d69a6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -645,6 +645,39 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho --- +### chirp_amplitude + +Peak chirp perturbation in degrees/second. Select a small excitation appropriate to the aircraft; the test does not change PID gains. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 1 | 30 | + +--- + +### chirp_axis + +Experimental multirotor rate chirp axis. OFF disables excitation. Requires manual-throttle ANGLE flight and CHIRP Blackbox logging. See Multirotor Chirp.md. + +| Allowed Values | | +| --- | --- | +| OFF | Default | +| ROLL | | +| PITCH | | +| YAW | | + +--- + +### chirp_trigger_channel + +Dedicated receiver switch channel for the experimental chirp test (5 = AUX1). Observe low (900-1300) in valid flight conditions, then high (1700-2100) to start one sweep. Do not share with other functions. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | 12 | + +--- + ### crsf_use_legacy_baro_packet CRSF telemetry: If `ON`, send altitude about start point in GPS telemetry packet. If `OFF`, GPS has ASL altitude, altitude about start point in separate packet. Default: 'OFF' @@ -803,6 +836,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | ESC | | | FW_TURN | | | MAG | | +| CHIRP | | --- diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 1cfd028c3f9..9da77c2a74b 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -380,6 +380,11 @@ main_sources(COMMON_SRC flight/secondary_dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h + flight/chirp.c + flight/chirp.h + flight/chirp_flight.c + flight/chirp_flight.h + flight/ez_tune.c flight/ez_tune.h flight/adaptive_filter.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 23caa6b6fa7..c661382c517 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -87,6 +87,7 @@ typedef enum { DEBUG_ESC, DEBUG_FW_TURN, DEBUG_MAG, + DEBUG_CHIRP, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index fe52a22e63f..6f402a2842a 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -136,7 +136,8 @@ #define PG_DRONECAN_CONFIG 1045 #define PG_TERRAIN_NAV_CONFIG 1046 #define PG_DRONECAN_DNA_SERVER 1047 // Separate PG so we don't wipe user settings if the allocation table changes -#define PG_INAV_END PG_DRONECAN_DNA_SERVER +#define PG_CHIRP_CONFIG 1048 +#define PG_INAV_END PG_CHIRP_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 73c65919b09..2236aefb07b 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -238,7 +238,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "TERRAIN_NAV", "ESC", "FW_TURN", - "MAG" + "MAG", + "CHIRP" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f805e60d658..ee1d19b9b6a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -93,7 +93,9 @@ tables: "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "MAG_CALIB", "VTOL_TRANSITION", "VTOL_MC_PROTECT", "TERRAIN_NAV", "ESC", - "FW_TURN", "MAG"] + "FW_TURN", "MAG", "CHIRP"] + - name: chirp_axis + values: ["OFF", "ROLL", "PITCH", "YAW"] - name: vtol_mc_protection_mode values: ["OFF", "NAV", "NAV_AND_STABILIZED"] - name: aux_operator @@ -1724,6 +1726,32 @@ groups: min: 0 max: 99 + - name: PG_CHIRP_CONFIG + headers: ["flight/chirp_flight.h"] + type: chirpConfig_t + condition: USE_CHIRP + members: + - name: chirp_axis + description: "Experimental multirotor rate chirp axis. OFF disables excitation. Requires manual-throttle ANGLE flight and CHIRP Blackbox logging. See Multirotor Chirp.md." + field: axis + type: uint8_t + table: chirp_axis + default_value: "OFF" + - name: chirp_trigger_channel + description: "Dedicated receiver switch channel for the experimental chirp test (5 = AUX1). Observe low (900-1300) in valid flight conditions, then high (1700-2100) to start one sweep. Do not share with other functions." + field: triggerChannel + type: uint8_t + min: 5 + max: 12 + default_value: 5 + - name: chirp_amplitude + description: "Peak chirp perturbation in degrees/second. Select a small excitation appropriate to the aircraft; the test does not change PID gains." + field: amplitude + type: uint8_t + min: 1 + max: 30 + default_value: 10 + - name: PG_EZ_TUNE headers: ["flight/ez_tune.h"] type: ezTuneSettings_t diff --git a/src/main/flight/chirp.c b/src/main/flight/chirp.c new file mode 100644 index 00000000000..915938b7b52 --- /dev/null +++ b/src/main/flight/chirp.c @@ -0,0 +1,94 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#include + +#include "flight/chirp.h" + +#define CHIRP_TWO_PI 6.28318530718f +#define CHIRP_LOG_FREQUENCY_RATIO 3.40119738166f // log(60 / 2) +#define CHIRP_FADE_US 1000000U + +void chirpUpdate(chirpState_t *state, uint32_t now, bool enabled, bool switchLow, + bool switchHigh, uint16_t inhibit, float amplitude) +{ + state->output = 0; + + if (!enabled) { + *state = (chirpState_t){0}; + return; + } + + if (!isfinite(amplitude) || amplitude <= 0 || amplitude > 30) { + inhibit |= CHIRP_INHIBIT_CONFIG; + } + if (state->phase == CHIRP_SETTLING || state->phase == CHIRP_RUNNING) { + if (now - state->updatedAt > CHIRP_MAX_INTERVAL_US) { + inhibit |= CHIRP_INHIBIT_TIMING; + } + } + + if (inhibit) { + state->phase = CHIRP_ABORTED; + state->inhibit = inhibit; + return; + } + + if (switchLow) { + *state = (chirpState_t){ .phase = CHIRP_READY, .updatedAt = now }; + return; + } + + if (!switchHigh) { + state->phase = CHIRP_ABORTED; + state->inhibit = CHIRP_INHIBIT_SWITCH; + return; + } + + if (state->phase == CHIRP_READY) { + state->phase = CHIRP_SETTLING; + state->startedAt = now; + state->updatedAt = now; + } else if (state->phase == CHIRP_IDLE) { + state->phase = CHIRP_ABORTED; + state->inhibit = CHIRP_INHIBIT_SWITCH; + } + + if (state->phase == CHIRP_SETTLING) { + state->updatedAt = now; + if (now - state->startedAt < CHIRP_SETTLE_US) { + return; + } + state->phase = CHIRP_RUNNING; + state->startedAt = now; + state->frequency = CHIRP_START_HZ; + state->angle = 0; + } + + if (state->phase != CHIRP_RUNNING) { + return; + } + + const uint32_t elapsed = now - state->startedAt; + if (elapsed >= CHIRP_DURATION_US) { + state->phase = CHIRP_DONE; + return; + } + + const float frequency = CHIRP_START_HZ * expf(CHIRP_LOG_FREQUENCY_RATIO * elapsed / CHIRP_DURATION_US); + const float dt = (now - state->updatedAt) * 1e-6f; + // Integrate with bounded phase to avoid loss of precision late in the sweep. + state->angle = fmodf(state->angle + CHIRP_TWO_PI * 0.5f * (state->frequency + frequency) * dt, CHIRP_TWO_PI); + state->updatedAt = now; + state->frequency = frequency; + + float envelope = 1; + if (elapsed < CHIRP_FADE_US) { + envelope = (float)elapsed / CHIRP_FADE_US; + } else if (CHIRP_DURATION_US - elapsed < CHIRP_FADE_US) { + envelope = (float)(CHIRP_DURATION_US - elapsed) / CHIRP_FADE_US; + } + state->output = amplitude * envelope * sinf(state->angle); +} diff --git a/src/main/flight/chirp.h b/src/main/flight/chirp.h new file mode 100644 index 00000000000..0e79b07f036 --- /dev/null +++ b/src/main/flight/chirp.h @@ -0,0 +1,51 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#pragma once + +#include +#include + +#define CHIRP_START_HZ 2.0f +#define CHIRP_END_HZ 60.0f +#define CHIRP_DURATION_US 20000000U +#define CHIRP_SETTLE_US 2000000U +#define CHIRP_MAX_INTERVAL_US 10000U + +typedef enum { + CHIRP_IDLE, + CHIRP_READY, + CHIRP_SETTLING, + CHIRP_RUNNING, + CHIRP_DONE, + CHIRP_ABORTED, +} chirpPhase_e; + +typedef enum { + CHIRP_INHIBIT_NONE = 0, + CHIRP_INHIBIT_FLIGHT = 1 << 0, + CHIRP_INHIBIT_PILOT = 1 << 1, + CHIRP_INHIBIT_LOGGING = 1 << 2, + CHIRP_INHIBIT_MOTION = 1 << 3, + CHIRP_INHIBIT_SATURATION = 1 << 4, + CHIRP_INHIBIT_CONFIG = 1 << 5, + CHIRP_INHIBIT_TIMING = 1 << 6, + CHIRP_INHIBIT_SWITCH = 1 << 7, +} chirpInhibit_e; + +typedef struct chirpState_s { + chirpPhase_e phase; + uint32_t startedAt; + uint32_t updatedAt; + uint16_t inhibit; + float frequency; + float angle; + float output; +} chirpState_t; + +// A low switch must be observed in valid flight conditions before every run. +// Neither booting with the switch high nor recovering from an abort starts a run. +void chirpUpdate(chirpState_t *state, uint32_t now, bool enabled, bool switchLow, + bool switchHigh, uint16_t inhibit, float amplitude); diff --git a/src/main/flight/chirp_flight.c b/src/main/flight/chirp_flight.c new file mode 100644 index 00000000000..7e4435f875e --- /dev/null +++ b/src/main/flight/chirp_flight.c @@ -0,0 +1,164 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_CHIRP) && defined(USE_BLACKBOX) + +#include "build/debug.h" +#include "common/axis.h" +#include "common/maths.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "drivers/time.h" +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" +#include "flight/chirp.h" +#include "flight/chirp_flight.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/mixer_profile.h" +#include "blackbox/blackbox.h" +#include "programming/logic_condition.h" +#include "rx/rx.h" +#include "sensors/gyro.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(chirpConfig_t, chirpConfig, PG_CHIRP_CONFIG, 0); + +PG_RESET_TEMPLATE(chirpConfig_t, chirpConfig, + .axis = SETTING_CHIRP_AXIS_DEFAULT, + .triggerChannel = SETTING_CHIRP_TRIGGER_CHANNEL_DEFAULT, + .amplitude = SETTING_CHIRP_AMPLITUDE_DEFAULT, +); + +static chirpState_t chirp; +static chirpConfig_t runConfig; +static uint8_t runProfile; +static uint8_t runMixerProfile; +static int16_t runThrottle; + +// Keep LTO from copying the experiment's state machine into pidController's limited ITCM. +void NOINLINE chirpFlightUpdate(float dt, bool controllerReady) +{ + const chirpConfig_t *config = chirpConfig(); + const bool enabled = config->axis != CHIRP_AXIS_OFF; + const uint32_t now = (uint32_t)micros(); + uint16_t inhibit = 0; + bool switchLow = false; + bool switchHigh = false; + + if (!enabled) { + chirpUpdate(&chirp, now, false, false, false, 0, 0); + } else { + // Channels 13-32 may carry an MSP auxiliary overlay rather than the receiver switch. + if (config->axis > CHIRP_AXIS_YAW || config->triggerChannel < 5 || + config->triggerChannel > rxRuntimeConfig.channelCount || config->triggerChannel > 12) { + inhibit |= CHIRP_INHIBIT_CONFIG; + } else { + const int channel = rxGetChannelValue(config->triggerChannel - 1); + switchLow = channel >= CHANNEL_RANGE_MIN && channel <= 1300; + switchHigh = channel >= 1700 && channel <= CHANNEL_RANGE_MAX; + } + + // Only manual-throttle ANGLE flight on a conventional multirotor is supported. + // Reject all other flight-mode bits, including navigation and failsafe. + if (!controllerReady || !ARMING_FLAG(ARMED) || !STATE(MULTIROTOR) || STATE(LANDING_DETECTED) || + mixerConfig()->platformType != PLATFORM_MULTIROTOR || isMixerTransitionMixing || + feature(FEATURE_REVERSIBLE_MOTORS) || flightModeFlags != ANGLE_MODE || + failsafeIsActive() || !failsafeIsReceivingRxData() || !rxIsReceivingSignal() || !rxAreFlightChannelsValid() || + IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)) { + inhibit |= CHIRP_INHIBIT_FLIGHT; + } + +#ifdef USE_PROGRAMMING_FRAMEWORK + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE | + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE | LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL | + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS)) { + inhibit |= CHIRP_INHIBIT_PILOT; + } +#endif + if (rcCommand[THROTTLE] <= 1200 || rcCommand[THROTTLE] >= 1800) { + inhibit |= CHIRP_INHIBIT_PILOT; + } + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + if (abs(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE) > 50 || + abs(rcCommand[axis]) > 50 || isFlightAxisAngleOverrideActive(axis) || isFlightAxisRateOverrideActive(axis)) { + inhibit |= CHIRP_INHIBIT_PILOT; + } + if (!isfinite(gyro.gyroADCf[axis]) || fabsf(gyro.gyroADCf[axis]) > 200) { + inhibit |= CHIRP_INHIBIT_MOTION; + } + } + if (abs(attitude.values.roll) > 200 || abs(attitude.values.pitch) > 200) { + inhibit |= CHIRP_INHIBIT_MOTION; + } + if (!isfinite(getMotorMixRange()) || getMotorMixRange() >= 0.9f) { + inhibit |= CHIRP_INHIBIT_SATURATION; + } + + // Require ten samples per cycle at the highest excitation frequency. + // No automatic filter or logging configuration changes are made in flight. + const blackboxConfig_t *logConfig = blackboxConfig(); + if (!isfinite(dt) || dt <= 0 || dt > 1.0f / (10 * CHIRP_END_HZ) || + debugMode != DEBUG_CHIRP || getBlackboxState() != BLACKBOX_STATE_RUNNING || + !logConfig->rate_num || !logConfig->rate_denom || + (float)logConfig->rate_num / logConfig->rate_denom < dt * (10 * CHIRP_END_HZ)) { + inhibit |= CHIRP_INHIBIT_LOGGING; + } + + const bool active = chirp.phase == CHIRP_SETTLING || chirp.phase == CHIRP_RUNNING; + if (active && abs(rcCommand[THROTTLE] - runThrottle) > 100) { + inhibit |= CHIRP_INHIBIT_PILOT; + } + if (active && (config->axis != runConfig.axis || config->triggerChannel != runConfig.triggerChannel || + config->amplitude != runConfig.amplitude || getConfigProfile() != runProfile || + getConfigMixerProfile() != runMixerProfile)) { + inhibit |= CHIRP_INHIBIT_CONFIG; + } + + chirpUpdate(&chirp, now, true, switchLow, switchHigh, inhibit, config->amplitude); + if (!active && chirp.phase == CHIRP_SETTLING) { + runConfig = *config; + runProfile = getConfigProfile(); + runMixerProfile = getConfigMixerProfile(); + runThrottle = rcCommand[THROTTLE]; + } + } + + DEBUG_SET(DEBUG_CHIRP, 0, chirp.phase); + DEBUG_SET(DEBUG_CHIRP, 1, config->axis); + DEBUG_SET(DEBUG_CHIRP, 2, lrintf(chirp.output * 100)); + DEBUG_SET(DEBUG_CHIRP, 3, lrintf(chirp.frequency * 100)); + DEBUG_SET(DEBUG_CHIRP, 4, chirp.inhibit); + DEBUG_SET(DEBUG_CHIRP, 5, 0); + DEBUG_SET(DEBUG_CHIRP, 6, 0); + DEBUG_SET(DEBUG_CHIRP, 7, 0); +} + +float chirpApplyRate(int axis, float rate) +{ + return rate + ((axis + 1 == chirpConfig()->axis) ? chirp.output : 0); +} + +void chirpLogResponse(int axis, float setpoint, float measurement, float output) +{ + if (axis + 1 == chirpConfig()->axis) { + DEBUG_SET(DEBUG_CHIRP, 5, lrintf(setpoint * 100)); + DEBUG_SET(DEBUG_CHIRP, 6, lrintf(measurement * 100)); + DEBUG_SET(DEBUG_CHIRP, 7, lrintf(output * 100)); + } +} + +#endif diff --git a/src/main/flight/chirp_flight.h b/src/main/flight/chirp_flight.h new file mode 100644 index 00000000000..6be6c11aa5e --- /dev/null +++ b/src/main/flight/chirp_flight.h @@ -0,0 +1,30 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#pragma once + +#include +#include + +#include "config/parameter_group.h" + +typedef enum { + CHIRP_AXIS_OFF, + CHIRP_AXIS_ROLL, + CHIRP_AXIS_PITCH, + CHIRP_AXIS_YAW, +} chirpAxis_e; + +typedef struct chirpConfig_s { + uint8_t axis; + uint8_t triggerChannel; // Receiver channel number, starting at 1 (5 = AUX1). + uint8_t amplitude; // Peak rate perturbation in degrees/second. +} chirpConfig_t; + +PG_DECLARE(chirpConfig_t, chirpConfig); + +void chirpFlightUpdate(float dt, bool controllerReady); +float chirpApplyRate(int axis, float rate); +void chirpLogResponse(int axis, float setpoint, float measurement, float output); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c39aef9cf88..4995c0e78de 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -41,6 +41,7 @@ #include "fc/settings.h" #include "flight/pid.h" +#include "flight/chirp_flight.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/mixer_profile.h" @@ -1749,7 +1750,11 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, float dT, float dT_inv) { - const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget); + float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget); +#if defined(USE_CHIRP) && defined(USE_BLACKBOX) + // Excite the actual rate controller after level/rate limiting and overrides. + rateTarget = chirpApplyRate(pidState->axis, rateTarget); +#endif const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); @@ -1788,6 +1793,9 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid applyItermLimiting(pidState); axisPID[pidState->axis] = newOutputLimited; +#if defined(USE_CHIRP) && defined(USE_BLACKBOX) + chirpLogResponse(pidState->axis, rateTarget, pidState->gyroRate, newOutputLimited); +#endif #ifdef USE_BLACKBOX axisPID_P[pidState->axis] = newPTerm; @@ -2059,6 +2067,9 @@ void FAST_CODE pidController(float dT) { const float dT_inv = 1.0f / dT; pidLoopNowMs = millis(); +#if defined(USE_CHIRP) && defined(USE_BLACKBOX) + chirpFlightUpdate(dT, pidFiltersConfigured && usedPidControllerType == PID_TYPE_PID); +#endif if (!pidFiltersConfigured) { #ifdef USE_AUTO_TRANSITION diff --git a/src/main/target/common.h b/src/main/target/common.h index fb46490362c..8208fb0f45f 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -92,6 +92,9 @@ #ifndef USE_BLACKBOX #define USE_BLACKBOX #endif +#if defined(USE_BLACKBOX) && !defined(USE_CHIRP) +#define USE_CHIRP +#endif #ifndef USE_GPS #define USE_GPS #endif diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 60bde216173..e30cd49d010 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -1,3 +1,6 @@ +set_property(SOURCE chirp_unittest.cc PROPERTY depends "flight/chirp.c") +set_property(SOURCE chirp_flight_unittest.cc PROPERTY depends "flight/chirp.c" "flight/chirp_flight.c") + # XXX: This should come from main project once everything # uses cmake set(MAIN_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../../src/main") diff --git a/src/test/unit/chirp_flight_unittest.cc b/src/test/unit/chirp_flight_unittest.cc new file mode 100644 index 00000000000..811abc3b337 --- /dev/null +++ b/src/test/unit/chirp_flight_unittest.cc @@ -0,0 +1,251 @@ +/* SPDX-License-Identifier: GPL-3.0-or-later */ + +#include +#include +#include +#include +#include "gtest/gtest.h" + +extern "C" { +#include "platform.h" +#include "build/debug.h" +#include "common/axis.h" +#include "common/maths.h" +#include "drivers/time.h" +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "flight/chirp.h" +#include "flight/chirp_flight.h" +#include "flight/imu.h" +#include "flight/mixer_profile.h" +#include "blackbox/blackbox.h" +#include "programming/logic_condition.h" +#include "rx/rx.h" +#include "sensors/gyro.h" + +uint32_t armingFlags; +uint32_t stateFlags; +uint32_t flightModeFlags; +uint64_t logicConditionsGlobalFlags; +int32_t debug[DEBUG32_VALUE_COUNT]; +uint8_t debugMode; +int16_t rcCommand[4]; +gyro_t gyro; +attitudeEulerAngles_t attitude; +rxRuntimeConfig_t rxRuntimeConfig; +systemConfig_t systemConfig_System; +mixerProfile_t mixerProfiles_SystemArray[MAX_MIXER_PROFILE_COUNT]; +blackboxConfig_t blackboxConfig_System; +bool isMixerTransitionMixing; + +static timeUs_t now; +static int16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +static bool receiving, validChannels, failsafe, receivingFailsafeData, mspOverride, axisOverride; +static uint32_t features; +static uint8_t profile, mixerProfile; +static float mixRange; +static BlackboxState logState; + +timeUs_t micros(void) { return now; } +bool feature(uint32_t mask) { return features & mask; } +bool failsafeIsActive(void) { return failsafe; } +bool failsafeIsReceivingRxData(void) { return receivingFailsafeData; } +bool rxIsReceivingSignal(void) { return receiving; } +bool rxAreFlightChannelsValid(void) { return validChannels; } +int16_t rxGetChannelValue(unsigned channel) { return channels[channel]; } +bool IS_RC_MODE_ACTIVE(boxId_e id) { return id == BOXMSPRCOVERRIDE && mspOverride; } +bool isFlightAxisAngleOverrideActive(uint8_t) { return axisOverride; } +bool isFlightAxisRateOverrideActive(uint8_t) { return axisOverride; } +float getMotorMixRange(void) { return mixRange; } +BlackboxState getBlackboxState(void) { return logState; } +uint8_t getConfigProfile(void) { return profile; } +uint8_t getConfigMixerProfile(void) { return mixerProfile; } +} + +class ChirpFlightTest : public testing::Test { +protected: + void SetUp() override + { + chirpConfigMutable()->axis = CHIRP_AXIS_OFF; + chirpFlightUpdate(0.001f, true); + now = 0; + armingFlags = ARMED; + stateFlags = MULTIROTOR; + flightModeFlags = ANGLE_MODE; + logicConditionsGlobalFlags = 0; + systemConfig_System = {}; + mixerProfiles_SystemArray[0] = {}; + mixerConfigMutable()->platformType = PLATFORM_MULTIROTOR; + chirpConfigMutable()->axis = CHIRP_AXIS_ROLL; + chirpConfigMutable()->triggerChannel = 5; + chirpConfigMutable()->amplitude = 10; + blackboxConfigMutable()->rate_num = 1; + blackboxConfigMutable()->rate_denom = 1; + rxRuntimeConfig.channelCount = 16; + for (auto &channel : channels) channel = 1500; + channels[4] = 1000; + std::memset(rcCommand, 0, sizeof(rcCommand)); + rcCommand[THROTTLE] = 1500; + gyro = {}; + attitude = {}; + receiving = receivingFailsafeData = validChannels = true; + failsafe = mspOverride = axisOverride = isMixerTransitionMixing = false; + features = profile = mixerProfile = 0; + mixRange = 0; + logState = BLACKBOX_STATE_RUNNING; + debugMode = DEBUG_CHIRP; + } + + void tick(float dt = 0.001f) + { + now += 1000; + chirpFlightUpdate(dt, true); + } + + void start() + { + channels[4] = 1000; + tick(); + ASSERT_EQ(debug[0], CHIRP_READY); + channels[4] = 2000; + for (int i = 0; i < 3333; ++i) tick(); + ASSERT_EQ(debug[0], CHIRP_RUNNING); + ASSERT_NE(chirpApplyRate(0, 0), 0); + } +}; + +TEST_F(ChirpFlightTest, AppliesOnlyToSelectedAxisAndRecordsActualResponse) +{ + start(); + EXPECT_NE(chirpApplyRate(0, 42), 42); + EXPECT_FLOAT_EQ(chirpApplyRate(1, 42), 42); + EXPECT_FLOAT_EQ(chirpApplyRate(2, 42), 42); + chirpLogResponse(0, 12.25f, -3.5f, 99); + EXPECT_EQ(debug[5], 1225); + EXPECT_EQ(debug[6], -350); + EXPECT_EQ(debug[7], 9900); + chirpLogResponse(1, 0, 0, 0); + EXPECT_EQ(debug[5], 1225); +} + +TEST_F(ChirpFlightTest, AllAxesCanBeSelected) +{ + for (int axis = 1; axis <= 3; ++axis) { + SetUp(); + chirpConfigMutable()->axis = axis; + channels[4] = 1000; + tick(); + channels[4] = 2000; + for (int i = 0; i < 3333; ++i) tick(); + ASSERT_EQ(debug[0], CHIRP_RUNNING); + for (int target = 0; target < 3; ++target) { + if (target + 1 == axis) EXPECT_NE(chirpApplyRate(target, 0), 0); + else EXPECT_FLOAT_EQ(chirpApplyRate(target, 0), 0); + } + } +} + +TEST_F(ChirpFlightTest, RealFlightGuardsStopOutputAndLatchUntilNewTrigger) +{ + const std::function failures[] = { + [] { armingFlags = 0; }, + [] { stateFlags = AIRPLANE; }, + [] { stateFlags |= LANDING_DETECTED; }, + [] { mixerConfigMutable()->platformType = PLATFORM_TRICOPTER; }, + [] { isMixerTransitionMixing = true; }, + [] { features = FEATURE_REVERSIBLE_MOTORS; }, + [] { flightModeFlags = 0; }, + [] { flightModeFlags |= NAV_RTH_MODE; }, + [] { flightModeFlags |= NAV_POSHOLD_MODE; }, + [] { flightModeFlags |= NAV_ALTHOLD_MODE; }, + [] { failsafe = true; }, + [] { receiving = false; }, + [] { validChannels = false; }, + [] { receivingFailsafeData = false; }, + [] { mspOverride = true; }, + [] { channels[0] = 1600; }, + [] { channels[2] = 1400; }, + [] { rcCommand[0] = 100; }, + [] { rcCommand[THROTTLE] = 1200; }, + [] { rcCommand[THROTTLE] = 1650; }, + [] { axisOverride = true; }, + [] { logicConditionsGlobalFlags = LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE; }, + [] { logicConditionsGlobalFlags = LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL; }, + [] { attitude.values.pitch = 201; }, + [] { attitude.values.roll = -201; }, + [] { gyro.gyroADCf[0] = 201; }, + [] { gyro.gyroADCf[1] = std::numeric_limits::quiet_NaN(); }, + [] { mixRange = 0.91f; }, + [] { mixRange = std::numeric_limits::quiet_NaN(); }, + [] { logState = BLACKBOX_STATE_PAUSED; }, + [] { blackboxConfigMutable()->rate_denom = 2; }, + [] { blackboxConfigMutable()->rate_num = 0; }, + [] { blackboxConfigMutable()->rate_denom = 0; }, + [] { profile = 1; }, + [] { mixerProfile = 1; }, + [] { chirpConfigMutable()->axis = CHIRP_AXIS_YAW; }, + [] { chirpConfigMutable()->amplitude = 20; }, + [] { chirpConfigMutable()->triggerChannel = 6; }, + [] { chirpConfigMutable()->triggerChannel = 255; }, + [] { chirpConfigMutable()->axis = 255; }, + }; + int index = 0; + for (const auto &fail : failures) { + SCOPED_TRACE(index++); + SetUp(); + start(); + fail(); + tick(); + ASSERT_EQ(debug[0], CHIRP_ABORTED); + ASSERT_NE(debug[4], 0); + for (int axis = 0; axis < 3; ++axis) EXPECT_FLOAT_EQ(chirpApplyRate(axis, 42), 42); + } +} + +TEST_F(ChirpFlightTest, ReceiverRecoveryDoesNotRestartWhileSwitchHigh) +{ + start(); + receiving = false; + tick(); + receiving = true; + for (int i = 0; i < 3000; ++i) tick(); + EXPECT_EQ(debug[0], CHIRP_ABORTED); + EXPECT_FLOAT_EQ(chirpApplyRate(0, 0), 0); + start(); +} + +TEST_F(ChirpFlightTest, MissingDebugLoggingAndInvalidLoopTimesStopOutput) +{ + start(); + debugMode = DEBUG_NONE; + tick(); + EXPECT_FLOAT_EQ(chirpApplyRate(0, 0), 0); + for (float dt : {0.0f, -0.001f, 0.002f, std::numeric_limits::quiet_NaN()}) { + SetUp(); + start(); + tick(dt); + EXPECT_EQ(debug[0], CHIRP_ABORTED); + EXPECT_FLOAT_EQ(chirpApplyRate(0, 0), 0); + } +} + +TEST_F(ChirpFlightTest, InvalidChannelsAndMspOverlayChannelsCannotTrigger) +{ + for (int channel : {0, 4, 13, 34, 255}) { + SetUp(); + chirpConfigMutable()->triggerChannel = channel; + tick(); + EXPECT_EQ(debug[0], CHIRP_ABORTED); + EXPECT_EQ(debug[4] & CHIRP_INHIBIT_CONFIG, CHIRP_INHIBIT_CONFIG); + } +} + +TEST_F(ChirpFlightTest, UnreadyOrWrongControllerCannotExcite) +{ + start(); + chirpFlightUpdate(0.001f, false); + EXPECT_EQ(debug[0], CHIRP_ABORTED); + EXPECT_FLOAT_EQ(chirpApplyRate(0, 0), 0); +} diff --git a/src/test/unit/chirp_unittest.cc b/src/test/unit/chirp_unittest.cc new file mode 100644 index 00000000000..1deebadcddf --- /dev/null +++ b/src/test/unit/chirp_unittest.cc @@ -0,0 +1,169 @@ +/* SPDX-License-Identifier: GPL-3.0-or-later */ + +#include +#include +#include +#include "gtest/gtest.h" + +extern "C" { +#include "flight/chirp.h" +} + +class ChirpTest : public testing::Test { +protected: + chirpState_t state = {}; + uint32_t now = 1000; + + void update(bool low, bool high, uint16_t inhibit = 0, float amplitude = 10) + { + chirpUpdate(&state, now, true, low, high, inhibit, amplitude); + now += 1000; + } + + void start() + { + update(true, false); + update(false, true); + for (unsigned i = 0; i < CHIRP_SETTLE_US / 1000; ++i) { + update(false, true); + } + ASSERT_EQ(state.phase, CHIRP_RUNNING); + } +}; + +TEST_F(ChirpTest, HighAtBootCannotStart) +{ + for (int i = 0; i < 3000; ++i) { + update(false, true); + EXPECT_FLOAT_EQ(state.output, 0); + } + EXPECT_EQ(state.phase, CHIRP_ABORTED); + EXPECT_EQ(state.inhibit, CHIRP_INHIBIT_SWITCH); + start(); +} + +TEST_F(ChirpTest, DisabledIgnoresTriggerAndClearsOutput) +{ + start(); + chirpUpdate(&state, now, false, false, true, 0, 10); + EXPECT_EQ(state.phase, CHIRP_IDLE); + EXPECT_FLOAT_EQ(state.output, 0); + update(false, true); + EXPECT_EQ(state.phase, CHIRP_ABORTED); +} + +TEST_F(ChirpTest, SweepIsBoundedFiniteMonotonicAndOneShot) +{ + start(); + float previousFrequency = 0; + int zeroCrossings = 0; + float previousOutput = 0; + for (unsigned i = 0; i < CHIRP_DURATION_US / 1000; ++i) { + update(false, true); + ASSERT_TRUE(std::isfinite(state.output)); + EXPECT_LE(std::fabs(state.output), 10); + EXPECT_GE(state.frequency, previousFrequency); + EXPECT_LE(state.frequency, CHIRP_END_HZ); + if (state.output > 0 && previousOutput <= 0) { + ++zeroCrossings; + } + previousFrequency = state.frequency; + previousOutput = state.output; + } + EXPECT_EQ(state.phase, CHIRP_DONE); + EXPECT_FLOAT_EQ(state.output, 0); + // Integral of 2 * exp(log(30) * t / 20) over 20 seconds is ~341 cycles. + EXPECT_NEAR(zeroCrossings, 341, 2); + for (int i = 0; i < 30000; ++i) { + update(false, true); + ASSERT_EQ(state.phase, CHIRP_DONE); + ASSERT_FLOAT_EQ(state.output, 0); + } + start(); +} + +TEST_F(ChirpTest, EveryInhibitStopsExcitationAndRequiresNewLow) +{ + for (unsigned mask = 1; mask <= CHIRP_INHIBIT_SWITCH; mask <<= 1) { + start(); + for (int i = 0; i < 1234; ++i) update(false, true); + update(false, true, mask); + ASSERT_EQ(state.phase, CHIRP_ABORTED); + ASSERT_EQ(state.inhibit, mask); + ASSERT_FLOAT_EQ(state.output, 0); + for (int i = 0; i < 3000; ++i) { + update(false, true); + ASSERT_EQ(state.phase, CHIRP_ABORTED); + ASSERT_FLOAT_EQ(state.output, 0); + } + } +} + +TEST_F(ChirpTest, InvalidConditionsWhileSwitchLowDoNotArmTheTest) +{ + update(true, false, CHIRP_INHIBIT_FLIGHT); + update(false, true); + EXPECT_EQ(state.phase, CHIRP_ABORTED); +} + +TEST_F(ChirpTest, SwitchLowAndMiddleStopImmediately) +{ + start(); + update(true, false); + EXPECT_EQ(state.phase, CHIRP_READY); + EXPECT_FLOAT_EQ(state.output, 0); + start(); + update(false, false); + EXPECT_EQ(state.phase, CHIRP_ABORTED); + EXPECT_FLOAT_EQ(state.output, 0); +} + +TEST_F(ChirpTest, TimingGapAbortsDuringSettlingAndDuringSweep) +{ + update(true, false); + update(false, true); + now += CHIRP_MAX_INTERVAL_US; + update(false, true); + EXPECT_EQ(state.inhibit, CHIRP_INHIBIT_TIMING); + EXPECT_EQ(state.phase, CHIRP_ABORTED); + start(); + now += CHIRP_MAX_INTERVAL_US; + update(false, true); + EXPECT_EQ(state.phase, CHIRP_ABORTED); + EXPECT_FLOAT_EQ(state.output, 0); +} + +TEST_F(ChirpTest, MicrosecondWrapDoesNotInterruptSweep) +{ + now = UINT32_MAX - CHIRP_SETTLE_US - 5000; + start(); + for (int i = 0; i < 2000; ++i) update(false, true); + EXPECT_EQ(state.phase, CHIRP_RUNNING); + EXPECT_TRUE(std::isfinite(state.output)); +} + +TEST_F(ChirpTest, InvalidAmplitudeNeverExcites) +{ + for (float amplitude : {0.0f, -1.0f, 31.0f, std::numeric_limits::infinity(), + std::numeric_limits::quiet_NaN()}) { + update(true, false, 0, amplitude); + update(false, true, 0, amplitude); + EXPECT_EQ(state.phase, CHIRP_ABORTED); + EXPECT_EQ(state.inhibit, CHIRP_INHIBIT_CONFIG); + EXPECT_FLOAT_EQ(state.output, 0); + } +} + +TEST_F(ChirpTest, FrequencyTracksElapsedTimeWithJitter) +{ + start(); + const uint32_t begin = state.startedAt; + for (int i = 0; i < 4000; ++i) { + now += (i % 2) ? 250 : 0; + const uint32_t sampleTime = now; + update(false, true); + const double expected = 2 * std::exp(std::log(30.0) * (sampleTime - begin) / 20000000.0); + ASSERT_NEAR(state.frequency, expected, 0.00002); + ASSERT_EQ(state.phase, CHIRP_RUNNING); + } +}