fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: base setpoint generation on nav_state

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/modules/rover_ackermann/RoverAckermann.cpp
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()

6
src/modules/rover_ackermann/RoverAckermann.hpp
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{};

57
src/modules/rover_differential/RoverDifferential.cpp
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()

6
src/modules/rover_differential/RoverDifferential.hpp
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{};

57
src/modules/rover_mecanum/RoverMecanum.cpp
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()

6
src/modules/rover_mecanum/RoverMecanum.hpp
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{};