From 5ec479579f08ba0b0143a0ea8686a57a3ed77299 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 14 Feb 2023 14:11:41 +0000 Subject: [PATCH] Plane: move guided roll, pitch and throttle overrides upto guided mode --- ArduPlane/Attitude.cpp | 55 --------------------------------- ArduPlane/mode_guided.cpp | 64 +++++++++++++++++++++++++++++++++++++-- 2 files changed, 61 insertions(+), 58 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0b51098b43..751ef332da 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -453,14 +453,6 @@ void Plane::calc_throttle() } float commanded_throttle = TECS_controller.get_throttle_demand(); - - // Received an external msg that guides throttle in the last 3 seconds? - if (control_mode->is_guided_mode() && - plane.guided_state.last_forced_throttle_ms > 0 && - millis() - plane.guided_state.last_forced_throttle_ms < 3000) { - commanded_throttle = plane.guided_state.forced_throttle; - } - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } @@ -587,17 +579,7 @@ void Plane::calc_nav_yaw_ground(void) */ void Plane::calc_nav_pitch() { - // Calculate the Pitch of the plane - // -------------------------------- int32_t commanded_pitch = TECS_controller.get_pitch_demand(); - - // Received an external msg that guides roll in the last 3 seconds? - if (control_mode->is_guided_mode() && - plane.guided_state.last_forced_rpy_ms.y > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { - commanded_pitch = plane.guided_state.forced_rpy_cd.y; - } - nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } @@ -608,43 +590,6 @@ void Plane::calc_nav_pitch() void Plane::calc_nav_roll() { int32_t commanded_roll = nav_controller->nav_roll_cd(); - - // Received an external msg that guides roll in the last 3 seconds? - if (control_mode->is_guided_mode() && - plane.guided_state.last_forced_rpy_ms.x > 0 && - millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { - commanded_roll = plane.guided_state.forced_rpy_cd.x; -#if OFFBOARD_GUIDED == ENABLED - // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) - } else if ((control_mode == &mode_guided) && (guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { - uint32_t tnow = AP_HAL::millis(); - float delta = (tnow - guided_state.target_heading_time_ms) * 1e-3f; - guided_state.target_heading_time_ms = tnow; - - float error = 0.0f; - if (guided_state.target_heading_type == GUIDED_HEADING_HEADING) { - error = wrap_PI(guided_state.target_heading - AP::ahrs().yaw); - } else { - Vector2f groundspeed = AP::ahrs().groundspeed_vector(); - error = wrap_PI(guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); - } - - float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; - - g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.? - - float i = g2.guidedHeading.get_i(); // get integrator TODO - if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) { - i = g2.guidedHeading.get_i(); - } - - float desired = g2.guidedHeading.get_p() + i + g2.guidedHeading.get_d(); - guided_state.target_heading_limit_low = (desired <= -bank_limit); - guided_state.target_heading_limit_high = (desired >= bank_limit); - commanded_roll = constrain_float(desired, -bank_limit, bank_limit); -#endif // OFFBOARD_GUIDED == ENABLED - } - nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd); update_load_factor(); } diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 900b084f8c..2d712c7598 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -35,9 +35,67 @@ void ModeGuided::update() return; } #endif - plane.calc_nav_roll(); - plane.calc_nav_pitch(); - plane.calc_throttle(); + + // Received an external msg that guides roll in the last 3 seconds? + if (plane.guided_state.last_forced_rpy_ms.x > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { + plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd); + plane.update_load_factor(); + +#if OFFBOARD_GUIDED == ENABLED + // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 ) + // This function is used in Guided and AvoidADSB, check for guided + } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) { + uint32_t tnow = AP_HAL::millis(); + float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f; + plane.guided_state.target_heading_time_ms = tnow; + + float error = 0.0f; + if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) { + error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().yaw); + } else { + Vector2f groundspeed = AP::ahrs().groundspeed_vector(); + error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); + } + + float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; + bank_limit = MIN(bank_limit, plane.roll_limit_cd); + + plane.g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.? + + float i = plane.g2.guidedHeading.get_i(); // get integrator TODO + if (((is_negative(error) && !plane.guided_state.target_heading_limit_low) || (is_positive(error) && !plane.guided_state.target_heading_limit_high))) { + i = plane.g2.guidedHeading.get_i(); + } + + float desired = plane.g2.guidedHeading.get_p() + i + plane.g2.guidedHeading.get_d(); + plane.guided_state.target_heading_limit_low = (desired <= -bank_limit); + plane.guided_state.target_heading_limit_high = (desired >= bank_limit); + + plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit); + plane.update_load_factor(); + +#endif // OFFBOARD_GUIDED == ENABLED + } else { + plane.calc_nav_roll(); + } + + if (plane.guided_state.last_forced_rpy_ms.y > 0 && + millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { + plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + } else { + plane.calc_nav_pitch(); + } + + // Received an external msg that guides throttle in the last 3 seconds? + if (plane.aparm.throttle_cruise > 1 && + plane.guided_state.last_forced_throttle_ms > 0 && + millis() - plane.guided_state.last_forced_throttle_ms < 3000) { + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); + } else { + plane.calc_throttle(); + } + } void ModeGuided::navigate()