diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3128263c98..8665424119 100644 --- a/ArduCopter/mode.h +++ b/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(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 27332609a3..f2d75db5cb 100644 --- a/ArduCopter/mode_auto.cpp +++ b/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); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 77837cd6af..30da6448ae 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -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(); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f8c502e13a..90c545b6cc 100644 --- a/ArduCopter/mode_guided.cpp +++ b/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_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 diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 5d7e6e6940..ff6cb0c999 100644 --- a/ArduCopter/mode_land.cpp +++ b/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); } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 56db29c4b4..430b71f105 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/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; } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index c491b826bf..b6cacd1608 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/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_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); diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 61044fe6c1..aa9358f37d 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/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_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 diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 67eb995405..7743d4b538 100644 --- a/ArduCopter/mode_throw.cpp +++ b/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; - Vector3f accel; + Vector2f vel; + Vector2f accel; pos_control->input_vel_accel_xy(vel, accel); pos_control->update_xy_controller(); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 57692af932..3941c71558 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/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); } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 427455c7f9..dc8c6b69e2 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -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) ||