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

Copter: use cleaned up APIs

This commit is contained in:
Andrew Tridgell 2021-06-21 17:22:48 +10:00
parent 52d20cedad
commit b43c58f6c3
11 changed files with 28 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/mode.h
View File

@ -380,7 +380,7 @@ public:
void takeoff_start(const Location& dest_loc);
void wp_start(const Location& dest_loc);
void land_start();
void land_start(const Vector3f& destination);
void land_start(const Vector2f& destination);
void circle_movetoedge_start(const Location &circle_center, float radius_m);
void circle_start();
void nav_guided_start();
@ -442,7 +442,7 @@ private:
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
void payload_place_start(const Vector3f& destination);
void payload_place_start(const Vector2f& destination);
void payload_place_run();
bool payload_place_run_should_run();
void payload_place_run_loiter();

12
ArduCopter/mode_auto.cpp
View File

@ -255,7 +255,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
void ModeAuto::land_start()
{
// set target to stopping point
Vector3f stopping_point;
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
// call location specific land start function
@ -263,7 +263,7 @@ void ModeAuto::land_start()
}
// auto_land_start - initialises controller to implement a landing
void ModeAuto::land_start(const Vector3f& destination)
void ModeAuto::land_start(const Vector2f& destination)
{
_mode = SubMode::LAND;
@ -392,7 +392,7 @@ bool ModeAuto::is_taking_off() const
void ModeAuto::payload_place_start()
{
// set target to stopping point
Vector3f stopping_point;
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
// call location specific place start function
@ -988,7 +988,7 @@ void ModeAuto::loiter_to_alt_run()
}
// auto_payload_place_start - initialises controller to implement placement of a load
void ModeAuto::payload_place_start(const Vector3f& destination)
void ModeAuto::payload_place_start(const Vector2f& destination)
{
_mode = SubMode::NAV_PAYLOAD_PLACE;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
@ -1253,7 +1253,7 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
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);
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;
@ -1588,7 +1588,7 @@ bool ModeAuto::verify_land()
// check if we've reached the location
if (copter.wp_nav->reached_wp_destination()) {
// get destination so we can use it for loiter target
const Vector3f& dest = copter.wp_nav->get_wp_destination();
const Vector2f& dest = copter.wp_nav->get_wp_destination().xy();
// initialise landing controller
land_start(dest);

4
ArduCopter/mode_brake.cpp
View File

@ -49,8 +49,8 @@ void ModeBrake::run()
}
// use position controller to stop
Vector3f vel;
Vector3f accel;
Vector2f vel;
Vector2f accel;
pos_control->input_vel_accel_xy(vel, accel);
pos_control->update_xy_controller();

8
ArduCopter/mode_guided.cpp
View File

@ -557,8 +557,8 @@ void ModeGuided::posvel_control_run()
guided_pos_target_cm += guided_vel_target_cms * pos_control->get_dt();
// send position and velocity targets to position controller
pos_control->input_pos_vel_accel_xy(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
pos_control->input_pos_vel_accel_z(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
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);
// run position controllers
pos_control->update_xy_controller();
@ -670,8 +670,8 @@ void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f
#endif
// update position controller with new target
pos_control->input_vel_accel_xy(curr_vel_des, Vector3f());
pos_control->input_vel_accel_z(curr_vel_des, Vector3f(), false);
pos_control->input_vel_accel_xy(curr_vel_des.xy(), Vector2f());
pos_control->input_vel_accel_z(curr_vel_des.z, 0, false);
}
// helper function to set yaw state and targets

2
ArduCopter/mode_land.cpp
View File

@ -7,7 +7,7 @@ bool ModeLand::init(bool ignore_checks)
control_position = copter.position_ok();
if (control_position) {
// set target to stopping point
Vector3f stopping_point;
Vector2f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
loiter_nav->init_target(stopping_point);
}

2
ArduCopter/mode_poshold.cpp
View File

@ -379,7 +379,7 @@ void ModePosHold::run()
pitch_mode = RPMode::BRAKE_TO_LOITER;
brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller
loiter_nav->init_target(inertial_nav.get_position());
loiter_nav->init_target(inertial_nav.get_position().xy());
// set delay to start of wind compensation estimate updates
wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
}

8
ArduCopter/mode_rtl.cpp
View File

@ -263,7 +263,7 @@ void ModeRTL::descent_start()
_state_complete = false;
// Set wp navigation target to above home
loiter_nav->init_target(wp_nav->get_wp_destination());
loiter_nav->init_target(wp_nav->get_wp_destination().xy());
// initialise altitude target to stopping point
pos_control->init_z_controller_stopping_point();
@ -356,7 +356,7 @@ void ModeRTL::land_start()
_state_complete = false;
// Set wp navigation target to above home
loiter_nav->init_target(wp_nav->get_wp_destination());
loiter_nav->init_target(wp_nav->get_wp_destination().xy());
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
@ -413,8 +413,8 @@ void ModeRTL::build_path()
{
// origin point is our stopping point
Vector3f stopping_point;
pos_control->get_stopping_point_xy_cm(stopping_point);
pos_control->get_stopping_point_z_cm(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.change_alt_frame(Location::AltFrame::ABOVE_HOME);

4
ArduCopter/mode_smart_rtl.cpp
View File

@ -17,8 +17,8 @@ bool ModeSmartRTL::init(bool ignore_checks)
// set current target to a reasonable stopping point
Vector3f stopping_point;
pos_control->get_stopping_point_xy_cm(stopping_point);
pos_control->get_stopping_point_z_cm(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);
// initialise yaw to obey user parameter

4
ArduCopter/mode_throw.cpp
View File

@ -171,8 +171,8 @@ void ModeThrow::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// use position controller to stop
Vector3f vel;
Vector3f accel;
Vector2f vel;
Vector2f accel;
pos_control->input_vel_accel_xy(vel, accel);
pos_control->update_xy_controller();

2
ArduCopter/mode_zigzag.cpp
View File

@ -241,7 +241,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
loiter_nav->clear_pilot_desired_acceleration();
if (maintain_target) {
const Vector3f& wp_dest = wp_nav->get_wp_destination();
loiter_nav->init_target(wp_dest);
loiter_nav->init_target(wp_dest.xy());
if (wp_nav->origin_and_destination_are_terrain_alt()) {
copter.surface_tracking.set_target_alt_cm(wp_dest.z);
}

10
ArduCopter/takeoff.cpp
View File

@ -79,15 +79,11 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
return;
}
Vector3f pos;
Vector3f vel;
Vector3f accel;
pos.z = take_off_complete_alt ;
vel.z = pilot_climb_rate_cm;
float pos_z = take_off_complete_alt;
float vel_z = pilot_climb_rate_cm;
// command the aircraft to the take off altitude and current pilot climb rate
copter.pos_control->input_pos_vel_accel_z(pos, vel, accel);
copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0);
// 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) ||