static void send_segment(struct transport_tx *trans, struct link_device *dev)
{
if (nav_in_segment) {
pprz_msg_send_SEGMENT(trans, dev, AC_ID,
&nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
}
}
static void send_segment(struct transport_tx *trans, struct link_device *dev)
{
if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
pprz_msg_send_SEGMENT(trans, dev, AC_ID,
&gvf_segment.x1, &gvf_segment.y1,
&gvf_segment.x2, &gvf_segment.y2);
}
}
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
{
float dist_home = sqrtf(dist2_to_home);
float dist_wp = sqrtf(dist2_to_wp);
pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
&block_time, &stage_time,
&dist_home, &dist_wp,
&nav_block, &nav_stage,
&horizontal_mode);
if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
} else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
float cx = POS_FLOAT_OF_BFP(nav_circle_center.x);
float cy = POS_FLOAT_OF_BFP(nav_circle_center.y);
float r = POS_FLOAT_OF_BFP(nav_circle_radius);
pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
}
}