// 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; }