mirror of https://github.com/ArduPilot/ardupilot
Copter: integrate AttControl method name changes
This commit is contained in:
parent
6880a6db80
commit
3ccd1ad170
|
@ -33,7 +33,7 @@ static void acro_run()
|
|||
pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in);
|
||||
|
||||
// run attitude controller
|
||||
attitude_control.ratebf_rpy(target_roll, target_pitch, target_yaw);
|
||||
attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
||||
|
||||
// output pilot's throttle without angle boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, false);
|
||||
|
@ -85,7 +85,7 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
|
|||
}
|
||||
|
||||
// convert earth-frame level rates to body-frame level rates
|
||||
attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level);
|
||||
attitude_control.frame_conversion_ef_to_bf(rate_ef_level, rate_bf_level);
|
||||
|
||||
// combine earth frame rate corrections with rate requests
|
||||
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
|
||||
|
|
|
@ -56,7 +56,7 @@ static void althold_run()
|
|||
attitude_control.set_throttle_out(0, false);
|
||||
}else{
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
// call throttle controller
|
||||
|
|
|
@ -105,7 +105,7 @@ static void auto_takeoff_run()
|
|||
pos_control.update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}
|
||||
|
||||
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
|
||||
|
@ -154,10 +154,10 @@ static void auto_wp_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -215,7 +215,7 @@ static void auto_land_run()
|
|||
pos_control.update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}
|
||||
|
||||
// auto_rtl_start - initialises RTL in AUTO flight mode
|
||||
|
@ -255,7 +255,7 @@ void auto_circle_run()
|
|||
pos_control.update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rpy(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
||||
}
|
||||
|
||||
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
|
||||
|
|
|
@ -261,7 +261,7 @@ static void autotune_run()
|
|||
|
||||
// if pilot override call attitude controller
|
||||
if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) {
|
||||
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
}else{
|
||||
// somehow get attitude requests from autotuning
|
||||
autotune_attitude_control();
|
||||
|
@ -288,7 +288,7 @@ static void autotune_attitude_control()
|
|||
attitude_control.limit_angle_to_rate_request(true);
|
||||
|
||||
// hold level attitude
|
||||
attitude_control.angleef_rp_rateef_y(0.0f, 0.0f, 0.0f);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(0.0f, 0.0f, 0.0f);
|
||||
|
||||
// hold the copter level for 0.25 seconds before we begin a twitch
|
||||
// reset counter if we are no longer level
|
||||
|
@ -321,16 +321,16 @@ static void autotune_attitude_control()
|
|||
if (autotune_state.axis == AUTOTUNE_AXIS_ROLL) {
|
||||
// request roll to 20deg
|
||||
if (autotune_state.positive_direction) {
|
||||
attitude_control.angleef_rp_rateef_y(AUTOTUNE_TARGET_ANGLE_CD, 0.0f, 0.0f);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(AUTOTUNE_TARGET_ANGLE_CD, 0.0f, 0.0f);
|
||||
}else{
|
||||
attitude_control.angleef_rp_rateef_y(-AUTOTUNE_TARGET_ANGLE_CD, 0.0f, 0.0f);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(-AUTOTUNE_TARGET_ANGLE_CD, 0.0f, 0.0f);
|
||||
}
|
||||
}else{
|
||||
// request pitch to 20deg
|
||||
if (autotune_state.positive_direction) {
|
||||
attitude_control.angleef_rp_rateef_y(0.0f, AUTOTUNE_TARGET_ANGLE_CD, 0.0f);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(0.0f, AUTOTUNE_TARGET_ANGLE_CD, 0.0f);
|
||||
}else{
|
||||
attitude_control.angleef_rp_rateef_y(0.0f, -AUTOTUNE_TARGET_ANGLE_CD, 0.0f);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(0.0f, -AUTOTUNE_TARGET_ANGLE_CD, 0.0f);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
|
|
|
@ -56,9 +56,9 @@ static void circle_run()
|
|||
|
||||
// call attitude controller
|
||||
if (circle_pilot_yaw_override) {
|
||||
attitude_control.angleef_rp_rateef_y(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
attitude_control.angleef_rpy(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
||||
}
|
||||
|
||||
// run altitude controller
|
||||
|
|
|
@ -69,7 +69,7 @@ static void drift_run()
|
|||
}
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
|
||||
// output pilot's throttle with angle boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, true);
|
||||
|
|
|
@ -121,7 +121,7 @@ static void flip_run()
|
|||
|
||||
case Flip_Start:
|
||||
// under 45 degrees request 400deg/sec roll
|
||||
attitude_control.ratebf_rpy(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
|
||||
attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
|
||||
// increase throttle
|
||||
throttle_out += FLIP_THR_INC;
|
||||
// beyond 45deg lean angle move to next stage
|
||||
|
@ -132,7 +132,7 @@ static void flip_run()
|
|||
|
||||
case Flip_Roll:
|
||||
// between 45deg ~ -90deg request 400deg/sec roll
|
||||
attitude_control.ratebf_rpy(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
|
||||
attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROLL_RATE * flip_dir, 0.0, 0.0);
|
||||
// decrease throttle
|
||||
throttle_out -= FLIP_THR_DEC;
|
||||
// beyond -90deg move on to recovery
|
||||
|
@ -143,7 +143,7 @@ static void flip_run()
|
|||
|
||||
case Flip_Recover:
|
||||
// use originally captured earth-frame angle targets to recover
|
||||
attitude_control.angleef_rpy(curr_ef_targets.x, curr_ef_targets.y, curr_ef_targets.z,false);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(curr_ef_targets.x, curr_ef_targets.y, curr_ef_targets.z,false);
|
||||
|
||||
// increase throttle to gain any lost alitude
|
||||
throttle_out += FLIP_THR_INC;
|
||||
|
|
|
@ -83,9 +83,9 @@ static void guided_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -79,7 +79,7 @@ static void land_gps_run()
|
|||
wp_nav.update_loiter();
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
|
||||
|
@ -125,7 +125,7 @@ static void land_nogps_run()
|
|||
}
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
|
||||
|
|
|
@ -67,7 +67,7 @@ static void loiter_run()
|
|||
wp_nav.update_loiter();
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
|
|
|
@ -68,7 +68,7 @@ static void ofloiter_run()
|
|||
target_pitch = get_of_pitch(target_pitch);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
|
||||
// run altitude controller
|
||||
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
|
||||
|
|
|
@ -153,10 +153,10 @@ static void rtl_climb_return_descent_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
|
@ -210,10 +210,10 @@ static void rtl_loiterathome_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
|
@ -266,7 +266,7 @@ static void rtl_land_run()
|
|||
pos_control.update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
rtl_state_complete = ap.land_complete;
|
||||
|
|
|
@ -74,7 +74,7 @@ static void sport_run()
|
|||
attitude_control.set_throttle_out(0, false);
|
||||
}else{
|
||||
// call attitude controller
|
||||
attitude_control.rateef_rpy(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
attitude_control.rate_ef_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
|
||||
// call throttle controller
|
||||
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
|
||||
|
|
|
@ -48,7 +48,7 @@ static void stabilize_run()
|
|||
attitude_control.init_targets();
|
||||
}else{
|
||||
// call attitude controller
|
||||
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue