35#ifndef TARGET_POS_TIMEOUT
36#define TARGET_POS_TIMEOUT 5000
40#ifndef TARGET_RTK_TIMEOUT
41#define TARGET_RTK_TIMEOUT 1000
44#ifndef TARGET_OFFSET_HEADING
45#define TARGET_OFFSET_HEADING 180.0
48#ifndef TARGET_OFFSET_DISTANCE
49#define TARGET_OFFSET_DISTANCE 12.0
52#ifndef TARGET_OFFSET_HEIGHT
53#define TARGET_OFFSET_HEIGHT -1.2
56#ifndef TARGET_INTEGRATE_XY
57#define TARGET_INTEGRATE_XY true
60#ifndef TARGET_INTEGRATE_Z
61#define TARGET_INTEGRATE_Z false
86 pprz_msg_send_TARGET_POS_INFO(trans,
dev, AC_ID,
102#if PERIODIC_TELEMETRY
111 uint32_t stamp __attribute__((unused)),
124 if(DL_TARGET_POS_ac_id(buf) != AC_ID)
130 target.pos.lla.lat = DL_TARGET_POS_lat(buf);
131 target.pos.lla.lon = DL_TARGET_POS_lon(buf);
132 target.pos.lla.alt = DL_TARGET_POS_alt(buf);
133 target.pos.ground_speed = DL_TARGET_POS_speed(buf);
134 target.pos.climb = DL_TARGET_POS_climb(buf);
135 target.pos.course = DL_TARGET_POS_course(buf);
136 target.pos.heading = DL_TARGET_POS_heading(buf);
148 struct NedCoor_i target_pos_cm, drone_pos_cm;
156 pos->
x = (target_pos_cm.
x - drone_pos_cm.
x) * 0.01;
157 pos->
y = (target_pos_cm.
y - drone_pos_cm.
y) * 0.01;
158 pos->
z = (target_pos_cm.
z - drone_pos_cm.
z) * 0.01;
169 if(
target.integrate_xy && got_vel) {
170 pos->
x = pos->
x + vel.
x * time_diff;
171 pos->
y = pos->
y + vel.
y * time_diff;
174 if(
target.integrate_z && got_vel) {
175 pos->
z = pos->
z + vel.
z * time_diff;
181 pos->
z -=
target.offset.height;
197 vel->
x =
target.pos.ground_speed * cosf(
target.pos.course/180.*M_PI);
198 vel->
y =
target.pos.ground_speed * sinf(
target.pos.course/180.*M_PI);
220 pos.
x = target_pos_cm.
x * 0.01;
221 pos.
y = target_pos_cm.
y * 0.01;
222 pos.
z = target_pos_cm.
z * 0.01;
224 target.offset.distance = sqrtf(powf(uav_pos.
x - pos.
x, 2) + powf(uav_pos.
y - pos.
y, 2));
225 target.offset.height = -(uav_pos.
z - pos.
z);
226 target.offset.heading = atan2f((uav_pos.
y - pos.
y), (uav_pos.
x - pos.
x))*180.0/M_PI -
target.pos.heading;
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ABI_BROADCAST
Broadcast address.
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
static struct uart_periph * dev
uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
Convert time in sys_time ticks to GPS time of week.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
data structure for GPS information
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
vector in North East Down coordinates
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
vector in North East Down coordinates Units: meters
API to get/set the generic vehicle states.
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
bool target_get_pos(struct NedCoor_f *pos, float *heading)
Get the current target position (NED) and heading.
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
#define TARGET_OFFSET_HEIGHT
void target_parse_target_pos(uint8_t *buf)
Receive a TARGET_POS message from the ground.
static void send_target_pos_info(struct transport_tx *trans, struct link_device *dev)
#define TARGET_OFFSET_HEADING
#define TARGET_POS_TIMEOUT
#define TARGET_INTEGRATE_Z
#define TARGET_OFFSET_DISTANCE
#define TARGET_INTEGRATE_XY
void target_pos_init(void)
#define TARGET_RTK_TIMEOUT
bool target_get_vel(struct NedCoor_f *vel)
Get the current target velocity (NED).
bool target_pos_set_current_offset(float unk)
Set the current measured distance and heading as offset.
struct target_pos_t pos
The target position message.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.