Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

vtol_att_control: use VT_MOT_ID instead of VT_MOT_COUNT

Browse Source 
- VT_MOT_COUNT assumed that motors were always the first outputs. This
does not have to be true always. VT_MOT_ID allows to specify precisely how
many motors we have for hovering and to which channel they are connected.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2019-06-27 16:25:23 +02:00
committed by Lorenz Meier
parent 2c74216028
commit afd95b14cf
5 changed files with 36 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+3 -11
View File
@@ -65,10 +65,10 @@ VtolAttitudeControl::VtolAttitudeControl()
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
_params.idle_pwm_mc = PWM_DEFAULT_MIN;
_params.vtol_motor_count = 0;
_params.vtol_motor_id = 0;
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
@@ -269,7 +269,7 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
/* vtol motor count */
param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
/* vtol fw permanent stabilization */
param_get(_params_handles.vtol_fw_permanent_stab, &l);
@@ -326,14 +326,6 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.diff_thrust_scale, &v);
_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
// standard vtol always needs to turn all mc motors off when going into fixed wing mode
// normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code
// did not use the interface of the VtolType class to disable motors we will have users flying around with a wrong
// parameter value. Therefore, explicitly set it here such that all motors will be disabled as expected.
if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
_params.fw_motors_off = 12345678;
}
// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
src/modules/vtol_att_control/vtol_att_control_main.h
+1 -1
View File
@@ -175,7 +175,7 @@ private:
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t vtol_motor_id;
param_t vtol_fw_permanent_stab;
param_t vtol_type;
param_t elevons_mc_lock;
src/modules/vtol_att_control/vtol_att_control_params.c
+11 -11
View File
@@ -39,17 +39,6 @@
* @author Sander Smeets <sander@droneslab.com>
*/
/**
* VTOL number of engines
*
* @min 0
* @max 8
* @increment 1
* @decimal 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_MOT_COUNT, 0);
/**
* Idle speed of VTOL when in multicopter mode
*
@@ -291,6 +280,17 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
*/
PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
/**
* The channel number of motors which provide lift during hover.
*
* @min 0
* @max 12345678
* @increment 1
* @decimal 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_MOT_ID, 0);
/**
* Differential thrust in forwards flight.
*
src/modules/vtol_att_control/vtol_type.cpp
+15 -15
View File
@@ -46,6 +46,7 @@
#include <px4_defines.h>
#include <matrix/math.hpp>
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(mode::ROTARY_WING)
@@ -248,8 +249,8 @@ bool VtolType::set_idle_mc()
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, _params->vtol_motor_id)) {
pwm_values.values[i] = pwm_value;
} else {
@@ -268,8 +269,8 @@ bool VtolType::set_idle_fw()
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, _params->vtol_motor_id)) {
pwm_values.values[i] = PWM_MOTOR_OFF;
} else {
@@ -315,10 +316,10 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
motor_state VtolType::set_motor_state(const motor_state current_state, const motor_state next_state, const int value)
{
struct pwm_output_values pwm_values = {};
pwm_values.channel_count = _params->vtol_motor_count;
pwm_values.channel_count = num_outputs_max;
// per default all motors are running
for (int i = 0; i < _params->vtol_motor_count; i++) {
for (int i = 0; i < num_outputs_max; i++) {
pwm_values.values[i] = _max_mc_pwm_values.values[i];
}
@@ -327,8 +328,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
break;
case motor_state::DISABLED:
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, _params->fw_motors_off)) {
pwm_values.values[i] = _disarmed_pwm_values.values[i];
}
}
@@ -337,8 +338,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
case motor_state::IDLE:
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, _params->vtol_motor_id)) {
pwm_values.values[i] = _params->idle_pwm_mc;
}
}
@@ -346,8 +347,8 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
break;
case motor_state::VALUE:
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, _params->fw_motors_off)) {
pwm_values.values[i] = value;
}
}
@@ -363,14 +364,13 @@ motor_state VtolType::set_motor_state(const motor_state current_state, const mot
}
}
bool VtolType::is_motor_off_channel(const int channel)
bool VtolType::is_channel_set(const int channel, const int target)
{
int channel_bitmap = 0;
int tmp;
int channels = _params->fw_motors_off;
int channels = target;
static constexpr int num_outputs_max = 8;
for (int i = 0; i < num_outputs_max; ++i) {
tmp = channels % 10;
src/modules/vtol_att_control/vtol_type.h
+6 -3
View File
@@ -49,7 +49,7 @@
struct Params {
int32_t idle_pwm_mc; // pwm value for idle in mc mode
int32_t vtol_motor_count; // number of motors
int32_t vtol_motor_id;
int32_t vtol_type;
bool elevons_mc_lock; // lock elevons in multicopter mode
float fw_min_alt; // minimum relative altitude for FW mode (QuadChute)
@@ -173,6 +173,8 @@ protected:
VtolAttitudeControl *_attc;
mode _vtol_mode;
static constexpr const int num_outputs_max = 8;
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
@@ -266,13 +268,14 @@ private:
bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type);
/**
* @brief Determines if this channel is one selected by VT_FW_MOT_OFFID
* @brief Determines if channel is set in target.
*
* @param[in] channel The channel
* @param[in] target The target to check on.
*
* @return True if motor off channel, False otherwise.
*/
bool is_motor_off_channel(const int channel);
bool is_channel_set(const int channel, const int target);
};