From 75b1bdb3219c19b9fbdcf8ea85508662a0b439f1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Feb 2019 17:42:13 +1100 Subject: [PATCH 1/2] Copter: remove position-vector methods Both were used in just one place --- ArduCopter/Copter.h | 4 ---- ArduCopter/GCS_Mavlink.cpp | 7 ++++++- ArduCopter/inertia.cpp | 14 +++++++------- ArduCopter/position_vector.cpp | 23 ----------------------- 4 files changed, 13 insertions(+), 35 deletions(-) delete mode 100644 ArduCopter/position_vector.cpp diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 390e63303e8ee..f0379f1caf412 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -788,10 +788,6 @@ class Copter : public AP_HAL::HAL::Callbacks { void convert_pid_parameters(void); void convert_lgr_parameters(void); - // position_vector.cpp - float pv_alt_above_origin(float alt_above_home_cm); - float pv_alt_above_home(float alt_above_origin_cm); - // precision_landing.cpp void init_precland(); void update_precland(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index df7a4fd046212..4455257da423d 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1033,7 +1033,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) pos_vector += copter.inertial_nav.get_position(); } else { // convert from alt-above-home to alt-above-ekf-origin - pos_vector.z = copter.pv_alt_above_origin(pos_vector.z); + if (!AP::ahrs().home_is_set()) { + break; + } + const Location &origin = copter.inertial_nav.get_origin(); + pos_vector.z += AP::ahrs().get_home().alt; + pos_vector.z -= origin.alt; } } diff --git a/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index eb46c8e03a2b6..9e69a7dd64c70 100644 --- a/ArduCopter/inertia.cpp +++ b/ArduCopter/inertia.cpp @@ -15,16 +15,16 @@ void Copter::read_inertia() return; } - // without home return alt above the EKF origin - if (!ahrs.home_is_set()) { - // with inertial nav we can update the altitude and climb rate at 50hz - current_loc.alt = inertial_nav.get_altitude(); + Location::ALT_FRAME frame; + if (ahrs.home_is_set()) { + frame = Location::ALT_FRAME_ABOVE_HOME; } else { - // with inertial nav we can update the altitude and climb rate at 50hz - current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude()); + // without home use alt above the EKF origin + frame = Location::ALT_FRAME_ABOVE_ORIGIN; } + current_loc.set_alt_cm(inertial_nav.get_altitude(), frame); + current_loc.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME); // set flags and get velocity - current_loc.relative_alt = true; climb_rate = inertial_nav.get_velocity_z(); } diff --git a/ArduCopter/position_vector.cpp b/ArduCopter/position_vector.cpp deleted file mode 100644 index 6c0e82aded58b..0000000000000 --- a/ArduCopter/position_vector.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "Copter.h" - -// Position vectors related utility functions - -// position vectors are Vector3f -// .x = latitude from home in cm -// .y = longitude from home in cm -// .z = altitude above home in cm - - -// pv_alt_above_origin - convert altitude above home to altitude above EKF origin -float Copter::pv_alt_above_origin(float alt_above_home_cm) -{ - const struct Location &origin = inertial_nav.get_origin(); - return alt_above_home_cm + (ahrs.get_home().alt - origin.alt); -} - -// pv_alt_above_home - convert altitude above EKF origin to altitude above home -float Copter::pv_alt_above_home(float alt_above_origin_cm) -{ - const struct Location &origin = inertial_nav.get_origin(); - return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt); -} From 79b430c373b47ed0a7627e1b202c1ee8c1b51fc9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 22 Feb 2019 16:29:13 +1100 Subject: [PATCH 2/2] Tools: autotest: add test for MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED --- Tools/autotest/arducopter.py | 90 ++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 41e7cf6d3fa35..14c851889d9dc 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2063,6 +2063,41 @@ def test_setting_modes_via_auxswitch(self): if ex is not None: raise ex + def fly_guided_stop(self, + timeout=20, + groundspeed_tolerance=0.05, + climb_tolerance=0.001): + """stop the vehicle moving in guided mode""" + self.progress("Stopping vehicle") + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Vehicle did not stop") + # send a position-control command + self.mav.mav.set_position_target_local_ned_send( + 0, # timestamp + 1, # target system_id + 1, # target component id + mavutil.mavlink.MAV_FRAME_BODY_NED, + 0b1111111111111000, # mask specifying use-only-x-y-z + 0, # x + 0, # y + 0, # z + 0, # vx + 0, # vy + 0, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0, # yawrate + ) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + print("%s" % str(m)) + if (m.groundspeed < groundspeed_tolerance and + m.climb < climb_tolerance): + break + def fly_guided_move_relative(self, lat, lon, alt): startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) @@ -2097,6 +2132,57 @@ def fly_guided_move_relative(self, lat, lon, alt): if delta > 10: break + def fly_guided_move_local(self, x, y, z_up, timeout=100): + """move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED""" + startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + self.progress("startpos=%s" % str(startpos)) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > timeout: + raise NotAchievedException("Did not start to move") + # send a position-control command + self.mav.mav.set_position_target_local_ned_send( + 0, # timestamp + 1, # target system_id + 1, # target component id + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + 0b1111111111111000, # mask specifying use-only-x-y-z + x, # x + y, # y + -z_up,# z + 0, # vx + 0, # vy + 0, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0, # yawrate + ) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + print("%s" % m) + if m.groundspeed > 0.5: + break + + self.progress("Waiting for vehicle to stop...") + self.wait_groundspeed(1, 100, timeout=timeout) + + stoppos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + self.progress("stop_pos=%s" % str(stoppos)) + + x_achieved = stoppos.x - startpos.x + if x_achieved - x > 1: + raise NotAchievedException("Did not achieve x position: want=%f got=%f" % (x, x_achieved)) + + y_achieved = stoppos.y - startpos.y + if y_achieved - y > 1: + raise NotAchievedException("Did not achieve y position: want=%f got=%f" % (y, y_achieved)) + + z_achieved = stoppos.z - startpos.z + if z_achieved - z_up > 1: + raise NotAchievedException("Did not achieve z position: want=%f got=%f" % (z_up, z_achieved)) + def earth_to_body(self, vector): m = self.mav.messages["ATTITUDE"] x = rotmat.Vector3(m.roll, m.pitch, m.yaw) @@ -2218,6 +2304,10 @@ def fly_guided_change_submode(self): """move the vehicle using set_position_target_global_int""" self.fly_guided_move_relative(5, 5, 10) + """move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED""" + self.fly_guided_stop() + self.fly_guided_move_local(5, 5, 10) + self.progress("Landing") self.mavproxy.send('switch 2\n') # land mode self.wait_mode('LAND')