Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

rover: base setpoint generation on nav_state
Build all targets / Scan for Board Targets (push) Has been cancelled
Details
Build all targets / Build Group [${{ matrix.group }}][${{ matrix.arch == 'nuttx' && 'x86' || 'arm64' }}] (push) Has been cancelled
Details
Build all targets / Upload Artifacts to S3 (push) Has been cancelled
Details
Build all targets / Create Release and Upload Artifacts (push) Has been cancelled
Details
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Details
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Details
Checks / build (check_format) (push) Has been cancelled
Details
Checks / build (check_newlines) (push) Has been cancelled
Details
Checks / build (module_documentation) (push) Has been cancelled
Details
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Details
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Details
Checks / build (shellcheck_all) (push) Has been cancelled
Details
Checks / build (tests) (push) Has been cancelled
Details
Checks / build (tests_coverage) (push) Has been cancelled
Details
Checks / build (validate_module_configs) (push) Has been cancelled
Details
Clang Tidy / build (push) Has been cancelled
Details
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
Details
MacOS build / build (px4_sitl) (push) Has been cancelled
Details
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Details
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Details
Container build / Set Tags and Variables (push) Has been cancelled
Details
Container build / Build Container (amd64) (push) Has been cancelled
Details
Container build / Build Container (arm64) (push) Has been cancelled
Details
Container build / Deploy To Registry (push) Has been cancelled
Details
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Details
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
Details
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
Details
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
Details
FLASH usage analysis / Publish Results (push) Has been cancelled
Details
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
Details
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
Details
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
Details
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
Details
MAVROS Mission Tests / build (map[mission:rover_mission_1 vehicle:rover]) (push) Has been cancelled
Details
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Details
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Details
Python CI Checks / build (push) Has been cancelled
Details
ROS Integration Tests / build (push) Has been cancelled
Details
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
Details
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
Details
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
Details
SITL Tests / Testing PX4 iris (push) Has been cancelled
Details
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Details
Handle stale issues and PRs / stale (push) Has been cancelled
Details

