Skip to content

Commit

Permalink
Update for addition of position controller inputs in sysid mode in 4.6
Browse files Browse the repository at this point in the history
  • Loading branch information
bnsgeyer authored and Hwurzburg committed Jan 19, 2025
1 parent 3caa375 commit a71afc1
Show file tree
Hide file tree
Showing 6 changed files with 24 additions and 3 deletions.
21 changes: 19 additions & 2 deletions copter/source/docs/systemid-mode-operation.rst
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,10 @@ Parameters

Values: 0:None (prevents entry into mode and display of other mode parameters), 1:Input Roll Angle, 2:Input Pitch Angle, 3:Input Yaw Angle, 4:Recovery Roll Angle,
5:Recovery Pitch Angle, 6:Recovery Yaw Angle, 7:Rate Roll, 8:Rate Pitch, 9:Rate Yaw, 10:Mixer Roll,
11:Mixer Pitch, 12:Mixer Yaw, 13:Mixer Thrust
11:Mixer Pitch, 12:Mixer Yaw, 13:Mixer Thrust, 14:Measured Lateral Position, 15:Measured Longitudinal Position,
16:Measured Lateral Velocity, 17:Measured Longitudinal Velocity, 18:Input Lateral Velocity, 19:Input Longitudinal Velocity

:ref:`SID_MAGNITUDE<SID_MAGNITUDE>`: System identification Chirp Magnitude. Depending on the injection point, units will be in either deg, deg/s, or 0-1 for mixer outputs. The magnitude can be changed in flight easily using the :ref:`tuning knob<TUNE>` using the 58 option.
:ref:`SID_MAGNITUDE<SID_MAGNITUDE>`: System identification Chirp Magnitude. Depending on the injection point, units will be in either deg, deg/s, 0-1 for mixer outputs, m/s for velocity, and m for position. The magnitude can be changed in flight easily using the :ref:`tuning knob<TUNE>` using the 58 option.

:ref:`SID_F_START_HZ<SID_F_START_HZ>`: System identification Start Frequency. Range: 0.01-100 Hz

Expand Down Expand Up @@ -113,6 +114,22 @@ The next figure depicts the :ref:`SID_AXIS<SID_AXIS>` injection points in yellow

ArduCopter control loops with ATC_RATE_FF_ENAB=1

Position controller diagram showing the chirp being input at the desired velocity. Both the velocity and position feedback loops are being used. This input would be used to analyze data to determine position or tracking bandwidth.

.. figure:: ../images/Pos_Ctrl_Pos_BW.png

Position controller diagram showing the chirp being input at the desired velocity. Only the velocity feedback loop is being used. This input would be used to analyze data to determine the velocity bandwidth.

.. figure:: ../images/Pos_Ctrl_Vel_BW.png

Position controller diagram showing the chirp being input at the measured position. Both the velocity and position feedback loops are being used. This input would be used to analyze data to determine the position disturbance rejection bandwidth.

.. figure:: ../images/Pos_Ctrl_Pos_DRB.png

Position controller diagram showing the chirp being input at the measured velocity. Only the velocity feedback loop is being used. This input would be used to analyze data to determine the velocity disturbance rejection bandwidth.

.. figure:: ../images/Pos_Ctrl_Vel_DRB.png

Identification of a Multicopter
===============================

Expand Down
6 changes: 5 additions & 1 deletion copter/source/docs/systemid-mode.rst
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,11 @@
System Identification Mode
==========================

This mode is for advanced users and provides a means to generate mathematical models of the vehicles flight behavior for model generation. It places the vehicle into a STABILIZE like mode, and generates bursts of signals ("chirps") injected into the control loops at various points and logs the results for math analysis and model generation later.
This mode is for advanced users and provides a means to characterize the response of a vehicle in the frequency domain. The flight data collected from this mode can be used in several ways:
- generating mathematical models of the vehicles flight behavior for model generation
- investigating and predicting the quality of the vehicle tune
- calculating flying quality metrics
It injects an input signal, called a chirp, at various points in the control loops. The chirp is a constant amplitude oscillation that increases in frequency from a user defined minimum to maximum frequency, generally referred to as a frequency sweep. The injection points are defined by the :ref:`SID_AXIS<SID_AXIS>` parameter and there are points in the Attitude and Position Controller. For any injection points in the Attitude Controller, the user is required to fly the aircraft through an underlying STABILIZE like flight mode. For any injection points in the Position Controller, the user controls are locked out and will not be able to affect the control of the vehicle. The resulting flight data is logged for math analysis and model generation after the flight.


Enabling the Mode
Expand Down
Binary file added copter/source/images/Pos_Ctrl_Pos_BW.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added copter/source/images/Pos_Ctrl_Pos_DRB.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added copter/source/images/Pos_Ctrl_Vel_BW.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added copter/source/images/Pos_Ctrl_Vel_DRB.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.

0 comments on commit a71afc1

Please sign in to comment.