void set_udb_pwIn(uint16_t pwm, int index)
{
#if (NORADIO != 1)
union longww pww;
pww.WW = __builtin_muluu(pwm, PWINSCALE);
#if (LEFTSHIFT > 0)
pww.WW <<= LEFTSHIFT;
#endif
pwm = pww._.W1;
if (FAILSAFE_INPUT_CHANNEL == index)
{
if ((pwm > FAILSAFE_INPUT_MIN) && (pwm < FAILSAFE_INPUT_MAX))
{
failSafePulses++;
}
else
{
noisePulses++;
}
}
#if (FLY_BY_DATALINK_ENABLED == 1)
// It's kind of a bad idea to override the radio mode input
if (MODE_SWITCH_INPUT_CHANNEL == index)
{
udb_pwIn[index] = pwm;
}
else
{
if (udb_pwIn[MODE_SWITCH_INPUT_CHANNEL] < MODE_SWITCH_THRESHOLD_LOW)
{
// if mode is in low mode, use pwm values that came in from external source
udb_pwIn[index] = get_fbdl_pwm(index);
}
else
{
udb_pwIn[index] = pwm;
}
}
#else
if (FAILSAFE_INPUT_CHANNEL == index)
{
//printf("FS: %u %u %u\r\n", pwm, failSafePulses, noisePulses);
#ifdef DEBUG_FAILSAFE_MIN_MAX
{
static uint8_t foo = 0;
if (!(++foo % 32))
{
printf("FS: %u\r\n", pwm);
}
}
#endif // DEBUG_FAILSAFE_MIN_MAX
}
udb_pwIn[index] = pwm;
#endif // FLY_BY_DATALINK_ENABLED
#endif // NOARADIO !=1
}
static int16_t relativeWingLoading(int16_t wingLoad, uint16_t airSpeed)
{
// wingLoad is(2**16)*((wing_load / mass*gravity) / 16)
// stallSpeed is the stall speed in centimeters per second
// airSpeed is the air speed in centimeters per second
uint16_t stallSpeed = STALL_SPEED_CM_SEC;
int16_t result = 0;
uint32_t long_unsigned_accum;
union longww long_signed_accum;
uint16_t unsigned_accum;
// if airspeed is less than or equal to stall speed, return zero
if (airSpeed <= stallSpeed)
{
return 0;
}
long_unsigned_accum = (uint32_t)stallSpeed;
long_unsigned_accum = long_unsigned_accum << 16; //(2**16)*V0, 32 bits unsigned
unsigned_accum = __builtin_divud(long_unsigned_accum, airSpeed); //(2**16)*(V0/V), 16 bits unsigned
long_unsigned_accum = __builtin_muluu(unsigned_accum, unsigned_accum); //(2**32)*(V0/V)**2, 32 bits unsigned
unsigned_accum = long_unsigned_accum >> 16; //(2**16)*(V0/V)**2, 16 bits unsigned
long_signed_accum.WW = __builtin_mulus(unsigned_accum, wingLoad); //(2**32)*(a/16g)*(V0/V)**2, 32 bits unsigned
if (abs(long_signed_accum._.W1) < 4095)
{
long_signed_accum.WW = long_signed_accum.WW << 3; // multiply by 8
result = long_signed_accum._.W1;
}
else
{
if (wingLoad > 0)
{
result = 32767;
}
else
{
result = -32767;
}
}
return result;
}
inline int scale_pwm_out(int channel) {
union longww pww;
pww.WW = __builtin_muluu(udb_pwOut[channel], (unsigned int)(65536 * PWMOUTSCALE / 4));
pww.WW <<= 2;
return pww._.W1;
}
void estLocation(void)
{
static int8_t cog_previous = 64;
static int16_t sog_previous = 0;
static int16_t climb_rate_previous = 0;
static uint16_t velocity_previous = 0;
static int16_t location_previous[] = { 0, 0, 0 };
union longbbbb accum;
union longww accum_velocity;
int8_t cog_circular;
int8_t cog_delta;
int16_t sog_delta;
int16_t climb_rate_delta;
#ifdef USE_EXTENDED_NAV
int32_t location[3];
#else
int16_t location[3];
#endif // USE_EXTENDED_NAV
int16_t location_deltaZ;
struct relative2D location_deltaXY;
struct relative2D velocity_thru_air;
int16_t velocity_thru_airz;
location_plane(&location[0]);
// convert GPS course of 360 degrees to a binary model with 256
accum.WW = __builtin_muluu(COURSEDEG_2_BYTECIR, cog_gps.BB) + 0x00008000;
// re-orientate from compass (clockwise) to maths (anti-clockwise) with 0 degrees in East
cog_circular = -accum.__.B2 + 64;
// compensate for GPS reporting latency.
// The dynamic model of the EM406 and uBlox is not well known.
// However, it seems likely much of it is simply reporting latency.
// This section of the code compensates for reporting latency.
// markw: what is the latency? It doesn't appear numerically or as a comment
// in the following code. Since this method is called at the GPS reporting rate
// it must be assumed to be one reporting interval?
if (dcm_flags._.gps_history_valid)
{
cog_delta = cog_circular - cog_previous;
sog_delta = sog_gps.BB - sog_previous;
climb_rate_delta = climb_gps.BB - climb_rate_previous;
location_deltaXY.x = location[0] - location_previous[0];
location_deltaXY.y = location[1] - location_previous[1];
location_deltaZ = location[2] - location_previous[2];
}
else
{
cog_delta = 0;
sog_delta = 0;
climb_rate_delta = 0;
location_deltaXY.x = location_deltaXY.y = location_deltaZ = 0;
}
dcm_flags._.gps_history_valid = 1;
actual_dir = cog_circular + cog_delta;
cog_previous = cog_circular;
// Note that all these velocities are in centimeters / second
ground_velocity_magnitudeXY = sog_gps.BB + sog_delta;
sog_previous = sog_gps.BB;
GPSvelocity.z = climb_gps.BB + climb_rate_delta;
climb_rate_previous = climb_gps.BB;
accum_velocity.WW = (__builtin_mulss(cosine(actual_dir), ground_velocity_magnitudeXY) << 2) + 0x00008000;
GPSvelocity.x = accum_velocity._.W1;
accum_velocity.WW = (__builtin_mulss(sine(actual_dir), ground_velocity_magnitudeXY) << 2) + 0x00008000;
GPSvelocity.y = accum_velocity._.W1;
rotate_2D(&location_deltaXY, cog_delta); // this is a key step to account for rotation effects!!
GPSlocation.x = location[0] + location_deltaXY.x;
GPSlocation.y = location[1] + location_deltaXY.y;
GPSlocation.z = location[2] + location_deltaZ;
location_previous[0] = location[0];
location_previous[1] = location[1];
location_previous[2] = location[2];
velocity_thru_air.y = GPSvelocity.y - estimatedWind[1];
velocity_thru_air.x = GPSvelocity.x - estimatedWind[0];
velocity_thru_airz = GPSvelocity.z - estimatedWind[2];
#if (HILSIM == 1)
air_speed_3DGPS = hilsim_airspeed.BB; // use Xplane as a pitot
#else
air_speed_3DGPS = vector3_mag(velocity_thru_air.x, velocity_thru_air.y, velocity_thru_airz);
#endif
calculated_heading = rect_to_polar(&velocity_thru_air);
// veclocity_thru_air.x becomes XY air speed as a by product of CORDIC routine in rect_to_polar()
air_speed_magnitudeXY = velocity_thru_air.x; // in cm / sec
#if (GPS_RATE == 4)
forward_acceleration = (air_speed_3DGPS - velocity_previous) << 2; // Ublox enters code 4 times per second
#elif (GPS_RATE == 2)
forward_acceleration = (air_speed_3DGPS - velocity_previous) << 1; // Ublox enters code 2 times per second
#else
forward_acceleration = (air_speed_3DGPS - velocity_previous); // EM406 standard GPS enters code once per second
#endif
velocity_previous = air_speed_3DGPS;
}
