// Calculate the groundspeed in cm/s
int16_t calc_groundspeed(void) // computes (1/2gravity)*( actual_speed^2 - desired_speed^2 )
{
int32_t gndspd2;
gndspd2 = __builtin_mulss ( IMUvelocityx._.W1 , IMUvelocityx._.W1 ) ;
gndspd2 += __builtin_mulss ( IMUvelocityy._.W1 , IMUvelocityy._.W1 ) ;
gndspd2 += __builtin_mulss ( IMUvelocityz._.W1 , IMUvelocityz._.W1 ) ;
return sqrt_long(gndspd2);
}
// Calculate the airspeed.
// Note that this airspeed is a magnitude regardless of direction.
// It is not a calculation of forward airspeed.
int16_t calc_airspeed(void)
{
int16_t speed_component ;
int32_t fwdaspd2;
speed_component = IMUvelocityx._.W1 - estimatedWind[0] ;
fwdaspd2 = __builtin_mulss ( speed_component , speed_component ) ;
speed_component = IMUvelocityy._.W1 - estimatedWind[1] ;
fwdaspd2 += __builtin_mulss ( speed_component , speed_component ) ;
speed_component = IMUvelocityz._.W1 - estimatedWind[2] ;
fwdaspd2 += __builtin_mulss ( speed_component , speed_component ) ;
airspeed = sqrt_long(fwdaspd2);
return airspeed;
}
static int16_t logo_value_for_identifier(uint8_t ident)
{
if (ident > 0 && ident <= NUM_INPUTS)
{
return udb_pwIn[(int16_t)ident]; // 2000 - 4000
}
switch (ident) {
case DIST_TO_HOME: // in m
return sqrt_long(IMUlocationx._.W1 * (int32_t)IMUlocationx._.W1 + IMUlocationy._.W1 * (int32_t)IMUlocationy._.W1);
case DIST_TO_GOAL: // in m
return tofinish_line;
case ALT: // in m
return IMUlocationz._.W1;
case CURRENT_ANGLE: // in degrees. 0-359 (clockwise, 0=North)
return get_current_angle();
case ANGLE_TO_HOME: // in degrees. 0-359 (clockwise, 0=North)
{
int16_t angle = get_angle_to_point(0,0);
angle += 180;
if (angle < 0) angle += 360;
if (angle > 360) angle -= 360;
return angle;
}
case ANGLE_TO_GOAL: // in degrees. 0-359 (clockwise, 0=North)
return get_angle_to_point(IMUlocationx._.W1, IMUlocationy._.W1);
case REL_ANGLE_TO_HOME: // in degrees. -180-179 (0=heading directly towards Home. Home to the right of the nose of the plane is positive)
{
int16_t angle = get_angle_to_point(0,0);
angle = get_current_angle() - angle;
angle += 180;
if (angle < -180) angle += 360;
if (angle >= 180) angle -= 360;
return -angle;
}
case REL_ANGLE_TO_GOAL: // in degrees. -180-179 (0=heading directly towards Goal. Goal to the right of the nose of the plane is positive)
{
int16_t angle = get_current_angle() - get_angle_to_point(IMUlocationx._.W1, IMUlocationy._.W1);
if (angle < -180) angle += 360;
if (angle >= 180) angle -= 360;
return -angle;
}
case GROUND_SPEED: // in cm/s
return ground_velocity_magnitudeXY;
case AIR_SPEED: // in cm/s
return air_speed_magnitudeXY;
case AIR_SPEED_Z: // in cm/s
return IMUvelocityz._.W1 - estimatedWind[2];
case WIND_SPEED: // in cm/s
return sqrt_long(estimatedWind[0] * (int32_t)estimatedWind[0] + estimatedWind[1] * (int32_t)estimatedWind[1]);
case WIND_SPEED_X: // in cm/s
return estimatedWind[0];
case WIND_SPEED_Y: // in cm/s
return estimatedWind[1];
case WIND_SPEED_Z: // in cm/s
return estimatedWind[2];
case WIND_FROM_ANGLE: // wind from in degrees 0-359, 0 = North
{
int16_t angle = get_angle_to_point(estimatedWind[0], estimatedWind[1]);
if (angle < 0) angle += 360;
if (angle >= 360) angle -= 360;
return angle;
}
case PARAM:
{
int16_t ind = get_current_stack_parameter_frame_index();
return logoStack[ind].arg;
}
}
return 0;
}
