// 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); }