diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 8792efd2488988..61e217718bd1c8 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -29,20 +29,23 @@ extern const AP_HAL::HAL& hal; -#define AP_FOLLOW_TIMEOUT_MS 3000 // position estimate timeout after 1 second #define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we have not heard from them in 10 seconds #define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame #define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading -#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE 1 // relative altitude is used by default +#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE 1 // home relative altitude is used by default #define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) -#define AP_FOLLOW_ALT_TYPE_DEFAULT 0 +#define AP_FOLLOW_ALT_TYPE_DEFAULT 0 // absolute/AMSL altitude +#define AP_FOLLOW_ALTITUDE_TYPE_ORIGIN 2 // origin relative altitude +#define AP_FOLLOW_ALTITUDE_TYPE_TERRAIN 3 // terrain relative altitude +#define AP_FOLLOW_TIMEOUT_DEFAULT 600 #else #define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE +#define AP_FOLLOW_TIMEOUT_DEFAULT 3000 #endif AP_Follow *AP_Follow::_singleton; @@ -129,7 +132,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @Param: _ALT_TYPE // @DisplayName: Follow altitude type // @Description: Follow altitude type - // @Values: 0:absolute, 1:relative + // @Values: 0:absolute, 1:relative, 2:origin (not used), 3:terrain (plane) // @User: Standard AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT), #endif @@ -141,6 +144,13 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @User: Standard AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0), + // @Param: _TIMEOUT + // @DisplayName: Follow timeout + // @Description: Follow position estimate timeout after x milliseconds + // @User: Standard + // @Units: ms + AP_GROUPINFO("_TIMEOUT", 12, AP_Follow, _timeout, AP_FOLLOW_TIMEOUT_DEFAULT), + AP_GROUPEND }; @@ -174,7 +184,7 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne } // check for timeout - if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) { + if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > (uint32_t)_timeout)) { return false; } @@ -263,7 +273,7 @@ bool AP_Follow::get_target_heading_deg(float &heading) const } // check for timeout - if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) { + if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > (uint32_t)_timeout)) { return false; } @@ -345,6 +355,27 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg) _target_location.lng = packet.lon; // select altitude source based on FOLL_ALT_TYPE param +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + switch(_alt_type) { + case AP_FOLLOW_ALT_TYPE_DEFAULT: + _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE); + break; + case AP_FOLLOW_ALTITUDE_TYPE_RELATIVE: + case AP_FOLLOW_ALTITUDE_TYPE_ORIGIN: + _target_location.set_alt_cm(packet.relative_alt * 0.1, static_cast((int)_alt_type)); + break; + case AP_FOLLOW_ALTITUDE_TYPE_TERRAIN: { + /// Altitude comes in AMSL + int32_t terrain_altitude_cm; + _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE); + // convert the incoming altitude to terrain altitude + if(_target_location.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terrain_altitude_cm)) { + _target_location.set_alt_cm(terrain_altitude_cm, Location::AltFrame::ABOVE_TERRAIN); + } + break; + } + } +#else if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) { // above home alt _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME); @@ -352,7 +383,7 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg) // absolute altitude _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE); } - +#endif _target_velocity_ned.x = packet.vx * 0.01f; // velocity north _target_velocity_ned.y = packet.vy * 0.01f; // velocity east _target_velocity_ned.z = packet.vz * 0.01f; // velocity down @@ -393,8 +424,8 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg) // FOLLOW_TARGET is always AMSL, change the provided alt to // above home if we are configured for relative alt - if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE && - !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { + if (_alt_type != AP_FOLLOW_ALT_TYPE_DEFAULT && + !new_loc.change_alt_frame(static_cast((int)_alt_type))) { return false; } _target_location = new_loc; @@ -559,11 +590,42 @@ bool AP_Follow::have_target(void) const } // check for timeout - if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) { + if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > (uint32_t)_timeout)) { + gcs().send_text(MAV_SEVERITY_NOTICE, "location timeout %d", (int)(AP_HAL::millis() - _last_location_update_ms)); + return false; + } + return true; +} + +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) +// create a single method to retrieve all the relevant values in one shot for Lua +/* replaces the following Lua calls + target_distance, target_distance_offsets, target_velocity = follow:get_target_dist_and_vel_ned() + target_location, target_velocity = follow:get_target_location_and_velocity() + target_location_offset, target_velocity = follow:get_target_location_and_velocity_ofs() + local xy_dist = follow:get_distance_to_target() -- this value is set by get_target_dist_and_vel_ned() - why do I have to know this? + local target_heading = follow:get_target_heading_deg() +*/ +bool AP_Follow::get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs, + Vector3f &target_vel_ned, Vector3f &target_vel_ned_ofs, + Location &target_loc, Location &target_loc_ofs, + float &target_dist_ofs + ) +{ + // The order here is VERY important. This needs to called first because it updates a lot of internal variables + if(!get_target_dist_and_vel_ned(dist_ned, dist_with_offs, target_vel_ned)) { return false; } + if(!get_target_location_and_velocity(target_loc,target_vel_ned)) { + return false; + } + if(!get_target_location_and_velocity_ofs(target_loc_ofs, target_vel_ned_ofs)) { + return false; + } + target_dist_ofs = _dist_to_target; return true; } +#endif namespace AP { diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 48c43fc9ff4706..14fca4c05eaf14 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -26,6 +26,8 @@ #include #include +#include + class AP_Follow { @@ -112,6 +114,16 @@ class AP_Follow // returns true if a follow option enabled bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; } + // + // Lua binding combo function + // + // try to get all the values from a single cycle and return them in a single call to Lua + bool get_target_info(Vector3f &dist_ned, Vector3f &dist_with_offs, + Vector3f &target_vel_ned, Vector3f &target_vel_ned_ofs, + Location &target_loc, Location &target_loc_ofs, + float &target_dist_ofs + ); + // parameter list static const struct AP_Param::GroupInfo var_info[]; @@ -153,6 +165,7 @@ class AP_Follow AP_Int8 _alt_type; // altitude source for follow mode AC_P _p_pos; // position error P controller AP_Int16 _options; // options for mount behaviour follow mode + AP_Int32 _timeout; // position estimate timeout after x milliseconds // local variables uint32_t _last_location_update_ms; // system time of last position update