From 0bcff6cec0ec30e16ce4621fe1db6b4aa0f0516b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 2 Aug 2024 18:31:50 +0900 Subject: [PATCH] Copter: support set_posvelaccel_offset in auto Co-authored-by: Leonard Hall --- ArduCopter/Copter.h | 6 +-- ArduCopter/mode.h | 10 ++--- ArduCopter/mode_auto.cpp | 68 ++++++++++++++++++++------------- ArduCopter/mode_guided.cpp | 20 ++++------ ArduCopter/mode_throw.cpp | 4 +- ArduCopter/mode_zigzag.cpp | 29 ++++---------- ArduCopter/surface_tracking.cpp | 42 ++++++-------------- ArduCopter/takeoff.cpp | 10 ++--- 8 files changed, 83 insertions(+), 106 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3bdba074d5..5718fc11f4 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -266,9 +266,9 @@ private: // measured ground or ceiling level measured using the range finder. void update_surface_offset(); - // get/set target altitude (in cm) above ground - bool get_target_alt_cm(float &target_alt_cm) const; - void set_target_alt_cm(float target_alt_cm); + // target has already been set by terrain following so do not initalise again + // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag) + void external_init(); // get target and actual distances (in m) for logging purposes bool get_target_dist_for_logging(float &target_dist) const; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6e87e9b4fe..68cc168574 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -182,12 +182,6 @@ public: // altitude fence float get_avoidance_adjusted_climbrate(float target_rate); - const Vector3f& get_vel_desired_cms() { - // note that position control isn't used in every mode, so - // this may return bogus data: - return pos_control->get_vel_desired_cms(); - } - // send output to the motors, can be overridden by subclasses virtual void output_to_motors(); @@ -651,6 +645,10 @@ private: bool shift_alt_to_current_alt(Location& target_loc) const; + // subtract position controller offsets from target location + // should be used when the location will be used as a target for the position controller + void subtract_pos_offsets(Location& target_loc) const; + void do_takeoff(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 5166053c89..6858c7a17a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1419,7 +1419,7 @@ void PayloadPlace::run() // vel_threshold_fraction * max velocity const float vel_threshold_fraction = 0.1; const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss(); - bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= descent_start_altitude_cm - stop_distance; + bool reached_altitude = copter.pos_control->get_pos_desired_z_cm() >= descent_start_altitude_cm - stop_distance; if (reached_altitude) { state = State::Done; } @@ -1472,6 +1472,8 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const (wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER)) { int32_t curr_rngfnd_alt_cm; if (copter.get_rangefinder_height_interpolated_cm(curr_rngfnd_alt_cm)) { + // subtract position offset (if any) + curr_rngfnd_alt_cm -= pos_control->get_pos_offset_z_cm(); // wp_nav is using rangefinder so use current rangefinder alt target_loc.set_alt_cm(MAX(curr_rngfnd_alt_cm, 200), Location::AltFrame::ABOVE_TERRAIN); return true; @@ -1486,11 +1488,21 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const return false; } - // set target_loc's alt - target_loc.set_alt_cm(currloc.alt, currloc.get_alt_frame()); + // set target_loc's alt minus position offset (if any) + target_loc.set_alt_cm(currloc.alt - pos_control->get_pos_offset_z_cm(), currloc.get_alt_frame()); return true; } +// subtract position controller offsets from target location +// should be used when the location will be used as a target for the position controller +void ModeAuto::subtract_pos_offsets(Location& target_loc) const +{ + // subtract position controller offsets from target location + const Vector3p& pos_ofs_neu_cm = pos_control->get_pos_offset_cm(); + Vector3p pos_ofs_ned_m = Vector3p{pos_ofs_neu_cm.x * 0.01, pos_ofs_neu_cm.y * 0.01, -pos_ofs_neu_cm.z * 0.01}; + target_loc.offset(-pos_ofs_ned_m); +} + /********************************************************************************/ // Nav (Must) commands /********************************************************************************/ @@ -1531,6 +1543,10 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) { // calculate default location used when lat, lon or alt is zero Location default_loc = copter.current_loc; + + // subtract position offsets + subtract_pos_offsets(default_loc); + if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { if (!wp_nav->get_wp_destination_loc(default_loc)) { // this should never happen @@ -1651,33 +1667,23 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) // note: caller should set yaw_mode void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { - // convert back to location - Location target_loc(cmd.content.location); + // calculate default location used when lat, lon or alt is zero + Location default_loc = copter.current_loc; - // use current location if not provided - if (target_loc.lat == 0 && target_loc.lng == 0) { - // To-Do: make this simpler - Vector3f temp_pos; - copter.wp_nav->get_wp_stopping_point_xy(temp_pos.xy()); - const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN); - target_loc.lat = temp_loc.lat; - target_loc.lng = temp_loc.lng; - } + // subtract position offsets + subtract_pos_offsets(default_loc); - // use current altitude if not provided - // To-Do: use z-axis stopping point instead of current alt - if (target_loc.alt == 0) { - // set to current altitude but in command's alt frame - int32_t curr_alt; - if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { - target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame()); - } else { - // default to current altitude as alt-above-home - target_loc.set_alt_cm(copter.current_loc.alt, - copter.current_loc.get_alt_frame()); + // use previous waypoint destination as default if available + if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { + if (!wp_nav->get_wp_destination_loc(default_loc)) { + // this should never happen + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } + // get waypoint's location from command and send to wp_nav + const Location target_loc = loc_from_cmd(cmd, default_loc); + // start way point navigator and provide it the desired location if (!wp_start(target_loc)) { // failure to set next destination can only be because of missing terrain data @@ -1689,7 +1695,13 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) // do_circle - initiate moving in a circle void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) { - const Location circle_center = loc_from_cmd(cmd, copter.current_loc); + // calculate default location used when lat, lon or alt is zero + Location default_loc = copter.current_loc; + + // subtract position offsets + subtract_pos_offsets(default_loc); + + const Location circle_center = loc_from_cmd(cmd, default_loc); // calculate radius uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 @@ -1757,6 +1769,10 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) { // calculate default location used when lat, lon or alt is zero Location default_loc = copter.current_loc; + + // subtract position offsets + subtract_pos_offsets(default_loc); + if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { if (!wp_nav->get_wp_destination_loc(default_loc)) { // this should never happen diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index c599f91cd1..f7ce7642a0 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -383,10 +383,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // convert origin to alt-above-terrain if necessary if (!guided_pos_terrain_alt) { // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - pos_control->set_pos_offset_z_cm(origin_terr_offset); + pos_control->init_pos_terrain_cm(origin_terr_offset); } } else { - pos_control->set_pos_offset_z_cm(0.0); + pos_control->init_pos_terrain_cm(0.0); } // set yaw state @@ -496,10 +496,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // convert origin to alt-above-terrain if necessary if (!guided_pos_terrain_alt) { // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - pos_control->set_pos_offset_z_cm(origin_terr_offset); + pos_control->init_pos_terrain_cm(origin_terr_offset); } } else { - pos_control->set_pos_offset_z_cm(0.0); + pos_control->init_pos_terrain_cm(0.0); } guided_pos_target_cm = pos_target_f.topostype(); @@ -825,8 +825,7 @@ void ModeGuided::velaccel_control_run() if (!stabilizing_vel_xy() && !do_avoid) { // set the current commanded xy vel to the desired vel - guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; - guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; + guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy(); } pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); if (!stabilizing_vel_xy() && !do_avoid) { @@ -903,14 +902,11 @@ void ModeGuided::posvelaccel_control_run() // send position and velocity targets to position controller if (!stabilizing_vel_xy()) { // set the current commanded xy pos to the target pos and xy vel to the desired vel - guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; - guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; - guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; - guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; + guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy(); + guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy(); } else if (!stabilizing_pos_xy()) { // set the current commanded xy pos to the target pos - guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; - guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; + guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy(); } pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); if (!stabilizing_vel_xy()) { diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 1fcfd24d32..835a4e27aa 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -72,9 +72,9 @@ void ModeThrow::run() // initialise the demanded height to 3m above the throw height // we want to rapidly clear surrounding obstacles if (g2.throw_type == ThrowType::Drop) { - pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100); + pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm() - 100); } else { - pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300); + pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm() + 300); } // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 57f274b2cc..44e8501174 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -245,8 +245,8 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) const Vector3f& wp_dest = wp_nav->get_wp_destination(); loiter_nav->init_target(wp_dest.xy()); #if AP_RANGEFINDER_ENABLED - if (wp_nav->origin_and_destination_are_terrain_alt()) { - copter.surface_tracking.set_target_alt_cm(wp_dest.z); + if (copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy()) { + copter.surface_tracking.external_init(); } #endif } else { @@ -455,16 +455,10 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve terrain_alt = wp_nav->origin_and_destination_are_terrain_alt(); next_dest.z = wp_nav->get_wp_destination().z; } else { - // if we have a downward facing range finder then use terrain altitude targets terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); - if (terrain_alt) { -#if AP_RANGEFINDER_ENABLED - if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) { - next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); - } -#endif - } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm(); + next_dest.z = pos_control->get_pos_desired_z_cm(); + if (!terrain_alt) { + next_dest.z += pos_control->get_pos_terrain_cm(); } } @@ -506,20 +500,11 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con // get distance from vehicle to start_pos const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()}; - next_dest.x = curr_pos2d.x + (AB_side.x * scalar); - next_dest.y = curr_pos2d.y + (AB_side.y * scalar); + next_dest.xy() = curr_pos2d + (AB_side * scalar); // if we have a downward facing range finder then use terrain altitude targets terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); - if (terrain_alt) { -#if AP_RANGEFINDER_ENABLED - if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) { - next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); - } -#endif - } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm(); - } + next_dest.z = pos_control->get_pos_desired_z_cm(); return true; } diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index ecb6a397d3..4851df6fcc 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -18,7 +18,7 @@ void Copter::SurfaceTracking::update_surface_offset() AP_SurfaceDistance &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; // update position controller target offset to the surface's alt above the EKF origin - copter.pos_control->set_pos_offset_target_z_cm(rf_state.terrain_offset_cm); + copter.pos_control->set_pos_terrain_target_cm(rf_state.terrain_offset_cm); last_update_ms = now_ms; valid_for_logging = true; @@ -28,7 +28,7 @@ void Copter::SurfaceTracking::update_surface_offset() if (timeout || reset_target || (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) { - copter.pos_control->set_pos_offset_z_cm(rf_state.terrain_offset_cm); + copter.pos_control->init_pos_terrain_cm(rf_state.terrain_offset_cm); reset_target = false; last_glitch_cleared_ms = rf_state.glitch_cleared_ms; } @@ -36,40 +36,22 @@ void Copter::SurfaceTracking::update_surface_offset() } else { // reset position controller offsets if surface tracking is inactive // flag target should be reset when/if it next becomes active - if (timeout) { - copter.pos_control->set_pos_offset_z_cm(0); - copter.pos_control->set_pos_offset_target_z_cm(0); + if (timeout && !reset_target) { + copter.pos_control->init_pos_terrain_cm(0); + valid_for_logging = false; reset_target = true; } } } - -// get target altitude (in cm) above ground -// returns true if there is a valid target -bool Copter::SurfaceTracking::get_target_alt_cm(float &target_alt_cm) const +// target has already been set by terrain following so do not initalise again +// this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag) +void Copter::SurfaceTracking::external_init() { - // fail if we are not tracking downwards - if (surface != Surface::GROUND) { - return false; + if ((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) { + last_update_ms = millis(); + reset_target = false; } - // check target has been updated recently - if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { - return false; - } - target_alt_cm = (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()); - return true; -} - -// set target altitude (in cm) above ground -void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm) -{ - // fail if we are not tracking downwards - if (surface != Surface::GROUND) { - return; - } - copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm); - last_update_ms = AP_HAL::millis(); } bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const @@ -79,7 +61,7 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co } const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; - target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f; + target_dist = dir * copter.pos_control->get_pos_desired_z_cm() * 0.01f; return true; } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index e31ea18710..943cea8d52 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -52,7 +52,7 @@ void Mode::_TakeOff::start(float alt_cm) { // initialise takeoff state _running = true; - take_off_start_alt = copter.pos_control->get_pos_target_z_cm(); + take_off_start_alt = copter.pos_control->get_pos_desired_z_cm(); take_off_complete_alt = take_off_start_alt + alt_cm; } @@ -87,7 +87,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) || (copter.pos_control->get_vel_desired_cms().z >= constrain_float(pilot_climb_rate_cm, copter.pos_control->get_max_speed_up_cms() * 0.1, copter.pos_control->get_max_speed_up_cms() * 0.5)) || - (is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_target_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) { + (is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_desired_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) { // throttle > 90% // acceleration > 50% maximum acceleration // velocity > 10% maximum velocity && commanded climb rate @@ -104,7 +104,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude if (is_negative(pilot_climb_rate_cm) || - (take_off_complete_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) { + (take_off_complete_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_desired_z_cm() - take_off_start_alt) { stop(); } } @@ -210,13 +210,13 @@ void _AutoTakeoff::run() const float vel_threshold_fraction = 0.1; // stopping distance from vel_threshold_fraction * max velocity const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss(); - bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance; + bool reached_altitude = copter.pos_control->get_pos_desired_z_cm() >= pos_z - stop_distance; bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction; complete = reached_altitude && reached_climb_rate; // calculate completion for location in case it is needed for a smooth transition to wp_nav if (complete) { - const Vector3p& _complete_pos = copter.pos_control->get_pos_target_cm(); + const Vector3p& _complete_pos = copter.pos_control->get_pos_desired_cm(); complete_pos = Vector3p{_complete_pos.x, _complete_pos.y, pos_z}; } }