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 ;
}
// Called every 1/40 second at low priority
void udb_heartbeat_40hz_callback(void)
{
if (udb_heartbeat_counter % 20 == 0)
{
if (charPosition == 256 && !didDisplay)
{
LED_GREEN = LED_ON;
LED_GREEN = LED_ON;
osd_spi_write(0x04, 0); // DMM set to 0
#if (OSD_VIDEO_FORMAT == OSD_NTSC)
osd_spi_write(0x0, 0x08); // VM0: enable display of OSD image, NTSC
#else
osd_spi_write(0x0, 0x48); // VM0: enable display of OSD image, PAL
#endif
int row;
for (row = 0; row < 11; row++)
{
osd_spi_write_location(OSD_LOC(row + 1, 3));
osd_spi_write(0x04, 1); // DMM: Enable auto-increment mode
int col;
for (col = 0; col < 24; col++)
{
osd_spi_write_byte(row * 24 + col);
}
osd_spi_write_byte(0xFF);
didDisplay = 1;
}
}
else
{
udb_led_toggle(LED_RED);
}
}
}
void osd_setup_screen( void )
{
#if (OSD_LOC_ALTITUDE != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_ALTITUDE) ;
osd_spi_write(0x7, 0xA6) ; // Altitude symbol
#endif
#if (OSD_LOC_CPU_LOAD != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_CPU_LOAD) ;
osd_spi_write(0x7, 0xBD) ; // CPU symbol
osd_spi_write_location(OSD_LOC_CPU_LOAD+4) ;
osd_spi_write(0x7, 0xA5) ; // % symbol
#endif
#if (OSD_LOC_DIST_TO_GOAL != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_DIST_TO_GOAL) ;
osd_spi_write(0x7, 0xA7) ; // Distance symbol
#endif
#if (OSD_LOC_HEADING_NUM != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_HEADING_NUM) ;
osd_spi_write(0x7, 0xAB) ; // Direction symbol
osd_spi_write_location(OSD_LOC_HEADING_NUM+4) ;
osd_spi_write(0x7, 0x4D) ; // Degrees symbol
#endif
#if (OSD_LOC_AIR_SPEED_M_S != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AIR_SPEED_M_S+3) ;
osd_spi_write(0x7, 0xDD) ; // m/s symbol
#endif
#if (OSD_LOC_AIR_SPEED_MI_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AIR_SPEED_MI_HR+3) ;
osd_spi_write(0x7, 0xDF) ; // mi/hr symbol
#endif
#if (OSD_LOC_AIR_SPEED_KM_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_AIR_SPEED_KM_HR+3) ;
osd_spi_write(0x7, 0xDE) ; // km/hr symbol
#endif
#if (OSD_LOC_GROUND_SPEED_M_S != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GROUND_SPEED_M_S+3) ;
osd_spi_write(0x7, 0xDD) ; // m/s symbol
#endif
#if (OSD_LOC_GROUND_SPEED_MI_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GROUND_SPEED_MI_HR+3) ;
osd_spi_write(0x7, 0xDF) ; // mi/hr symbol
#endif
#if (OSD_LOC_GROUND_SPEED_KM_HR != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_GROUND_SPEED_KM_HR+3) ;
osd_spi_write(0x7, 0xDE) ; // km/hr symbol
#endif
#if (OSD_SHOW_CENTER_DOT == 1)
osd_spi_write_location(OSD_LOC(OSD_SPACING + 3, 14)) ;
osd_spi_write(0x7, 0x4E) ; // center dot
osd_spi_write_location(OSD_LOC(OSD_SPACING + 3, 15)) ;
osd_spi_write(0x7, 0x4F) ; // center dot
#endif
#if (OSD_SHOW_HORIZON == 1)
osd_spi_write_location(OSD_LOC(OSD_SPACING + 3, 14-OSD_HORIZON_WIDTH)) ;
osd_spi_write(0x7, 0xF1) ; // horizon center
osd_spi_write_location(OSD_LOC(OSD_SPACING + 3, 15+OSD_HORIZON_WIDTH)) ;
osd_spi_write(0x7, 0xF0) ; // horizon center
#endif
#if (OSD_LOC_CALLSIGN_HORIZ != OSD_LOC_DISABLED)
osd_spi_write_location(OSD_LOC_CALLSIGN_HORIZ) ;
osd_spi_write_string(callsign) ; // callsign
#endif
#if (OSD_LOC_CALLSIGN_VERT != OSD_LOC_DISABLED)
osd_spi_write_vertical_string_at_location(OSD_LOC_CALLSIGN_VERT, callsign) ; // callsign
#endif
return ;
}
void osd_set_xy(int x, int y)
{
int loc = OSD_LOC(x, y);
osd_spi_write(0x05, (uint8_t)(loc >> 8)); // DMAH
osd_spi_write(0x06, (uint8_t)(loc & 0xFF)); // DMAL
}
