Copter: use cleaned up APIs

ArduCopter/mode.h

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

ArduCopter/mode_auto.cpp

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

ArduCopter/mode_brake.cpp

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

ArduCopter/mode_guided.cpp

@@ -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_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), Vector2f());
-    pos_control->input_pos_vel_accel_z(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
+    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_xy(curr_vel_des.xy(), Vector2f());
-    pos_control->input_vel_accel_z(curr_vel_des, Vector3f(), false);
+    pos_control->input_vel_accel_z(curr_vel_des.z, 0, false);
 }
 
 // helper function to set yaw state and targets

ArduCopter/mode_land.cpp

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

ArduCopter/mode_poshold.cpp

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

ArduCopter/mode_rtl.cpp

@@ -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_xy_cm(stopping_point.xy());
-    pos_control->get_stopping_point_z_cm(stopping_point);
+    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);
 

ArduCopter/mode_smart_rtl.cpp

@@ -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_xy_cm(stopping_point.xy());
-        pos_control->get_stopping_point_z_cm(stopping_point);
+        pos_control->get_stopping_point_z_cm(stopping_point.z);
         wp_nav->set_wp_destination(stopping_point);
 
         // initialise yaw to obey user parameter

ArduCopter/mode_throw.cpp

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

ArduCopter/mode_zigzag.cpp

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

ArduCopter/takeoff.cpp

@@ -79,15 +79,11 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
         return;
     }
 
-    Vector3f pos;
+    float pos_z = take_off_complete_alt;
-    Vector3f vel;
+    float vel_z = pilot_climb_rate_cm;
-    Vector3f accel;
-
-    pos.z = take_off_complete_alt ;
-    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) ||