/* measure psi by projecting magnetic vector in local tangeant plan */
__attribute__((always_inline)) static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est,
int32_t theta_est, struct Int32Vect3 *mag)
{
int32_t sphi;
PPRZ_ITRIG_SIN(sphi, phi_est);
int32_t cphi;
PPRZ_ITRIG_COS(cphi, phi_est);
int32_t stheta;
PPRZ_ITRIG_SIN(stheta, theta_est);
int32_t ctheta;
PPRZ_ITRIG_COS(ctheta, theta_est);
int32_t sphi_stheta = (sphi * stheta) >> INT32_TRIG_FRAC;
int32_t cphi_stheta = (cphi * stheta) >> INT32_TRIG_FRAC;
//int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
//int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
//const int32_t md =
// -stheta * mag->x +
// sphi_ctheta * mag->y +
// cphi_ctheta * mag->z;
float m_psi = -atan2(me, mn);
*psi_meas = ((m_psi - ahrs_ice.mag_offset) * (float)(1 << (INT32_ANGLE_FRAC)) * F_UPDATE);
}
void stabilization_attitude_set_failsafe_setpoint(void) {
/* set failsafe to zero roll/pitch and current heading */
int32_t heading2 = stabilization_attitude_get_heading_i() / 2;
PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2);
stab_att_sp_quat.qx = 0;
stab_att_sp_quat.qy = 0;
PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) {
/* Rotate horizontal commands to body frame by psi */
int32_t psi = stateGetNedToBodyEulers_i()->psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.psi = heading;
}
/* measures phi and theta assuming no dynamic acceleration ?!! */
__attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
INT32_ATAN2(*phi_meas, -accel.y, -accel.z);
int32_t cphi;
PPRZ_ITRIG_COS(cphi, *phi_meas);
int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC);
INT32_ATAN2(*theta_meas, -cphi_ax, -accel.z);
*phi_meas *= F_UPDATE;
*theta_meas *= F_UPDATE;
}
/** Read attitude setpoint from RC as euler angles.
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi;
int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) {
// stab_att_sp_euler.psi still used in ref..
stab_att_sp_euler.psi = heading;
// compute sp_euler phi/theta for debugging/telemetry
/* Rotate horizontal commands to body frame by psi */
int32_t psi = stateGetNedToBodyEulers_i()->psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
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;
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
if (in_flight) {
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
if(abs(sp->phi) < max_phi)
omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
else //max 60 degrees roll, then take constant omega
omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = heading + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
static float last_ts = 0.f;
sp->phi = get_rc_roll();
sp->theta = get_rc_pitch();
if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
/* make sure nothing drastically weird happens, bound dt to 0.5sec */
Bound(dt, 0, 0.5);
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw() * dt;
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
if (abs(sp->phi) < max_phi) {
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
} else { //max 60 degrees roll
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
}
sp->psi += omega * dt;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit) {
sp->psi = heading + delta_limit;
} else if (delta_psi < -delta_limit) {
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}
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;
}
}
