src/main/main.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "blackbox/blackbox.h"
  26
  27 #include "common/axis.h"
  28 #include "common/color.h"
  29 #include "common/maths.h"
  30 #include "common/printf.h"
  31
  32 #include "drivers/nvic.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/system.h"
  36 #include "drivers/dma.h"
  37 #include "drivers/gpio.h"
  38 #include "drivers/io.h"
  39 #include "drivers/light_led.h"
  40 #include "drivers/sound_beeper.h"
  41 #include "drivers/timer.h"
  42 #include "drivers/serial.h"
  43 #include "drivers/serial_softserial.h"
  44 #include "drivers/serial_uart.h"
  45 #include "drivers/accgyro.h"
  46 #include "drivers/compass.h"
  47 #include "drivers/pwm_rx.h"
  48 #include "drivers/pwm_output.h"
  49 #include "drivers/adc.h"
  50 #include "drivers/bus_i2c.h"
  51 #include "drivers/bus_spi.h"
  52 #include "drivers/inverter.h"
  53 #include "drivers/flash_m25p16.h"
  54 #include "drivers/sonar_hcsr04.h"
  55 #include "drivers/sdcard.h"
  56 #include "drivers/usb_io.h"
  57 #include "drivers/transponder_ir.h"
  58 #include "drivers/io.h"
  59 #include "drivers/exti.h"
  60 #include "drivers/vtx_soft_spi_rtc6705.h"
  61
  62 #ifdef USE_BST
  63 #include "bus_bst.h"
  64 #endif
  65
  66 #include "fc/config.h"
  67 #include "fc/fc_tasks.h"
  68 #include "fc/fc_msp.h"
  69 #include "fc/rc_controls.h"
  70 #include "fc/runtime_config.h"
  71
  72 #include "msp/msp_serial.h"
  73
  74 #include "rx/rx.h"
  75 #include "rx/spektrum.h"
  76
  77 #include "io/beeper.h"
  78 #include "io/serial.h"
  79 #include "io/flashfs.h"
  80 #include "io/gps.h"
  81 #include "io/motors.h"
  82 #include "io/servos.h"
  83 #include "io/gimbal.h"
  84 #include "io/ledstrip.h"
  85 #include "io/dashboard.h"
  86 #include "io/asyncfatfs/asyncfatfs.h"
  87 #include "io/serial_cli.h"
  88 #include "io/transponder_ir.h"
  89 #include "io/osd.h"
  90 #include "io/vtx.h"
  91
  92 #include "scheduler/scheduler.h"
  93
  94 #include "sensors/sensors.h"
  95 #include "sensors/sonar.h"
  96 #include "sensors/barometer.h"
  97 #include "sensors/compass.h"
  98 #include "sensors/acceleration.h"
  99 #include "sensors/gyro.h"
 100 #include "sensors/battery.h"
 101 #include "sensors/boardalignment.h"
 102 #include "sensors/initialisation.h"
 103
 104 #include "telemetry/telemetry.h"
 105
 106 #include "flight/pid.h"
 107 #include "flight/imu.h"
 108 #include "flight/mixer.h"
 109 #include "flight/failsafe.h"
 110 #include "flight/navigation.h"
 111
 112 #include "config/config_eeprom.h"
 113 #include "config/config_profile.h"
 114 #include "config/config_master.h"
 115 #include "config/feature.h"
 116
 117 #ifdef USE_HARDWARE_REVISION_DETECTION
 118 #include "hardware_revision.h"
 119 #endif
 120
 121 #include "build/build_config.h"
 122 #include "build/debug.h"
 123
 124 extern uint8_t motorControlEnable;
 125
 126 #ifdef SOFTSERIAL_LOOPBACK
 127 serialPort_t *loopbackPort;
 128 #endif
 129
 130 typedef enum {
 131     SYSTEM_STATE_INITIALISING   = 0,
 132     SYSTEM_STATE_CONFIG_LOADED  = (1 << 0),
 133     SYSTEM_STATE_SENSORS_READY  = (1 << 1),
 134     SYSTEM_STATE_MOTORS_READY   = (1 << 2),
 135     SYSTEM_STATE_TRANSPONDER_ENABLED = (1 << 3),
 136     SYSTEM_STATE_READY          = (1 << 7)
 137 } systemState_e;
 138
 139 static uint8_t systemState = SYSTEM_STATE_INITIALISING;
 140
 141 void init(void)
 142 {
 143 #ifdef USE_HAL_DRIVER
 144     HAL_Init();
 145 #endif
 146
 147     printfSupportInit();
 148
 149     initEEPROM();
 150
 151     ensureEEPROMContainsValidData();
 152     readEEPROM();
 153
 154     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 155
 156     systemInit();
 157
 158     //i2cSetOverclock(masterConfig.i2c_overclock);
 159
 160     // initialize IO (needed for all IO operations)
 161     IOInitGlobal();
 162
 163     debugMode = masterConfig.debug_mode;
 164
 165 #ifdef USE_HARDWARE_REVISION_DETECTION
 166     detectHardwareRevision();
 167 #endif
 168
 169     // Latch active features to be used for feature() in the remainder of init().
 170     latchActiveFeatures();
 171
 172 #ifdef ALIENFLIGHTF3
 173     ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
 174 #else
 175     ledInit(false);
 176 #endif
 177     LED2_ON;
 178
 179 #ifdef USE_EXTI
 180     EXTIInit();
 181 #endif
 182
 183 #if defined(BUTTONS)
 184     gpio_config_t buttonAGpioConfig = {
 185         BUTTON_A_PIN,
 186         Mode_IPU,
 187         Speed_2MHz
 188     };
 189     gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
 190
 191     gpio_config_t buttonBGpioConfig = {
 192         BUTTON_B_PIN,
 193         Mode_IPU,
 194         Speed_2MHz
 195     };
 196     gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
 197
 198     // Check status of bind plug and exit if not active
 199     delayMicroseconds(10);  // allow GPIO configuration to settle
 200
 201     if (!isMPUSoftReset()) {
 202         uint8_t secondsRemaining = 5;
 203         bool bothButtonsHeld;
 204         do {
 205             bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
 206             if (bothButtonsHeld) {
 207                 if (--secondsRemaining == 0) {
 208                     resetEEPROM();
 209                     systemReset();
 210                 }
 211                 delay(1000);
 212                 LED0_TOGGLE;
 213             }
 214         } while (bothButtonsHeld);
 215     }
 216 #endif
 217
 218 #ifdef SPEKTRUM_BIND
 219     if (feature(FEATURE_RX_SERIAL)) {
 220         switch (masterConfig.rxConfig.serialrx_provider) {
 221             case SERIALRX_SPEKTRUM1024:
 222             case SERIALRX_SPEKTRUM2048:
 223                 // Spektrum satellite binding if enabled on startup.
 224                 // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
 225                 // The rest of Spektrum initialization will happen later - via spektrumInit()
 226                 spektrumBind(&masterConfig.rxConfig);
 227                 break;
 228         }
 229     }
 230 #endif
 231
 232     delay(100);
 233
 234     timerInit();  // timer must be initialized before any channel is allocated
 235
 236 #if defined(AVOID_UART1_FOR_PWM_PPM)
 237     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
 238             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
 239 #elif defined(AVOID_UART2_FOR_PWM_PPM)
 240     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
 241             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
 242 #elif defined(AVOID_UART3_FOR_PWM_PPM)
 243     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
 244             feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
 245 #else
 246     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 247 #endif
 248
 249     mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
 250 #ifdef USE_SERVOS
 251     servoMixerInit(masterConfig.customServoMixer);
 252 #endif
 253
 254     uint16_t idlePulse = masterConfig.motorConfig.mincommand;
 255     if (feature(FEATURE_3D)) {
 256         idlePulse = masterConfig.flight3DConfig.neutral3d;
 257     }
 258
 259     if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
 260         featureClear(FEATURE_3D);
 261         idlePulse = 0; // brushed motors
 262     }
 263
 264 #ifdef USE_QUAD_MIXER_ONLY
 265     motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
 266 #else
 267     motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount);
 268 #endif
 269
 270 #ifdef USE_SERVOS
 271     if (isMixerUsingServos()) {
 272         //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
 273         servoInit(&masterConfig.servoConfig);
 274     }
 275 #endif
 276
 277 #ifndef SKIP_RX_PWM_PPM
 278     if (feature(FEATURE_RX_PPM)) {
 279         ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
 280     } else if (feature(FEATURE_RX_PARALLEL_PWM)) {
 281         pwmRxInit(&masterConfig.pwmConfig);
 282     }
 283     pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode);
 284 #endif
 285
 286     mixerConfigureOutput();
 287 #ifdef USE_SERVOS
 288     servoConfigureOutput();
 289 #endif
 290     systemState |= SYSTEM_STATE_MOTORS_READY;
 291
 292 #ifdef BEEPER
 293     beeperInit(&masterConfig.beeperConfig);
 294 #endif
 295 /* temp until PGs are implemented. */
 296 #ifdef INVERTER
 297     initInverter();
 298 #endif
 299
 300 #ifdef USE_BST
 301     bstInit(BST_DEVICE);
 302 #endif
 303
 304 #ifdef USE_SPI
 305 #ifdef USE_SPI_DEVICE_1
 306     spiInit(SPIDEV_1);
 307 #endif
 308 #ifdef USE_SPI_DEVICE_2
 309     spiInit(SPIDEV_2);
 310 #endif
 311 #ifdef USE_SPI_DEVICE_3
 312 #ifdef ALIENFLIGHTF3
 313     if (hardwareRevision == AFF3_REV_2) {
 314         spiInit(SPIDEV_3);
 315     }
 316 #else
 317     spiInit(SPIDEV_3);
 318 #endif
 319 #endif
 320 #ifdef USE_SPI_DEVICE_4
 321     spiInit(SPIDEV_4);
 322 #endif
 323 #endif
 324
 325 #ifdef VTX
 326     vtxInit();
 327 #endif
 328
 329 #ifdef USE_HARDWARE_REVISION_DETECTION
 330     updateHardwareRevision();
 331 #endif
 332
 333 #if defined(NAZE)
 334     if (hardwareRevision == NAZE32_SP) {
 335         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 336     } else  {
 337         serialRemovePort(SERIAL_PORT_USART3);
 338     }
 339 #endif
 340
 341 #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
 342     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 343         serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
 344     }
 345 #endif
 346
 347 #if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
 348 #if defined(SONAR) && defined(USE_SOFTSERIAL1)
 349     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 350         serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
 351     }
 352 #endif
 353 #endif
 354
 355 #ifdef USE_I2C
 356 #if defined(NAZE)
 357     if (hardwareRevision != NAZE32_SP) {
 358         i2cInit(I2C_DEVICE);
 359     } else {
 360         if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 361             i2cInit(I2C_DEVICE);
 362         }
 363     }
 364 #elif defined(CC3D)
 365     if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
 366         i2cInit(I2C_DEVICE);
 367     }
 368 #else
 369     i2cInit(I2C_DEVICE);
 370 #endif
 371 #endif
 372
 373 #ifdef USE_ADC
 374     drv_adc_config_t adc_params;
 375
 376     adc_params.enableVBat = feature(FEATURE_VBAT);
 377     adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
 378     adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
 379     adc_params.enableExternal1 = false;
 380 #ifdef OLIMEXINO
 381     adc_params.enableExternal1 = true;
 382 #endif
 383 #ifdef NAZE
 384     // optional ADC5 input on rev.5 hardware
 385     adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
 386 #endif
 387
 388     adcInit(&adc_params);
 389 #endif
 390
 391
 392     initBoardAlignment(&masterConfig.boardAlignment);
 393
 394 #ifdef USE_DASHBOARD
 395     if (feature(FEATURE_DASHBOARD)) {
 396         dashboardInit(&masterConfig.rxConfig);
 397     }
 398 #endif
 399
 400 #ifdef USE_RTC6705
 401     if (feature(FEATURE_VTX)) {
 402         rtc6705_soft_spi_init();
 403         current_vtx_channel = masterConfig.vtx_channel;
 404         rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
 405         rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
 406     }
 407 #endif
 408
 409 #ifdef OSD
 410     if (feature(FEATURE_OSD)) {
 411         osdInit();
 412     }
 413 #endif
 414
 415     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
 416             masterConfig.acc_hardware,
 417             masterConfig.mag_hardware,
 418             masterConfig.baro_hardware,
 419             masterConfig.mag_declination,
 420             masterConfig.gyro_lpf,
 421             masterConfig.gyro_sync_denom)) {
 422         // if gyro was not detected due to whatever reason, we give up now.
 423         failureMode(FAILURE_MISSING_ACC);
 424     }
 425
 426     systemState |= SYSTEM_STATE_SENSORS_READY;
 427
 428     LED1_ON;
 429     LED0_OFF;
 430     LED2_OFF;
 431
 432     for (int i = 0; i < 10; i++) {
 433         LED1_TOGGLE;
 434         LED0_TOGGLE;
 435         delay(25);
 436         if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
 437         delay(25);
 438         BEEP_OFF;
 439     }
 440     LED0_OFF;
 441     LED1_OFF;
 442
 443 #ifdef MAG
 444     if (sensors(SENSOR_MAG))
 445         compassInit();
 446 #endif
 447
 448     imuInit();
 449
 450     mspFcInit();
 451     mspSerialInit();
 452
 453 #ifdef USE_CLI
 454     cliInit(&masterConfig.serialConfig);
 455 #endif
 456
 457     failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 458
 459     rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
 460
 461 #ifdef GPS
 462     if (feature(FEATURE_GPS)) {
 463         gpsInit(
 464             &masterConfig.serialConfig,
 465             &masterConfig.gpsConfig
 466         );
 467         navigationInit(
 468             &masterConfig.gpsProfile,
 469             &currentProfile->pidProfile
 470         );
 471     }
 472 #endif
 473
 474 #ifdef SONAR
 475     if (feature(FEATURE_SONAR)) {
 476         sonarInit(&masterConfig.sonarConfig);
 477     }
 478 #endif
 479
 480 #ifdef LED_STRIP
 481     ledStripInit(&masterConfig.ledStripConfig);
 482
 483     if (feature(FEATURE_LED_STRIP)) {
 484         ledStripEnable();
 485     }
 486 #endif
 487
 488 #ifdef TELEMETRY
 489     if (feature(FEATURE_TELEMETRY)) {
 490         telemetryInit();
 491     }
 492 #endif
 493
 494 #ifdef USB_CABLE_DETECTION
 495     usbCableDetectInit();
 496 #endif
 497
 498 #ifdef TRANSPONDER
 499     if (feature(FEATURE_TRANSPONDER)) {
 500         transponderInit(masterConfig.transponderData);
 501         transponderEnable();
 502         transponderStartRepeating();
 503         systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
 504     }
 505 #endif
 506
 507 #ifdef USE_FLASHFS
 508 #ifdef NAZE
 509     if (hardwareRevision == NAZE32_REV5) {
 510         m25p16_init(IO_TAG_NONE);
 511     }
 512 #elif defined(USE_FLASH_M25P16)
 513     m25p16_init(IO_TAG_NONE);
 514 #endif
 515
 516     flashfsInit();
 517 #endif
 518
 519 #ifdef USE_SDCARD
 520     bool sdcardUseDMA = false;
 521
 522     sdcardInsertionDetectInit();
 523
 524 #ifdef SDCARD_DMA_CHANNEL_TX
 525
 526 #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
 527     // Ensure the SPI Tx DMA doesn't overlap with the led strip
 528 #if defined(STM32F4) || defined(STM32F7)
 529     sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
 530 #else
 531     sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
 532 #endif
 533 #else
 534     sdcardUseDMA = true;
 535 #endif
 536
 537 #endif
 538
 539     sdcard_init(sdcardUseDMA);
 540
 541     afatfs_init();
 542 #endif
 543
 544     if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
 545         masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
 546         masterConfig.gyro_sync_denom = 1;
 547     }
 548
 549     setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
 550
 551 #ifdef BLACKBOX
 552     initBlackbox();
 553 #endif
 554
 555     if (masterConfig.mixerMode == MIXER_GIMBAL) {
 556         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 557     }
 558     gyroSetCalibrationCycles();
 559 #ifdef BARO
 560     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 561 #endif
 562
 563     // start all timers
 564     // TODO - not implemented yet
 565     timerStart();
 566
 567     ENABLE_STATE(SMALL_ANGLE);
 568     DISABLE_ARMING_FLAG(PREVENT_ARMING);
 569
 570 #ifdef SOFTSERIAL_LOOPBACK
 571     // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
 572     loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
 573     if (!loopbackPort->vTable) {
 574         loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
 575     }
 576     serialPrint(loopbackPort, "LOOPBACK\r\n");
 577 #endif
 578
 579     // Now that everything has powered up the voltage and cell count be determined.
 580
 581     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
 582         batteryInit(&masterConfig.batteryConfig);
 583
 584 #ifdef USE_DASHBOARD
 585     if (feature(FEATURE_DASHBOARD)) {
 586 #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
 587         dashboardShowFixedPage(PAGE_GPS);
 588 #else
 589         dashboardResetPageCycling();
 590         dashboardEnablePageCycling();
 591 #endif
 592     }
 593 #endif
 594
 595 #ifdef CJMCU
 596     LED2_ON;
 597 #endif
 598
 599     // Latch active features AGAIN since some may be modified by init().
 600     latchActiveFeatures();
 601     motorControlEnable = true;
 602
 603     fcTasksInit();
 604     systemState |= SYSTEM_STATE_READY;
 605 }
 606
 607 #ifdef SOFTSERIAL_LOOPBACK
 608 void processLoopback(void) {
 609     if (loopbackPort) {
 610         uint8_t bytesWaiting;
 611         while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
 612             uint8_t b = serialRead(loopbackPort);
 613             serialWrite(loopbackPort, b);
 614         };
 615     }
 616 }
 617 #else
 618 #define processLoopback()
 619 #endif
 620
 621
 622 void main_step(void)
 623 {
 624     scheduler();
 625     processLoopback();
 626 }
 627
 628 #ifndef NOMAIN
 629 int main(void)
 630 {
 631     init();
 632     while (true) {
 633         main_step();
 634     }
 635 }
 636 #endif
 637
 638 #ifdef DEBUG_HARDFAULTS
 639 //from: https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/
 640 /**
 641  * hard_fault_handler_c:
 642  * This is called from the HardFault_HandlerAsm with a pointer the Fault stack
 643  * as the parameter. We can then read the values from the stack and place them
 644  * into local variables for ease of reading.
 645  * We then read the various Fault Status and Address Registers to help decode
 646  * cause of the fault.
 647  * The function ends with a BKPT instruction to force control back into the debugger
 648  */
 649 void hard_fault_handler_c(unsigned long *hardfault_args)
 650 {
 651   volatile unsigned long stacked_r0 ;
 652   volatile unsigned long stacked_r1 ;
 653   volatile unsigned long stacked_r2 ;
 654   volatile unsigned long stacked_r3 ;
 655   volatile unsigned long stacked_r12 ;
 656   volatile unsigned long stacked_lr ;
 657   volatile unsigned long stacked_pc ;
 658   volatile unsigned long stacked_psr ;
 659   volatile unsigned long _CFSR ;
 660   volatile unsigned long _HFSR ;
 661   volatile unsigned long _DFSR ;
 662   volatile unsigned long _AFSR ;
 663   volatile unsigned long _BFAR ;
 664   volatile unsigned long _MMAR ;
 665
 666   stacked_r0 = ((unsigned long)hardfault_args[0]) ;
 667   stacked_r1 = ((unsigned long)hardfault_args[1]) ;
 668   stacked_r2 = ((unsigned long)hardfault_args[2]) ;
 669   stacked_r3 = ((unsigned long)hardfault_args[3]) ;
 670   stacked_r12 = ((unsigned long)hardfault_args[4]) ;
 671   stacked_lr = ((unsigned long)hardfault_args[5]) ;
 672   stacked_pc = ((unsigned long)hardfault_args[6]) ;
 673   stacked_psr = ((unsigned long)hardfault_args[7]) ;
 674
 675   // Configurable Fault Status Register
 676   // Consists of MMSR, BFSR and UFSR
 677   _CFSR = (*((volatile unsigned long *)(0xE000ED28))) ;
 678
 679   // Hard Fault Status Register
 680   _HFSR = (*((volatile unsigned long *)(0xE000ED2C))) ;
 681
 682   // Debug Fault Status Register
 683   _DFSR = (*((volatile unsigned long *)(0xE000ED30))) ;
 684
 685   // Auxiliary Fault Status Register
 686   _AFSR = (*((volatile unsigned long *)(0xE000ED3C))) ;
 687
 688   // Read the Fault Address Registers. These may not contain valid values.
 689   // Check BFARVALID/MMARVALID to see if they are valid values
 690   // MemManage Fault Address Register
 691   _MMAR = (*((volatile unsigned long *)(0xE000ED34))) ;
 692   // Bus Fault Address Register
 693   _BFAR = (*((volatile unsigned long *)(0xE000ED38))) ;
 694
 695   __asm("BKPT #0\n") ; // Break into the debugger
 696 }
 697
 698 #else
 699 void HardFault_Handler(void)
 700 {
 701     LED2_ON;
 702
 703     // fall out of the sky
 704     uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
 705     if ((systemState & requiredStateForMotors) == requiredStateForMotors) {
 706         stopMotors();
 707     }
 708 #ifdef TRANSPONDER
 709     // prevent IR LEDs from burning out.
 710     uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED;
 711     if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) {
 712         transponderIrDisable();
 713     }
 714 #endif
 715
 716     LED1_OFF;
 717     LED0_OFF;
 718
 719     while (1) {
 720 #ifdef LED2
 721         delay(50);
 722         LED2_TOGGLE;
 723 #endif
 724     }
 725 }
 726 #endif