void hoverPitchCntrl(void)
{
union longww pitchAccum;
if (flags._.pitch_feedback)
{
pitchAccum.WW = (__builtin_mulss(-rmat[7] , omegagyro[0])
- __builtin_mulss(rmat[6] , omegagyro[1])) << 1;
pitchrate = pitchAccum._.W1;
int16_t elevInput = (udb_flags._.radio_on == 1) ?
REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) : 0;
int16_t manualPitchOffset = elevInput * (int16_t)(RMAX/600);
int32_t pitchToWP;
if (flags._.GPS_steering)
{
pitchToWP = (tofinish_line > HOVER_NAV_MAX_PITCH_RADIUS) ?
HOVERPTOWP : (HOVERPTOWP / HOVER_NAV_MAX_PITCH_RADIUS * tofinish_line);
}
else
{
pitchToWP = 0;
}
pitchAccum.WW = __builtin_mulsu(rmat[8] + HOVERPOFFSET - pitchToWP + manualPitchOffset , hoverpitchgain)
+ __builtin_mulus(hoverpitchkd , pitchrate);
}
else
{
pitchAccum.WW = 0;
}
pitch_control = (int32_t)pitchAccum._.W1;
}
// Called at 40 Hz, before sending servo pulses
void dcm_servo_callback_prepare_outputs(void)
{
if (!dcm_flags._.calib_finished)
{
udb_pwOut[ROLL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[PITCH_OUTPUT_CHANNEL] = 3000;
udb_pwOut[YAW_OUTPUT_CHANNEL] = 3000;
}
else
{
union longww accum;
accum.WW = __builtin_mulss(rmat[6], 4000);
udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(rmat[7], 4000);
udb_pwOut[PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(rmat[1], 4000);
udb_pwOut[YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
}
// Serial output at 10Hz
if ((udb_heartbeat_counter % (HEARTBEAT_HZ/10)) == 0)
{
if (dcm_flags._.calib_finished)
{
send_debug_line();
}
}
}
// 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);
}
void normalRollCntrl(void)
{
union longww rollAccum = { 0 } ;
union longww gyroRollFeedback ;
union longww gyroYawFeedback ;
fractional rmat6 ;
fractional omegaAccum2 ;
if ( !canStabilizeInverted() || !desired_behavior._.inverted )
{
rmat6 = rmat[6] ;
omegaAccum2 = omegaAccum[2] ;
}
else
{
rmat6 = -rmat[6] ;
omegaAccum2 = -omegaAccum[2] ;
}
#ifdef TestGains
flags._.GPS_steering = 0 ; // turn off navigation
#endif
if ( AILERON_NAVIGATION && flags._.GPS_steering )
{
rollAccum._.W1 = determine_navigation_deflection( 'a' ) ;
}
#ifdef TestGains
flags._.pitch_feedback = 1 ;
#endif
if ( ROLL_STABILIZATION_AILERONS && flags._.pitch_feedback )
{
gyroRollFeedback.WW = __builtin_mulss( rollkd , omegaAccum[1] ) ;
rollAccum.WW += __builtin_mulss( rmat6 , rollkp ) ;
}
else
{
gyroRollFeedback.WW = 0 ;
}
if ( YAW_STABILIZATION_AILERON && flags._.pitch_feedback )
{
gyroYawFeedback.WW = __builtin_mulss( yawkdail, omegaAccum2 ) ;
}
else
{
gyroYawFeedback.WW = 0 ;
}
roll_control = (long)rollAccum._.W1 - (long)gyroRollFeedback._.W1 - (long)gyroYawFeedback._.W1 ;
// Servo reversing is handled in servoMix.c
return ;
}
void hoverRollCntrl(void)
{
int rollNavDeflection ;
union longww gyroRollFeedback ;
if ( flags._.pitch_feedback )
{
if ( AILERON_NAVIGATION && flags._.GPS_steering )
{
rollNavDeflection = (tofinish_line > HOVER_NAV_MAX_PITCH_RADIUS/2) ? determine_navigation_deflection( 'h' ) : 0 ;
}
else
{
rollNavDeflection = 0 ;
}
gyroRollFeedback.WW = __builtin_mulss( hoverrollkd , omegaAccum[1] ) ;
}
else
{
rollNavDeflection = 0 ;
gyroRollFeedback.WW = 0 ;
}
roll_control = rollNavDeflection -(long)gyroRollFeedback._.W1 ;
return ;
}
static int16_t wingLift(int16_t X, int16_t Z, int16_t XoverZ)
{
// compute (1/16*(Z + X *(X/Z)))*2^16
union longww lift;
union longww accum;
// compute the first term. X is already scaled by 1/4, so apply another 1/4:
accum._.W1 = Z;
accum._.W0 = 0;
accum.WW = accum.WW >> 2; // divide by 4
lift.WW = accum.WW; // Z/16
// compute the second term in result, X *(X/Z)
// X has been divided by 4
// X/Z has been divided by 8
// so we need to multiply by 2
accum.WW = __builtin_mulss(X, XoverZ);
accum.WW += accum.WW; // multiply by 2
// add the two terms
lift.WW += accum.WW;
return lift._.W1;
}
// 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;
}
// Calculate the expected climb rate depending on a throttle setting and airspeed
// glide_ratio. rate and airspeed in cm/s. Descent rate is positive falling.
// Returned value is positive rising in cm/s
int16_t feedforward_climb_rate(fractional throttle, int16_t glide_descent_rate, int16_t airspeed)
{
// TODO - Correct the assumption that scale changes with descent rate.
int16_t rateScale = (MAX_THROTTLE_CLIMB_RATE * 100) + glide_descent_rate;
union longww temp;
temp.WW = __builtin_mulss( throttle, rateScale);
temp.WW <<= 2;
temp._.W1 -= glide_descent_rate;
return temp._.W1;
}
void AggiornaDatiVelocita(void)
{
long tmp;
/*
* RPMruota = RPMmotore * ( RapportoRiduzioneMotore:RapportoRiduzioneAsse)
*
* MotoreX.I_MotorAxelSpeed : RPM asse motore, un motore "standard" ruota nel range 0-10000RPM, il dato è un intero.
* MotoreX.I_GearAxelSpeed : RPM uscita riduttore, considerando un rapporto riduzione minimo di 1:100 per motori da
* 10000RPM e di 1:10 per motori sotto i 3000RPM il dato varierà al max nel range 0-300
* **** Il dato GearAxelSpeed è un intero moltiplicato per un fattore 100 ******
* **** Range 0-30000 con due cifre decimali in visualizzazione ******
* I calcoli vengono effettuati passando per un LONG per evitare problemi di overflow nelle moltiplicazioni tra interi
*/
tmp = __builtin_mulss(Motore1.I_MotorAxelSpeed, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT]);
tmp *= 100; // Per ottenere I_GearAxelSpeed riscalato di un fattore 100
tmp = __builtin_divsd(tmp, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT]);
Motore1.I_GearAxelSpeed = (int) tmp;
tmp = __builtin_mulss(Motore2.I_MotorAxelSpeed, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT]);
tmp *= 100; // Per ottenere I_GearAxelSpeed riscalato di un fattore 100
tmp = __builtin_divsd(tmp, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT]);
Motore2.I_GearAxelSpeed = (int) tmp;
/* Calcolo la velocità di rotazione delle ruote espressa in centesimi di radianti al secondo
* MotoreX.L_WheelSpeed deriva da I_GearAxelSpeed ed è ritornato alla GUI moltiplicato per 100
*
* EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_xxxx : Raggio espresso in decimi di millimetro 360 = 3,6Cm
* */
tmp = __builtin_mulss(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT], Motore1.I_GearAxelSpeed);
tmp *= TWOPI;
tmp = __builtin_divsd(tmp, 6000);
Motore1.L_WheelSpeed = tmp;
tmp = __builtin_mulss(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT], Motore2.I_GearAxelSpeed);
tmp *= TWOPI;
tmp = __builtin_divsd(tmp, 6000);
Motore2.L_WheelSpeed = tmp;
}
struct relative3D dcm_absolute_to_relative(struct waypoint3D absolute)
{
struct relative3D rel ;
union longww accum_nav ;
rel.z = absolute.z ;
rel.y = (absolute.y - lat_origin.WW)/90 ; // in meters
accum_nav.WW = ((absolute.x - long_origin.WW)/90) ; // in meters
accum_nav.WW = ((__builtin_mulss ( cos_lat , accum_nav._.W0 )<<2)) ;
rel.x = accum_nav._.W1 ;
return rel ;
}
void dcm_set_origin_location(int32_t o_lon, int32_t o_lat, int32_t o_alt)
{
union longbbbb accum_nav;
unsigned char lat_cir;
lat_origin.WW = o_lat;
lon_origin.WW = o_lon;
alt_origin.WW = o_alt;
// scale the low 16 bits of latitude from GPS units to gentleNAV units
accum_nav.WW = __builtin_mulss(LONGDEG_2_BYTECIR, lat_origin._.W1);
lat_cir = accum_nav.__.B2; // effectively divides by 256
// estimate the cosine of the latitude, which is used later computing desired course
cos_lat = cosine(lat_cir);
}
void dcm_set_origin_location(long o_long, long o_lat, long o_alt)
{
union longbbbb accum_nav ;
lat_origin.WW = o_lat ;
long_origin.WW = o_long ;
alt_origin.WW = o_alt;
// scale the latitude from GPS units to gentleNAV units
accum_nav.WW = __builtin_mulss( LONGDEG_2_BYTECIR , lat_origin._.W1 ) ;
lat_cir = accum_nav.__.B2 ;
// estimate the cosine of the latitude, which is used later computing desired course
cos_lat = cosine ( lat_cir ) ;
return ;
}
// Called at HEARTBEAT_HZ, before sending servo pulses
void udb_heartbeat_callback(void)
{
if (calib_countdown) {
udb_pwOut[ROLL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[PITCH_OUTPUT_CHANNEL] = 3000;
udb_pwOut[YAW_OUTPUT_CHANNEL] = 3000;
udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = 3000;
} else if (eepromSuccess == 0 && eepromFailureFlashCount) {
// eeprom failure!
DPRINT("eeprom failure!\r\n");
if (udb_heartbeat_counter % 6 == 0) {
udb_led_toggle(LED_RED);
udb_led_toggle(LED_GREEN);
udb_led_toggle(LED_BLUE);
udb_led_toggle(LED_ORANGE);
eepromFailureFlashCount--;
}
} else {
union longww accum;
accum.WW = __builtin_mulss(y_rate, 4000);
udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(x_rate, 4000);
udb_pwOut[PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(z_rate, 4000);
udb_pwOut[YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(x_accel, 4000);
udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(y_accel, 4000);
udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(z_accel, 4000);
udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
if ((udb_heartbeat_counter / 600) % 2 == 0) {
led_on(LED_RED);
((abs(udb_pwOut[ROLL_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
((abs(udb_pwOut[PITCH_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
((abs(udb_pwOut[YAW_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
} else {
led_off(LED_RED);
((abs(udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
((abs(udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
((abs(udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
}
}
}
int main(void)
{
//int i = sin90[1];
int z;
int a = 10, b = 5, c;
//_Fract fr;
//fr = 0.6;
unsigned int i = 0xFFFF;
float y = 0.5;
unsigned int x;
x = (unsigned int)(y * i);
x++;
z = MPY(10000, 5000);
x++;
c = __builtin_mulss(a, b);
c = a * b;
return z;
}
void dead_reckon(void)
{
int16_t air_speed_x, air_speed_y, air_speed_z;
union longww accum;
union longww energy;
if (dcm_flags._.dead_reckon_enable == 1) // wait for startup of GPS
{
// integrate the accelerometers to update IMU velocity
IMUintegralAccelerationx.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[0]);
IMUintegralAccelerationy.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[1]);
IMUintegralAccelerationz.WW += __builtin_mulss(((int16_t)(ACCEL2DELTAV)), accelEarth[2]);
// integrate IMU velocity to update the IMU location
IMUlocationx.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationx._.W1)>>4);
IMUlocationy.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationy._.W1)>>4);
IMUlocationz.WW += (__builtin_mulss(((int16_t)(VELOCITY2LOCATION)), IMUintegralAccelerationz._.W1)>>4);
if (dead_reckon_clock > 0)
// apply drift adjustments only while valid GPS data is in force.
// This is done with a countdown clock that gets reset each time new data comes in.
{
dead_reckon_clock --;
IMUintegralAccelerationx.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[0]);
IMUintegralAccelerationy.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[1]);
IMUintegralAccelerationz.WW += __builtin_mulss(DR_FILTER_GAIN, velocityErrorEarth[2]);
IMUlocationx.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[0]);
IMUlocationy.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[1]);
IMUlocationz.WW += __builtin_mulss(DR_FILTER_GAIN, locationErrorEarth[2]);
IMUvelocityx.WW = IMUintegralAccelerationx.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[0]);
IMUvelocityy.WW = IMUintegralAccelerationy.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[1]);
IMUvelocityz.WW = IMUintegralAccelerationz.WW +
__builtin_mulus(ONE_OVER_TAU, 100*locationErrorEarth[2]);
}
else // GPS has gotten disconnected
{
yaw_drift_reset();
dcm_flags._.gps_history_valid = 0; // restart GPS history variables
IMUvelocityx.WW = IMUintegralAccelerationx.WW;
IMUvelocityy.WW = IMUintegralAccelerationy.WW;
IMUvelocityz.WW = IMUintegralAccelerationz.WW;
}
if (gps_nav_valid() && (dcm_flags._.reckon_req == 1))
{
// compute error indications and restart the dead reckoning clock to apply them
dcm_flags._.reckon_req = 0;
dead_reckon_clock = DR_PERIOD;
locationErrorEarth[0] = GPSlocation.x - IMUlocationx._.W1;
locationErrorEarth[1] = GPSlocation.y - IMUlocationy._.W1;
locationErrorEarth[2] = GPSlocation.z - IMUlocationz._.W1;
velocityErrorEarth[0] = GPSvelocity.x - IMUintegralAccelerationx._.W1;
velocityErrorEarth[1] = GPSvelocity.y - IMUintegralAccelerationy._.W1;
velocityErrorEarth[2] = GPSvelocity.z - IMUintegralAccelerationz._.W1;
}
}
void normalPitchCntrl(void)
{
union longww pitchAccum;
int16_t rtlkick;
// int16_t aspd_adj;
// fractional aspd_err, aspd_diff;
fractional rmat6;
fractional rmat7;
fractional rmat8;
if (!canStabilizeInverted() || current_orientation != F_INVERTED)
{
rmat6 = rmat[6];
rmat7 = rmat[7];
rmat8 = rmat[8];
}
else
{
rmat6 = -rmat[6];
rmat7 = -rmat[7];
rmat8 = -rmat[8];
pitchAltitudeAdjust = -pitchAltitudeAdjust - INVNPITCH;
}
navElevMix = 0;
if (flags._.pitch_feedback)
{
if (RUDDER_OUTPUT_CHANNEL != CHANNEL_UNUSED && RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED) {
pitchAccum.WW = __builtin_mulsu(rmat6 , rudderElevMixGain) << 1;
pitchAccum.WW = __builtin_mulss(pitchAccum._.W1 ,
REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, udb_pwTrim[RUDDER_INPUT_CHANNEL] - udb_pwOut[RUDDER_OUTPUT_CHANNEL])) << 3;
navElevMix += pitchAccum._.W1;
}
pitchAccum.WW = __builtin_mulsu(rmat6 , rollElevMixGain) << 1;
pitchAccum.WW = __builtin_mulss(pitchAccum._.W1 , rmat[6]) >> 3;
navElevMix += pitchAccum._.W1;
}
pitchAccum.WW = (__builtin_mulss(rmat8 , omegagyro[0])
- __builtin_mulss(rmat6 , omegagyro[2])) << 1;
pitchrate = pitchAccum._.W1;
if (!udb_flags._.radio_on && flags._.GPS_steering)
{
rtlkick = RTLKICK;
}
else
{
rtlkick = 0;
}
#if(GLIDE_AIRSPEED_CONTROL == 1)
fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
#endif
if (PITCH_STABILIZATION && flags._.pitch_feedback)
{
#if(GLIDE_AIRSPEED_CONTROL == 1)
pitchAccum.WW = __builtin_mulsu(rmat7 - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust, pitchgain)
+ __builtin_mulus(pitchkd , pitchrate);
#else
pitchAccum.WW = __builtin_mulsu(rmat7 - rtlkick + pitchAltitudeAdjust, pitchgain)
+ __builtin_mulus(pitchkd , pitchrate);
#endif
}
else
{
pitchAccum.WW = 0;
}
pitch_control = (int32_t)pitchAccum._.W1 + navElevMix;
}
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;
}
void motor_get_vind(int * u) {
u[0] =__builtin_divsd(__builtin_mulss(vind_filtered[0],64), settings.mot256[0]);
u[1] =__builtin_divsd(__builtin_mulss(vind_filtered[1],64), settings.mot256[1]);
}
void osd_update_values( void )
{
switch (osd_phase)
{
case 0:
{
#if (OSD_LOC_ALTITUDE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_ALTITUDE+1) ;
osd_spi_write_number(IMUlocationz._.W1, 0, 0, NUM_FLAG_SIGNED, 0, 0) ; // Altitude
#endif
#if (OSD_LOC_CPU_LOAD != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_CPU_LOAD+1) ;
osd_spi_write_number(udb_cpu_load(), 3, 0, 0, 0, 0) ; // CPU
#endif
#if (OSD_LOC_VARIO_NUM != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_VARIO_NUM) ;
osd_spi_write_number(IMUvelocityz._.W1, 0, 0, NUM_FLAG_SIGNED, 0, 0) ; // Variometer
#endif
#if (OSD_LOC_VARIO_ARROW != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_VARIO_ARROW) ;
if (IMUvelocityz._.W1 <= -VARIOMETER_HIGH)
osd_spi_write(0x7, 0xD4) ; // Variometer down fast
else if (IMUvelocityz._.W1 <= -VARIOMETER_LOW)
osd_spi_write(0x7, 0xD2) ; // Variometer down slowly
else if (IMUvelocityz._.W1 < VARIOMETER_LOW)
osd_spi_write(0x7, 0x00) ; // Variometer flat (was 0xD0)
else if (IMUvelocityz._.W1 < VARIOMETER_HIGH)
osd_spi_write(0x7, 0xD1) ; // Variometer up slowly
else if (IMUvelocityz._.W1 >= VARIOMETER_HIGH)
osd_spi_write(0x7, 0xD3) ; // Variometer up fast
#endif
#if (OSD_LOC_AP_MODE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AP_MODE) ;
if (!flags._.pitch_feedback)
osd_spi_write(0x7, 0x97) ; // M : Manual Mode
else if (!flags._.GPS_steering)
osd_spi_write(0x7, 0x9D) ; // S : Stabilized Mode
else if (udb_flags._.radio_on && !flags._.rtl_hold)
osd_spi_write(0x7, 0xA1) ; // W : Waypoint Mode
else if (flags._.rtl_hold && udb_flags._.radio_on)
osd_spi_write(0x7, 0x92) ; // H : RTL Hold, has signal
else
osd_spi_write(0x7, 0x9C) ; // R : RTL Mode, lost signal
#endif
break ;
}
case 1:
{
signed char dir_to_goal ;
int dist_to_goal ;
struct relative2D curHeading ;
curHeading.x = -rmat[1] ;
curHeading.y = rmat[4] ;
signed char earth_yaw = rect_to_polar(&curHeading) ;// 0-255 (0=East, ccw)
if (flags._.GPS_steering)
{
dir_to_goal = desired_dir - earth_yaw ;
dist_to_goal = abs(tofinish_line) ;
}
else
{
struct relative2D toGoal ;
toGoal.x = 0 - IMUlocationx._.W1 ;
toGoal.y = 0 - IMUlocationy._.W1 ;
dir_to_goal = rect_to_polar ( &toGoal ) - earth_yaw ;
dist_to_goal = toGoal.x ;
}
#if (OSD_LOC_DIST_TO_GOAL != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_DIST_TO_GOAL+1) ;
osd_spi_write_number(dist_to_goal, 0, 0, 0, 0, 0) ; // Distance to wp/home
#endif
#if (OSD_LOC_ARROW_TO_GOAL != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_ARROW_TO_GOAL) ;
osd_write_arrow(dir_to_goal) ;
#endif
#if (OSD_LOC_HEADING_NUM != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_HEADING_NUM+1) ;
// earth_yaw // 0-255 (0=East, ccw)
int angle = (earth_yaw * 180 + 64) >> 7 ; // 0-359 (0=East, ccw)
angle = -angle + 90; // 0-359 (0=North, clockwise)
if (angle < 0) angle += 360 ; // 0-359 (0=North, clockwise)
osd_spi_write_number(angle, 3, 0, NUM_FLAG_ZERO_PADDED, 0, 0) ; // heading
#endif
#if (OSD_LOC_HEADING_CARDINAL != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_HEADING_CARDINAL) ;
osd_spi_write_string(heading_strings[((unsigned char)(earth_yaw+8))>>4]) ; // heading
#endif
#if (OSD_LOC_VERTICAL_ANGLE_HOME != OSD_LOC_DISABLED)
// Vertical angle from origin to plane
int verticalAngle = 0 ;
if (dist_to_goal != 0)
{
struct relative2D componentsToPlane ;
componentsToPlane.x = dist_to_goal ;
componentsToPlane.y = IMUlocationz._.W1 ;
verticalAngle = rect_to_polar(&componentsToPlane) ; // binary angle (0 - 256 = 360 degrees)
verticalAngle = (verticalAngle * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
}
osd_spi_write_location(OSD_LOC_VERTICAL_ANGLE_HOME) ;
osd_spi_write_number(verticalAngle, 0, 0, NUM_FLAG_SIGNED, 0, 0x4D); // Footer: Degree symbol
#endif
#if (OSD_LOC_ROLL_RATE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_ROLL_RATE) ;
osd_spi_write_number(abs(omegagyro[1])/DEGPERSEC, 3, 0, 0, 0, 0) ; // roll rate in degrees/sec/sec
#endif
#if (OSD_LOC_PITCH_RATE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_PITCH_RATE) ;
osd_spi_write_number(abs(omegagyro[0])/DEGPERSEC, 3, 0, 0, 0, 0) ; // pitch rate in degrees/sec/sec
#endif
#if (OSD_LOC_YAW_RATE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_YAW_RATE) ;
osd_spi_write_number(abs(omegagyro[2])/DEGPERSEC, 3, 0, 0, 0, 0) ; // yaw rate in degrees/sec/sec
#endif
#if (ANALOG_CURRENT_INPUT_CHANNEL != CHANNEL_UNUSED)
#if (OSD_LOC_BATT_CURRENT != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_BATT_CURRENT) ;
osd_spi_write_number(battery_current._.W1, 3, 1, 0, 0, 0xB4) ; // tenths of Amps being used right now
#endif
#if (OSD_LOC_BATT_USED != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_BATT_USED) ;
osd_spi_write_number(battery_mAh_used._.W1, 4, 0, 0, 0, 0xB7) ; // mAh used so far
#endif
#endif
#if (ANALOG_VOLTAGE_INPUT_CHANNEL != CHANNEL_UNUSED)
#if (OSD_LOC_BATT_VOLTAGE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_BATT_VOLTAGE) ;
osd_spi_write_number(battery_voltage._.W1, 3, 1, 0, 0, 0xA0) ; // tenths of Volts
#endif
#endif
#if (ANALOG_RSSI_INPUT_CHANNEL != CHANNEL_UNUSED)
#if (OSD_LOC_RSSI != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_RSSI) ;
osd_spi_write_number(rc_signal_strength, 3, 0, 0, 0, 0xB3) ; // RC Receiver signal strength as 0-100%
#endif
#endif
break ;
}
case 2:
{
#if (OSD_SHOW_HORIZON == 1)
osd_update_horizon() ;
#endif
break ;
}
case 3:
{
#if (OSD_LOC_AIR_SPEED_M_S != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AIR_SPEED_M_S) ;
osd_spi_write_number(air_speed_3DIMU/100, 3, 0, 0, 0, 0) ; // speed in m/s
#endif
#if (OSD_LOC_AIR_SPEED_MI_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AIR_SPEED_MI_HR) ;
osd_spi_write_number(air_speed_3DIMU/45, 3, 0, 0, 0, 0) ; // speed in mi/hr
#endif
#if (OSD_LOC_AIR_SPEED_KM_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AIR_SPEED_KM_HR) ;
osd_spi_write_number(air_speed_3DIMU/28, 3, 0, 0, 0, 0) ; // speed in km/hr
#endif
#if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED || OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED || OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED)
unsigned int ground_speed_3DIMU =
vector3_mag ( IMUvelocityx._.W1 ,
IMUvelocityy._.W1 ,
IMUvelocityz._.W1 ) ;
#endif
#if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GROUND_SPEED_M_S) ;
osd_spi_write_number(ground_speed_3DIMU/100, 3, 0, 0, 0, 0) ; // speed in m/s
#endif
#if (OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GROUND_SPEED_MI_HR) ;
osd_spi_write_number(ground_speed_3DIMU/45, 3, 0, 0, 0, 0) ; // speed in mi/hr
#endif
#if (OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GROUND_SPEED_KM_HR) ;
osd_spi_write_number(ground_speed_3DIMU/28, 3, 0, 0, 0, 0) ; // speed in km/hr
#endif
#if (OSD_LOC_VERTICAL_ACCEL != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_VERTICAL_ACCEL) ;
union longww gravity_z ;
gravity_z.WW = __builtin_mulss(GRAVITY, rmat[8]) << 2;
osd_spi_write_number((ZACCEL_VALUE - gravity_z._.W1)/(100*ACCELSCALE), 3, 0, NUM_FLAG_SIGNED, 0, 0) ; // vertical acceleration rate in units of m/sec/sec
#endif
#if (OSD_LOC_VERTICAL_WIND_SPEED != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_VERTICAL_WIND_SPEED) ;
osd_spi_write_number(estimatedWind[2]/10, 4, 1, NUM_FLAG_SIGNED, 0, 0) ; // vertical wind speed in m/s
#endif
#if (OSD_LOC_TOTAL_ENERGY != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_TOTAL_ENERGY) ;
osd_spi_write_number(total_energy, 4, 0, NUM_FLAG_SIGNED, 0, 0) ; // total energy in meters above the origin
#endif
#if (OSD_AUTO_HIDE_GPS == 1)
boolean showGPS = ( IMUlocationz._.W1 < 20 || ground_velocity_magnitudeXY < 150) ;
#else
boolean showGPS = 1 ;
#endif
#if (OSD_LOC_NUM_SATS != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_NUM_SATS) ;
if (showGPS)
{
osd_spi_write_number(svs, 0, 0, 0, 0xEB, 0) ; // Num satelites locked, with SatDish icon header
}
else
{
osd_spi_erase_chars(3) ;
}
#endif
#if (OSD_LOC_GPS_LAT != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GPS_LAT) ;
if (showGPS)
{
osd_spi_write_number(labs(lat_gps.WW/10), 8, 6, 0, 0, (lat_gps.WW >= 0) ? 0x98 : 0x9D) ; // Footer: N/S
}
else
{
osd_spi_erase_chars(9) ;
}
#endif
#if (OSD_LOC_GPS_LONG != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GPS_LONG) ;
if (showGPS)
{
osd_spi_write_number(labs(long_gps.WW/10), 9, 6, 0, 0, (long_gps.WW >= 0) ? 0x8F : 0xA1) ; // Footer: E/W
}
else
{
osd_spi_erase_chars(10) ;
}
#endif
break ;
}
}
return ;
}
static boolean process_one_instruction(struct logoInstructionDef instr)
{
if (instr.use_param)
{
// Use the subroutine's parameter instead of the instruction's arg value
int16_t ind = get_current_stack_parameter_frame_index();
instr.arg = logoStack[ind].arg;
}
switch (instr.cmd)
{
case 1: // Repeat
switch (instr.subcmd)
{
case 0: // Repeat N times (or forever if N == -1)
if (logoStackIndex < LOGO_STACK_DEPTH-1)
{
logoStackIndex++;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_REPEAT;
logoStack[logoStackIndex].arg = instr.arg;
logoStack[logoStackIndex].returnInstructionIndex = instructionIndex;
}
break;
case 1: // End
if (logoStackIndex > 0)
{
if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_REPEAT)
{
// END REPEAT
if (logoStack[logoStackIndex].arg > 1 || logoStack[logoStackIndex].arg == -1)
{
if (logoStack[logoStackIndex].arg != -1)
{
logoStack[logoStackIndex].arg--;
}
instructionIndex = logoStack[logoStackIndex].returnInstructionIndex;
}
else
{
logoStackIndex--;
}
}
else if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_SUBROUTINE)
{
// END SUBROUTINE
instructionIndex = logoStack[logoStackIndex].returnInstructionIndex;
logoStackIndex--;
if (logoStackIndex < interruptStackBase)
{
interruptStackBase = 0;
instructionsProcessed = MAX_INSTRUCTIONS_PER_CYCLE; // stop processing instructions after finishing interrupt
}
}
else if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_IF)
{
// Do nothing at the end of an IF block
logoStackIndex--;
}
}
else
{
// Extra, unmatched END goes back to the start of the program
instructionIndex = logoStack[0].returnInstructionIndex;
logoStackIndex = 0;
interruptStackBase = 0;
}
break;
case 3: // Else
if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_IF)
{
instructionIndex = find_end_of_current_if_block();
logoStackIndex--;
}
break;
case 2: // To (define a function)
{
// Shouldn't ever run these lines.
// If we do get here, restart from the top of the logo program.
instructionIndex = logoStack[0].returnInstructionIndex;
logoStackIndex = 0;
interruptStackBase = 0;
}
break;
}
break;
case 10: // Exec (reset the stack and then call a subroutine)
instructionIndex = find_start_of_subroutine(instr.subcmd);
logoStack[0].returnInstructionIndex = instructionIndex;
logoStackIndex = 0;
interruptStackBase = 0;
break;
case 2: // Do (call a subroutine)
if (logoStackIndex < LOGO_STACK_DEPTH-1)
{
logoStackIndex++;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_SUBROUTINE;
logoStack[logoStackIndex].arg = instr.arg;
logoStack[logoStackIndex].returnInstructionIndex = instructionIndex;
}
instructionIndex = find_start_of_subroutine(instr.subcmd);
break;
case 3: // Forward/Back
switch (instr.subcmd)
{
case 0: // Forward
{
int16_t cangle = turtleAngles[currentTurtle]; // 0-359 (clockwise, 0=North)
int8_t b_angle = (cangle * 182 + 128) >> 8; // 0-255 (clockwise, 0=North)
b_angle = -b_angle - 64; // 0-255 (ccw, 0=East)
turtleLocations[currentTurtle].x.WW += (__builtin_mulss(-cosine(b_angle), instr.arg) << 2);
turtleLocations[currentTurtle].y.WW += (__builtin_mulss(-sine(b_angle), instr.arg) << 2);
}
break;
}
break;
case 4: // Rotate
switch (instr.subcmd)
{
case 0: // Right
{
int16_t angle = turtleAngles[currentTurtle] + instr.arg;
while (angle < 0) angle += 360;
angle = angle % 360;
turtleAngles[currentTurtle] = angle;
break;
}
case 1: // Set Angle
turtleAngles[currentTurtle] = instr.arg;
break;
case 2: // Use current angle
{
turtleAngles[currentTurtle] = get_current_angle();
break;
}
case 3: // Use angle to goal
{
turtleAngles[currentTurtle] = get_angle_to_point(IMUlocationx._.W1, IMUlocationy._.W1);
break;
}
}
break;
case 5: // MV/SET location - X, Y, and Z
switch (instr.subcmd)
{
case 0: // Move X
turtleLocations[currentTurtle].x._.W1 += instr.arg;
break;
case 1: // Set X location
turtleLocations[currentTurtle].x._.W0 = 0;
turtleLocations[currentTurtle].x._.W1 = instr.arg;
break;
case 2: // Move Y
turtleLocations[currentTurtle].y._.W1 += instr.arg;
break;
case 3: // Set Y location
turtleLocations[currentTurtle].y._.W0 = 0;
turtleLocations[currentTurtle].y._.W1 = instr.arg;
break;
case 4: // Move Z
turtleLocations[currentTurtle].z += instr.arg;
break;
case 5: // Set Z location
turtleLocations[currentTurtle].z = instr.arg;
break;
case 6: // Use current position (for x and y)
turtleLocations[currentTurtle].x._.W0 = 0;
turtleLocations[currentTurtle].x._.W1 = IMUlocationx._.W1;
turtleLocations[currentTurtle].y._.W0 = 0;
turtleLocations[currentTurtle].y._.W1 = IMUlocationy._.W1;
break;
case 7: // HOME
turtleAngles[currentTurtle] = 0;
turtleLocations[currentTurtle].x.WW = 0;
turtleLocations[currentTurtle].y.WW = 0;
break;
case 8: // Absolute set high value
absoluteHighWord = instr.arg;
break;
case 9: // Absolute set low X value
{
absoluteXLong._.W1 = absoluteHighWord;
absoluteXLong._.W0 = instr.arg;
break;
}
case 10: // Absolute set low Y value
{
struct waypoint3D wp;
struct relative3D rel;
union longww absoluteYLong;
absoluteYLong._.W1 = absoluteHighWord;
absoluteYLong._.W0 = instr.arg;
wp.x = absoluteXLong.WW;
wp.y = absoluteYLong.WW;
wp.z = 0;
rel = dcm_absolute_to_relative(wp);
turtleLocations[currentTurtle].x._.W0 = 0;
turtleLocations[currentTurtle].x._.W1 = rel.x;
turtleLocations[currentTurtle].y._.W0 = 0;
turtleLocations[currentTurtle].y._.W1 = rel.y;
break;
}
}
break;
case 6: // Flags
switch (instr.subcmd)
{
case 0: // Flag On
setBehavior(desired_behavior.W | instr.arg);
break;
case 1: // Flag Off
setBehavior(desired_behavior.W & ~instr.arg);
break;
case 2: // Flag Toggle
setBehavior(desired_behavior.W ^ instr.arg);
break;
}
break;
case 7: // Pen Up/Down
switch (instr.subcmd)
{
case 0: // Pen Up
penState++;
break;
case 1: // Pen Down
if (penState > 0)
penState--;
break;
case 2: // Pen Toggle
penState = (penState == 0);
if (penState == 0) instr.do_fly = 1; // Set the Fly Flag
break;
}
break;
case 8: // Set Turtle (choose plane or camera target)
currentTurtle = (instr.arg == CAMERA) ? CAMERA : PLANE;
break;
case 9: // Modify PARAM
switch (instr.subcmd)
{
case 0: // Set param
{
int16_t ind = get_current_stack_parameter_frame_index();
logoStack[ind].arg = instr.arg;
break;
}
case 1: // Add to param
{
int16_t ind = get_current_stack_parameter_frame_index();
logoStack[ind].arg += instr.arg;
break;
}
case 2: // Multiply param
{
int16_t ind = get_current_stack_parameter_frame_index();
logoStack[ind].arg *= instr.arg;
break;
}
case 3: // Divide param
{
int16_t ind = get_current_stack_parameter_frame_index();
if (instr.arg != 0) // Avoid divide by 0!
{
logoStack[ind].arg /= instr.arg;
}
break;
}
}
break;
case 11: // Speed
#if (SPEED_CONTROL == 1)
switch (instr.subcmd)
{
case 0: // Increase Speed
desiredSpeed += instr.arg * 10;
break;
case 1: // Set Speed
desiredSpeed = instr.arg * 10;
break;
}
if (desiredSpeed < 0) desiredSpeed = 0;
#endif
break;
case 12: // Interrupts
switch (instr.subcmd) {
case 1: // Set
interruptIndex = find_start_of_subroutine(instr.arg);
break;
case 0: // Clear
interruptIndex = 0;
break;
}
break;
case 13: // Load to PARAM
{
int16_t ind = get_current_stack_parameter_frame_index();
logoStack[ind].arg = logo_value_for_identifier(instr.subcmd);
break;
}
case 14: // IF commands
case 15:
case 16:
case 17:
case 18:
case 19:
{
int16_t val = logo_value_for_identifier(instr.subcmd);
boolean condTrue = false;
if (instr.cmd == 14 && val == instr.arg) condTrue = true; // IF_EQ
else if (instr.cmd == 15 && val != instr.arg) condTrue = true; // IF_NE
else if (instr.cmd == 16 && val > instr.arg) condTrue = true; // IF_GT
else if (instr.cmd == 17 && val < instr.arg) condTrue = true; // IF_LT
else if (instr.cmd == 18 && val >= instr.arg) condTrue = true; // IF_GE
else if (instr.cmd == 19 && val <= instr.arg) condTrue = true; // IF_LE
if (condTrue)
{
if (logoStackIndex < LOGO_STACK_DEPTH-1)
{
logoStackIndex++;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_IF;
}
}
else
{
// jump to the matching END or ELSE
instructionIndex = find_end_of_current_if_block();
if (currentInstructionSet[instructionIndex].subcmd == 3) // is entering an ELSE block
{
if (logoStackIndex < LOGO_STACK_DEPTH-1)
{
logoStackIndex++;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_IF;
}
}
}
break;
}
}
return instr.do_fly;
}
void helicalTurnCntrl(void)
{
union longww accum;
int16_t pitchAdjustAngleOfAttack;
int16_t rollErrorVector[3];
int16_t rtlkick;
int16_t desiredPitch;
int16_t steeringInput;
int16_t desiredTiltVector[3];
int16_t desiredRotationRateGyro[3];
uint16_t airSpeed;
union longww desiredTilt;
int16_t desiredPitchVector[2];
int16_t desiredPerpendicularPitchVector[2];
int16_t actualPitchVector[2];
int16_t pitchDot;
int16_t pitchCross;
int16_t pitchError;
int16_t pitchEarthBodyProjection[2];
int16_t angleOfAttack;
#ifdef TestGains
state_flags._.GPS_steering = 0; // turn off navigation
state_flags._.pitch_feedback = 1; // turn on stabilization
airSpeed = 981; // for testing purposes, an airspeed is needed
#else
airSpeed = air_speed_3DIMU;
if (airSpeed < TURN_CALC_MINIMUM_AIRSPEED) airSpeed = TURN_CALC_MINIMUM_AIRSPEED;
#endif
// determine the desired turn rate as the sum of navigation and fly by wire.
// this allows the pilot to override navigation if needed.
steeringInput = 0 ; // just in case no airframe type is specified or radio is off
if (udb_flags._.radio_on == 1)
{
#if ( (AIRFRAME_TYPE == AIRFRAME_STANDARD) || (AIRFRAME_TYPE == AIRFRAME_GLIDER) )
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[ AILERON_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
steeringInput = udb_pwIn[ RUDDER_INPUT_CHANNEL ] - udb_pwTrim[ RUDDER_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, steeringInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_STANDARD
#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
// use aileron channel if it is available, otherwise use rudder
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
// unmix the Vtail
int16_t rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, (udb_pwIn[ RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = (-rudderInput + elevatorInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_VTAIL
#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
// delta wing must have an aileron input, so use that
// unmix the elevons
int16_t aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, ((elevatorInput - aileronInput)));
#endif // AIRFRAME_DELTA
}
if (steeringInput > MAX_INPUT) steeringInput = MAX_INPUT;
if (steeringInput < - MAX_INPUT) steeringInput = - MAX_INPUT;
// note that total steering is the sum of pilot input and waypoint navigation,
// so that the pilot always has some say in the matter
accum.WW = __builtin_mulsu(steeringInput, turngainfbw) /(2*MAX_INPUT);
if ((settings._.AileronNavigation || settings._.RudderNavigation) && state_flags._.GPS_steering)
{
accum.WW +=(int32_t) navigate_determine_deflection('t');
}
if (accum.WW >(int32_t) 2*(int32_t) RMAX - 1) accum.WW =(int32_t) 2*(int32_t) RMAX - 1;
if (accum.WW < -(int32_t) 2*(int32_t) RMAX + 1) accum.WW = -(int32_t) 2*(int32_t) RMAX + 1;
desiredTurnRateRadians = accum._.W0;
// compute the desired tilt from desired turn rate and air speed
// range for acceleration is plus minus 4 times gravity
// range for turning rate is plus minus 4 radians per second
// desiredTilt is the ratio(-rmat[6]/rmat[8]), times RMAX/2 required for the turn
// desiredTilt = desiredTurnRate * airSpeed / gravity
// desiredTilt = RMAX/2*"real desired tilt"
// desiredTurnRate = RMAX/2*"real desired turn rate", desired turn rate in radians per second
// airSpeed is air speed centimeters per second
// gravity is 981 centimeters per second per second
desiredTilt.WW = - __builtin_mulsu(desiredTurnRateRadians, airSpeed);
desiredTilt.WW /= GRAVITYCMSECSEC;
// limit the lateral acceleration to +- 4 times gravity, total wing loading approximately 4.12 times gravity
if (desiredTilt.WW > (int32_t)2 * (int32_t)RMAX - 1)
{
desiredTilt.WW = (int32_t)2 * (int32_t)RMAX - 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
else if (desiredTilt.WW < -(int32_t)2 * (int32_t)RMAX + 1)
{
desiredTilt.WW = -(int32_t)2 * (int32_t)RMAX + 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
// Compute the amount of lift needed to perform the desired turn
// Tests show that the best estimate of lift is obtained using
// actual values of rmat[6] and rmat[8], and the commanded value of their ratio
estimatedLift = wingLift(rmat[6], rmat[8], desiredTilt._.W0);
// compute angle of attack and elevator trim based on relative wing loading.
// relative wing loading is the ratio of wing loading divided by the stall wing loading, as a function of air speed
// both angle of attack and trim are computed by a linear approximation as a function of relative loading:
// y = (2m)*(x/2) + b, y is either angle of attack or elevator trim.
// x is relative wing loading. (x/2 is computed instead of x)
// 2m and b are determined from values of angle of attack and trim at stall speed, normal and inverted.
// b = (y_normal + y_inverted) / 2.
// 2m = (y_normal - y_inverted).
// If airspeed is greater than stall speed, compute angle of attack and elevator trim,
// otherwise set AoA and trim to zero.
if (air_speed_3DIMU > STALL_SPEED_CM_SEC)
{
// compute "x/2", the relative wing loading
relativeLoading = relativeWingLoading(estimatedLift, air_speed_3DIMU);
// multiply x/2 by 2m for angle of attack
accum.WW = __builtin_mulss(AOA_SLOPE, relativeLoading);
// add mx to b
angleOfAttack = AOA_OFFSET + accum._.W1;
// project angle of attack into the earth frame
accum.WW =(__builtin_mulss(angleOfAttack, rmat[8])) << 2;
pitchAdjustAngleOfAttack = accum._.W1;
// similarly, compute elevator trim
accum.WW = __builtin_mulss(ELEVATOR_TRIM_SLOPE, relativeLoading);
elevatorLoadingTrim = ELEVATOR_TRIM_OFFSET + accum._.W1;
}
else
{
angleOfAttack = 0;
pitchAdjustAngleOfAttack = 0;
elevatorLoadingTrim = 0;
}
// SetAofA(angleOfAttack); // removed by helicalTurns
// convert desired turn rate from radians/second to gyro units
accum.WW = (((int32_t)desiredTurnRateRadians) << 4); // desired turn rate in radians times 16 to provide resolution for the divide to follow
accum.WW = accum.WW / RADSTOGYRO; // at this point accum._.W0 has 2 times the required gyro signal for the turn.
// compute desired rotation rate vector in body frame, scaling is same as gyro signal
VectorScale(3, desiredRotationRateGyro, &rmat[6], accum._.W0); // this operation has side effect of dividing by 2
// compute desired rotation rate vector in body frame, scaling is in RMAX/2*radians/sec
VectorScale(3, desiredRotationRateRadians, &rmat[6], desiredTurnRateRadians); // this produces half of what we want
VectorAdd(3, desiredRotationRateRadians, desiredRotationRateRadians, desiredRotationRateRadians); // double
// incorporate roll into desired tilt vector
desiredTiltVector[0] = desiredTilt._.W0;
desiredTiltVector[1] = 0;
desiredTiltVector[2] = RMAX/2; // the divide by 2 is to account for the RMAX/2 scaling in both tilt and rotation rate
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// incorporate pitch into desired tilt vector
// compute return to launch pitch down kick for unpowered RTL
if (!udb_flags._.radio_on && state_flags._.GPS_steering)
{
rtlkick = RTLKICK;
}
else
{
rtlkick = 0;
}
// Compute Matt's glider pitch adjustment
#if (GLIDE_AIRSPEED_CONTROL == 1)
fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
#endif
// Compute total desired pitch
#if (GLIDE_AIRSPEED_CONTROL == 1)
desiredPitch = - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust;
#else
desiredPitch = - rtlkick + pitchAltitudeAdjust;
#endif
// Adjustment for inverted flight
if (!canStabilizeInverted() || !desired_behavior._.inverted)
{
// normal flight
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack;
}
else
{
// inverted flight
desiredTiltVector[0] = - desiredTiltVector[0];
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack - INVNPITCH; // only one of the adjustments is not zero
desiredTiltVector[2] = - desiredTiltVector[2];
}
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// compute roll error
VectorCross(rollErrorVector, &rmat[6], desiredTiltVector); // compute tilt orientation error
if (VectorDotProduct(3, &rmat[6], desiredTiltVector) < 0) // more than 90 degree error
{
vector3_normalize(rollErrorVector, rollErrorVector); // for more than 90 degrees, make the tilt error vector parallel to desired axis, with magnitude RMAX
}
tiltError[1] = rollErrorVector[1];
// compute pitch error
// start by computing the projection of earth frame pitch error to body frame
pitchEarthBodyProjection[0] = rmat[6];
pitchEarthBodyProjection[1] = rmat[8];
// normalize the projection vector and compute the cosine of the actual pitch as a side effect
actualPitchVector[1] =(int16_t) vector2_normalize(pitchEarthBodyProjection, pitchEarthBodyProjection);
// complete the actual pitch vector
actualPitchVector[0] = rmat[7];
// compute the desired pitch vector
desiredPitchVector[0] = - desiredPitch;
desiredPitchVector[1] = RMAX;
vector2_normalize(desiredPitchVector, desiredPitchVector);
// rotate desired pitch vector by 90 degrees to be able to compute cross product using VectorDot
desiredPerpendicularPitchVector[0] = desiredPitchVector[1];
desiredPerpendicularPitchVector[1] = - desiredPitchVector[0];
// compute pitchDot, the dot product of actual and desired pitch vector
// (the 2* that appears in several of the following expressions is a result of the Q2.14 format)
pitchDot = 2*VectorDotProduct(2, actualPitchVector, desiredPitchVector);
// compute pitchCross, the cross product of the actual and desired pitch vector
pitchCross = 2*VectorDotProduct(2, actualPitchVector, desiredPerpendicularPitchVector);
if (pitchDot > 0)
{
pitchError = pitchCross;
}
else
{
if (pitchCross > 0)
{
pitchError = RMAX;
}
else
{
pitchError = - RMAX;
}
}
// multiply the normalized rmat[6], rmat[8] vector by the pitch error
VectorScale(2, pitchEarthBodyProjection, pitchEarthBodyProjection, pitchError);
tiltError[0] = 2*pitchEarthBodyProjection[1];
tiltError[2] = - 2*pitchEarthBodyProjection[0];
// compute the rotation rate error vector
VectorSubtract(3, rotationRateError, omegaAccum, desiredRotationRateGyro);
}
