Skip to content

Commit

Permalink
[offboard] add thrust and torque control mode
Browse files Browse the repository at this point in the history 
New thrust and torque control mode added which replaces the previous
actuator mode,
in this mode the rate controllers are disables, the control allocator
is enabled and the used externally provices
vehicle_thrust_setpoints and vehicle_torque_setpoints.

New direct_actuator mode
In direct_actuator mode the control allocator module does not publish
actuator_motors and actuator_servos messages which must instead be
provided extrernally by the user.

Removed old direct mode.

Signed-off-by: Beniamino Pozzan <[email protected]>
  • Loading branch information
@beniaminopozzan @dagar
beniaminopozzan authored and dagar committed Nov 23, 2023
1 parent 6cc3877 commit 1c8f31f
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 7 deletions.
3 changes: 2 additions & 1 deletion msg/OffboardControlMode.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,4 +7,5 @@ bool velocity
bool acceleration
bool attitude
bool body_rate
bool actuator
bool thrust_and_torque
bool direct_actuator
2 changes: 1 addition & 1 deletion src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)

bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity
|| offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
|| offboard_control_mode.actuator) && data_is_recent;
|| offboard_control_mode.thrust_and_torque || offboard_control_mode.direct_actuator) && data_is_recent;

if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
offboard_available = false;
Expand Down
24 changes: 19 additions & 5 deletions src/modules/commander/ModeUtil/control_mode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,19 +46,20 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
const offboard_control_mode_s &offboard_control_mode,
vehicle_control_mode_s &vehicle_control_mode)
{
vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default

switch (nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
vehicle_control_mode.flag_control_manual_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = stabilization_required(vehicle_type);
vehicle_control_mode.flag_control_attitude_enabled = stabilization_required(vehicle_type);
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_STAB:
vehicle_control_mode.flag_control_manual_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
Expand All @@ -67,6 +68,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_altitude_enabled = true;
vehicle_control_mode.flag_control_climb_rate_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_POSCTL:
Expand All @@ -77,6 +79,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode.flag_control_climb_rate_enabled = true;
vehicle_control_mode.flag_control_position_enabled = true;
vehicle_control_mode.flag_control_velocity_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
Expand All @@ -93,18 +96,21 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode.flag_control_climb_rate_enabled = true;
vehicle_control_mode.flag_control_position_enabled = true;
vehicle_control_mode.flag_control_velocity_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_ACRO:
vehicle_control_mode.flag_control_manual_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_DESCEND:
vehicle_control_mode.flag_control_auto_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_climb_rate_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;
break;

case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
Expand All @@ -121,28 +127,36 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode.flag_control_altitude_enabled = true;
vehicle_control_mode.flag_control_climb_rate_enabled = true;
vehicle_control_mode.flag_control_acceleration_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;

} else if (offboard_control_mode.velocity) {
vehicle_control_mode.flag_control_velocity_enabled = true;
vehicle_control_mode.flag_control_altitude_enabled = true;
vehicle_control_mode.flag_control_climb_rate_enabled = true;
vehicle_control_mode.flag_control_acceleration_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;

} else if (offboard_control_mode.acceleration) {
vehicle_control_mode.flag_control_acceleration_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;

} else if (offboard_control_mode.attitude) {
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_attitude_enabled = true;
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;

} else if (offboard_control_mode.body_rate) {
vehicle_control_mode.flag_control_rates_enabled = true;
vehicle_control_mode.flag_control_allocation_enabled = true;

} else if (offboard_control_mode.thrust_and_torque) {
vehicle_control_mode.flag_control_allocation_enabled = true;
}

break;
Expand Down

0 comments on commit 1c8f31f

Please sign in to comment.