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

add FF to FW rate controllers

This commit is contained in:
bresch 2021-10-01 17:24:26 +02:00 committed by Mathieu Bresciani
parent 55f0860c31
commit 6af0856558
8 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_att_control/ecl_controller.h
View File

@ -80,7 +80,7 @@ public:
virtual ~ECL_Controller() = default;
virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) = 0;
virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) = 0;
virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0;
/* Setters */

4
src/modules/fw_att_control/ecl_pitch_controller.cpp
View File

@ -111,11 +111,11 @@ float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlDat
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);

2
src/modules/fw_att_control/ecl_pitch_controller.h
View File

@ -61,7 +61,7 @@ public:
~ECL_PitchController() = default;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
/* Additional Setters */

4
src/modules/fw_att_control/ecl_roll_controller.cpp
View File

@ -108,10 +108,10 @@ float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);

2
src/modules/fw_att_control/ecl_roll_controller.h
View File

@ -59,7 +59,7 @@ public:
~ECL_RollController() = default;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
};

2
src/modules/fw_att_control/ecl_wheel_controller.h
View File

@ -62,7 +62,7 @@ public:
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; }
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override { (void)ctl_data; return 0; }
};
#endif // ECL_HEADING_CONTROLLER_H

4
src/modules/fw_att_control/ecl_yaw_controller.cpp
View File

@ -142,11 +142,11 @@ float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff;
set_bodyrate_setpoint(_bodyrate_setpoint);

2
src/modules/fw_att_control/ecl_yaw_controller.h
View File

@ -59,7 +59,7 @@ public:
~ECL_YawController() = default;
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
protected: