Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: AC_AttitudeControl_Sub: Helper function to ensure that the vehicle reaches the target orientation with the desired yaw rate.

Browse Source
This commit is contained in:
Rakesh Vivekanandan 2023-07-25 17:52:42 -07:00 committed by Willian Galvani
parent e12ed0fe1c
commit cacbb23c0b
2 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
View File

@ -378,3 +378,33 @@ void AC_AttitudeControl_Sub::parameter_sanity_check()
_thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT);
}
}
// This function ensures that the ROV reaches the target orientation with the desired yaw rate
void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float target_yaw_rate)
{
// Convert from centidegrees on public interface to radians
const float euler_yaw_angle = wrap_PI(radians(euler_yaw_angle_cd * 0.01f));
const float current_yaw = AP::ahrs().get_yaw();
// Compute angle error
const float yaw_error = wrap_PI(euler_yaw_angle - current_yaw);
int direction = 0;
if (yaw_error < 0){
direction = -1;
} else {
direction = 1;
}
target_yaw_rate *= direction;
if (fabsf(yaw_error) > MAX_YAW_ERROR) {
// rotate the rov with desired yaw rate towards the target yaw
input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, target_yaw_rate);
} else {
// holds the rov's angles
input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, true);
}
}

5
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h
View File

@ -23,6 +23,8 @@
#define AC_ATC_SUB_RATE_YAW_IMAX 0.222f
#define AC_ATC_SUB_RATE_YAW_FILT_HZ 5.0f
#define MAX_YAW_ERROR radians(5)
class AC_AttitudeControl_Sub : public AC_AttitudeControl {
public:
AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors);
@ -60,6 +62,9 @@ public:
// sanity check parameters. should be called once before take-off
void parameter_sanity_check() override;
// This function ensures that the ROV reaches the target orientation with the desired yaw rate
void input_euler_angle_roll_pitch_slew_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float slew_yaw);
// user settable parameters
static const struct AP_Param::GroupInfo var_info[];