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

Copter: update for new double precision position APIs

This commit is contained in:
Andrew Tridgell 2021-06-18 11:20:26 +10:00
parent 1d00cab9e6
commit bada2670a6
4 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode_auto.cpp
View File

@ -324,7 +324,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &circle_center_neu = copter.circle_nav->get_center();
const Vector3p &circle_center_neu = copter.circle_nav->get_center();
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
// initialise yaw

10
ArduCopter/mode_guided.cpp
View File

@ -13,7 +13,7 @@
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates
#define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
static Vector3f guided_pos_target_cm; // position target (used by posvel controller only)
static Vector3p guided_pos_target_cm; // position target (used by posvel controller only)
static Vector3f guided_vel_target_cms; // velocity target (used by velocity controller and posvel controller)
static uint32_t posvel_update_time_ms; // system time of last target update to posvel controller (i.e. position and velocity update)
static uint32_t vel_update_time_ms; // system time of last target update to velocity controller
@ -347,7 +347,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
posvel_update_time_ms = millis();
guided_pos_target_cm = destination;
guided_pos_target_cm = destination.topostype();
guided_vel_target_cms = velocity;
copter.pos_control->set_pos_vel_accel(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
@ -554,11 +554,13 @@ void ModeGuided::posvel_control_run()
}
// advance position target using velocity target
guided_pos_target_cm += guided_vel_target_cms * pos_control->get_dt();
guided_pos_target_cm += (guided_vel_target_cms * pos_control->get_dt()).topostype();
// send position and velocity targets to position controller
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), Vector2f());
pos_control->input_pos_vel_accel_z(guided_pos_target_cm.z, guided_vel_target_cms.z, 0);
float pz = guided_pos_target_cm.z;
pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, 0);
guided_pos_target_cm.z = pz;
// run position controllers
pos_control->update_xy_controller();

4
ArduCopter/mode_rtl.cpp
View File

@ -412,10 +412,10 @@ void ModeRTL::land_run(bool disarm_on_land)
void ModeRTL::build_path()
{
// origin point is our stopping point
Vector3f stopping_point;
Vector3p stopping_point;
pos_control->get_stopping_point_xy_cm(stopping_point.xy());
pos_control->get_stopping_point_z_cm(stopping_point.z);
rtl_path.origin_point = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN);
rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN);
rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
// compute return target

4
ArduCopter/mode_smart_rtl.cpp
View File

@ -16,10 +16,10 @@ bool ModeSmartRTL::init(bool ignore_checks)
wp_nav->wp_and_spline_init();
// set current target to a reasonable stopping point
Vector3f stopping_point;
Vector3p stopping_point;
pos_control->get_stopping_point_xy_cm(stopping_point.xy());
pos_control->get_stopping_point_z_cm(stopping_point.z);
wp_nav->set_wp_destination(stopping_point);
wp_nav->set_wp_destination(stopping_point.tofloat());
// initialise yaw to obey user parameter
auto_yaw.set_mode_to_default(true);