Gimbal: add pitch stabilization and asymmetrical pitch ranges

[perrrre]

Solved Problem

When using an asymmetrical angular range angle setpoints are wrong. Additionally adding 1-axis gimbal stabilization for pitch.

Solution

• Add another option to stabilize a 1-axis gimbal
• Refactor MNT_RANGE_PITCH to MNT_MAX_PITCH and MNT_MIN_PITCH to account for asymmetrical angular ranges on the pitch axis
• Remove MNT_OFF_ROLL / MNT_OFF_PITCH / MNT_OFF_YAW as PWM_CENT takes care of offsets
• Fixed a bug in simulation where it was possible to continuously subtract vehicle attitude before any setpoint was sent.

Test coverage

• Tested with skynode S and servo motor
  • without offset but symmetrical angular ranges
  • without offset but asymmetrical angular ranges
  • with offset but symmetrical angular ranges
  • with offset but asymmetrical angular ranges
• Simulation: sitl make px4_sitl gz_x500_gimbal, but I had to revert _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); to see that the simulation still works with QGC. This PR changed the gimbal attitude.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 1160 byte (0.06 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +1.13Ki  +0.1% +1.13Ki    .text
  +148%    +236  +148%    +236    gimbal::OutputRC::_stream_device_attitude_status()
  [NEW]    +208  [NEW]    +208    gimbal::OutputRC::anglesMappedToOutput()
  +141%    +192  +141%    +192    AlphaFilter<>::updateCalculation()
  [NEW]    +184  [NEW]    +184    gimbal::OutputBase::set_last_valid_setpoint()
  +0.1%    +164  +0.1%    +164    g_cromfs_image
   +25%    +128   +25%    +128    param_modify_on_import()
   +14%    +104   +14%    +104    gimbal::OutputBase::_calculate_angle_output()
   +32%     +52   +32%     +52    gimbal::InputMavlinkGimbalV2::_stream_gimbal_manager_information()
  +0.0%     +48  +0.0%     +48    [section .text]
  +3.3%     +36  +3.3%     +36    matrix::Matrix<>::operator=()
   +12%     +24   +12%     +24    gimbal::OutputBase::OutputBase()
  +4.5%     +16  +4.5%     +16    gimbal::InputRC::_read_control_data_from_subscription()
  +6.1%      +8  +6.1%      +8    gimbal::OutputBase::_set_angle_setpoints()
 -97.2%      +6 -97.2%      +6    [3 Others]
  +4.8%      +4  +4.8%      +4    FlightTask
  -0.1%      -8  -0.1%      -8    px4::parameters
  -4.7%     -10  -4.7%     -10    update_params()
  -1.2%     -16  -1.2%     -16    gimbal_thread_main()
 -11.1%     -16 -11.1%     -16    px4::wq_configurations::lp_default
  -9.4%     -36  -9.4%     -36    gimbal::OutputMavlinkV1::update()
 -49.4%    -164 -49.4%    -164    gimbal::OutputRC::update()
+0.0%    +587  [ = ]       0    .debug_abbrev
+0.0%     +80  [ = ]       0    .debug_aranges
+0.0%    +240  [ = ]       0    .debug_frame
+0.1% +19.4Ki  [ = ]       0    .debug_info
+0.1% +4.20Ki  [ = ]       0    .debug_line
   +67%      +2  [ = ]       0    [Unmapped]
  +0.1% +4.20Ki  [ = ]       0    [section .debug_line]
+0.2% +6.21Ki  [ = ]       0    .debug_loclists
+0.2%    +962  [ = ]       0    .debug_rnglists
+0.0%    +262  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%    +222  [ = ]       0    .strtab
  +100%     +64  [ = ]       0    AlphaFilter<>::updateCalculation()
   +80%     +16  [ = ]       0    __nxsem_post_veneer
 -40.0%     -16  [ = ]       0    __stm32_dmastart_veneer
  [NEW]     +70  [ = ]       0    gimbal::OutputBase::set_last_valid_setpoint()
  [NEW]     +44  [ = ]       0    gimbal::OutputRC::anglesMappedToOutput()
  +7.5%     +44  [ = ]       0    matrix::Matrix<>::operator=()
+0.0%    +192  [ = ]       0    .symtab
  +100%     +32  [ = ]       0    AlphaFilter<>::updateCalculation()
 -50.0%     -16  [ = ]       0    ParamAutosave::enable()
   +25%     +16  [ = ]       0    Tailsitter::Tailsitter()
  +0.6%     +64  [ = ]       0    [section .symtab]
   +67%     +32  [ = ]       0    __nxsem_post_veneer
 -40.0%     -32  [ = ]       0    __stm32_dmastart_veneer
  [NEW]     +48  [ = ]       0    gimbal::OutputBase::set_last_valid_setpoint()
 -33.3%     -16  [ = ]       0    gimbal::OutputRC
   +50%     +16  [ = ]       0    gimbal::OutputRC::_stream_device_attitude_status()
  [NEW]     +48  [ = ]       0    gimbal::OutputRC::anglesMappedToOutput()
 -66.7%     -32  [ = ]       0    gimbal::OutputRC::update()
  +3.6%     +64  [ = ]       0    matrix::Matrix<>::operator=()
 -16.7%     -16  [ = ]       0    matrix::Vector<>::unit()
 -50.0%     -16  [ = ]       0    perf_reset
   +33%     +16  [ = ]       0    px4::atomic<>::store()
 -33.3%     -16  [ = ]       0    update_params()
 +36% +2.87Ki  [ = ]       0    [Unmapped]
+0.1% +36.3Ki  +0.1% +1.13Ki    TOTAL

px4_fmu-v6x [Total VM Diff: 1152 byte (0.06 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +1.12Ki  +0.1% +1.12Ki    .text
  +148%    +236  +148%    +236    gimbal::OutputRC::_stream_device_attitude_status()
  [NEW]    +208  [NEW]    +208    gimbal::OutputRC::anglesMappedToOutput()
  +141%    +192  +141%    +192    AlphaFilter<>::updateCalculation()
  [NEW]    +184  [NEW]    +184    gimbal::OutputBase::set_last_valid_setpoint()
  +0.1%    +160  +0.1%    +160    g_cromfs_image
   +25%    +128   +25%    +128    param_modify_on_import()
   +14%    +104   +14%    +104    gimbal::OutputBase::_calculate_angle_output()
   +32%     +52   +32%     +52    gimbal::InputMavlinkGimbalV2::_stream_gimbal_manager_information()
  +3.3%     +36  +3.3%     +36    matrix::Matrix<>::operator=()
  +0.0%     +24  +0.0%     +24    [section .text]
   +12%     +24   +12%     +24    gimbal::OutputBase::OutputBase()
  +4.5%     +16  +4.5%     +16    gimbal::InputRC::_read_control_data_from_subscription()
  +6.1%      +8  +6.1%      +8    gimbal::OutputBase::_set_angle_setpoints()
 -92.4%      +6 -92.4%      +6    [3 Others]
  +4.8%      +4  +4.8%      +4    FlightTask
  +4.8%      +4  +4.8%      +4    ParamAutosave::enable()
  -0.1%      -8  -0.1%      -8    px4::parameters
  -4.7%     -10  -4.7%     -10    update_params()
  -1.2%     -16  -1.2%     -16    gimbal_thread_main()
  -9.4%     -36  -9.4%     -36    gimbal::OutputMavlinkV1::update()
 -49.4%    -164 -49.4%    -164    gimbal::OutputRC::update()
+0.0%    +587  [ = ]       0    .debug_abbrev
+0.0%     +80  [ = ]       0    .debug_aranges
+0.0%    +240  [ = ]       0    .debug_frame
+0.1% +19.5Ki  [ = ]       0    .debug_info
+0.1% +4.20Ki  [ = ]       0    .debug_line
   +40%      +2  [ = ]       0    [Unmapped]
  +0.1% +4.20Ki  [ = ]       0    [section .debug_line]
+0.2% +6.09Ki  [ = ]       0    .debug_loclists
+0.2%    +962  [ = ]       0    .debug_rnglists
+0.0%    +262  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%    +222  [ = ]       0    .strtab
  +100%     +64  [ = ]       0    AlphaFilter<>::updateCalculation()
  [NEW]     +70  [ = ]       0    gimbal::OutputBase::set_last_valid_setpoint()
  [NEW]     +44  [ = ]       0    gimbal::OutputRC::anglesMappedToOutput()
  +7.5%     +44  [ = ]       0    matrix::Matrix<>::operator=()
+0.0%    +192  [ = ]       0    .symtab
  +100%     +32  [ = ]       0    AlphaFilter<>::updateCalculation()
 -50.0%     -16  [ = ]       0    ParamAutosave::enable()
   +25%     +16  [ = ]       0    Tailsitter::Tailsitter()
  +0.4%     +48  [ = ]       0    [section .symtab]
  [NEW]     +48  [ = ]       0    gimbal::OutputBase::set_last_valid_setpoint()
 -33.3%     -16  [ = ]       0    gimbal::OutputRC
   +50%     +16  [ = ]       0    gimbal::OutputRC::_stream_device_attitude_status()
  [NEW]     +48  [ = ]       0    gimbal::OutputRC::anglesMappedToOutput()
 -66.7%     -32  [ = ]       0    gimbal::OutputRC::update()
  +3.6%     +64  [ = ]       0    matrix::Matrix<>::operator=()
 -16.7%     -16  [ = ]       0    matrix::Vector<>::unit()
   +33%     +16  [ = ]       0    px4::atomic<>::store()
 -33.3%     -16  [ = ]       0    update_params()
-18.8% -1.12Ki  [ = ]       0    [Unmapped]
+0.1% +32.3Ki  +0.1% +1.12Ki    TOTAL

Updated: 2026-01-15T17:43:13

[sfuhrer]

Moving from the "range" to "min/max" makes sense for pitch makes total sense, though I wonder if for roll and yaw you would ever need it? If you think it's highly unlikely that it will ever be needed then I'd prefer to keep "range" for roll and yaw, for simplicity's sake and contain the flash increase.

If the flash is still over-flowing on v6s after a rebase on latest main, I would disable CONFIG_MODULES_TEMPERATURE_COMPENSATION in the v6s default build. Other v6 boards already have it disabled, with modern IMUs it's generally not required anymore.

[perrrre]

Moving from the "range" to "min/max" makes sense for pitch makes total sense, though I wonder if for roll and yaw you would ever need it? If you think it's highly unlikely that it will ever be needed then I'd prefer to keep "range" for roll and yaw, for simplicity's sake and contain the flash increase.

Thanks for the review. Good point, I think roll is very unlikely, but yaw could possibly have a blind zone. There is no actual use case at the moment so I reverted both yaw and roll.

[sfuhrer]

I only have comments for definitions/comments to avoid confusions later on.

[dakejahl]

Simulation: sitl make px4_sitl gz_x500_gimbal, but I had to revert _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); to see that the simulation still works with QGC. This #25754 changed the gimbal attitude.

Is this still the case? Or did you figure this out? Let's get it sorted.

[perrrre]

Simulation: sitl make px4_sitl gz_x500_gimbal, but I had to revert _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); to see that the simulation still works with QGC. This #25754 changed the gimbal attitude.

Is this still the case? Or did you figure this out? Let's get it sorted.

The image clicking works now: ✔️

• clicking up -> pitch up
• clicking down -> pitch down
• click right -> yaw positive
• click left -> yaw negative

Also tested to connect an RC to use as gimbal controls, works to send rate inputs ✔️

Note for future: Reason for 90 deg offset between gimbal_device_attitude_status and gimbal_device_set_attitude is that it should be aligned with the vehicle heading.

listener gimbal_device_attitude_status

TOPIC: gimbal_device_attitude_status
 gimbal_device_attitude_status
    timestamp: 1684064000 (0.060000 seconds ago)
    q: [-0.70711, -0.00000, 0.00000, -0.70711] (Roll: -0.0 deg, Pitch: -0.0 deg, Yaw: 90.0 deg)
    angular_velocity_x: -0.00102
    angular_velocity_y: 0.00017
    angular_velocity_z: 0.00060
    failure_flags: 0
    delta_yaw: 0.00000
    delta_yaw_velocity: 0.00000
    device_flags: 64 (0b100'0000)
    target_system: 0
    target_component: 0
    gimbal_device_id: 0
    received_from_mavlink: False

pxh> listener gimbal_device_set_attitude

TOPIC: gimbal_device_set_attitude
 gimbal_device_set_attitude
    timestamp: 1688008000 (0.012000 seconds ago)
    q: [1.00000, 0.00000, 0.00000, 0.00000] (Roll: 0.0 deg, Pitch: -0.0 deg, Yaw: 0.0 deg)
    angular_velocity_x: nan
    angular_velocity_y: nan
    angular_velocity_z: nan
    flags: 12 (0b1100)
    target_system: 1
    target_component: 1
pxh> 

The report of the angles in QGC is just different, and can surpass 360 deg. @dakejahl Do you know where those angles are calculated?

[julianoes]

See inline comment regarding the enum.

[sfuhrer]

Okay to merge from my side, all my comments are addressed and it was flight tested.
Squash merge or clean up commits.

The requested change (for a clean up) would include an additional feature (separate roll stabilization) and should thus be done in a follow up PR.

[sfuhrer]

@perrrre we need to free up 300 bytes