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

FligtTaskManualAltitude: split functionality such that scale_stick values and the update_setpoints can be overwritten

Browse Source
This commit is contained in:
Dennis Mannhart 2017-12-09 13:36:03 +01:00 committed by Beat Küng
parent ea61af3164
commit b5c19f6483
2 changed files with 68 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
View File

@ -54,37 +54,66 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent,
bool FlightTaskManualAltitude::activate()
{
_pos_sp_z = _position(2);
_yaw_sp = _yaw;
_pos_sp_predicted = _pos_sp_z = _position(2);
_yaw_sp_predicted = _yaw_sp = _yaw;
_yaw_rate_sp = _vel_sp_z = NAN;
return FlightTaskManual::activate();
}
void FlightTaskManualAltitude::scale_sticks()
{
/* map stick to velocity */
const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
_vel_sp_z = vel_max_z * _sticks_expo(2);
_yaw_rate_sp = _sticks(3) * _yaw_rate_scaling.get();
}
void FlightTaskManualAltitude::update_heading_setpoints()
{
if (fabsf(_sticks(3) < FLT_EPSILON)) {
/* want to hold yaw */
_yaw_rate_sp = NAN;
_yaw_sp = _yaw_sp_predicted;
} else {
/* want to change yaw based on yaw-rate */
_yaw_sp = NAN;
_yaw_sp_predicted = _wrap_pi(_yaw_sp_predicted + _yaw_rate_sp * _deltatime);
}
}
void FlightTaskManualAltitude::update_z_setpoints()
{
if (fabsf(_sticks(2)) < FLT_EPSILON) {
/* want to hold altitude */
_vel_sp_z = NAN;
_pos_sp_z = _pos_sp_predicted;
} else {
/* want to change altitude through velocity */
_pos_sp_z = NAN;
_pos_sp_predicted = _position(2) + _vel_sp_z * _deltatime;
}
}
void FlightTaskManualAltitude::update_setpoints()
{
update_heading_setpoints();
update_z_setpoints();
}
bool FlightTaskManualAltitude::update()
{
/* map stick to velocity */
const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
float vel_sp_z = vel_max_z * _sticks_expo(2);
scale_sticks();
update_setpoints();
const float yaw_rate_sp = _sticks(3) * _yaw_rate_scaling;
if(fabsf(_sticks(2)) < FLT_EPSILON) {
_setPositionSetpoint(Vector3f(NAN, NAN, _pos_sp_z));
_setVelocitySetpoint(Vector3f(NAN, NAN, NAN));
} else {
_setPositionSetpoint(Vector3f(NAN, NAN, NAN));
_setVelocitySetpoint(Vector3f(NAN, NAN, vel_sp_z));
_pos_sp_z = _position(2) + vel_sp_z * _deltatime;
}
if(fabsf(_sticks(3) < FLT_EPSILON)) {
_setYawspeedSetpoint(NAN);
_setYawSetpoint(_yaw_sp);
} else {
_setYawspeedSetpoint(yaw_rate_sp);
_setYawSetpoint(NAN);
_yaw_sp = _wrap_pi(_yaw_sp + yaw_rate_sp * _deltatime)
}
_setPositionSetpoint(Vector3f(NAN, NAN, _pos_sp_z));
_setVelocitySetpoint(Vector3f(NAN, NAN, _vel_sp_z));
_setYawSetpoint(_yaw_sp);
_setYawspeedSetpoint(_yaw_rate_sp);
return true;
}

17
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
View File

@ -54,13 +54,24 @@ public:
bool update() override;
protected:
float _vel_sp_z{0.0f}; /**< scaled velocity directly from stick */
float _yaw_rate_sp{0.0f}; /** scaled yaw rate directly from stick */
float _pos_sp_z{0.0f};
float _yaw_sp{0.0f};
private:
control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */
control::BlockParamFloat _yaw_rate_scaling;
control::BlockParamFloat _yaw_rate_scaling; /**< scaling factor from stick to yaw rate */
virtual void update_setpoints(); /**< updates all setpoints */
virtual void scale_sticks(); /**< scales sticks to velocity */
private:
void update_heading_setpoints(); /**< sets yaw or yaw speed */
void update_z_setpoints(); /**< sets position or velocity setpoint */
float _pos_sp_predicted{0.0f}; /**< position setpoint computed in set_z_setpoints */
float _yaw_sp_predicted{0.0f}; /**< yaw setpoint computed in set_heading_setpoints */
};