24 #define AP_FOLLOW_TIMEOUT_MS 3000 // position estimate timeout after 1 second 25 #define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we haave not heard from them in 10 seconds 26 #define AP_GCS_INTERVAL_MS 1000 // interval between updating GCS on position of vehicle 28 #define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame 29 #define AP_FOLLOW_OFFSET_TYPE_RELATIVE 0 // offsets are relative to lead vehicle's heading 31 #define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default 148 last_loc.
alt -= vel_ned.
z * 10.0f * dt;
160 if (!
AP::ahrs().get_position(current_loc)) {
190 dist_with_offs = dist_vec + offsets;
238 if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
248 mavlink_global_position_int_t packet;
249 mavlink_msg_global_position_int_decode(&msg, &packet);
252 if ((packet.lat == 0 && packet.lon == 0)) {
263 if (packet.hdg <= 36000) {
272 gcs().
send_text(MAV_SEVERITY_INFO,
"Foll: %u %ld %ld %4.2f\n",
281 "TimeUS,Lat,Lon,Alt,VelX,VelY,VelZ,LatE,LonE,AltE",
334 offset.
x = (off.
x * veh_cos_yaw) - (off.
y * veh_sin_yaw);
335 offset.
y = (off.
y * veh_cos_yaw) + (off.
x * veh_sin_yaw);
#define AP_FOLLOW_POS_P_DEFAULT
#define AP_FOLLOW_SYSID_TIMEOUT_MS
#define AP_PARAM_FLAG_ENABLE
bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned)
uint32_t _last_location_sent_to_gcs
Interface definition for the various Ground Control System.
#define AP_GROUPINFO(name, idx, clazz, element, def)
bool is_positive(const T fVal1)
void location_offset(struct Location &loc, float ofs_north, float ofs_east)
#define AP_FOLLOW_OFFSET_TYPE_NED
bool get_velocity_ned(Vector3f &vel_ned, float dt) const
int32_t lat
param 3 - Latitude * 10**7
bool get_offsets_ned(Vector3f &offsets) const
Object managing one P controller.
#define AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags)
#define AP_GCS_INTERVAL_MS
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
void Log_Write(const char *name, const char *labels, const char *fmt,...)
mavlink_system_t mavlink_system
MAVLink system definition.
static DataFlash_Class * instance(void)
Vector3f _target_accel_ned
bool get_target_heading(float &heading) const
void init_offsets_if_required(const Vector3f &dist_vec_ned)
void send_text(MAV_SEVERITY severity, const char *fmt,...)
#define UNUSED_RESULT(expr_)
#define AP_FOLLOW_TIMEOUT_MS
int32_t lng
param 4 - Longitude * 10**7
Vector3f location_3d_diff_NED(const struct Location &loc1, const struct Location &loc2)
static constexpr float radians(float deg)
static const struct AP_Param::GroupInfo var_info[]
Vector3f _target_velocity_ned
uint32_t _last_heading_update_ms
uint32_t _last_location_update_ms
#define AP_SUBGROUPINFO(element, name, idx, thisclazz, elclazz)
Location _target_location
void handle_msg(const mavlink_message_t &msg)
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)