mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Copter: call renamed functions in AC_AttitudeControl
This commit is contained in:
parent
e8345fbaab
commit
709fcf37cc
@ -296,7 +296,7 @@ void NOINLINE Copter::send_location(mavlink_channel_t chan)
|
||||
|
||||
void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
|
||||
{
|
||||
const Vector3f &targets = attitude_control.angle_ef_targets();
|
||||
const Vector3f &targets = attitude_control.get_att_target_euler_cd();
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
targets.x / 1.0e2f,
|
||||
|
@ -354,7 +354,7 @@ void Copter::Log_Write_Performance()
|
||||
// Write an attitude packet
|
||||
void Copter::Log_Write_Attitude()
|
||||
{
|
||||
Vector3f targets = attitude_control.angle_ef_targets();
|
||||
Vector3f targets = attitude_control.get_att_target_euler_cd();
|
||||
targets.z = wrap_360_cd_float(targets.z);
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
|
||||
|
@ -125,7 +125,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
|
||||
}
|
||||
|
||||
// convert earth-frame level rates to body-frame level rates
|
||||
attitude_control.euler_rate_to_ang_vel(attitude_control.angle_ef_targets()*radians(0.01f), rate_ef_level, rate_bf_level);
|
||||
attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
|
||||
|
||||
// combine earth frame rate corrections with rate requests
|
||||
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
|
||||
|
@ -569,7 +569,7 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
|
||||
void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
|
||||
{
|
||||
// get current yaw target
|
||||
int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
|
||||
int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;
|
||||
|
||||
// get final angle, 1 = Relative, 0 = Absolute
|
||||
if (relative_angle == 0) {
|
||||
|
@ -33,7 +33,7 @@ void Copter::crash_check()
|
||||
}
|
||||
|
||||
// check for angle error over 30 degrees
|
||||
const Vector3f angle_error = attitude_control.angle_bf_error();
|
||||
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
|
||||
if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
|
||||
crash_counter = 0;
|
||||
return;
|
||||
@ -99,7 +99,7 @@ void Copter::parachute_check()
|
||||
}
|
||||
|
||||
// check for angle error over 30 degrees
|
||||
const Vector3f angle_error = attitude_control.angle_bf_error();
|
||||
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
|
||||
if (pythagorous2(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
|
||||
control_loss_count = 0;
|
||||
return;
|
||||
|
@ -131,11 +131,11 @@ void Copter::update_throttle_thr_mix()
|
||||
// autopilot controlled throttle
|
||||
|
||||
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
|
||||
const Vector3f angle_target = attitude_control.angle_ef_targets();
|
||||
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
|
||||
bool large_angle_request = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
|
||||
|
||||
// check for large external disturbance - angle error over 30 degrees
|
||||
const Vector3f angle_error = attitude_control.angle_bf_error();
|
||||
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
|
||||
bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
|
||||
|
||||
// check for large acceleration - falling or high turbulence
|
||||
|
Loading…
Reference in New Issue
Block a user