void osd_update_horizon( void )
{
// TODO: Change away from using roll degrees. Use tangent as the slope.
struct relative2D matrix_accum ;
matrix_accum.x = rmat[8] ;
matrix_accum.y = rmat[6] ;
long earth_roll = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
earth_roll = (-earth_roll * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
#if (OSD_HORIZON_ROLL_REVERSED == 1)
earth_roll = -earth_roll ;
#endif
matrix_accum.y = rmat[7] ;
long earth_pitch = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
earth_pitch = (-earth_pitch * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
#if (OSD_HORIZON_PITCH_REVERSED == 1)
earth_pitch = -earth_pitch ;
#endif
char i ;
for (i = -OSD_HORIZON_WIDTH; i<OSD_HORIZON_WIDTH; i++)
{
int h = earth_roll * i - earth_pitch * 16 + 60 ;
char height = h / 120 ;
char subHeight = ((h % 120) * 16 / 120) ;
if (h < 0) { height-- ; subHeight-- ; }
subHeight &= 0x0F ;
h = lastRoll * i - lastPitch * 16 + 60 ;
char lastHeight = h / 120 ;
if (h < 0) lastHeight-- ;
if (height != 0 || OSD_SHOW_CENTER_DOT != 1 || (i != -1 && i != 0))
{
if (height >= -OSD_SPACING && height <= OSD_SPACING)
{
osd_spi_write_location(OSD_LOC(OSD_SPACING + 3 - height, 15+i)) ;
osd_spi_write(0x7, 0xC0 + subHeight) ; // DMDI: Write a '-'
}
}
if (lastHeight != 0 || OSD_SHOW_CENTER_DOT != 1 || (i != -1 && i != 0))
{
if (height != lastHeight && lastHeight >= -OSD_SPACING && lastHeight <= OSD_SPACING)
{
osd_spi_write_location(OSD_LOC(OSD_SPACING + 3 - lastHeight, 15+i)) ;
osd_spi_write(0x7, 0x00) ; // DMDI: Write a ' '
}
}
}
lastRoll = earth_roll ;
lastPitch = earth_pitch ;
return ;
}
// In the future, we could include more than 2 flight plans...
// flightplanNum is 0 for the main lgo instructions, and 1 for RTL instructions
//void init_flightplan(int16_t flightplanNum)
void flightplan_logo_begin(int16_t flightplanNum)
{
struct relative2D curHeading;
struct relative3D IMUloc;
int8_t earth_yaw;
int16_t angle;
if (flightplanNum == 1) // RTL instructions set
{
currentInstructionSet = (struct logoInstructionDef*)rtlInstructions;
numInstructionsInCurrentSet = NUM_RTL_INSTRUCTIONS;
}
else if (flightplanNum == 0) // Main instructions set
{
currentInstructionSet = (struct logoInstructionDef*)instructions;
numInstructionsInCurrentSet = NUM_INSTRUCTIONS;
}
instructionIndex = 0;
logoStackIndex = 0;
logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_SUBROUTINE;
logoStack[logoStackIndex].arg = 0;
logoStack[logoStackIndex].returnInstructionIndex = -1; // When starting over, begin on instruction 0
currentTurtle = PLANE;
penState = 0; // 0 means down. more than 0 means up
turtleLocations[PLANE].x._.W1 = IMUlocationx._.W1;
turtleLocations[PLANE].y._.W1 = IMUlocationy._.W1;
turtleLocations[PLANE].z = IMUlocationz._.W1;
turtleLocations[CAMERA].x._.W1 = IMUlocationx._.W1;
turtleLocations[CAMERA].y._.W1 = IMUlocationy._.W1;
turtleLocations[CAMERA].z = IMUlocationz._.W1;
// Calculate heading from Direction Cosine Matrix (rather than GPS),
// So that this code works when the plane is static. e.g. at takeoff
curHeading.x = -rmat[1];
curHeading.y = rmat[4];
earth_yaw = rect_to_polar(&curHeading); // (0=East, ccw)
angle = (earth_yaw * 180 + 64) >> 7; // (ccw, 0=East)
angle = -angle + 90; // (clockwise, 0=North)
turtleAngles[PLANE] = turtleAngles[CAMERA] = angle;
setBehavior(0);
IMUloc.x = IMUlocationx._.W1;
IMUloc.y = IMUlocationy._.W1;
IMUloc.z = IMUlocationz._.W1;
update_goal_from(IMUloc);
interruptIndex = 0;
interruptStackBase = 0;
process_instructions();
}
int main(void)
{
Rect_V input;
Polar_V result;
printf("Enter x and y coordinates (q to quit): ");
while (scanf("%lf %lf", &input.x, &input.y) == 2) {
result = rect_to_polar(input);
printf("Magnitude: %.2f, angle: %.2f\n", result.magnitude, result.angle);
printf("Enter x and y coordinates (q to quit): ");
}
puts("Bye");
return 0;
}
int main()
{
RECT_V input;
POLAR_V result;
puts("enter x,y coordinates: enter q to quit:");
while(scanf("%lf %lf",&input.x, &input.y) == 2)
{
result = rect_to_polar(input);
printf("magnitude = %0.2f, angle = %0.2f\n",result.magnitude, result.angle);
}
puts("Bye\n");
return 0;
}
static int16_t get_current_angle(void)
{
// Calculate heading from Direction Cosine Matrix (rather than GPS),
// So that this code works when the plane is static. e.g. at takeoff
struct relative2D curHeading;
int8_t earth_yaw;
int16_t angle;
curHeading.x = -rmat[1];
curHeading.y = rmat[4];
earth_yaw = rect_to_polar(&curHeading); // (0=East, ccw)
angle = (earth_yaw * 180 + 64) >> 7; // (ccw, 0=East)
angle = -angle + 90; // (clockwise, 0=North)
if (angle < 0) angle += 360;
return angle;
}
static int16_t get_angle_to_point(int16_t x, int16_t y)
{
struct relative2D vectorToGoal;
int8_t dir_to_goal;
int16_t angle;
vectorToGoal.x = turtleLocations[currentTurtle].x._.W1 - x;
vectorToGoal.y = turtleLocations[currentTurtle].y._.W1 - y;
dir_to_goal = rect_to_polar (&vectorToGoal);
// dir_to_goal // 0-255 (ccw, 0=East)
angle = (dir_to_goal * 180 + 64) >> 7; // 0-359 (ccw, 0=East)
angle = -angle + 90; // 0-359 (clockwise, 0=North)
if (angle < 0) angle += 360;
if (angle > 360) angle -= 360;
return angle;
}
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 ;
}
void serial_output_8hz( void )
{
unsigned int mode ;
struct relative2D matrix_accum ;
union longbbbb accum ;
int desired_dir_deg ; // desired_dir converted to a bearing (0-360)
long earth_pitch ; // pitch in binary angles ( 0-255 is 360 degreres)
long earth_roll ; // roll of the plane with respect to earth frame
//long earth_yaw ; // yaw with respect to earth frame
accum.WW = ( desired_dir * BYTECIR_TO_DEGREE ) + 32768 ;
desired_dir_deg = accum._.W1 - 90 ; // "Convert UAV DevBoad Earth" to Compass Bearing
if ( desired_dir_deg < 0 ) desired_dir_deg += 360 ;
if (flags._.GPS_steering == 0 && flags._.pitch_feedback == 0)
mode = 1 ;
else if (flags._.GPS_steering == 0 && flags._.pitch_feedback == 1)
mode = 2 ;
else if (flags._.GPS_steering == 1 && flags._.pitch_feedback == 1 && udb_flags._.radio_on == 1)
mode = 3 ;
else if (flags._.GPS_steering == 1 && flags._.pitch_feedback == 1 && udb_flags._.radio_on == 0)
mode = 0 ;
else
mode = 99 ; // Unknown
// Roll
// Earth Frame of Reference
matrix_accum.x = rmat[8] ;
matrix_accum.y = rmat[6] ;
earth_roll = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
earth_roll = (-earth_roll * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
// Pitch
// Earth Frame of Reference
// Note that we are using the matrix_accum.x
// left over from previous rect_to_polar in this calculation.
// so this Pitch calculation must follow the Roll calculation
matrix_accum.y = rmat[7] ;
earth_pitch = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
earth_pitch = (-earth_pitch * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
// Yaw
// Earth Frame of Reference
// Ardustation does not use yaw in degrees
// matrix_accum.x = rmat[4] ;
// matrix_accum.y = rmat[1] ;
// earth_yaw = rect_to_polar(&matrix_accum) ; // binary angle (0 - 256 = 360 degrees)
// earth_yaw = (earth_yaw * BYTECIR_TO_DEGREE) >> 16 ; // switch polarity, convert to -180 - 180 degrees
// The Ardupilot GroundStation protocol is mostly documented here:
// http://diydrones.com/profiles/blogs/ardupilot-telemetry-protocol
if (udb_heartbeat_counter % 40 == 0) // Every 8 runs (5 heartbeat counts per 8Hz)
{
serial_output("!!!LAT:%li,LON:%li,SPD:%.2f,CRT:%.2f,ALT:%li,ALH:%i,CRS:%.2f,BER:%i,WPN:%i,DST:%i,BTV:%.2f***\r\n"
"+++THH:%i,RLL:%li,PCH:%li,STT:%i,***\r\n",
lat_gps.WW / 10 , long_gps.WW / 10 , (float)(sog_gps.BB / 100.0), (float)(climb_gps.BB / 100.0),
(alt_sl_gps.WW - alt_origin.WW) / 100, desiredHeight, (float)(cog_gps.BB / 100.0), desired_dir_deg,
waypointIndex, tofinish_line, (float)(voltage_milis.BB / 100.0),
(int)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),
earth_roll, earth_pitch,
mode
) ;
}
else if (udb_heartbeat_counter % 10 == 0) // Every 2 runs (5 heartbeat counts per 8Hz)
{
serial_output("+++THH:%i,RLL:%li,PCH:%li,STT:%i,***\r\n",
(int)((udb_pwOut[THROTTLE_OUTPUT_CHANNEL] - udb_pwTrim[THROTTLE_OUTPUT_CHANNEL])/20),
earth_roll, earth_pitch,
mode
) ;
}
return ;
}
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;
}
