void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
{
struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
VECT2_DIFF(wp_diff, *wp_end, *wp_start);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
// go back to metric precision or values are too large
VECT2_COPY(wp_diff_prec, wp_diff);
INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC);
uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
nav_leg_length = int32_sqrt(leg_length2);
nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress;
int32_t prog_2 = nav_leg_length;
Bound(nav_leg_progress, 0, prog_2);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
VECT2_SUM(navigation_target, *wp_start, progress_pos);
nav_segment_start = *wp_start;
nav_segment_end = *wp_end;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
dist2_to_wp = get_dist2_to_point(wp_end);
}
void stateCalcHorizontalSpeedNorm_i(void)
{
if (bit_is_set(state.speed_status, SPEED_HNORM_I)) {
return;
}
if (bit_is_set(state.speed_status, SPEED_HNORM_F)) {
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
} else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x +
state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC;
state.h_speed_norm_i = int32_sqrt(n2);
} else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
