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 gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { struct Int64Vect2 new_pos; new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); new_pos.y = ((int64_t)pos.y)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); gh_pos_ref = new_pos; INT32_VECT2_RSHIFT(gh_speed_ref, speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); INT32_VECT2_RSHIFT(gh_accel_ref, accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); }
static void guidance_h_update_reference(void) { /* compute reference even if usage temporarily disabled via guidance_h_use_ref */ #if GUIDANCE_H_USE_REF if (bit_is_set(guidance_h.sp.mask, 5)) { gh_update_ref_from_speed_sp(guidance_h.sp.speed); } else { gh_update_ref_from_pos_sp(guidance_h.sp.pos); } #endif /* either use the reference or simply copy the pos setpoint */ if (guidance_h.use_ref) { /* convert our reference to generic representation */ INT32_VECT2_RSHIFT(guidance_h.ref.pos, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC)); INT32_VECT2_LSHIFT(guidance_h.ref.speed, gh_ref.speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); INT32_VECT2_LSHIFT(guidance_h.ref.accel, gh_ref.accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); } else { VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos); INT_VECT2_ZERO(guidance_h.ref.speed); INT_VECT2_ZERO(guidance_h.ref.accel); } #if GUIDANCE_H_USE_SPEED_REF if (guidance_h.mode == GUIDANCE_H_MODE_HOVER) { VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only } #endif /* update heading setpoint from rate */ if (bit_is_set(guidance_h.sp.mask, 7)) { guidance_h.sp.heading += (guidance_h.sp.heading_rate >> (INT32_ANGLE_FRAC - INT32_RATE_FRAC)) / PERIODIC_FREQUENCY; INT32_ANGLE_NORMALIZE(guidance_h.sp.heading); }
bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time) { uint16_t time_at_wp; uint32_t dist_to_point; static uint16_t wp_entry_time = 0; static bool_t wp_reached = FALSE; static struct EnuCoor_i wp_last = { 0, 0, 0 }; struct Int32Vect2 diff; if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) { wp_reached = FALSE; wp_last = *wp; } VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); dist_to_point = int32_vect2_norm(&diff); if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { if (!wp_reached) { wp_reached = TRUE; wp_entry_time = autopilot_flight_time; time_at_wp = 0; } else { time_at_wp = autopilot_flight_time - wp_entry_time; } } else { time_at_wp = 0; wp_reached = FALSE; } if (time_at_wp > stay_time) { INT_VECT3_ZERO(wp_last); return TRUE; } return FALSE; }
void ins_update_gps(void) { #ifdef USE_GPS if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { if (!ins_ltp_initialised) { ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos); ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; ins_ltp_def.hmsl = booz_gps_state.hmsl; ins_ltp_initialised = TRUE; } ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos); ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel); #ifdef USE_HFF VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); if (ins_hf_realign) { ins_hf_realign = FALSE; #ifdef SITL struct FloatVect2 true_pos, true_speed; VECT2_COPY(true_pos, fdm.ltpprz_pos); VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); b2_hff_realign(true_pos, true_speed); #else const struct FloatVect2 zero = {0.0, 0.0}; b2_hff_realign(ins_gps_pos_m_ned, zero); #endif } b2_hff_update_gps(); #ifndef USE_VFF /* vff not used */ ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; #endif /* vff not used */ #endif /* hff used */ #ifndef USE_HFF /* hff not used */ #ifndef USE_VFF /* neither hf nor vf used */ INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #else /* only vff used */ INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #endif #ifdef USE_GPS_LAG_HACK VECT2_COPY(d_pos, ins_ltp_speed); INT32_VECT2_RSHIFT(d_pos, d_pos, 11); VECT2_ADD(ins_ltp_pos, d_pos); #endif #endif /* hff not used */ INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos); INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed); INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel); } #endif /* USE_GPS */ }
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; }
void rotorcraft_cam_periodic(void) { switch (rotorcraft_cam_mode) { case ROTORCRAFT_CAM_MODE_NONE: #if ROTORCRAFT_CAM_USE_TILT rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; #endif #if ROTORCRAFT_CAM_USE_PAN rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi; #endif break; case ROTORCRAFT_CAM_MODE_MANUAL: // nothing to do here, just apply tilt pwm at the end break; case ROTORCRAFT_CAM_MODE_HEADING: #if ROTORCRAFT_CAM_USE_TILT_ANGLES Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX); rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); #endif #if ROTORCRAFT_CAM_USE_PAN INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); nav_heading = rotorcraft_cam_pan; #endif break; case ROTORCRAFT_CAM_MODE_WP: #ifdef ROTORCRAFT_CAM_TRACK_WP { struct Int32Vect2 diff; VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC); rotorcraft_cam_pan = int32_atan2(diff.x, diff.y); nav_heading = rotorcraft_cam_pan; #if ROTORCRAFT_CAM_USE_TILT_ANGLES int32_t dist, height; dist = INT32_VECT2_NORM(diff); height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; rotorcraft_cam_tilt = int32_atan2(height, dist); Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); #endif } #endif break; default: break; } #if ROTORCRAFT_CAM_USE_TILT ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); #endif }
void booz_cam_periodic(void) { switch (booz_cam_mode) { case BOOZ_CAM_MODE_NONE: #ifdef BOOZ_CAM_USE_TILT booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; #endif #ifdef BOOZ_CAM_USE_PAN booz_cam_pan = ahrs.ltp_to_body_euler.psi; #endif break; case BOOZ_CAM_MODE_MANUAL: #ifdef BOOZ_CAM_USE_TILT Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif break; case BOOZ_CAM_MODE_HEADING: #ifdef BOOZ_CAM_USE_TILT_ANGLES Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX); booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif #ifdef BOOZ_CAM_USE_PAN Bound(booz_cam_pan, CP_MIN, CP_MAX); nav_heading = booz_cam_pan; #endif break; case BOOZ_CAM_MODE_WP: #ifdef WP_CAM { struct Int32Vect2 diff; VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos); INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); INT32_ATAN2(booz_cam_pan,diff.x,diff.y); nav_heading = booz_cam_pan; #ifdef BOOZ_CAM_USE_TILT_ANGLES int32_t dist, height; INT32_VECT2_NORM(dist, diff); height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC; INT32_ATAN2(booz_cam_tilt, height, dist); Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif } #endif break; } #ifdef BOOZ_CAM_USE_TILT BOOZ_CAM_SetPwm(booz_cam_tilt_pwm); #endif }
void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) { if (radius == 0) { VECT2_COPY(navigation_target, *wp_center); dist2_to_wp = get_dist2_to_point(wp_center); } else { struct Int32Vect2 pos_diff; VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center); // go back to half metric precision or values are too large //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2); // store last qdr int32_t last_qdr = nav_circle_qdr; // compute qdr nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x); // increment circle radians if (nav_circle_radians != 0) { int32_t angle_diff = nav_circle_qdr - last_qdr; INT32_ANGLE_NORMALIZE(angle_diff); nav_circle_radians += angle_diff; } else { // Smallest angle to increment at next step nav_circle_radians = 1; } // direction of rotation int8_t sign_radius = radius > 0 ? 1 : -1; // absolute radius int32_t abs_radius = abs(radius); // carrot_angle int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius); Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4); carrot_angle = nav_circle_qdr - sign_radius * carrot_angle; int32_t s_carrot, c_carrot; PPRZ_ITRIG_SIN(s_carrot, carrot_angle); PPRZ_ITRIG_COS(c_carrot, carrot_angle); // compute setpoint VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot); INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC); VECT2_SUM(navigation_target, *wp_center, pos_diff); } nav_circle_center = *wp_center; nav_circle_radius = radius; horizontal_mode = HORIZONTAL_MODE_CIRCLE; }
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { VECT2_ADD(gh_pos_ref, gh_speed_ref); VECT2_ADD(gh_speed_ref, gh_accel_ref); // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp) struct Int32Vect2 speed; INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA); INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC); // compute pos error in pos_sp resolution struct Int32Vect2 pos_err; INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC)); VECT2_DIFF(pos_err, pos_err, pos_sp); // convert to accel resolution INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC)); // compute the "pos part" of accel struct Int32Vect2 pos; VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2)); INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC); // sum accel VECT2_SUM(gh_accel_ref, speed, pos); /* Compute route reference before saturation */ // use metric precision or values are too large INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x); /* Compute North and East route components */ PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref); c_route_ref = abs(c_route_ref); s_route_ref = abs(s_route_ref); /* Compute maximum acceleration*/ gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); /* Compute maximum speed*/ gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); /* restore gh_speed_ref range (Q14.17) */ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); /* Saturate accelerations */ if (gh_accel_ref.x <= -gh_max_accel_ref.x) { gh_accel_ref.x = -gh_max_accel_ref.x; } else if (gh_accel_ref.x >= gh_max_accel_ref.x) { gh_accel_ref.x = gh_max_accel_ref.x; } if (gh_accel_ref.y <= -gh_max_accel_ref.y) { gh_accel_ref.y = -gh_max_accel_ref.y; } else if (gh_accel_ref.y >= gh_max_accel_ref.y) { gh_accel_ref.y = gh_max_accel_ref.y; } /* Saturate speed and adjust acceleration accordingly */ if (gh_speed_ref.x <= -gh_max_speed_ref.x) { gh_speed_ref.x = -gh_max_speed_ref.x; if (gh_accel_ref.x < 0) gh_accel_ref.x = 0; } else if (gh_speed_ref.x >= gh_max_speed_ref.x) { gh_speed_ref.x = gh_max_speed_ref.x; if (gh_accel_ref.x > 0) gh_accel_ref.x = 0; } if (gh_speed_ref.y <= -gh_max_speed_ref.y) { gh_speed_ref.y = -gh_max_speed_ref.y; if (gh_accel_ref.y < 0) gh_accel_ref.y = 0; } else if (gh_speed_ref.y >= gh_max_speed_ref.y) { gh_speed_ref.y = gh_max_speed_ref.y; if (gh_accel_ref.y > 0) gh_accel_ref.y = 0; } }
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { /* WARNING: SPEED SATURATION UNTESTED */ VECT2_ADD(gh_pos_ref, gh_speed_ref); VECT2_ADD(gh_speed_ref, gh_accel_ref); // compute speed error struct Int32Vect2 speed_err; INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); VECT2_DIFF(speed_err, gh_speed_ref, speed_err); // convert to accel resolution INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); // compute accel from speed_sp VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU); INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC); /* Compute route reference before saturation */ // use metric precision or values are too large INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x); /* Compute North and East route components */ PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref); c_route_ref = abs(c_route_ref); s_route_ref = abs(s_route_ref); /* Compute maximum acceleration*/ gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); /* Compute maximum speed*/ gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); /* restore gh_speed_ref range (Q14.17) */ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); /* Saturate accelerations */ if (gh_accel_ref.x <= -gh_max_accel_ref.x) { gh_accel_ref.x = -gh_max_accel_ref.x; } else if (gh_accel_ref.x >= gh_max_accel_ref.x) { gh_accel_ref.x = gh_max_accel_ref.x; } if (gh_accel_ref.y <= -gh_max_accel_ref.y) { gh_accel_ref.y = -gh_max_accel_ref.y; } else if (gh_accel_ref.y >= gh_max_accel_ref.y) { gh_accel_ref.y = gh_max_accel_ref.y; } /* Saturate speed and adjust acceleration accordingly */ if (gh_speed_ref.x <= -gh_max_speed_ref.x) { gh_speed_ref.x = -gh_max_speed_ref.x; if (gh_accel_ref.x < 0) gh_accel_ref.x = 0; } else if (gh_speed_ref.x >= gh_max_speed_ref.x) { gh_speed_ref.x = gh_max_speed_ref.x; if (gh_accel_ref.x > 0) gh_accel_ref.x = 0; } if (gh_speed_ref.y <= -gh_max_speed_ref.y) { gh_speed_ref.y = -gh_max_speed_ref.y; if (gh_accel_ref.y < 0) gh_accel_ref.y = 0; } else if (gh_speed_ref.y >= gh_max_speed_ref.y) { gh_speed_ref.y = gh_max_speed_ref.y; if (gh_accel_ref.y > 0) gh_accel_ref.y = 0; } }