Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: support set_posvelaccel_offset in auto 

Co-authored-by: Leonard Hall <leonardthall@gmail.com>
This commit is contained in:
Randy Mackay 2024-08-02 18:31:50 +09:00
parent df2ae532c2
commit 0bcff6cec0
8 changed files with 83 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Copter.h
View File

@ -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;

10
ArduCopter/mode.h
View File

@ -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);

68
ArduCopter/mode_auto.cpp
View File

@ -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

20
ArduCopter/mode_guided.cpp
View File

@ -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()) {

4
ArduCopter/mode_throw.cpp
View File

@ -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

29
ArduCopter/mode_zigzag.cpp
View File

@ -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;
}

42
ArduCopter/surface_tracking.cpp
View File

@ -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;
}

10
ArduCopter/takeoff.cpp
View File

@ -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};
}
}