From f6c6e4f8ba04e3b1c5ca6e044f445f5213876343 Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Thu, 1 Sep 2016 11:23:20 +0300 Subject: [PATCH] Boot-time event logging implementation (#536) * Initial cut on boot-time event logging * More verbose PWM init logging; more verbose HMC5883 error logging * Disable NMEA GPS on CC3D due to flash size issues --- Makefile | 1 + src/main/drivers/compass_hmc5883l.c | 25 +++++++ src/main/drivers/logging.c | 133 ++++++++++++++++++++++++++++++++++++ src/main/drivers/logging.h | 42 ++++++++++++ src/main/drivers/logging_codes.h | 55 +++++++++++++++ src/main/drivers/pwm_mapping.c | 47 ++++++++++--- src/main/io/serial_cli.c | 49 +++++++++++++ src/main/main.c | 32 ++++++--- src/main/sensors/initialisation.c | 14 ++++ src/main/target/CC3D/target.h | 6 +- src/main/target/common.h | 2 + 11 files changed, 388 insertions(+), 18 deletions(-) create mode 100755 src/main/drivers/logging.c create mode 100755 src/main/drivers/logging.h create mode 100755 src/main/drivers/logging_codes.h diff --git a/Makefile b/Makefile index 23390d09c..2ae9aa1c2 100644 --- a/Makefile +++ b/Makefile @@ -372,6 +372,7 @@ COMMON_SRC = \ common/streambuf.c \ config/config.c \ fc/runtime_config.c \ + drivers/logging.c \ drivers/adc.c \ drivers/buf_writer.c \ drivers/bus_i2c_soft.c \ diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 2aef9575c..8d09e2572 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -36,6 +36,8 @@ #include "bus_i2c.h" #include "light_led.h" +#include "logging.h" + #include "sensor.h" #include "compass.h" @@ -189,6 +191,8 @@ bool hmc5883lInit(void) int16_t magADC[3]; int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow. bool bret = true; // Error indicator + bool error_read = false; + bool error_saturation = false; delay(50); i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias @@ -211,6 +215,7 @@ bool hmc5883lInit(void) xyz_total[Z] += magADC[Z]; // Detect saturation. if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { + error_saturation = true; bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } @@ -219,10 +224,15 @@ bool hmc5883lInit(void) } LED1_TOGGLE; } + if (failedSamples >= INIT_MAX_FAILURES) { bret = false; } + if (failedSamples > 0) { + error_read = true; + } + // Apply the negative bias. (Same gain) i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. validSamples = 0; @@ -238,6 +248,7 @@ bool hmc5883lInit(void) xyz_total[Z] -= magADC[Z]; // Detect saturation. if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) { + error_saturation = true; bret = false; break; // Breaks out of the for loop. No sense in continuing if we saturated. } @@ -246,10 +257,15 @@ bool hmc5883lInit(void) } LED1_TOGGLE; } + if (failedSamples >= INIT_MAX_FAILURES) { bret = false; } + if (failedSamples > 0) { + error_read = true; + } + if (bret) { magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]); magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]); @@ -268,6 +284,15 @@ bool hmc5883lInit(void) delay(100); hmc5883lConfigureDataReadyInterruptHandling(); + + if (error_read) { + addBootlogEvent2(BOOT_EVENT_HMC5883L_READ_FAILED, BOOT_EVENT_FLAGS_WARNING); + } + + if (error_saturation) { + addBootlogEvent2(BOOT_EVENT_HMC5883L_SATURATION, BOOT_EVENT_FLAGS_ERROR); + } + return bret; } diff --git a/src/main/drivers/logging.c b/src/main/drivers/logging.c new file mode 100755 index 000000000..ab2dd7779 --- /dev/null +++ b/src/main/drivers/logging.c @@ -0,0 +1,133 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef BOOTLOG + +#include "logging.h" +#include "system.h" + +#define MAX_BOOTLOG_ENTRIES 64 + +static bootLogEntry_t events[MAX_BOOTLOG_ENTRIES]; +static int eventCount; +static bool eventOverflow; + +#ifdef BOOTLOG_DESCRIPTIONS +static const char * eventDescription[BOOT_EVENT_CODE_COUNT] = { + [BOOT_EVENT_CONFIG_LOADED] = "CONFIG_LOADED", + [BOOT_EVENT_SYSTEM_INIT_DONE] = "SYSTEM_INIT_DONE", + [BOOT_EVENT_PWM_INIT_DONE] = "PWM_INIT_DONE", + [BOOT_EVENT_EXTRA_BOOT_DELAY] = "EXTRA_BOOT_DELAY", + [BOOT_EVENT_SENSOR_INIT_DONE] = "SENSOR_INIT_DONE", + [BOOT_EVENT_GPS_INIT_DONE] = "GPS_INIT_DONE", + [BOOT_EVENT_LEDSTRIP_INIT_DONE] = "LEDSTRIP_INIT_DONE", + [BOOT_EVENT_TELEMETRY_INIT_DONE] = "TELEMETRY_INIT_DONE", + [BOOT_EVENT_SYSTEM_READY] = "SYSTEM_READY", + [BOOT_EVENT_GYRO_DETECTION] = "GYRO_DETECTION", + [BOOT_EVENT_ACC_DETECTION] = "ACC_DETECTION", + [BOOT_EVENT_BARO_DETECTION] = "BARO_DETECTION", + [BOOT_EVENT_MAG_DETECTION] = "MAG_DETECTION", + [BOOT_EVENT_RANGEFINDER_DETECTION] = "RANGEFINDER_DETECTION", + [BOOT_EVENT_MAG_INIT_FAILED] = "MAG_INIT_FAILED", + [BOOT_EVENT_HMC5883L_READ_FAILED] = "HMC5883L_READ_FAILED", + [BOOT_EVENT_HMC5883L_SATURATION] = "HMC5883L_SATURATION", + [BOOT_EVENT_TIMER_CH_SKIPPED] = "TIMER_CHANNEL_SKIPPED", + [BOOT_EVENT_TIMER_CH_MAPPED] = "TIMER_CHANNEL_MAPPED" +}; + +const char * getBootlogEventDescription(bootLogEventCode_e eventCode) +{ + if (eventCode < BOOT_EVENT_CODE_COUNT) { + return eventDescription[eventCode]; + } + else { + return NULL; + } +} +#endif + +void initBootlog(void) +{ + memset(events, 0, sizeof(events)); + eventCount = 0; + eventOverflow = false; +} + +int getBootlogEventCount(void) +{ + return eventCount; +} + +bootLogEntry_t * getBootlogEvent(int index) +{ + if (index <= eventCount) { + return &events[index]; + } + else { + return 0; + } +} + +static void addBootlogEntry(bootLogEntry_t * event) +{ + if (eventCount >= MAX_BOOTLOG_ENTRIES) { + eventOverflow = true; + return; + } + + memcpy(&events[eventCount], event, sizeof(bootLogEntry_t)); + events[eventCount].timestamp = millis(); + + eventCount++; +} + +void addBootlogEvent2(bootLogEventCode_e eventCode, uint16_t eventFlags) +{ + bootLogEntry_t event; + event.eventCode = eventCode; + event.eventFlags = eventFlags; + addBootlogEntry(&event); +} + +void addBootlogEvent4(bootLogEventCode_e eventCode, uint16_t eventFlags, uint32_t param1, uint32_t param2) +{ + bootLogEntry_t event; + event.eventCode = eventCode; + event.eventFlags = eventFlags | BOOT_EVENT_FLAGS_PARAM32; + event.params.u32[0] = param1; + event.params.u32[1] = param2; + addBootlogEntry(&event); +} + +void addBootlogEvent6(bootLogEventCode_e eventCode, uint16_t eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4) +{ + bootLogEntry_t event; + event.eventCode = eventCode; + event.eventFlags = eventFlags | BOOT_EVENT_FLAGS_PARAM16; + event.params.u16[0] = param1; + event.params.u16[1] = param2; + event.params.u16[2] = param3; + event.params.u16[3] = param4; + addBootlogEntry(&event); +} +#endif diff --git a/src/main/drivers/logging.h b/src/main/drivers/logging.h new file mode 100755 index 000000000..b5ee64910 --- /dev/null +++ b/src/main/drivers/logging.h @@ -0,0 +1,42 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#ifdef BOOTLOG + +#include "logging_codes.h" + +typedef struct bootLogEntry_s { + uint32_t timestamp; + uint16_t eventCode; + uint16_t eventFlags; + union { + uint16_t u16[4]; + uint32_t u32[2]; + } params; +} bootLogEntry_t; + +void initBootlog(void); +int getBootlogEventCount(void); +bootLogEntry_t * getBootlogEvent(int index); +const char * getBootlogEventDescription(bootLogEventCode_e eventCode); +void addBootlogEvent2(bootLogEventCode_e eventCode, bootLogFlags_e eventFlags); +void addBootlogEvent4(bootLogEventCode_e eventCode, bootLogFlags_e eventFlags, uint32_t param1, uint32_t param2); +void addBootlogEvent6(bootLogEventCode_e eventCode, bootLogFlags_e eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4); + +#endif diff --git a/src/main/drivers/logging_codes.h b/src/main/drivers/logging_codes.h new file mode 100755 index 000000000..bc6455b0b --- /dev/null +++ b/src/main/drivers/logging_codes.h @@ -0,0 +1,55 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#ifdef BOOTLOG + +typedef enum { + BOOT_EVENT_FLAGS_NONE = 0, + BOOT_EVENT_FLAGS_WARNING = 1 << 0, + BOOT_EVENT_FLAGS_ERROR = 1 << 1, + + BOOT_EVENT_FLAGS_PARAM16 = 1 << 14, + BOOT_EVENT_FLAGS_PARAM32 = 1 << 15 +} bootLogFlags_e; + +typedef enum { + BOOT_EVENT_CONFIG_LOADED = 0, + BOOT_EVENT_SYSTEM_INIT_DONE = 1, + BOOT_EVENT_PWM_INIT_DONE = 2, + BOOT_EVENT_EXTRA_BOOT_DELAY = 3, + BOOT_EVENT_SENSOR_INIT_DONE = 4, + BOOT_EVENT_GPS_INIT_DONE = 5, + BOOT_EVENT_LEDSTRIP_INIT_DONE = 6, + BOOT_EVENT_TELEMETRY_INIT_DONE = 7, + BOOT_EVENT_SYSTEM_READY = 8, + BOOT_EVENT_GYRO_DETECTION = 9, + BOOT_EVENT_ACC_DETECTION = 10, + BOOT_EVENT_BARO_DETECTION = 11, + BOOT_EVENT_MAG_DETECTION = 12, + BOOT_EVENT_RANGEFINDER_DETECTION = 13, + BOOT_EVENT_MAG_INIT_FAILED = 14, + BOOT_EVENT_HMC5883L_READ_FAILED = 15, + BOOT_EVENT_HMC5883L_SATURATION = 16, + BOOT_EVENT_TIMER_CH_SKIPPED = 17, // 1 - MAX_MOTORS exceeded, 2 - MAX_SERVOS exceeded, 3 - feature clash + BOOT_EVENT_TIMER_CH_MAPPED = 18, // 0 - PPM, 1 - PWM, 2 - MOTOR, 3 - SERVO + + BOOT_EVENT_CODE_COUNT +} bootLogEventCode_e; + +#endif diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 64020362d..62e0de1bc 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -26,6 +26,8 @@ #include "io_impl.h" #include "timer.h" +#include "logging.h" + #include "pwm_output.h" #include "pwm_rx.h" #include "pwm_mapping.h" @@ -118,39 +120,53 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E - if (timerIndex == PWM2) + if (timerIndex == PWM2) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #endif #ifdef STM32F10X // skip UART2 ports - if (init->useUART2 && (timerIndex == PWM3 || timerIndex == PWM4)) + if (init->useUART2 && (timerIndex == PWM3 || timerIndex == PWM4)) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #endif #if defined(STM32F303xC) && defined(USE_UART3) // skip UART3 ports (PB10/PB11) - if (init->useUART3 && (timerHardwarePtr->tag == IO_TAG(UART3_TX_PIN) || timerHardwarePtr->tag == IO_TAG(UART3_RX_PIN))) + if (init->useUART3 && (timerHardwarePtr->tag == IO_TAG(UART3_TX_PIN) || timerHardwarePtr->tag == IO_TAG(UART3_RX_PIN))) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #endif #ifdef SOFTSERIAL_1_TIMER - if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_1_TIMER) + if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_1_TIMER) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #endif #ifdef SOFTSERIAL_2_TIMER - if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_2_TIMER) + if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_2_TIMER) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #endif #ifdef WS2811_TIMER // skip LED Strip output if (init->useLEDStrip) { - if (timerHardwarePtr->tim == WS2811_TIMER) + if (timerHardwarePtr->tim == WS2811_TIMER) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #if defined(STM32F303xC) && defined(WS2811_PIN) - if (timerHardwarePtr->tag == IO_TAG(WS2811_PIN)) + if (timerHardwarePtr->tag == IO_TAG(WS2811_PIN)) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } #endif } @@ -158,18 +174,21 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) #ifdef VBAT_ADC_PIN if (init->useVbat && timerHardwarePtr->tag == IO_TAG(VBAT_ADC_PIN)) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; } #endif #ifdef RSSI_ADC_GPIO if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; } #endif #ifdef CURRENT_METER_ADC_GPIO if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; } #endif @@ -180,6 +199,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) timerHardwarePtr->tag == init->sonarIOConfig.triggerTag || timerHardwarePtr->tag == init->sonarIOConfig.echoTag )) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; } #endif @@ -323,6 +343,8 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) ppmInConfig(timerHardwarePtr); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PPM; pwmIOConfiguration.ppmInputCount++; + + addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 0); #endif } else if (type == MAP_TO_PWM_INPUT) { #ifndef SKIP_RX_PWM_PPM @@ -330,18 +352,23 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PWM; pwmIOConfiguration.pwmInputCount++; channelIndex++; + + addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 1); #endif } else if (type == MAP_TO_MOTOR_OUTPUT) { /* Check if we already configured maximum supported number of motors and skip the rest */ if (pwmIOConfiguration.motorCount >= MAX_MOTORS) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 1); continue; } #if defined(CC3D) && !defined(CC3D_PPM1) if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed - if (timerHardwarePtr->tim == TIM2) + if (timerHardwarePtr->tim == TIM2) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); continue; + } } #endif if (init->useOneshot) { @@ -365,8 +392,10 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) pwmIOConfiguration.motorCount++; + addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 2); } else if (type == MAP_TO_SERVO_OUTPUT) { if (pwmIOConfiguration.servoCount >= MAX_SERVOS) { + addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 2); continue; } @@ -378,6 +407,8 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr; pwmIOConfiguration.servoCount++; + + addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); #endif } else { continue; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 70af0a864..bcb439e18 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -36,6 +36,8 @@ #include "common/color.h" #include "common/typeconversion.h" +#include "drivers/logging.h" + #include "drivers/system.h" #include "drivers/sensor.h" @@ -113,6 +115,10 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 16]; static void cliAssert(char *cmdline); #endif +#if defined(BOOTLOG) +static void cliBootlog(char *cmdline); +#endif + static void cliAux(char *cmdline); static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); @@ -269,6 +275,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("assert", "", NULL, cliAssert), #endif CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), +#if defined(BOOTLOG) + CLI_COMMAND_DEF("bootlog", "show boot events", NULL, cliBootlog), +#endif #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), @@ -1021,6 +1030,46 @@ static void cliAssert(char *cmdline) } #endif +#if defined(BOOTLOG) +static void cliBootlog(char *cmdline) +{ + UNUSED(cmdline); + + int bootEventCount = getBootlogEventCount(); + +#if defined(BOOTLOG_DESCRIPTIONS) + cliPrintf("Time Evt Description Parameters\r\n"); +#else + cliPrintf("Time Evt Parameters\r\n"); +#endif + + for (int idx = 0; idx < bootEventCount; idx++) { + bootLogEntry_t * event = getBootlogEvent(idx); + +#if defined(BOOTLOG_DESCRIPTIONS) + const char * eventDescription = getBootlogEventDescription(event->eventCode); + if (!eventDescription) { + eventDescription = ""; + } + + cliPrintf("%4d: %2d %22s ", event->timestamp, event->eventCode, eventDescription); +#else + cliPrintf("%4d: %2d ", event->timestamp, event->eventCode); +#endif + + if (event->eventFlags & BOOT_EVENT_FLAGS_PARAM16) { + cliPrintf(" (%d, %d, %d, %d)\r\n", event->params.u16[0], event->params.u16[1], event->params.u16[2], event->params.u16[3]); + } + else if (event->eventFlags & BOOT_EVENT_FLAGS_PARAM32) { + cliPrintf(" (%d, %d)\r\n", event->params.u32[0], event->params.u32[1]); + } + else { + cliPrintf("\r\n"); + } + } +} +#endif + static void cliAux(char *cmdline) { int i, val = 0; diff --git a/src/main/main.c b/src/main/main.c index 0ba1e8fe2..ac1357b39 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -31,6 +31,8 @@ #include "common/maths.h" #include "common/printf.h" +#include "drivers/logging.h" + #include "drivers/nvic.h" #include "drivers/sensor.h" @@ -145,6 +147,8 @@ void flashLedsAndBeep(void) void init(void) { + initBootlog(); + printfSupportInit(); initEEPROM(); @@ -152,6 +156,7 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); + addBootlogEvent2(BOOT_EVENT_CONFIG_LOADED, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_CONFIG_LOADED; systemInit(); @@ -178,6 +183,8 @@ void init(void) EXTIInit(); #endif + addBootlogEvent2(BOOT_EVENT_SYSTEM_INIT_DONE, BOOT_EVENT_FLAGS_NONE); + #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { @@ -282,6 +289,7 @@ void init(void) if (!feature(FEATURE_ONESHOT125)) motorControlEnable = true; + addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER @@ -394,6 +402,8 @@ void init(void) /* Extra 500ms delay prior to initialising hardware if board is cold-booting */ #if defined(GPS) || defined(MAG) if (!isMPUSoftReset()) { + addBootlogEvent2(BOOT_EVENT_EXTRA_BOOT_DELAY, BOOT_EVENT_FLAGS_NONE); + LED1_ON; LED0_OFF; @@ -436,6 +446,7 @@ void init(void) failureMode(FAILURE_MISSING_ACC); } + addBootlogEvent2(BOOT_EVENT_SENSOR_INIT_DONE, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); @@ -458,18 +469,20 @@ void init(void) &masterConfig.serialConfig, &masterConfig.gpsConfig ); + + addBootlogEvent2(BOOT_EVENT_GPS_INIT_DONE, BOOT_EVENT_FLAGS_NONE); } #endif #ifdef NAV - navigationInit( - &masterConfig.navConfig, - ¤tProfile->pidProfile, - ¤tProfile->rcControlsConfig, - &masterConfig.rxConfig, - &masterConfig.flight3DConfig, - &masterConfig.escAndServoConfig - ); + navigationInit( + &masterConfig.navConfig, + ¤tProfile->pidProfile, + ¤tProfile->rcControlsConfig, + &masterConfig.rxConfig, + &masterConfig.flight3DConfig, + &masterConfig.escAndServoConfig + ); #endif #ifdef LED_STRIP @@ -477,12 +490,14 @@ void init(void) if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); + addBootlogEvent2(BOOT_EVENT_LEDSTRIP_INIT_DONE, BOOT_EVENT_FLAGS_NONE); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); + addBootlogEvent2(BOOT_EVENT_TELEMETRY_INIT_DONE, BOOT_EVENT_FLAGS_NONE); } #endif @@ -561,6 +576,7 @@ void init(void) latchActiveFeatures(); motorControlEnable = true; + addBootlogEvent2(BOOT_EVENT_SYSTEM_READY, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_READY; } diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index cdca0cf0b..8be272e87 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/axis.h" +#include "drivers/logging.h" + #include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" @@ -313,6 +315,8 @@ bool detectGyro(void) gyroHardware = GYRO_NONE; } + addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0); + if (gyroHardware == GYRO_NONE) { return false; } @@ -451,6 +455,7 @@ retry: goto retry; } + addBootlogEvent6(BOOT_EVENT_ACC_DETECTION, BOOT_EVENT_FLAGS_NONE, accHardware, 0, 0, 0); if (accHardware == ACC_NONE) { return false; @@ -529,6 +534,8 @@ static bool detectBaro(baroSensor_e baroHardwareToUse) break; } + addBootlogEvent6(BOOT_EVENT_BARO_DETECTION, BOOT_EVENT_FLAGS_NONE, baroHardware, 0, 0, 0); + if (baroHardware == BARO_NONE) { return false; } @@ -651,6 +658,8 @@ static bool detectMag(magSensor_e magHardwareToUse) break; } + addBootlogEvent6(BOOT_EVENT_MAG_DETECTION, BOOT_EVENT_FLAGS_NONE, magHardware, 0, 0, 0); + // If not in autodetect mode and detected the wrong chip - disregard the compass even if detected if ((magHardwareToUse != MAG_DEFAULT && magHardware != magHardwareToUse) || (magHardware == MAG_NONE)) { return false; @@ -680,6 +689,9 @@ static rangefinderType_e detectRangefinder(void) rangefinderType = RANGEFINDER_SRF10; } #endif + + addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderType, 0, 0, 0); + detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; if (rangefinderType != RANGEFINDER_NONE) { @@ -724,6 +736,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, if (!detectGyro()) { return false; } + // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.targetLooptime = gyroSetSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator); // Set gyro sample rate before initialisation gyro.init(gyroLpf); // driver initialisation @@ -747,6 +760,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, if (detectMag(magHardwareToUse)) { // calculate magnetic declination if (!compassInit(magDeclinationFromConfig)) { + addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR); sensorsClear(SENSOR_MAG); } } diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a35ef870b..d059314da 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -171,7 +171,9 @@ #undef TELEMETRY_HOTT #undef TELEMETRY_SMARTPORT +#undef GPS_PROTO_NMEA #undef GPS_PROTO_NAZA +#undef GPS_PROTO_I2C_NAV #ifdef OPBL @@ -208,8 +210,8 @@ #define MAX_PWM_OUTPUT_PORTS 11 // DEBUG -#define USE_ASSERT // include assertion support code -#define USE_ASSERT_FULL // Provide file information +//#define USE_ASSERT // include assertion support code +//#define USE_ASSERT_FULL // Provide file information //#define USE_ASSERT_STOP // stop on failed assertion //#define USE_ASSERT_CHECK // include assertion check code (should in general a per-file define) diff --git a/src/main/target/common.h b/src/main/target/common.h index 8f533f651..82eb58598 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -28,6 +28,7 @@ #define SKIP_TASK_STATISTICS #define SKIP_CLI_COMMAND_HELP #else +#define BOOTLOG #define BLACKBOX #define GPS #define GPS_PROTO_NMEA @@ -46,6 +47,7 @@ #define DISPLAY #define DISPLAY_ARMED_BITMAP #define TELEMETRY_MAVLINK +#define BOOTLOG_DESCRIPTIONS #else #define SKIP_CLI_COMMAND_HELP #define SKIP_RX_MSP -- 2.11.4.GIT