Skip to content

Conversation

chfriedrich98
Copy link
Contributor

@chfriedrich98 chfriedrich98 commented Apr 30, 2025

Solved Problem

This PR updates the ackermann module to the following control structure:
rover_graphics-rover_refactor_5

This is done to prepare the rover modules (and the RoverSpecificSetpoints) for access through API and is achieved through the following steps:

  1. Seperate Actuator Control: This streamlines the Input/Output flow of the rover specific uORB messages.
  2. Update Position Control: So far, in auto and manual position mode, position control directly published RoverVelocitySetpoints.
    Position control now always turns a RoverPositionSetpoint into a RoverVelocitySetpoint which further streamlines the Input/Output flow of the rover specific uORB messages.
    To achieve this, the RoverPositionSetpoint is extended with some optional fields.
  3. Centralize mode management, resets and checks: All arming, control mode and nav state checks are now done in RoverAckermann. Based on the control mode, it will generate RoverSetpoints and udpate the necessary controllers . Further, sanity checks are now only performed on parameter or control mode changes.
  4. Cleanup and NAN value handling: Align function/parameter names, comments and remove unused variables. Protect the controllers from NAN values in RoverSetpoints.

Alternative

All functions that generate roverSetpoints (manual modes, offboard, auto, ...) could also be moved to RoverAckermann.
This way the controllers would only handle their setpoint and publish the setpoint that is one level lower in the hierarchy.
rover_refactor6

Tests

Tested in SITL
Hardware tests:

Copy link

github-actions bot commented Apr 30, 2025

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -208 byte (-0.01 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
-0.1% -1.70Ki  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  [DEL]    -900  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]    -900  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]    -900  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]    +900  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
-0.0%     -72  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
  [DEL]     -32  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +32  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
-0.0%     -96  [ = ]       0    .debug_frame
-0.1% -27.2Ki  [ = ]       0    .debug_info
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/Subscription.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/SubscriptionInterval.cpp
  -0.0%     -14  [ = ]       0    ../../platforms/common/uORB/uORB.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/uORBDeviceMaster.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/uORBDeviceNode.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/uORBManager.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/adc/ads1115/ads1115_main.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/adc/board_adc/ADC.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/barometer/bmp388/bmp388.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/barometer/ms5611/ms5611.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/camera_capture/camera_capture.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/camera_trigger/camera_trigger.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/differential_pressure/ms4525do/MS4525DO.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/differential_pressure/sdp3x/SDP3X.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
 -100.1% -26.9Ki  [ = ]       0    [477 Others]
-0.1% -2.78Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.0%      -1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  -0.9%     -56  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.0%      +1  [ = ]       0    ../../src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
  [DEL] -1.31Ki  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL] -1.32Ki  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL] -1.31Ki  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW] +1.31Ki  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
  -1.2%     -71  [ = ]       0    msg/topics_sources/uORBTopics.cpp
  +0.4%      +4  [ = ]       0    task/task_cancelpt.c
-0.0%    -455  [ = ]       0    .debug_loc
  +0.0%     +15  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  -0.0%     -30  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  +0.6%     +48  [ = ]       0    ../../src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
  +0.0%     +16  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
  -0.7%    -186  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  -1.9%    -197  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  +0.1%     +75  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  +0.1%     +30  [ = ]       0    ../../src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
  -0.1%    -246  [ = ]       0    [section .debug_loc]
  +0.0%     +20  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%     -47  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.5%      -8  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  [DEL]     -16  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -16  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -16  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +16  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
  +1.5%      +1  [ = ]       0    task/task_cancelpt.c
-0.0%    -318  [ = ]       0    .debug_str
  -0.2%    -107  [ = ]       0    
  -0.3%     -61  [ = ]       0    ../../src/drivers/adc/ads1115/ads1115_main.cpp
  [DEL]     -90  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -70  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -65  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
   +38%     +24  [ = ]       0    msg/topics_sources/rover_position_setpoint.cpp
  [NEW]     +63  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
  -5.3%     -12  [ = ]       0    msg/topics_sources/uORBTopics.cpp
+0.9%      +2  [ = ]       0    .shstrtab
-0.0%    -142  [ = ]       0    .strtab
  -0.3%     -69  [ = ]       0    ../../platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.1%     +32  [ = ]       0    [section .strtab]
  [DEL]     -34  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -37  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +30  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
-0.0%    -128  [ = ]       0    .symtab
  -0.1%     -32  [ = ]       0    ../../platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c
  -5.4%     -48  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +32  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
+0.3%    +208  [ = ]       0    [Unmapped]
-0.0%    -208  -0.0%    -208    .text
  [NEW]     +16  [NEW]     +16    msg/topics_sources/rover_velocity_setpoint.cpp
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.7%      -8  -0.7%      -8    msg/topics_sources/uORBTopics.cpp
  [DEL]     -16  [DEL]     -16    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -16  [DEL]     -16    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [DEL]     -28  [DEL]     -28    msg/topics_sources/differential_velocity_setpoint.cpp
  -0.6%     -32  -0.6%     -32    ../../src/modules/logger/logged_topics.cpp
  -0.0%    -127  -0.0%    -127    [section .text]
-0.1% -32.9Ki  -0.0%    -208    TOTAL

px4_fmu-v6x [Total VM Diff: -232 byte (-0.01 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
-0.1% -1.70Ki  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  [DEL]    -900  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]    -900  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]    -900  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]    +900  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
-0.0%     -72  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
  [DEL]     -32  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +32  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
