// 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));
}
}
}
static void gps_id3(uint8_t gpschar)
{
XOR ^= gpschar;
if (id1 == 'R' && id2 == 'M' && gpschar == 'C') // "$GPRMC"
{
rmc_counter = 1; // Next rmc message after the comma
msg_parse = &gps_comma; // A comma ',' is expected now
#ifdef DEBUG_NMEA
// msg_parse = &msg_start;
strcpy(debug_RMC, "$GPRMC");
udb_led_toggle(LED_BLUE);
RMCpos = 6;
#endif
}
else if (id1 == 'G' && id2 == 'G' && gpschar == 'A') // "$GPGGA"
{
gga_counter = 1; // Next gga message after the comma
msg_parse = &gps_comma; // A comma ',' is expected now
#ifdef DEBUG_NMEA
GGApos = 6;
// msg_parse = &msg_start;
strcpy(debug_GGA, "$GPGGA");
#endif
}
else // ID not detected, abort
{
msg_parse = &msg_start;
}
}
// Called every 1/40 second at high priority
void udb_background_callback_periodic(void)
{
static int count = 0;
if (!dcm_flags._.calib_finished)
{
// If still calibrating, blink RED
if (++count > HEARTBEAT_HZ/2) {
count = 0;
udb_led_toggle(LED_RED);
}
}
else
{
// No longer calibrating: solid RED and send debug output
LED_RED = LED_ON;
}
}
void osd_update_glyph(void)
{
osd_spi_write(0x9, charPosition); // CMAH: set glyph to overwrite
unsigned char i;
for (i = 0; i < 54; i++)
{
osd_spi_write(0xA, i); // CMAL: set the 4-px chunk of the glyph to overwrite
osd_spi_write(0xB, font_data[charPosition * 64 + i]); // CMDI: update the data representing the 4-px chunk of the glyph
}
osd_spi_write(0x8, 0xA0); // CMM: write glyph to NVRAM
charPosition++;
udb_led_toggle(LED_GREEN); // Flash the green LED after each char is updated
}
void calibrateS(void)
{
if ( udb_flags._.radio_on )
{
#if ( LED_RED_MAG_CHECK == 0 )
udb_led_toggle(LED_RED) ;
#endif
calib_timer--;
if (calib_timer <= 0)
ent_acquiringS() ;
}
else
{
ent_calibrateS() ;
}
return ;
}
void waypointS(void)
{
#if ( LED_RED_MAG_CHECK == 0 )
udb_led_toggle(LED_RED) ;
#endif
if ( udb_flags._.radio_on )
{
if ( flags._.man_req )
ent_manualS() ;
else if ( flags._.auto_req )
ent_stabilizedS() ;
}
else
{
ent_returnS() ;
}
return ;
}
void waypointS(void)
{
#if ( LED_RED_MAG_CHECK == 0 )
udb_led_toggle(LED_RED) ;
#endif
if ( udb_flags._.radio_on )
{
if ( flight_mode_switch_manual() )
ent_manualS() ;
else if ( flight_mode_switch_auto() )
ent_stabilizedS() ;
}
else
{
ent_returnS() ;
}
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);
}
}
}
// Called every time we get gps data (1, 2, or 4 Hz, depending on GPS config)
void dcm_callback_gps_location_updated(void)
{
// Blink GREEN led to show that the GPS is communicating
udb_led_toggle(LED_GREEN);
}
