static void send_fp(void) {
int32_t carrot_up = -guidance_v_z_sp;
DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h_pos_sp.y,
&guidance_h_pos_sp.x,
&carrot_up,
&guidance_h_heading_sp,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
static void send_fp(struct transport_tx *trans, struct link_device *dev)
{
int32_t carrot_up = -guidance_v_z_sp;
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
&(stateGetPositionEnu_i()->x),
&(stateGetPositionEnu_i()->y),
&(stateGetPositionEnu_i()->z),
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
&(stateGetNedToBodyEulers_i()->phi),
&(stateGetNedToBodyEulers_i()->theta),
&(stateGetNedToBodyEulers_i()->psi),
&guidance_h.sp.pos.y,
&guidance_h.sp.pos.x,
&carrot_up,
&guidance_h.sp.heading,
&stabilization_cmd[COMMAND_THRUST],
&autopilot_flight_time);
}
bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
{
int32_t dist_to_point;
struct Int32Vect2 diff;
struct EnuCoor_i *pos = stateGetPositionEnu_i();
/* if an approaching_time is given, estimate diff after approching_time secs */
if (approaching_time > 0) {
struct Int32Vect2 estimated_pos;
struct Int32Vect2 estimated_progress;
struct EnuCoor_i *speed = stateGetSpeedEnu_i();
VECT2_SMUL(estimated_progress, *speed, approaching_time);
INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
VECT2_SUM(estimated_pos, *pos, estimated_progress);
VECT2_DIFF(diff, *wp, estimated_pos);
}
/* else use current position */
else {
VECT2_DIFF(diff, *wp, *pos);
}
/* compute distance of estimated/current pos to target wp
* distance with half metric precision (6.25 cm)
*/
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
dist_to_point = int32_vect2_norm(&diff);
/* return TRUE if we have arrived */
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
return TRUE;
}
/* if coming from a valid waypoint */
if (from != NULL) {
/* return TRUE if normal line at the end of the segment is crossed */
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, *wp, *from);
INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
}
return FALSE;
}
