fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 09:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

control_allocator: push VTOL flight phase into ActuatorEffectiveness

Browse Source
This commit is contained in:
Daniel Agar
2021-10-27 13:42:22 -04:00
parent eb362f2f63
commit 43d7d83a4b
8 changed files with 98 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
+1 -33
View File
@@ -44,7 +44,6 @@
#include <ControlAllocation/ControlAllocation.hpp>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_actuator_setpoint.h>
class ActuatorEffectiveness
{
@@ -55,23 +54,6 @@ public:
static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
enum class FlightPhase {
HOVER_FLIGHT = 0,
FORWARD_FLIGHT = 1,
TRANSITION_HF_TO_FF = 2,
TRANSITION_FF_TO_HF = 3
};
/**
* Set the current flight phase
*
* @param Flight phase
*/
virtual void setFlightPhase(const FlightPhase &flight_phase)
{
_flight_phase = flight_phase;
}
/**
* Get the control effectiveness matrix if updated
*
@@ -84,20 +66,7 @@ public:
*
* @return Actuator trims
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
{
return _trim;
}
/**
* Get the current flight phase
*
* @return Flight phase
*/
const FlightPhase &getFlightPhase() const
{
return _flight_phase;
}
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const { return _trim; }
/**
* Get the number of actuators
@@ -106,5 +75,4 @@ public:
protected:
matrix::Vector<float, NUM_ACTUATORS> _trim; ///< Actuator trim
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase
};
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+31 -14
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,15 +41,40 @@
#include "ActuatorEffectivenessStandardVTOL.hpp"
ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
{
setFlightPhase(FlightPhase::HOVER_FLIGHT);
}
bool
ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
// Check if the current flight phase is HOVER or FIXED_WING
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
flight_phase = FlightPhase::HOVER_FLIGHT;
} else {
flight_phase = FlightPhase::FORWARD_FLIGHT;
}
// Special cases for VTOL in transition
if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
if (vehicle_status.in_transition_to_fw) {
flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
} else {
flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
}
}
if (flight_phase != _flight_phase) {
_flight_phase = flight_phase;
_updated = true;
}
}
if (!(_updated || force)) {
return false;
}
@@ -99,11 +124,3 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float,
_updated = false;
return true;
}
void
ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
{
ActuatorEffectiveness::setFlightPhase(flight_phase);
_updated = true;
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
+17 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -43,22 +43,30 @@
#include "ActuatorEffectiveness.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
{
public:
ActuatorEffectivenessStandardVTOL();
ActuatorEffectivenessStandardVTOL() = default;
virtual ~ActuatorEffectivenessStandardVTOL() = default;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
/**
* Set the current flight phase
*
* @param Flight phase
*/
void setFlightPhase(const FlightPhase &flight_phase) override;
int numActuators() const override { return 7; }
protected:
enum class FlightPhase {
HOVER_FLIGHT = 0,
FORWARD_FLIGHT = 1,
TRANSITION_HF_TO_FF = 2,
TRANSITION_FF_TO_HF = 3
};
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
bool _updated{true};
};
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+31 -13
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,14 +41,40 @@
#include "ActuatorEffectivenessTiltrotorVTOL.hpp"
ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
{
setFlightPhase(FlightPhase::HOVER_FLIGHT);
}
bool
ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
bool force)
{
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
// Check if the current flight phase is HOVER or FIXED_WING
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
flight_phase = FlightPhase::HOVER_FLIGHT;
} else {
flight_phase = FlightPhase::FORWARD_FLIGHT;
}
// Special cases for VTOL in transition
if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
if (vehicle_status.in_transition_to_fw) {
flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
} else {
flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
}
}
if (flight_phase != _flight_phase) {
_flight_phase = flight_phase;
_updated = true;
}
}
if (!(_updated || force)) {
return false;
}
@@ -106,11 +132,3 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float,
_updated = false;
return true;
}
void
ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
{
ActuatorEffectiveness::setFlightPhase(flight_phase);
_updated = true;
}
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+17 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -43,22 +43,30 @@
#include "ActuatorEffectiveness.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
{
public:
ActuatorEffectivenessTiltrotorVTOL();
ActuatorEffectivenessTiltrotorVTOL() = default;
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
/**
* Set the current flight phase
*
* @param Flight phase
*/
void setFlightPhase(const FlightPhase &flight_phase) override;
int numActuators() const override { return 10; }
protected:
enum class FlightPhase {
HOVER_FLIGHT = 0,
FORWARD_FLIGHT = 1,
TRANSITION_HF_TO_FF = 2,
TRANSITION_FF_TO_HF = 3
};
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
bool _updated{true};
};
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
+1 -2
View File
@@ -70,7 +70,6 @@
#pragma once
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_actuator_setpoint.h>
class ControlAllocation
{
@@ -81,7 +80,7 @@ public:
static constexpr uint8_t NUM_ACTUATORS = 16;
static constexpr uint8_t NUM_AXES = 6;
typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
enum ControlAxis {
ROLL = 0,
src/modules/control_allocator/ControlAllocator.cpp
-28
View File
@@ -260,34 +260,6 @@ ControlAllocator::Run()
return;
}
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
// Check if the current flight phase is HOVER or FIXED_WING
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
} else {
flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
}
// Special cases for VTOL in transition
if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
if (vehicle_status.in_transition_to_fw) {
flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
} else {
flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
}
}
// Forward to effectiveness source
_actuator_effectiveness->setFlightPhase(flight_phase);
}
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
src/modules/control_allocator/ControlAllocator.hpp
-2
View File
@@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_actuator_setpoint.h>
#include <uORB/topics/vehicle_status.h>
class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::WorkItem
{
@@ -147,7 +146,6 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
matrix::Vector3f _torque_sp;
matrix::Vector3f _thrust_sp;