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

Sub: update for new double precision position APIs

This commit is contained in:
Andrew Tridgell 2021-06-18 11:20:27 +10:00
parent 50e6d67a66
commit 70f874e288
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/control_auto.cpp
View File

@ -205,7 +205,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
} }
// if we are outside the circle, point at the edge, otherwise hold yaw // if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &circle_center_neu = circle_nav.get_center(); const Vector3p &circle_center_neu = circle_nav.get_center();
const Vector3f &curr_pos = inertial_nav.get_position(); 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); float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {

14
ArduSub/control_guided.cpp
View File

@ -11,7 +11,7 @@
#define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates #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 #define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates
static Vector3f posvel_pos_target_cm; static Vector3p posvel_pos_target_cm;
static Vector3f posvel_vel_target_cms; static Vector3f posvel_vel_target_cms;
static uint32_t posvel_update_time_ms; static uint32_t posvel_update_time_ms;
static uint32_t vel_update_time_ms; static uint32_t vel_update_time_ms;
@ -214,11 +214,13 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
#endif #endif
posvel_update_time_ms = AP_HAL::millis(); posvel_update_time_ms = AP_HAL::millis();
posvel_pos_target_cm = destination; posvel_pos_target_cm = destination.topostype();
posvel_vel_target_cms = velocity; posvel_vel_target_cms = velocity;
pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
pos_control.input_pos_vel_accel_z(posvel_pos_target_cm.z, posvel_vel_target_cms.z, 0); float dz = posvel_pos_target_cm.z;
pos_control.input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = dz;
// log target // log target
Log_Write_GuidedTarget(guided_mode, destination, velocity); Log_Write_GuidedTarget(guided_mode, destination, velocity);
@ -418,11 +420,13 @@ void Sub::guided_posvel_control_run()
} }
// advance position target using velocity target // advance position target using velocity target
posvel_pos_target_cm += posvel_vel_target_cms * pos_control.get_dt(); posvel_pos_target_cm += (posvel_vel_target_cms * pos_control.get_dt()).topostype();
// send position and velocity targets to position controller // send position and velocity targets to position controller
pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
pos_control.input_pos_vel_accel_z(posvel_pos_target_cm.z, posvel_vel_target_cms.z, 0); float pz = posvel_pos_target_cm.z;
pos_control.input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = pz;
// run position controller // run position controller
pos_control.update_xy_controller(); pos_control.update_xy_controller();