Browse Source
This commit is contained in:
chfriedrich98
2025-07-01 13:56:53 +02:00
committed by chfriedrich98
parent 90a4b4798d
commit 9a0b666deb
6 changed files with 108 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/RoverAckermann.cpp
+32 -26
View File
@@ -67,10 +67,7 @@ void RoverAckermann::Run()
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz)
if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled ||
_vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled ||
_vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled ||
_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
if (_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
_vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled ||
_vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled ||
_vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled ||
@@ -82,24 +79,11 @@ void RoverAckermann::Run()
} else {
_vehicle_control_mode = vehicle_control_mode;
}
}
if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
_was_armed = true;
// Generate setpoints
if (_vehicle_control_mode.flag_control_manual_enabled) {
manualControl();
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
_auto_mode.autoControl();
} else if (_vehicle_control_mode.flag_control_offboard_enabled) {
_offboard_mode.offboardControl();
}
generateSetpoints();
updateControllers();
} else if (_was_armed) { // Reset all controllers and stop the vehicle
@@ -110,20 +94,42 @@ void RoverAckermann::Run()
}
void RoverAckermann::manualControl()
void RoverAckermann::generateSetpoints()
{
if (_vehicle_control_mode.flag_control_position_enabled) {
_manual_mode.position();
vehicle_status_s vehicle_status{};
_vehicle_status_sub.update(&vehicle_status);
} else if (_vehicle_control_mode.flag_control_attitude_enabled) {
_manual_mode.stab();
switch (vehicle_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_auto_mode.autoControl();
break;
} else if (_vehicle_control_mode.flag_control_rates_enabled) {
_manual_mode.acro();
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
_offboard_mode.offboardControl();
break;
} else if (_vehicle_control_mode.flag_control_allocation_enabled) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
_manual_mode.manual();
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
_manual_mode.acro();
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
_manual_mode.stab();
break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
_manual_mode.position();
break;
default:
break;
}
}
void RoverAckermann::updateControllers()
src/modules/rover_ackermann/RoverAckermann.hpp
+4 -2
View File
@@ -48,6 +48,7 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
// Local includes
#include "AckermannActControl/AckermannActControl.hpp"
@@ -90,9 +91,9 @@ private:
void Run() override;
/**
* @brief Generate and publish roverSetpoints from manualControlSetpoints.
* @brief Generate rover setpoints from supported PX4 internal modes
*/
void manualControl();
void generateSetpoints();
/**
* @brief Update the active controllers.
@@ -115,6 +116,7 @@ private:
// uORB subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
vehicle_control_mode_s _vehicle_control_mode{};
src/modules/rover_differential/RoverDifferential.cpp
+32 -25
View File
@@ -67,10 +67,7 @@ void RoverDifferential::Run()
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz)
if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled ||
_vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled ||
_vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled ||
_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
if (_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
_vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled ||
_vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled ||
_vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled ||
@@ -82,24 +79,12 @@ void RoverDifferential::Run()
} else {
_vehicle_control_mode = vehicle_control_mode;
}
}
if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
_was_armed = true;
// Generate setpoints
if (_vehicle_control_mode.flag_control_manual_enabled) {
manualControl();
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
_auto_mode.autoControl();
} else if (_vehicle_control_mode.flag_control_offboard_enabled) {
_offboard_mode.offboardControl();
}
generateSetpoints();
updateControllers();
} else if (_was_armed) { // Reset all controllers and stop the vehicle
@@ -110,20 +95,42 @@ void RoverDifferential::Run()
}
void RoverDifferential::manualControl()
void RoverDifferential::generateSetpoints()
{
if (_vehicle_control_mode.flag_control_position_enabled) {
_manual_mode.position();
vehicle_status_s vehicle_status{};
_vehicle_status_sub.update(&vehicle_status);
} else if (_vehicle_control_mode.flag_control_attitude_enabled) {
_manual_mode.stab();
switch (vehicle_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_auto_mode.autoControl();
break;
} else if (_vehicle_control_mode.flag_control_rates_enabled) {
_manual_mode.acro();
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
_offboard_mode.offboardControl();
break;
} else if (_vehicle_control_mode.flag_control_allocation_enabled) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
_manual_mode.manual();
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
_manual_mode.acro();
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
_manual_mode.stab();
break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
_manual_mode.position();
break;
default:
break;
}
}
void RoverDifferential::updateControllers()
src/modules/rover_differential/RoverDifferential.hpp
+4 -2
View File
@@ -47,6 +47,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
// Local includes
#include "DifferentialActControl/DifferentialActControl.hpp"
@@ -89,9 +90,9 @@ private:
void Run() override;
/**
* @brief Handle manual control
* @brief Generate rover setpoints from supported PX4 internal modes
*/
void manualControl();
void generateSetpoints();
/**
* @brief Update the controllers
@@ -114,6 +115,7 @@ private:
// uORB subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
vehicle_control_mode_s _vehicle_control_mode{};
src/modules/rover_mecanum/RoverMecanum.cpp
+32 -25
View File
@@ -67,10 +67,7 @@ void RoverMecanum::Run()
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz)
if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled ||
_vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled ||
_vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled ||
_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
if (_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
_vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled ||
_vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled ||
_vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled ||
@@ -82,24 +79,12 @@ void RoverMecanum::Run()
} else {
_vehicle_control_mode = vehicle_control_mode;
}
}
if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
_was_armed = true;
// Generate setpoints
if (_vehicle_control_mode.flag_control_manual_enabled) {
manualControl();
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
_auto_mode.autoControl();
} else if (_vehicle_control_mode.flag_control_offboard_enabled) {
_offboard_mode.offboardControl();
}
generateSetpoints();
updateControllers();
} else if (_was_armed) { // Reset all controllers and stop the vehicle
@@ -110,20 +95,42 @@ void RoverMecanum::Run()
}
void RoverMecanum::manualControl()
void RoverMecanum::generateSetpoints()
{
if (_vehicle_control_mode.flag_control_position_enabled) {
_manual_mode.position();
vehicle_status_s vehicle_status{};
_vehicle_status_sub.update(&vehicle_status);
} else if (_vehicle_control_mode.flag_control_attitude_enabled) {
_manual_mode.stab();
switch (vehicle_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_auto_mode.autoControl();
break;
} else if (_vehicle_control_mode.flag_control_rates_enabled) {
_manual_mode.acro();
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
_offboard_mode.offboardControl();
break;
} else if (_vehicle_control_mode.flag_control_allocation_enabled) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
_manual_mode.manual();
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
_manual_mode.acro();
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
_manual_mode.stab();
break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
_manual_mode.position();
break;
default:
break;
}
}
void RoverMecanum::updateControllers()
src/modules/rover_mecanum/RoverMecanum.hpp
+4 -2
View File
@@ -47,6 +47,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
// Local includes
#include "MecanumActControl/MecanumActControl.hpp"
@@ -89,9 +90,9 @@ private:
void Run() override;
/**
* @brief Handle manual control
* @brief Generate rover setpoints from supported PX4 internal modes
*/
void manualControl();
void generateSetpoints();
/**
* @brief Update the controllers
@@ -114,6 +115,7 @@ private:
// uORB subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
vehicle_control_mode_s _vehicle_control_mode{};