dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

StickAccelerationXY: factor in stricter tilt limit on takeoff

This commit is contained in:
Matthias Grob 2021-04-13 17:16:58 +02:00
parent 29e07b1e52
commit 4865d027f9
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
View File

@ -72,7 +72,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get());
acceleration_scale *= 2.f; // because of drag the average aceleration is half
acceleration_scale *= 2.f; // because of drag the average acceleration is half
// Map stick input to acceleration
Sticks::limitStickUnitLengthXY(stick_xy);
@ -144,9 +144,13 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo
void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
{
// fetch the tilt limit which is lower than the maximum during takeoff
takeoff_status_s takeoff_status{};
_takeoff_status_sub.copy(&takeoff_status);
// Check if acceleration would exceed the tilt limit
const float acc = acceleration.length();
const float acc_tilt_max = tanf(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()) * CONSTANTS_ONE_G;
const float acc_tilt_max = tanf(takeoff_status.tilt_limit) * CONSTANTS_ONE_G;
if (acc > acc_tilt_max) {
acceleration *= acc_tilt_max / acc;

5
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
View File

@ -40,9 +40,10 @@
#pragma once
#include <px4_platform_common/module_params.h>
#include <float.h> // TODO add this include to AlphaFilter since it's used there
#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
#include <matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/takeoff_status.h>
#include "SlewRate.hpp"
@ -66,6 +67,8 @@ private:
void applyTiltLimit(matrix::Vector2f &acceleration);
void lockPosition(const matrix::Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt);
uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
SlewRate<float> _acceleration_slew_rate_x;
SlewRate<float> _acceleration_slew_rate_y;
AlphaFilter<float> _brake_boost_filter;