-0.0%     -96  [ = ]       0    .debug_frame
-0.1% -26.9Ki  [ = ]       0    .debug_info
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/Subscription.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/SubscriptionInterval.cpp
  -0.0%     -14  [ = ]       0    ../../platforms/common/uORB/uORB.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/uORBDeviceMaster.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/uORBDeviceNode.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/common/uORB/uORBManager.cpp
  -0.1%     -14  [ = ]       0    ../../platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/adc/ads1115/ads1115_main.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/adc/board_adc/ADC.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/barometer/bmp388/bmp388.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/barometer/ms5611/ms5611.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/camera_capture/camera_capture.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/camera_trigger/camera_trigger.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/differential_pressure/ms4525do/MS4525DO.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/differential_pressure/sdp3x/SDP3X.cpp
  -0.1%     -14  [ = ]       0    ../../src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp
  -0.0%     -14  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
 -100.2% -26.6Ki  [ = ]       0    [461 Others]
-0.1% -2.79Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.0%      +1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  -0.9%     -56  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  [DEL] -1.31Ki  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL] -1.32Ki  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL] -1.31Ki  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW] +1.31Ki  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
  -1.2%     -71  [ = ]       0    msg/topics_sources/uORBTopics.cpp
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
-0.0%    -299  [ = ]       0    .debug_loc
  -0.0%     -29  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  -0.0%     -13  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  +0.1%     +16  [ = ]       0    ../../src/lib/mixer_module/mixer_module.cpp
  +0.1%     +16  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  -0.6%     -48  [ = ]       0    ../../src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
  +0.6%    +180  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  +0.0%     +16  [ = ]       0    ../../src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
  -0.0%     -16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.9%    -197  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  -0.1%    -238  [ = ]       0    [section .debug_loc]
  +0.0%     +29  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.0%     -15  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
-0.0%     -51  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.5%      -8  [ = ]       0    ../../src/modules/logger/logged_topics.cpp
  [DEL]     -16  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -16  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -16  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +16  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
  -4.5%      -3  [ = ]       0    task/task_cancelpt.c
-0.0%    -318  [ = ]       0    .debug_str
  -0.2%    -107  [ = ]       0    
  -0.3%     -61  [ = ]       0    ../../src/drivers/adc/ads1115/ads1115_main.cpp
  [DEL]     -90  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -70  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -65  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
   +38%     +24  [ = ]       0    msg/topics_sources/rover_position_setpoint.cpp
  [NEW]     +63  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
  -5.3%     -12  [ = ]       0    msg/topics_sources/uORBTopics.cpp
+0.9%      +2  [ = ]       0    .shstrtab
-0.0%    -142  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -37  [ = ]       0    [section .strtab]
  [DEL]     -34  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -37  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -32  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +30  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
-0.0%    -128  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.0%     +16  [ = ]       0    [section .symtab]
  [DEL]     -48  [ = ]       0    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -48  [ = ]       0    msg/topics_sources/differential_velocity_setpoint.cpp
  [DEL]     -48  [ = ]       0    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [NEW]     +48  [ = ]       0    msg/topics_sources/rover_velocity_setpoint.cpp
+0.3%    +232  [ = ]       0    [Unmapped]
-0.0%    -232  -0.0%    -232    .text
  [NEW]     +16  [NEW]     +16    msg/topics_sources/rover_velocity_setpoint.cpp
  -0.7%      -8  -0.7%      -8    msg/topics_sources/uORBTopics.cpp
  [DEL]     -16  [DEL]     -16    msg/topics_sources/ackermann_velocity_setpoint.cpp
  [DEL]     -16  [DEL]     -16    msg/topics_sources/mecanum_velocity_setpoint.cpp
  [DEL]     -28  [DEL]     -28    msg/topics_sources/differential_velocity_setpoint.cpp
  -0.6%     -32  -0.6%     -32    ../../src/modules/logger/logged_topics.cpp
  -0.1%    -148  -0.1%    -148    [section .text]
-0.1% -32.5Ki  -0.0%    -232    TOTAL

Updated: 2025-05-12T13:12:48

@chfriedrich98 chfriedrich98 force-pushed the pr-ackermann_task_manager branch 3 times, most recently from 86a8e71 to b1465a3 Compare May 6, 2025 11:54
@chfriedrich98 chfriedrich98 force-pushed the pr-ackermann_task_manager branch 2 times, most recently from bc7c3f8 to bce5b56 Compare May 12, 2025 09:41
@chfriedrich98 chfriedrich98 force-pushed the pr-ackermann_task_manager branch 2 times, most recently from d44786d to e7b8c3e Compare May 12, 2025 12:53
@chfriedrich98 chfriedrich98 force-pushed the pr-ackermann_task_manager branch from e7b8c3e to c4e0a40 Compare May 12, 2025 13:06
@chfriedrich98 chfriedrich98 moved this to 🏗 In Progress in PX4 Rover May 12, 2025
@chfriedrich98
Copy link
Contributor Author

Deprecated by #24836

@github-project-automation github-project-automation bot moved this from 🏗 In Progress to ✅ Done in PX4 Rover May 13, 2025
@chfriedrich98 chfriedrich98 deleted the pr-ackermann_task_manager branch June 16, 2025 10:11
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

Status: ✅ Done

Development

Successfully merging this pull request may close these issues.

1 participant