diff --git a/AK8963.cpp b/AK8963.cpp index 8d3a5d8..8a7d69a 100644 --- a/AK8963.cpp +++ b/AK8963.cpp @@ -56,22 +56,20 @@ uint8_t AK8963::getStatusByte() { bool AK8963::startMeasurement() { ready = false; data_to_send[0] = AK8963_XOUT_L; - i2c->addTransfer((uint8_t)AK8963_ADDRESS, (uint8_t)1, &data_to_send[0], (uint8_t)7, &data_to_read[0], this); + i2c->addTransfer(AK8963_ADDRESS, 1, data_to_send, 7, data_to_read, this); return true; } -void AK8963::processCallback(uint8_t count, uint8_t *rawData) { - // count should always be 7 if we wanted to check... - - uint8_t c = rawData[6]; // ST2 register +void AK8963::triggerCallback() { + uint8_t c = data_to_read[6]; // ST2 register if (!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data // convert from REGISTER system to IC/PCB system // "Measurement data is stored in two’s complement and Little Endian format." // be careful not to misinterpret 2's complement registers int16_t registerValues[3]; - registerValues[0] = (int16_t)(((uint16_t)rawData[1]) << 8) | (uint16_t)rawData[0]; // low byte, high byte - registerValues[1] = (int16_t)(((uint16_t)rawData[3]) << 8) | (uint16_t)rawData[2]; - registerValues[2] = (int16_t)(((uint16_t)rawData[5]) << 8) | (uint16_t)rawData[4]; + registerValues[0] = (int16_t)(((uint16_t)data_to_read[1]) << 8) | (uint16_t)data_to_read[0]; // low byte, high byte + registerValues[1] = (int16_t)(((uint16_t)data_to_read[3]) << 8) | (uint16_t)data_to_read[2]; + registerValues[2] = (int16_t)(((uint16_t)data_to_read[5]) << 8) | (uint16_t)data_to_read[4]; magCount[0] = MAG_XSIGN * registerValues[MAG_XDIR]; magCount[1] = MAG_YSIGN * registerValues[MAG_YDIR]; magCount[2] = MAG_ZSIGN * registerValues[MAG_ZDIR]; diff --git a/AK8963.h b/AK8963.h index 0c51b16..2e1bc38 100644 --- a/AK8963.h +++ b/AK8963.h @@ -31,7 +31,7 @@ class AK8963 : public CallbackProcessor { bool ready; bool startMeasurement(); // writes values to state (when data is ready) - void processCallback(uint8_t count, uint8_t *rawData); // handles return for getAccelGryo() + void triggerCallback(); // handles return for getAccelGryo() uint8_t getID(); diff --git a/BMP280.cpp b/BMP280.cpp index 1b33d89..ed2ba19 100644 --- a/BMP280.cpp +++ b/BMP280.cpp @@ -75,15 +75,14 @@ boolean BMP280::validateCalibation() { bool BMP280::startMeasurement(void) { ready = false; data_to_send[0] = BMP280_REG_RESULT; - i2c->addTransfer((uint8_t)BMP280_ADDR, (uint8_t)1, data_to_send, (uint8_t)6, data_to_read, this); + i2c->addTransfer(BMP280_ADDR, 1, data_to_send, 6, data_to_read, this); return true; } -void BMP280::processCallback(uint8_t count, uint8_t *data) { - // count should always be 6 if we wanted to check... +void BMP280::triggerCallback() { int32_t rawP, rawT; - rawP = (((int32_t)data[0]) << 12) + (((int32_t)data[1]) << 4) + (((int32_t)data[2]) >> 4); - rawT = (((int32_t)data[3]) << 12) + (((int32_t)data[4]) << 4) + (((int32_t)data[5]) >> 4); + rawP = (((int32_t)data_to_read[0]) << 12) + (((int32_t)data_to_read[1]) << 4) + (((int32_t)data_to_read[2]) >> 4); + rawT = (((int32_t)data_to_read[3]) << 12) + (((int32_t)data_to_read[4]) << 4) + (((int32_t)data_to_read[5]) >> 4); state->temperature = compensate_T_int32(rawT); // calculate temp first to update t_fine state->pressure = compensate_P_int64(rawP); ready = true; diff --git a/BMP280.h b/BMP280.h index 57d1b4d..17a5525 100644 --- a/BMP280.h +++ b/BMP280.h @@ -49,7 +49,7 @@ class BMP280 : public CallbackProcessor { uint8_t getID(); bool startMeasurement(); - void processCallback(uint8_t count, uint8_t *rawData); // handles return for getPT() + void triggerCallback(); // handles return for getPT() private: State *state; diff --git a/MPU9250.cpp b/MPU9250.cpp index a2e151a..09a1826 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -12,6 +12,7 @@ #include #include #include +#include "board.h" #include "state.h" // we have three coordinate systems here: @@ -40,7 +41,7 @@ MPU9250::MPU9250(State *__state, I2CManager *__i2c) { state = __state; i2c = __i2c; ready = false; - pinMode(MPU_INTERRUPT, INPUT); + pinMode(board::MPU_INTERRUPT, INPUT); } uint8_t MPU9250::getID() { @@ -54,7 +55,7 @@ void MPU9250::restart() { } bool MPU9250::dataReadyInterrupt() { - return digitalRead(MPU_INTERRUPT); // use the interrupt pin -- be sure to set INT_PIN_CFG + return digitalRead(board::MPU_INTERRUPT); // use the interrupt pin -- be sure to set INT_PIN_CFG } uint8_t MPU9250::getStatusByte() { @@ -165,33 +166,31 @@ bool MPU9250::startMeasurement() { if (dataReadyInterrupt()) { ready = false; data_to_send[0] = ACCEL_XOUT_H; - i2c->addTransfer((uint8_t)MPU9250_ADDRESS, (uint8_t)1, &data_to_send[0], (uint8_t)14, &data_to_read[0], this); + i2c->addTransfer(MPU9250_ADDRESS, 1, data_to_send, 14, data_to_read, this); return true; } return false; } -void MPU9250::processCallback(uint8_t count, uint8_t *rawData) { - // count should always be 14 if we wanted to check... - +void MPU9250::triggerCallback() { // convert from REGISTER system to IC/PCB system int16_t registerValuesAccel[3]; // be careful not to misinterpret 2's complement registers - registerValuesAccel[0] = (int16_t)(((uint16_t)rawData[0]) << 8) | (uint16_t)rawData[1]; // high byte, low byte - registerValuesAccel[1] = (int16_t)(((uint16_t)rawData[2]) << 8) | (uint16_t)rawData[3]; - registerValuesAccel[2] = (int16_t)(((uint16_t)rawData[4]) << 8) | (uint16_t)rawData[5]; + registerValuesAccel[0] = (int16_t)(((uint16_t)data_to_read[0]) << 8) | (uint16_t)data_to_read[1]; // high byte, low byte + registerValuesAccel[1] = (int16_t)(((uint16_t)data_to_read[2]) << 8) | (uint16_t)data_to_read[3]; + registerValuesAccel[2] = (int16_t)(((uint16_t)data_to_read[4]) << 8) | (uint16_t)data_to_read[5]; accelCount[0] = ACCEL_XSIGN * registerValuesAccel[ACCEL_XDIR]; accelCount[1] = ACCEL_YSIGN * registerValuesAccel[ACCEL_YDIR]; accelCount[2] = ACCEL_ZSIGN * registerValuesAccel[ACCEL_ZDIR]; // be careful not to misinterpret 2's complement registers - temperatureCount[0] = (int16_t)(((uint16_t)rawData[6]) << 8) | (uint16_t)rawData[7]; + temperatureCount[0] = (int16_t)(((uint16_t)data_to_read[6]) << 8) | (uint16_t)data_to_read[7]; int16_t registerValuesGyro[3]; // be careful not to misinterpret 2's complement registers - registerValuesGyro[0] = (int16_t)(((uint16_t)rawData[8]) << 8) | (uint16_t)rawData[9]; // high byte, low byte - registerValuesGyro[1] = (int16_t)(((uint16_t)rawData[10]) << 8) | (uint16_t)rawData[11]; - registerValuesGyro[2] = (int16_t)(((uint16_t)rawData[12]) << 8) | (uint16_t)rawData[13]; + registerValuesGyro[0] = (int16_t)(((uint16_t)data_to_read[8]) << 8) | (uint16_t)data_to_read[9]; // high byte, low byte + registerValuesGyro[1] = (int16_t)(((uint16_t)data_to_read[10]) << 8) | (uint16_t)data_to_read[11]; + registerValuesGyro[2] = (int16_t)(((uint16_t)data_to_read[12]) << 8) | (uint16_t)data_to_read[13]; gyroCount[0] = GYRO_XSIGN * registerValuesGyro[GYRO_XDIR]; gyroCount[1] = GYRO_YSIGN * registerValuesGyro[GYRO_YDIR]; gyroCount[2] = GYRO_ZSIGN * registerValuesGyro[GYRO_ZDIR]; diff --git a/MPU9250.h b/MPU9250.h index e41125a..3dd34d8 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -34,7 +34,7 @@ class MPU9250 : public CallbackProcessor { void forgetBiasValues(); // discard bias values bool startMeasurement(); - void processCallback(uint8_t count, uint8_t *rawData); // handles return for getAccelGryo() + void triggerCallback(); // handles return for getAccelGryo() float getTemp() { return (float)temperatureCount[0] / 333.87 + 21.0; @@ -73,8 +73,6 @@ class MPU9250 : public CallbackProcessor { #define DEG2RAD 0.01745329251f -#define MPU_INTERRUPT 17 // 36 - //************************************************************* // // MPU Registers (See Table 1 Register Map on page 7) diff --git a/R415X.cpp b/R415X.cpp index b0e4317..21c0b66 100644 --- a/R415X.cpp +++ b/R415X.cpp @@ -5,6 +5,7 @@ */ #include "R415X.h" +#include "board.h" volatile uint16_t RX[RC_CHANNEL_COUNT]; // filled by the interrupt with valid data volatile uint16_t RX_errors = 0; // count dropped frames @@ -13,12 +14,12 @@ volatile uint16_t RX_buffer[RC_CHANNEL_COUNT]; // buffer data in anticipation o volatile uint8_t RX_channel = 0; // we are collecting data for this channel R415X::R415X() { - attemptToBind(50); - initialize_isr(); + attemptToBind(50); //this calls initialize_isr + //initialize_isr(); } void R415X::initialize_isr(void) { - pinMode(RX_DAT, INPUT); // WE ARE ASSUMING RX_DAT IS PIN 3 IN FTM1 SETUP! + pinMode(board::RX_DAT, INPUT); // WE ARE ASSUMING RX_DAT IS PIN 3 IN FTM1 SETUP! for (uint8_t i = 0; i <= RC_CHANNEL_COUNT; i++) { RX[i] = 1100; @@ -73,10 +74,10 @@ extern "C" void ftm1_isr(void) { } void R415X::attemptToBind(uint16_t milliseconds) { - pinMode(RX_DAT, OUTPUT); - digitalWrite(RX_DAT, LOW); + pinMode(board::RX_DAT, OUTPUT); + digitalWrite(board::RX_DAT, LOW); delay(milliseconds); - pinMode(RX_DAT, INPUT); // WE ARE ASSUMING RX_DAT IS PIN 3 IN FTM1 SETUP! + pinMode(board::RX_DAT, INPUT); // WE ARE ASSUMING RX_DAT IS PIN 3 IN FTM1 SETUP! // after we bind, we must setup our timer again. initialize_isr(); diff --git a/R415X.h b/R415X.h index 94bae74..2c8d74a 100644 --- a/R415X.h +++ b/R415X.h @@ -32,8 +32,6 @@ extern volatile uint16_t startPulse; // keeps track of the last received pulse extern volatile uint16_t RX_buffer[RC_CHANNEL_COUNT]; // buffer data in anticipation of a valid frame extern volatile uint8_t RX_channel; // we are collecting data for this channel -// pin definitions -#define RX_DAT 3 // 28 --- MUST BE PIN 3 #define RX_PPM_SYNCPULSE_MIN 7500 // 2.5ms #define RX_PPM_SYNCPULSE_MAX 48000 // 16 ms (seems to be about 13.4ms on the scope) diff --git a/README.md b/README.md index 4003d3f..bcc555a 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,18 @@ # flybrix-firmware -Flybrix is still in alpha, but we're excited to annouce our first open source code release! +Flybrix is in beta and still changing from time to time. Dependencies: * arduino 1.6.7 from https://www.arduino.cc/en/Main/Software * teensyduino libraries from https://www.pjrc.com/teensy/teensyduino.html * teensy loader from https://www.pjrc.com/teensy/loader.html +* SDFat library must be patched to use nonstandard pins (see 'SdSpiTeensy3.cpp.diff') Tips: -If you can't compile because it appears that you're missing the ADC or i2c_t3 libraries, +If you can't compile because it appears that you're missing libraries, be sure that you have set up the Arduino IDE to target the correct board ("Teensy 3.2 / 3.1")! The Arduino IDE defaults to expand tabs with 2 spaces. To change that edit your preferences file. diff --git a/SdSpiTeensy3.cpp.diff b/SdSpiTeensy3.cpp.diff new file mode 100644 index 0000000..566bfba --- /dev/null +++ b/SdSpiTeensy3.cpp.diff @@ -0,0 +1,8 @@ +91,93c91,93 +< CORE_PIN11_CONFIG = PORT_PCR_DSE | PORT_PCR_MUX(2); +< CORE_PIN12_CONFIG = PORT_PCR_MUX(2); +< CORE_PIN13_CONFIG = PORT_PCR_DSE | PORT_PCR_MUX(2); +--- +> CORE_PIN7_CONFIG = PORT_PCR_DSE | PORT_PCR_MUX(2); +> CORE_PIN8_CONFIG = PORT_PCR_MUX(2); +> CORE_PIN14_CONFIG = PORT_PCR_DSE | PORT_PCR_MUX(2); diff --git a/airframe.cpp b/airframe.cpp index 3854bb7..05c308f 100644 --- a/airframe.cpp +++ b/airframe.cpp @@ -5,13 +5,11 @@ */ #include "airframe.h" - +#include #include "config.h" //CONFIG variable - #include "state.h" -Airframe::Airframe(State* __state) { - state = __state; +Airframe::Airframe(State* state) : state(state) { } uint16_t Airframe::mix(int32_t mFz, int32_t mTx, int32_t mTy, int32_t mTz) { @@ -20,12 +18,6 @@ uint16_t Airframe::mix(int32_t mFz, int32_t mTx, int32_t mTy, int32_t mTz) { } void Airframe::updateMotorsMix() { - state->MotorOut[0] = mix(CONFIG.data.mixTableFz[0], CONFIG.data.mixTableTx[0], CONFIG.data.mixTableTy[0], CONFIG.data.mixTableTz[0]); - state->MotorOut[1] = mix(CONFIG.data.mixTableFz[1], CONFIG.data.mixTableTx[1], CONFIG.data.mixTableTy[1], CONFIG.data.mixTableTz[1]); - state->MotorOut[2] = mix(CONFIG.data.mixTableFz[2], CONFIG.data.mixTableTx[2], CONFIG.data.mixTableTy[2], CONFIG.data.mixTableTz[2]); - state->MotorOut[3] = mix(CONFIG.data.mixTableFz[3], CONFIG.data.mixTableTx[3], CONFIG.data.mixTableTy[3], CONFIG.data.mixTableTz[3]); - state->MotorOut[4] = mix(CONFIG.data.mixTableFz[4], CONFIG.data.mixTableTx[4], CONFIG.data.mixTableTy[4], CONFIG.data.mixTableTz[4]); - state->MotorOut[5] = mix(CONFIG.data.mixTableFz[5], CONFIG.data.mixTableTx[5], CONFIG.data.mixTableTy[5], CONFIG.data.mixTableTz[5]); - state->MotorOut[6] = mix(CONFIG.data.mixTableFz[6], CONFIG.data.mixTableTx[6], CONFIG.data.mixTableTy[6], CONFIG.data.mixTableTz[6]); - state->MotorOut[7] = mix(CONFIG.data.mixTableFz[7], CONFIG.data.mixTableTx[7], CONFIG.data.mixTableTy[7], CONFIG.data.mixTableTz[7]); + for (size_t i = 0; i < 8; ++i) + state->MotorOut[i] = mix(CONFIG.data.mixTableFz[i], CONFIG.data.mixTableTx[i], CONFIG.data.mixTableTy[i], CONFIG.data.mixTableTz[i]); } diff --git a/airframe.h b/airframe.h index c523087..cedd238 100644 --- a/airframe.h +++ b/airframe.h @@ -11,7 +11,7 @@ #ifndef airframe_h #define airframe_h -#include "Arduino.h" +#include class State; diff --git a/board.h b/board.h new file mode 100644 index 0000000..5fc305d --- /dev/null +++ b/board.h @@ -0,0 +1,157 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Definitions of pins used in boards. +*/ + +#ifndef BOARD_H +#define BOARD_H + +#include +#include + +#define BETA +#define BOARD_ADC_REF ADC_REF_3V3 //ADC_REF_1V2 + +namespace board { +#ifdef ALPHA +inline +#endif + namespace alpha { +enum Pins : uint8_t { + LED_B_BLU = 30, // 56 + LED_B_GRN = 26, // 2 + LED_B_RED = 31, // 1 + + LED_A_BLU = 11, // 51 + LED_A_GRN = 12, // 52 + LED_A_RED = 28, // 53 + + GREEN_LED = 13, // 50 + RED_LED = 27, // 54 + + V0_DETECT = A13, // ADC0_DM3 + I0_DETECT = A10, // ADC0_DP0 + I1_DETECT = A11, // ADC0_DM0 + + RX_DAT = 3, // 28 --- MUST BE PIN 3 + + MPU_INTERRUPT = 17, // 36 +}; + +constexpr i2c_pins I2C_PINS{I2C_PINS_18_19}; +constexpr i2c_pullup I2C_PULLUP{I2C_PULLUP_EXT}; + +constexpr uint8_t PWM[]{ + 25, // 42 + 32, // 41 + 22, // 44 + 20, // 62 + 23, // 45 + 21, // 63 + 9, // 46 + 5, // 64 +}; + +constexpr uint8_t FTM[]{ + // TODO: properly consider right FTM pins + 25, // 42 | PWM[0] + 22, // 44 | PWM[2] +}; + +namespace led { +enum PositionSimpleName : int8_t { + LEFT = -1, + RIGHT = 1, +}; +struct Position { + int8_t x; // left < 0 < right + int8_t y; // back < 0 < front +}; + +constexpr uint8_t COUNT{2}; +constexpr Position POSITION[]{ + {RIGHT, 0}, {LEFT, 0}, +}; +} +} + +#ifndef ALPHA +inline +#endif + namespace beta { +enum Pins : uint8_t { + GREEN_LED = 13, // 50 + RED_LED = 27, // 54 + + V0_DETECT = A13, // ADC0_DM3 + I0_DETECT = A10, // ADC0_DP0 + I1_DETECT = A11, // ADC0_DM0 + + RX_DAT = 3, // 28 --- MUST BE PIN 3 + + MPU_INTERRUPT = 17, // 36 +}; + +constexpr i2c_pins I2C_PINS{I2C_PINS_18_19}; +constexpr i2c_pullup I2C_PULLUP{I2C_PULLUP_EXT}; + +constexpr uint8_t PWM[]{ + 25, // 42 + 32, // 41 + 22, // 44 + 10, // 49 + 23, // 45 + 21, // 63 + 9, // 46 + 5, // 64 +}; + +constexpr uint8_t FTM[]{ + // TODO: properly consider right FTM pins + 25, // 42 | PWM[0] + 22, // 44 | PWM[2] +}; + +namespace spi { +enum SdCardPins : uint8_t { + MOSI = 7, + MISO = 8, + SCK = 14, +}; +enum ChipSelect : uint8_t { + SD_CARD = 15, // 43 + EXT = 2, // 57 +}; +} + +namespace led { +enum PositionSimpleName : int8_t { + FRONT = 1, + BACK = -1, + LEFT = -1, + RIGHT = 1, +}; +struct Position { + int8_t x; // left < 0 < right + int8_t y; // back < 0 < front +}; + +constexpr uint8_t DATA_PIN{11}; // 51 +constexpr uint8_t COUNT{4}; +constexpr Position POSITION[]{ + {LEFT, BACK}, {LEFT, FRONT}, {RIGHT, FRONT}, {RIGHT, BACK}, +}; +} + +namespace bluetooth { +constexpr uint8_t RESET{28}; +} +} +} + +#endif diff --git a/cardManagement.cpp b/cardManagement.cpp new file mode 100644 index 0000000..5c5b138 --- /dev/null +++ b/cardManagement.cpp @@ -0,0 +1,220 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + General interaction with the SD card, in the forms of logging or reading. +*/ + +#include "cardManagement.h" + +#include +#include +#include +#include +#include "board.h" +#include "debug.h" +#include "version.h" + +#ifdef ALPHA +#define SKIP_SD +#endif + +namespace sdcard { +namespace { +SdFat sd; + +bool openSDHardwarePort() { +#ifdef SKIP_SD + return false; +#else + bool opened = sd.begin(board::spi::SD_CARD, SPI_FULL_SPEED); + if (!opened) + DebugPrint("Failed to open connection to SD card!"); + return opened; +#endif +} + +bool openSD() { + static bool sd_open{openSDHardwarePort()}; + return sd_open; +} + +SdBaseFile binFile; +uint32_t bgnBlock, endBlock; +uint8_t* cache; + +uint32_t block_number = 0; + +// Number of 512B blocks that the file can contain +// The limit can be changed as needed, and currently it's ~128MB +constexpr uint32_t FILE_BLOCK_COUNT = 256000; +constexpr uint32_t ERASE_SIZE = 262144L; +// Number of blocks in the buffer +// Increase this number if there is trouble with buffer overflows caused by +// random delays in the SD card +// This can not fix overflows caused by too high data rates +// This causes the program to take up 0.5kB of RAM per buffer block +#ifdef SKIP_SD +constexpr uint32_t BUFFER_BLOCK_COUNT = 0; +#else +constexpr uint32_t BUFFER_BLOCK_COUNT = 4; +#endif + +class WritingBuffer { + public: + void write(const uint8_t* data, size_t length); + bool hasBlock() const; + uint8_t* popBlock(); + + private: + uint8_t block[BUFFER_BLOCK_COUNT][512]; + uint16_t startBlock{0}; + uint16_t currentBlock{0}; + uint16_t currentPointer{0}; + bool overbuffered{false}; +} writingBuffer; + +void WritingBuffer::write(const uint8_t* data, size_t length) { + if (overbuffered) + return; + for (size_t i = 0; i < length; ++i) { + block[currentBlock][currentPointer++] = data[i]; + if (currentPointer < 512) + continue; + currentPointer = 0; + if (++currentBlock >= BUFFER_BLOCK_COUNT) + currentBlock = 0; + if (currentBlock == startBlock) { + overbuffered = true; + DebugPrint("SD card buffer is full"); + return; + } + } +} + +bool WritingBuffer::hasBlock() const { + return overbuffered || (startBlock != currentBlock); +} + +uint8_t* WritingBuffer::popBlock() { + if (!hasBlock()) + return nullptr; + uint8_t* retval{block[startBlock]}; + if (++startBlock >= BUFFER_BLOCK_COUNT) + startBlock = 0; + overbuffered = false; + return retval; +} + +bool openFileHelper(const char* filename) { + binFile.close(); + if (!binFile.createContiguous(sd.vwd(), filename, 512 * FILE_BLOCK_COUNT)) + return false; + + if (!binFile.contiguousRange(&bgnBlock, &endBlock)) + return false; + + cache = (uint8_t*)sd.vol()->cacheClear(); + if (cache == 0) + return false; + + uint32_t bgnErase = bgnBlock; + uint32_t endErase; + while (bgnErase < endBlock) { + endErase = bgnErase + ERASE_SIZE; + if (endErase > endBlock) + endErase = endBlock; + if (!sd.card()->erase(bgnErase, endErase)) + return false; + bgnErase = endErase + 1; + } + + DebugPrint("Starting file write"); + if (!sd.card()->writeStart(bgnBlock, FILE_BLOCK_COUNT)) + return false; + + return true; +} + +bool openFile(const char* base_name) { + if (!openSD()) + return false; + if (binFile.isOpen()) + return false; + char file_dir[64]; + char filename[64]; + // Create the directory /__ if it doesn't exist + sprintf(file_dir, "%d_%d_%d", FIRMWARE_VERSION_A, FIRMWARE_VERSION_B, FIRMWARE_VERSION_C); + if (!sd.exists(file_dir) && !sd.mkdir(file_dir)) { + DebugPrintf("Failed to create directory %s on SD card!", file_dir); + return false; + } + for (int idx = 0; true; ++idx) { + sprintf(filename, "%s/%s_%d.bin", file_dir, base_name, idx); + // Look for the first non-existing filename of the format /__/_.bin + if (!sd.exists(filename)) { + bool success = openFileHelper(filename); + if (!success) + DebugPrintf("Failed to open file %s on SD card!", filename); + return success; + } + } + return false; +} +} // namespace + +void startup() { + openSD(); +} + +void openFile() { + if (!openSD()) + return; + openFile("st"); +} + +bool isOpen() { + return binFile.isOpen(); +} + +void write(const uint8_t* data, size_t length) { + if (!openSD()) + return; + if (!binFile.isOpen()) + return; + if (block_number == FILE_BLOCK_COUNT) + return; + if (length > 0) + writingBuffer.write(data, length); + if (sd.card()->isBusy()) + return; + if (!writingBuffer.hasBlock()) + return; + if (!sd.card()->writeData(writingBuffer.popBlock())) + DebugPrint("Failed to write data!"); + block_number++; +} + +void closeFile() { + if (!openSD()) + return; + if (!binFile.isOpen()) + return; + if (!sd.card()->writeStop()) { + DebugPrint("Write stop failed"); + return; + } + if (block_number != FILE_BLOCK_COUNT) { + if (!binFile.truncate(512L * block_number)) { + DebugPrint("Truncating failed"); + return; + } + } + block_number = 0; + binFile.close(); + DebugPrint("File closing successful"); +} +} // namespace sdcard diff --git a/cardManagement.h b/cardManagement.h new file mode 100644 index 0000000..0b43171 --- /dev/null +++ b/cardManagement.h @@ -0,0 +1,31 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + General interaction with the SD card, in the forms of logging or reading. +*/ + +#ifndef CARD_MANAGEMENT_H +#define CARD_MANAGEMENT_H + +#include + +namespace sdcard { +// Card startup takes a long time to perform +void startup(); + +// Opening and clearing the file takes a long time to perform +void openFile(); + +bool isOpen(); + +void write(const uint8_t* data, size_t length); + +// File closing (saving and truncating the file) takes a long time to perform +void closeFile(); +} + +#endif diff --git a/cobs.h b/cobs.h index c22d359..ea10f1c 100644 --- a/cobs.h +++ b/cobs.h @@ -140,4 +140,6 @@ class CobsPayload final { std::size_t l{1}; }; +using CobsReaderBuffer = CobsReader<500>; + #endif diff --git a/command.cpp b/command.cpp index 986701f..fab3468 100644 --- a/command.cpp +++ b/command.cpp @@ -10,6 +10,8 @@ #include "state.h" +#include "cardManagement.h" + PilotCommand::PilotCommand(State* __state) : state{__state}, throttle_command{&state->command_throttle}, pitch_command{&state->command_pitch}, roll_command{&state->command_roll}, yaw_command{&state->command_yaw} { } @@ -53,14 +55,14 @@ void PilotCommand::loadRxData() { roll.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[2]]; yaw.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[3]]; AUX1.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[4]]; - AUX2.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[5]]; + AUX2.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[5]]; } sei(); // enable interrupts // Set the AUX bit mask // bitfield order is {AUX1_low, AUX1_mid, AUX1_high, AUX2_low, AUX2_mid, AUX2_high, x, x} (LSB-->MSB) - + state->AUX_chan_mask = 0x00; // reset the AUX mode bitmask if (AUX1.isLow()) { @@ -94,10 +96,13 @@ void PilotCommand::processCommands(void) { state->processMotorEnablingIteration(); recentlyEnabled = true; throttleHoldOff = 80; // @40Hz -- hold for 2 sec + if (state->is(STATUS_ENABLED)) + sdcard::openFile(); } } else if (AUX1.isHigh() && !state->is(STATUS_OVERRIDE)) { if (state->is(STATUS_ENABLED) || state->is(STATUS_FAIL_STABILITY) || state->is(STATUS_FAIL_ANGLE)) { state->disableMotors(); + sdcard::closeFile(); } } @@ -111,6 +116,11 @@ void PilotCommand::processCommands(void) { if (recentlyEnabled && (throttleHoldOff == 0)) { recentlyEnabled = false; } + } else if (serialInput) { + *throttle_command = throttle_man; + *pitch_command = pitch_man; + *roll_command = roll_man; + *yaw_command = yaw_man; } else { // // ppm channels range from about 900-1200 usec, but we want to keep the bottom ~10% of throttle reserved for "off" @@ -123,11 +133,11 @@ void PilotCommand::processCommands(void) { *pitch_command = constrain((1-2*((CONFIG.data.commandInversion >> 0) & 1))*(pitch.val - CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[1]]) * 4095 / (pitch.max - pitch.min), -2047, 2047); *roll_command = constrain((1-2*((CONFIG.data.commandInversion >> 1) & 1))*( roll.val - CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[2]]) * 4095 / (roll.max - roll.min), -2047, 2047); *yaw_command = constrain((1-2*((CONFIG.data.commandInversion >> 2) & 1))*( yaw.val - CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[3]]) * 4095 / (yaw.max - yaw.min), -2047, 2047); - + // // in some cases it is impossible to get a ppm channel to be exactly 1500 usec because the controller trim is too coarse to correct a small error // we can get around by creating a small dead zone on the commands that are potentially effected - + *pitch_command = *pitch_command > 0 ? max(0, *pitch_command - (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[1]]) : min(*pitch_command + (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[1]], 0); *roll_command = *roll_command > 0 ? max(0, *roll_command - (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[2]]) : min(*roll_command + (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[2]], 0); *yaw_command = *yaw_command > 0 ? max(0, *yaw_command - (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[3]]) : min(*yaw_command + (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[3]], 0); @@ -139,3 +149,14 @@ void PilotCommand::processCommands(void) { // in the future, this would be the place to look for other combination inputs or for AUX levels that mean something } + +void PilotCommand::useSerialInput(bool useSerial) { + serialInput = useSerial; +} + +void PilotCommand::setRCValues(int16_t throttle, int16_t pitch, int16_t roll, int16_t yaw) { + throttle_man = throttle; + pitch_man = pitch; + roll_man = roll; + yaw_man = yaw; +} diff --git a/command.h b/command.h index 7eaf976..fea595f 100644 --- a/command.h +++ b/command.h @@ -61,6 +61,8 @@ class PilotCommand { public: PilotCommand(State* state); void processCommands(void); + void useSerialInput(bool); + void setRCValues(int16_t throttle, int16_t pitch, int16_t roll, int16_t yaw); private: State* state; @@ -81,6 +83,9 @@ class PilotCommand { int16_t* pitch_command; int16_t* roll_command; int16_t* yaw_command; + + bool serialInput{false}; + int16_t throttle_man{0}, pitch_man{0}, roll_man{0}, yaw_man{0}; }; #endif diff --git a/config.cpp b/config.cpp index abe7bae..5235f19 100644 --- a/config.cpp +++ b/config.cpp @@ -8,6 +8,7 @@ #include "version.h" void(*config_handler)(CONFIG_struct&) {nullptr}; +bool(*config_verifier)(const CONFIG_struct&) {nullptr}; CONFIG_union CONFIG; // global CONFIG variable @@ -34,24 +35,29 @@ void initializeEEPROM(void) { // Default Settings CONFIG.data.pcbTranslation[1] = 0.0f; // y (mm) CONFIG.data.pcbTranslation[2] = 0.0f; // z (mm) - // default "x quad" configuration: - // * CH0 ( CW: red +, blue -, type B prop) at front right - // * CH1 (CCW: wht +, blk -, type A prop) at front left - // * CH2 (CCW: wht +, blk -, type A prop) at rear right - // * CH3 ( CW: red +, blue -, type B prop) at rear left + // default configuration is the flat8 octocopter: + // * CH0 ( CW: red +, blue -, type A prop) at full front right + // * CH2 (CCW: wht +, blk -, type B prop) at mid front right + // * CH4 ( CW: red +, blue -, type A prop) at mid rear right + // * CH6 (CCW: wht +, blk -, type B prop) at full rear right + // * CH1 (CCW: wht +, blk -, type B prop) at full front left + // * CH3 ( CW: red +, blue -, type A prop) at mid front left + // * CH5 (CCW: wht +, blk -, type B prop) at mid rear left + // * CH7 ( CW: red +, blue -, type A prop) at rull rear left + // Note that the same mixtable can be used to build a quad on CH0, CH6, CH1, CH7 // - // pitch positive (nose up) needs a Tx negative restoring torque --> (Tx<0) should drop the nose by increasing 2 & 3 - // roll positive (RHS down) needs a Ty negative restoring torque --> (Ty<0) should raise the RHS by increasing 0 & 2 - // yaw positive (CCW rotation from top down) needs a Tz negative restoring torque --> (Tz<0) should decrease [1,2] CCW b/w motors & increase [0,3] CW r/b motors + // pitch positive (nose up) needs a Tx negative restoring torque --> (Tx<0) should drop the nose by increasing rear channels and decreasing front channels + // roll positive (right side down) needs a Ty negative restoring torque --> (Ty<0) should raise the right side by increasing right channels and decreasing left channels + // yaw positive (CCW rotation from top down) needs a Tz negative restoring torque --> (Tz<0) should decrease CCW motors & increase CW motors // - CONFIG.data.mixTableFz[0] = 1; CONFIG.data.mixTableTx[0] = 1; CONFIG.data.mixTableTy[0] = -1; CONFIG.data.mixTableTz[0] = -1; - CONFIG.data.mixTableFz[1] = 1; CONFIG.data.mixTableTx[1] = 1; CONFIG.data.mixTableTy[1] = 1; CONFIG.data.mixTableTz[1] = 1; - CONFIG.data.mixTableFz[2] = 1; CONFIG.data.mixTableTx[2] = -1; CONFIG.data.mixTableTy[2] = -1; CONFIG.data.mixTableTz[2] = 1; - CONFIG.data.mixTableFz[3] = 1; CONFIG.data.mixTableTx[3] = -1; CONFIG.data.mixTableTy[3] = 1; CONFIG.data.mixTableTz[3] = -1; - CONFIG.data.mixTableFz[4] = 0; CONFIG.data.mixTableTx[4] = 0; CONFIG.data.mixTableTy[4] = 0; CONFIG.data.mixTableTz[4] = 0; - CONFIG.data.mixTableFz[5] = 0; CONFIG.data.mixTableTx[5] = 0; CONFIG.data.mixTableTy[5] = 0; CONFIG.data.mixTableTz[5] = 0; - CONFIG.data.mixTableFz[6] = 0; CONFIG.data.mixTableTx[6] = 0; CONFIG.data.mixTableTy[6] = 0; CONFIG.data.mixTableTz[6] = 0; - CONFIG.data.mixTableFz[7] = 0; CONFIG.data.mixTableTx[7] = 0; CONFIG.data.mixTableTy[7] = 0; CONFIG.data.mixTableTz[7] = 0; + CONFIG.data.mixTableFz[0] = 1; CONFIG.data.mixTableTx[0] = 1; CONFIG.data.mixTableTy[0] = -1; CONFIG.data.mixTableTz[0] = 1; + CONFIG.data.mixTableFz[1] = 1; CONFIG.data.mixTableTx[1] = 1; CONFIG.data.mixTableTy[1] = 1; CONFIG.data.mixTableTz[1] = -1; + CONFIG.data.mixTableFz[2] = 1; CONFIG.data.mixTableTx[2] = 1; CONFIG.data.mixTableTy[2] = -1; CONFIG.data.mixTableTz[2] = -1; + CONFIG.data.mixTableFz[3] = 1; CONFIG.data.mixTableTx[3] = 1; CONFIG.data.mixTableTy[3] = 1; CONFIG.data.mixTableTz[3] = 1; + CONFIG.data.mixTableFz[4] = 1; CONFIG.data.mixTableTx[4] = -1; CONFIG.data.mixTableTy[4] = -1; CONFIG.data.mixTableTz[4] = 1; + CONFIG.data.mixTableFz[5] = 1; CONFIG.data.mixTableTx[5] = -1; CONFIG.data.mixTableTy[5] = 1; CONFIG.data.mixTableTz[5] = -1; + CONFIG.data.mixTableFz[6] = 1; CONFIG.data.mixTableTx[6] = -1; CONFIG.data.mixTableTy[6] = -1; CONFIG.data.mixTableTz[6] = -1; + CONFIG.data.mixTableFz[7] = 1; CONFIG.data.mixTableTx[7] = -1; CONFIG.data.mixTableTy[7] = 1; CONFIG.data.mixTableTz[7] = 1; CONFIG.data.magBias[0] = 0.0f; // Bx (milligauss) CONFIG.data.magBias[1] = 0.0f; // By (milligauss) @@ -66,7 +72,7 @@ void initializeEEPROM(void) { // Default Settings CONFIG.data.assignedChannel[5] = 5; // map AUX2 to LHS click CONFIG.data.commandInversion = 3; //invert pitch and roll - + CONFIG.data.channelMidpoint[0] = 1515; CONFIG.data.channelMidpoint[1] = 1515; CONFIG.data.channelMidpoint[2] = 1500; @@ -146,7 +152,7 @@ void initializeEEPROM(void) { // Default Settings CONFIG.data.yawSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz) CONFIG.data.yawSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz) CONFIG.data.yawSlavePIDParameters[6] = 240.0f; // (deg/sec / full stick action) - + CONFIG.data.pidBypass = BYPASS_THRUST_MASTER | BYPASS_THRUST_SLAVE | BYPASS_YAW_MASTER; //AHRS/Horizon mode CONFIG.data.stateEstimationParameters[0] = 1.00f; // 2*kp or BETA @@ -169,8 +175,12 @@ void writeEEPROM(void) { config_handler(CONFIG.data); } +bool isEmptyEEPROM() { + return EEPROM.read(0) == 255; +} + void readEEPROM(void) { - if (EEPROM.read(0) == 255) { + if (isEmptyEEPROM()) { // No EEPROM values detected, re-initialize to default values initializeEEPROM(); writeEEPROM(); // store the default values diff --git a/config.h b/config.h index 4999194..2c1134a 100644 --- a/config.h +++ b/config.h @@ -66,14 +66,13 @@ union CONFIG_union { uint8_t raw[sizeof(struct CONFIG_struct)]; }; -#define EEPROM_LOG_START 500 -#define EEPROM_LOG_END 2048 - extern void(*config_handler)(CONFIG_struct&); +extern bool(*config_verifier)(const CONFIG_struct&); extern CONFIG_union CONFIG; extern void initializeEEPROM(void); extern void writeEEPROM(void); extern void readEEPROM(void); +bool isEmptyEEPROM(); #endif diff --git a/control.cpp b/control.cpp index 9107d9c..c88065b 100644 --- a/control.cpp +++ b/control.cpp @@ -6,6 +6,7 @@ #include "control.h" #include "config.h" +#include "debug.h" #include "state.h" namespace { @@ -30,6 +31,16 @@ Control::Control(State* __state, CONFIG_struct& config) parseConfig(config); } +bool Control::verifyConfig(const CONFIG_struct& config) const { + bool retval{true}; + if (!(config.pidBypass & (1 << THRUST_SLAVE))) { + // If the thrust slave is enabled + DebugPrint("The slave PID of the thrust regulator must be disabled for now"); + retval = false; + } + return retval; +} + void Control::parseConfig(CONFIG_struct& config) { thrust_pid = {config.thrustMasterPIDParameters, config.thrustSlavePIDParameters}; pitch_pid = {config.pitchMasterPIDParameters, config.pitchSlavePIDParameters}; diff --git a/control.h b/control.h index bde0abf..93e7353 100644 --- a/control.h +++ b/control.h @@ -23,13 +23,14 @@ class Control { public: Control(State *state, CONFIG_struct& config); + bool verifyConfig(const CONFIG_struct& config) const; void parseConfig(CONFIG_struct& config); void calculateControlVectors(); State *state; uint32_t lastUpdateMicros = 0; // 1.2 hrs should be enough - + // unpack config.pidBypass for convenience bool pidEnabled[8]{false, false, false, false, false, false, false, false}; diff --git a/debug.cpp b/debug.cpp index 05fb00f..07c8033 100644 --- a/debug.cpp +++ b/debug.cpp @@ -9,31 +9,5 @@ */ #include "debug.h" -#include -#include SerialComm* debug_serial_comm{nullptr}; - -namespace debugdata { - -std::vector delta_times; - -unsigned long previous_timestamp = micros(); - -void catchTime(size_t index) { - unsigned long timestamp = micros(); - while (index >= delta_times.size()) - delta_times.push_back(-1.0f); // Use -1.0f as a flag that the variable is uninitialized - if (delta_times[index] < 0.0f) { - delta_times[index] = static_cast(timestamp - previous_timestamp); - } else { - delta_times[index] = 0.99f * delta_times[index] + 0.01f * (timestamp - previous_timestamp); - previous_timestamp = timestamp; - } -} - -} // namespace debugdata - -float READ_TIME(size_t index) { - return index < debugdata::delta_times.size() ? debugdata::delta_times[index] : 0.0f; -} diff --git a/debug.h b/debug.h index a8ec313..f1a66b8 100644 --- a/debug.h +++ b/debug.h @@ -15,18 +15,16 @@ #include "serial.h" -class SerialComm; - extern SerialComm* debug_serial_comm; -template +template void DebugPrint(T... args) { if (!debug_serial_comm) return; debug_serial_comm->SendDebugString(String(args...)); } -template +template void DebugPrintf(T... args) { if (!debug_serial_comm) return; @@ -35,39 +33,4 @@ void DebugPrintf(T... args) { DebugPrint(print_buffer); } -#ifdef DEBUG -#define assert(condition, message) \ - do { \ - if (!(condition)) { \ - Serial.print("DEBUG: "); \ - Serial.print("Assertion `"); \ - Serial.print(#condition); \ - Serial.print("` failed in "); \ - Serial.print((strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__)); \ - Serial.print(" line "); \ - Serial.print(__LINE__); \ - Serial.print(": "); \ - Serial.println(message); \ - } \ - } while (false) -namespace debugdata { -void catchTime(size_t index); -} // namespace debugdata - -#define CATCH_TIME(index) \ - do { \ - debugdata::catchTime(index); \ - } while (false) -float READ_TIME(size_t index); -#define DEBUGGING_ENABLED -#else -#define assert(condition, message) \ - do { \ - } while (false) -#define CATCH_TIME(index) \ - do { \ - } while (false) -float READ_TIME(size_t index); -#endif - #endif diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 85bf9da..788a528 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -1,10 +1,14 @@ +#include +#include +#include /* - * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * Flybrix Flight Controller -- Copyright 2015, 2016 Flying Selfie Inc. * * License and other details available at: http://www.flybrix.com/firmware - Credit is due to several other projects, including: + Credit for inspiration and guidance is due to several other projects, including: - multiwii ("https://github.com/multiwii") + - cleanflight ("https://github.com/cleanflight") - phoenix flight controller ("https://github.com/cTn-dev/Phoenix-FlightController") */ @@ -35,6 +39,10 @@ #include "localization.h" #include "debug.h" #include "version.h" +#include "board.h" +#include "cardManagement.h" +#include "serialFork.h" +#include "testMode.h" struct Systems { // subsystem objects initialize pins when created @@ -70,25 +78,31 @@ Systems::Systems() airframe{&state}, pilot{&state}, control{&state, CONFIG.data}, - conf{&state, RX, &control, &CONFIG, &led} // listen for configuration inputs + conf{&state, RX, &control, &CONFIG, &led, &pilot} // listen for configuration inputs { } void setup() { + config_handler = [&](CONFIG_struct& config){ sys.control.parseConfig(config); }; - debug_serial_comm = &sys.conf; + config_verifier = [&](const CONFIG_struct& config) { + bool retval{true}; + retval &= sys.control.verifyConfig(config); + return retval; + }; - // setup USB debug serial - Serial.begin(9600); // USB is always 12 Mbit/sec + debug_serial_comm = &sys.conf; // MPU9250 is limited to 400kHz bus speed. - Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400); // For I2C pins 18 and 19 + Wire.begin(I2C_MASTER, 0x00, board::I2C_PINS, board::I2C_PULLUP, I2C_RATE_400); // For I2C pins 18 and 19 sys.state.set(STATUS_BOOT); sys.led.update(); + bool go_to_test_mode{isEmptyEEPROM()}; + // load stored settings (this will reinitialize if there is no data in the EEPROM! readEEPROM(); sys.state.resetState(); @@ -126,40 +140,42 @@ void setup() { ; } + sys.led.update(); + + // factory test pattern runs only once + if (go_to_test_mode) + runTestMode(sys.state, sys.led, sys.motors); + + // Perform intial check for an SD card + sdcard::startup(); + sys.state.clear(STATUS_BOOT); sys.state.set(STATUS_IDLE); sys.led.update(); } -// Main loop variables +uint32_t low_battery_counter = 0; -uint32_t start_time = 0; -uint32_t state_updates = 0; -uint32_t interrupt_waits = 0; -uint32_t control_updates = 0; -uint32_t mpu_reads = 0; -uint32_t mag_reads = 0; -uint32_t bmp_reads = 0; -uint32_t pwr_reads = 0; +template +uint32_t RunProcess(uint32_t start); -uint32_t low_battery_counter = 0; +template +struct freqlist{}; -#define DEF_PROCESS_VARIABLES(F) uint32_t iterations_at_##F##Hz = 0; +void RunProcessesHelper(freqlist<>) { +} -/* F is target frequency, D is a tuning factor */ -#define RUN_PROCESS(F) \ - iterations_at_##F##Hz = RunProcess(micros()); \ +template +void RunProcessesHelper(freqlist) { + RunProcess(micros()); sys.i2c.update(); + RunProcessesHelper(freqlist()); +} -DEF_PROCESS_VARIABLES(1000) -DEF_PROCESS_VARIABLES(500) -DEF_PROCESS_VARIABLES(100) -DEF_PROCESS_VARIABLES(40) -DEF_PROCESS_VARIABLES(10) -DEF_PROCESS_VARIABLES(1) - -template -uint32_t RunProcess(uint32_t start); +template +void RunProcesses() { + RunProcessesHelper(freqlist()); +} template bool ProcessTask(); @@ -170,21 +186,14 @@ void loop() { sys.state.loopCount++; - if (start_time == 0) { - start_time = micros(); - } - sys.i2c.update(); // manages a queue of requests for mpu, mag, bmp if (sys.mpu.ready) { if (!skip_state_update) { sys.state.updateStateIMU(micros()); // update state as often as we can - state_updates++; } else { - interrupt_waits++; } if (sys.mpu.startMeasurement()) { - mpu_reads++; skip_state_update = false; } else { skip_state_update = true; // stop updating state until we queue another mpu measurement @@ -197,43 +206,35 @@ void loop() { sys.motors.updateAllChannels(); } else { sys.control.calculateControlVectors(); - control_updates++; sys.airframe.updateMotorsMix(); sys.motors.updateAllChannels(); } - RUN_PROCESS(1000) - RUN_PROCESS(500) - RUN_PROCESS(500) - RUN_PROCESS(100) - RUN_PROCESS(40) - RUN_PROCESS(10) - RUN_PROCESS(1) + RunProcesses<1000, 100, 40, 35, 30, 10, 1>(); } -uint32_t eeprom_log_start = EEPROM_LOG_START; - template <> bool ProcessTask<1000>() { - static uint16_t counter{0}; - if (++counter > sys.conf.GetSendStateDelay() - 1) { - counter = 0; - sys.conf.SendState(micros(), [&](uint8_t* data, size_t length) { - if (eeprom_log_start + length >= EEPROM_LOG_END) { - sys.state.set(STATUS_LOG_FULL); - return; - } - for (size_t i = 0; i < length; ++i) - EEPROM.write(eeprom_log_start++, data[i]); - }); + static uint16_t counterSerial{0}; + static uint16_t counterSdCard{0}; + if (++counterSerial > sys.conf.GetSendStateDelay() - 1) { + counterSerial = 0; + sys.conf.SendState(micros()); + } + if (++counterSdCard > sys.conf.GetSdCardStateDelay() - 1) { + counterSdCard = 0; + sys.conf.SendState(micros(), 0xFFFFFFFF, true); } - counter %= 1000; + counterSerial %= 1000; + counterSdCard %= 1000; + if (LEDFastUpdate) + LEDFastUpdate(); return true; } template <> -bool ProcessTask<500>() { +bool ProcessTask<30>() { sys.led.update(); // update quickly to support color dithering return true; } @@ -243,7 +244,6 @@ bool ProcessTask<100>() { if (sys.bmp.ready) { sys.state.updateStatePT(micros()); sys.bmp.startMeasurement(); - bmp_reads++; } else { return false; } @@ -257,17 +257,22 @@ bool ProcessTask<100>() { sys.state.clear(STATUS_SET_MPU_BIAS); } - sys.conf.ReadData(); // Respond to commands from the Configurator chrome extension + sys.conf.Read(); // Respond to commands from the Configurator chrome extension return true; } +template <> +bool ProcessTask<35>() { + flushSerial(); + return true; +} + template <> bool ProcessTask<40>() { sys.pilot.processCommands(); sys.pwr.measureRawLevels(); // read all ADCs - pwr_reads++; // check for low voltage condition if ( ((1/50)/0.003*1.2/65536 * sys.state.I1_raw ) > 1.0f ){ //if total battery current > 1A @@ -300,7 +305,6 @@ template <> bool ProcessTask<10>() { if (sys.mag.ready) { sys.mag.startMeasurement(); - mag_reads++; } else { return false; } @@ -310,39 +314,6 @@ bool ProcessTask<10>() { template <> bool ProcessTask<1>() { -#ifdef DEBUG - float elapsed_seconds = (micros() - start_time) / 1000000.0; - Serial.print("DEBUG: elapsed time (seconds) = "); - Serial.println(elapsed_seconds); - Serial.print("DEBUG: main loop rate (Hz) = "); - Serial.println(sys.state.loopCount / elapsed_seconds); - Serial.print("DEBUG: state update rate (Hz) = "); - Serial.println(state_updates / elapsed_seconds); - Serial.print("DEBUG: control update rate (Hz) = "); - Serial.println(control_updates / elapsed_seconds); - Serial.print("DEBUG: mpu read rate (Hz) = "); - Serial.println(mpu_reads / elapsed_seconds); - Serial.print("DEBUG: mag read rate (Hz) = "); - Serial.println(mag_reads / elapsed_seconds); - Serial.print("DEBUG: bmp read rate (Hz) = "); - Serial.println(bmp_reads / elapsed_seconds); - Serial.print("DEBUG: pwr read rate (Hz) = "); - Serial.println(pwr_reads / elapsed_seconds); - Serial.print("DEBUG: 500Hz rate (Hz) = "); - Serial.println(iterations_at_500Hz / elapsed_seconds); - Serial.print("DEBUG: 100Hz rate (Hz) = "); - Serial.println(iterations_at_100Hz / elapsed_seconds); - Serial.print("DEBUG: 40Hz rate (Hz) = "); - Serial.println(iterations_at_40Hz / elapsed_seconds); - Serial.print("DEBUG: 10Hz rate (Hz) = "); - Serial.println(iterations_at_10Hz / elapsed_seconds); - Serial.print("DEBUG: 1Hz rate (Hz) = "); - Serial.println(iterations_at_1Hz / elapsed_seconds); - Serial.print("DEBUG: interrupt wait rate (Hz) = "); - Serial.println(interrupt_waits / elapsed_seconds); - Serial.println(""); -#endif - return true; } diff --git a/i2cManager.cpp b/i2cManager.cpp index 55a13e7..ed86b8f 100644 --- a/i2cManager.cpp +++ b/i2cManager.cpp @@ -9,53 +9,49 @@ #include "i2cManager.h" #include -void I2CManager::addTransfer(uint8_t address, uint8_t send_count, uint8_t *send_data, uint8_t receive_count, uint8_t *receive_data, CallbackProcessor *cb_object) { - I2CTransfer *new_transfer = (I2CTransfer *)malloc(sizeof(I2CTransfer)); - new_transfer->address = address; - new_transfer->send_count = send_count; - new_transfer->send_data = send_data; - new_transfer->receive_count = receive_count; - new_transfer->receive_data = receive_data; - new_transfer->cb_object = cb_object; - new_transfer->next = NULL; - - if (!transfer) { - transfer = new_transfer; - } else { - I2CTransfer *last = transfer; - while (last->next) { - last = last->next; - } - last->next = new_transfer; +I2CTransfer& I2CManager::TransferQueue::front() const { + return firstItem->item; +} + +void I2CManager::TransferQueue::push(I2CTransfer&& newItem) { + std::unique_ptr* lastItem = &firstItem; + while (*lastItem) { + lastItem = &((*lastItem)->nextItem); } + lastItem->reset(new I2CManager::QueueItem{newItem, nullptr}); +} + +void I2CManager::TransferQueue::pop() { + if (empty()) + return; + firstItem.reset(firstItem->nextItem.release()); +} + +bool I2CManager::TransferQueue::empty() const { + return !firstItem; +} + +void I2CManager::addTransfer(uint8_t address, uint8_t send_count, uint8_t* send_data, uint8_t receive_count, uint8_t* receive_data, CallbackProcessor* cb_object) { + transfers.push(I2CTransfer{address, send_count, send_data, receive_count, receive_data, cb_object}); } void I2CManager::update() { if (waiting_for_data) { - if (Wire.available() == transfer->receive_count) { - for (uint8_t i = 0; i < transfer->receive_count; i++) { - transfer->receive_data[i] = Wire.read(); - } - transfer->cb_object->processCallback(transfer->receive_count, transfer->receive_data); - - I2CTransfer *completed_transfer = transfer; - if (transfer->next) { - transfer = transfer->next; - free(completed_transfer); - } else { - free(transfer); - transfer = NULL; - } + if (Wire.available() == transfers.front().receive_count) { + for (uint8_t i = 0; i < transfers.front().receive_count; i++) + transfers.front().receive_data[i] = Wire.read(); + transfers.front().cb_object->triggerCallback(); + transfers.pop(); waiting_for_data = false; } - } else if (transfer && !waiting_for_data) { + } else if (!transfers.empty() && !waiting_for_data) { // begin new transfer - Wire.beginTransmission(transfer->address); - Wire.write(transfer->send_data, transfer->send_count); + Wire.beginTransmission(transfers.front().address); + Wire.write(transfers.front().send_data, transfers.front().send_count); uint8_t error = Wire.endTransmission(); - if (error == 0 && transfer->receive_count > 0) { + if (error == 0 && transfers.front().receive_count > 0) { waiting_for_data = true; - Wire.requestFrom(transfer->address, transfer->receive_count); + Wire.requestFrom(transfers.front().address, transfers.front().receive_count); } else { // how do we want to handle errors? ignore for now } @@ -70,16 +66,9 @@ uint8_t I2CManager::readByte(uint8_t address, uint8_t subAddress) { return 0; } -uint8_t I2CManager::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest) { +uint8_t I2CManager::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t* dest) { int error; -#ifdef BMP280_SERIAL_DEBUG - Serial.print("attempting read register 0x"); - Serial.print(subAddress, HEX); - Serial.print(" from address 0x"); - Serial.println(address, HEX); -#endif - Wire.beginTransmission(address); Wire.write(subAddress); error = Wire.endTransmission(); @@ -87,18 +76,13 @@ uint8_t I2CManager::readBytes(uint8_t address, uint8_t subAddress, uint8_t count Wire.requestFrom(address, count); while (Wire.available() != count) ; // wait until bytes are ready - for (int i = 0; i < count; i++) { - dest[i] = Wire.read(); -#ifdef BMP280_SERIAL_DEBUG - Serial.print("received 0x"); - Serial.println(dest[i], HEX); -#endif - } + for (int i = 0; i < count; i++) + dest[i] = Wire.read(); - return (1); + return 1; } - return (0); + return 0; } uint8_t I2CManager::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) { diff --git a/i2cManager.h b/i2cManager.h index fa5f24d..66f96a7 100644 --- a/i2cManager.h +++ b/i2cManager.h @@ -12,41 +12,49 @@ #ifndef i2cManager_h #define i2cManager_h +#include #include "Arduino.h" -#include class CallbackProcessor { public: - virtual void processCallback(uint8_t count, uint8_t *data); + virtual void triggerCallback(); }; struct I2CTransfer { uint8_t address; uint8_t send_count; - uint8_t *send_data; + uint8_t* send_data; uint8_t receive_count; - uint8_t *receive_data; - CallbackProcessor *cb_object; - I2CTransfer *next; + uint8_t* receive_data; + CallbackProcessor* cb_object; }; class I2CManager { public: - I2CManager() { - waiting_for_data = false; - transfer = NULL; - }; - void update(); - void addTransfer(uint8_t address, uint8_t send_count, uint8_t *send_data, uint8_t receive_count, uint8_t *receive_data, CallbackProcessor *cb_object); + void addTransfer(uint8_t address, uint8_t send_count, uint8_t* send_data, uint8_t receive_count, uint8_t* receive_data, CallbackProcessor* cb_object); uint8_t readByte(uint8_t address, uint8_t subAddress); - uint8_t readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest); + uint8_t readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t* dest); uint8_t writeByte(uint8_t address, uint8_t subAddress, uint8_t data); private: - I2CTransfer *transfer; - bool waiting_for_data; + struct QueueItem { + I2CTransfer item; + std::unique_ptr nextItem; + }; + class TransferQueue { + public: + I2CTransfer& front() const; + void push(I2CTransfer&& newItem); + void pop(); + bool empty() const; + + private: + std::unique_ptr firstItem{nullptr}; + }; + TransferQueue transfers; + bool waiting_for_data{false}; }; // class I2CManager diff --git a/led.cpp b/led.cpp index 4978ae4..0efb072 100644 --- a/led.cpp +++ b/led.cpp @@ -4,45 +4,169 @@ * License and other details available at: http://www.flybrix.com/firmware */ -//#define LED_SERIAL_DEBUG - #include "led.h" #include "state.h" -LED::LED(State* __state) : state(__state) { - // RGB leds A and B are inverted - pinMode(LED_A_RED, OUTPUT); - pinMode(LED_A_GRN, OUTPUT); - pinMode(LED_A_BLU, OUTPUT); - pinMode(LED_B_RED, OUTPUT); - pinMode(LED_B_GRN, OUTPUT); - pinMode(LED_B_BLU, OUTPUT); +void (*LEDFastUpdate)(){nullptr}; - // indicator leds are not inverted - pinMode(GREEN_LED, OUTPUT); - pinMode(RED_LED, OUTPUT); +namespace { +class LEDDriver { + public: + LEDDriver(); + void setPattern(LED::Pattern pattern); + void setColor(CRGB color, board::led::Position lower_left = {-128, -128}, board::led::Position upper_right = {127, 127}); + void set(LED::Pattern pattern, CRGB color); + void update(); // fire this off at 30Hz + + uint8_t getCycleIndex() const; + uint8_t getPattern() const; + + private: + void writeToDisplay(); + void updateBeacon(); // 2sec periodic double pulse + void updateFlash(); //~3Hz flasher + void updateBreathe(); // 4sec periodic breathe + void updateSolid(); // maintain constant light level + + uint8_t cycleIndex{0}; + uint8_t pattern; + uint8_t scale{0}; + CRGB leds[board::led::COUNT]; + bool hasChanges{true}; +} LED_driver; + +uint8_t scrambleCycle(uint8_t x) { + x = (((x & 0xAA) >> 1) | ((x & 0x55) << 1)); + x = (((x & 0xCC) >> 2) | ((x & 0x33) << 2)); + return (x >> 4) | (x << 4); } -void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator) { - override = pattern != LED::NO_OVERRIDE; - if (!override) +inline uint8_t scaleLight(uint8_t light, uint8_t scale) { + return (static_cast(light) * static_cast(scale)) >> 8; +} + +LEDDriver::LEDDriver() { + setColor(CRGB::Black); + setPattern(LED::SOLID); +#ifndef ALPHA + FastLED.addLeds(leds, board::led::COUNT); +#else + pinMode(board::LED_A_RED, OUTPUT); + pinMode(board::LED_A_GRN, OUTPUT); + pinMode(board::LED_A_BLU, OUTPUT); + pinMode(board::LED_B_RED, OUTPUT); + pinMode(board::LED_B_GRN, OUTPUT); + pinMode(board::LED_B_BLU, OUTPUT); + LEDFastUpdate = []() { + static uint8_t cycle; + uint8_t threshold = scrambleCycle(++cycle); + digitalWriteFast(board::LED_A_RED, (scaleLight(LED_driver.leds[0].red, LED_driver.scale) > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_B_RED, (scaleLight(LED_driver.leds[1].red, LED_driver.scale) > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_A_GRN, (scaleLight(LED_driver.leds[0].green, LED_driver.scale) > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_B_GRN, (scaleLight(LED_driver.leds[1].green, LED_driver.scale) > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_A_BLU, (scaleLight(LED_driver.leds[0].blue, LED_driver.scale) > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_B_BLU, (scaleLight(LED_driver.leds[1].blue, LED_driver.scale) > threshold) ? LOW : HIGH); + }; +#endif +} + +uint8_t LEDDriver::getCycleIndex() const { + return cycleIndex; +} + +uint8_t LEDDriver::getPattern() const { + return pattern; +} + +inline bool isInside(const board::led::Position& p, const board::led::Position& p_min, const board::led::Position& p_max) { + return p.x >= p_min.x && p.y >= p_min.y && p.x <= p_max.x && p.y <= p_max.y; +} + +void LEDDriver::setColor(CRGB color, board::led::Position lower_left, board::led::Position upper_right) { +#ifndef ALPHA + color = CRGB(color.green, color.red, color.blue); +#endif + for (size_t idx = 0; idx < board::led::COUNT; ++idx) { + if (!isInside(board::led::POSITION[idx], lower_left, upper_right)) + continue; + if (leds[idx].red == color.red && leds[idx].green == color.green && leds[idx].blue == color.blue) + continue; + hasChanges = true; + leds[idx] = color; + } +} + +void LEDDriver::setPattern(LED::Pattern pattern) { + if (pattern == this->pattern) return; - oldStatus = 0; - red_indicator ? indicatorRedOn() : indicatorRedOff(); - green_indicator ? indicatorGreenOn() : indicatorGreenOff(); - use(pattern, red_a, green_a, blue_a, red_b, green_b, blue_b); + this->pattern = pattern; + hasChanges = true; + cycleIndex = 255; } -void LED::update() { - cycleIndex++; // 500Hz update - cycleIndex &= 4095; // ~8 second period +void LEDDriver::set(LED::Pattern pattern, CRGB color) { + setColor(color); + setPattern(pattern); +} - if (!override && oldStatus != state->status) { - oldStatus = state->status; - changeLights(); +void LEDDriver::update() { + ++cycleIndex; + writeToDisplay(); + if (!hasChanges) + return; +#ifndef ALPHA + FastLED.show(scale); +#else + if (LEDFastUpdate) + LEDFastUpdate(); +#endif + hasChanges = false; +} + +void LEDDriver::updateFlash() { + if (cycleIndex & 3) + return; + scale = (cycleIndex & 4) ? 0 : 255; + hasChanges = true; +} + +void LEDDriver::updateBeacon() { + switch ((cycleIndex & 63) >> 2) { // two second period + case 1: + case 4: + scale = 255; + hasChanges = true; + break; + case 2: + case 5: + scale = 0; + hasChanges = true; + break; + default: + break; } +} + +void LEDDriver::updateBreathe() { + uint16_t multiplier = cycleIndex & 127; + if (multiplier > 31) + return; + scale = multiplier; + if (scale > 15) + scale = 31 - scale; + scale <<= 4; + hasChanges = true; +} + +void LEDDriver::updateSolid() { + if (scale == 255) + return; + scale = 255; + hasChanges = true; +} - switch (lightType) { +void LEDDriver::writeToDisplay() { + switch (pattern) { case LED::FLASH: updateFlash(); break; @@ -53,160 +177,134 @@ void LED::update() { updateBreathe(); break; case LED::ALTERNATE: - updateAlternate(); - break; + // Alternate is handled outside of the driver + // and here it's just a solid light case LED::SOLID: updateSolid(); break; } } +} // namespace -void LED::use(Pattern pattern, uint8_t red, uint8_t green, uint8_t blue) { - use(pattern, red, green, blue, red, green, blue); +LED::LED(State* __state) : state(__state) { + // indicator leds are not inverted + pinMode(board::GREEN_LED, OUTPUT); + pinMode(board::RED_LED, OUTPUT); } -void LED::use(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) { - lightType = pattern; - this->red_a_ = red_a; - this->green_a_ = green_a; - this->blue_a_ = blue_a; - this->red_b_ = red_b; - this->green_b_ = green_b; - this->blue_b_ = blue_b; +void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator) { + set(pattern, {red_a, green_a, blue_a}, {red_b, green_b, blue_b}, red_indicator, green_indicator); } -void LED::changeLights() { - lightType = 0; - - if (state->is(STATUS_MPU_FAIL)) { - allOff(); // no dithering during setup! - digitalWriteFast(LED_A_RED, LOW); - indicatorRedOn(); - } else if (state->is(STATUS_BMP_FAIL)) { - allOff(); // no dithering during setup! - digitalWriteFast(LED_B_RED, LOW); - indicatorRedOn(); - } else if (state->is(STATUS_BOOT)) { - allOff(); // no dithering during setup! - digitalWriteFast(LED_A_GRN, LOW); - digitalWriteFast(LED_B_GRN, LOW); - } else if (state->is(STATUS_UNPAIRED)) { - // use(LED::FLASH, 255,180,20); //orange - use(LED::FLASH, 255, 255, 255); // for pcba testing purposes -- a "good" board will end up in this state - indicatorGreenOn(); // for pcba testing purposes -- a "good" board will end up in this state - } else if (state->is(STATUS_RX_FAIL)) { - use(LED::FLASH, 255, 0, 0); // red - } else if (state->is(STATUS_FAIL_STABILITY) || state->is(STATUS_FAIL_ANGLE)) { - use(LED::FLASH, 100, 110, 250); // yellow - } else if (state->is(STATUS_OVERRIDE)) { - use(LED::BEACON, 255, 0, 0); // red - } else if (state->is(STATUS_TEMP_WARNING)) { - use(LED::FLASH, 100, 150, 250); // yellow - } else if (state->is(STATUS_BATTERY_LOW)) { - use(LED::BEACON, 255, 180, 20); - } else if (state->is(STATUS_ENABLING)) { - use(LED::FLASH, 0, 0, 255); // blue - } else if (state->is(STATUS_ENABLED)) { - use(LED::BEACON, 0, 0, 255); // blue for enable - } else if (state->is(STATUS_IDLE)) { - indicatorRedOff(); // clear boot test - use(LED::BEACON, 0, 255, 0); // breathe instead? - } else { - // ERROR: ("NO STATUS BITS SET???"); - } +void LED::set(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indicator, bool green_indicator) { + override = pattern != LED::NO_OVERRIDE; + oldStatus = 0; + if (!override) + return; + use(pattern, color_right, color_left, red_indicator, green_indicator); } -uint8_t LED::getLightThreshold() { - uint8_t x = cycleIndex & 0xFF; - x = (((x & 0xAA) >> 1) | ((x & 0x55) << 1)); - x = (((x & 0xCC) >> 2) | ((x & 0x33) << 2)); - return (x >> 4) | (x << 4); +void LED::set(Pattern pattern, CRGB color, bool red_indicator, bool green_indicator) { + set(pattern, color, color, red_indicator, green_indicator); } -void LED::rgb() { - rgb(red_a_, green_a_, blue_a_, red_b_, green_b_, blue_b_); +void LED::update() { + if (!override && oldStatus != state->status) { + oldStatus = state->status; + changeLights(); + } + if (LED_driver.getPattern() == LED::ALTERNATE && !(LED_driver.getCycleIndex() & 15)) { + if (LED_driver.getCycleIndex() & 16) { + LED_driver.setColor(colorRight, {0, -128}, {127, 127}); + LED_driver.setColor(CRGB::Black, {-128, -128}, {0, 127}); + } else { + LED_driver.setColor(CRGB::Black, {0, -128}, {127, 127}); + LED_driver.setColor(colorLeft, {-128, -128}, {0, 127}); + } + } + LED_driver.update(); } -void LED::rgb(uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) { - uint8_t threshold = getLightThreshold(); - digitalWriteFast(LED_A_RED, (red_a > threshold) ? LOW : HIGH); - digitalWriteFast(LED_B_RED, (red_b > threshold) ? LOW : HIGH); - digitalWriteFast(LED_A_GRN, (green_a > threshold) ? LOW : HIGH); - digitalWriteFast(LED_B_GRN, (green_b > threshold) ? LOW : HIGH); - digitalWriteFast(LED_A_BLU, (blue_a > threshold) ? LOW : HIGH); - digitalWriteFast(LED_B_BLU, (blue_b > threshold) ? LOW : HIGH); +void LED::use(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indicator, bool green_indicator) { + colorRight = color_right; + colorLeft = color_left; + red_indicator ? indicatorRedOn() : indicatorRedOff(); + green_indicator ? indicatorGreenOn() : indicatorGreenOff(); + LED_driver.setPattern(pattern); + LED_driver.setColor(color_right, {0, -128}, {127, 127}); + LED_driver.setColor(color_left, {-128, -128}, {0, 127}); } -void LED::updateFlash() { - uint16_t cycleState = (cycleIndex & 511) >> 4; // flash at ~3 HZ - if (cycleState < 5 || (cycleState < 15 && cycleState > 9) || (cycleState < 26 && cycleState > 20)) - rgb(); - else - allOff(); +void LED::setWhite(board::led::Position lower_left, board::led::Position upper_right, bool red_indicator, bool green_indicator) { + colorRight = CRGB::Black; + colorLeft = CRGB::Black; + override = true; + oldStatus = 0; + red_indicator ? indicatorRedOn() : indicatorRedOff(); + green_indicator ? indicatorGreenOn() : indicatorGreenOff(); + LED_driver.setPattern(LED::SOLID); + LED_driver.setColor(CRGB::Black); + LED_driver.setColor(CRGB::White, lower_left, upper_right); } -void LED::updateBeacon() { - switch ((cycleIndex & 1023) >> 6) // two second period - { - case 1: - case 4: - rgb(); - break; +namespace { +struct LightCase { + LightCase(uint16_t status, LED::Pattern pattern, CRGB colorRight, CRGB colorLeft, bool indicatorRed = false, bool indicatorGreen = false); + LightCase(uint16_t status, LED::Pattern pattern, CRGB color); + uint16_t status; + LED::Pattern pattern; + CRGB colorRight; + CRGB colorLeft; + bool indicatorRed; + bool indicatorGreen; +}; - case 2: - case 5: - allOff(); - break; - - default: - break; - } +LightCase::LightCase(uint16_t status, LED::Pattern pattern, CRGB colorRight, CRGB colorLeft, bool indicatorRed, bool indicatorGreen) + : status{status}, pattern{pattern}, colorRight{colorRight}, colorLeft{colorLeft}, indicatorRed{indicatorRed}, indicatorGreen{indicatorGreen} { } -void LED::updateBreathe() { - uint16_t multiplier = cycleIndex & 2047; - if (cycleIndex > 511) { - allOff(); - return; - } - if (multiplier > 255) - multiplier = 512 - multiplier; - - rgb((multiplier * red_a_) >> 8, (multiplier * green_a_) >> 8, (multiplier * blue_a_) >> 8, (multiplier * red_b_) >> 8, (multiplier * green_b_) >> 8, (multiplier * blue_b_) >> 8); +LightCase::LightCase(uint16_t status, LED::Pattern pattern, CRGB color) : LightCase{status, pattern, color, color, false, false} { } -void LED::updateAlternate() { - if (cycleIndex & 128) - rgb(red_a_, green_a_, blue_a_, 0, 0, 0); - else - rgb(0, 0, 0, red_b_, green_b_, blue_b_); -} +const LightCase INDICATIONS[]{ + {STATUS_MPU_FAIL, LED::SOLID, CRGB::Black, CRGB::Red, true}, + {STATUS_BMP_FAIL, LED::SOLID, CRGB::Red, CRGB::Black, true}, + {STATUS_BOOT, LED::SOLID, CRGB::Green}, + {STATUS_UNPAIRED, LED::FLASH, CRGB::Orange, CRGB::Orange}, + {STATUS_RX_FAIL, LED::FLASH, CRGB::Red}, + {STATUS_FAIL_STABILITY, LED::FLASH, CRGB::Black, CRGB::Blue}, + {STATUS_FAIL_ANGLE, LED::FLASH, CRGB::Blue, CRGB::Black}, + {STATUS_OVERRIDE, LED::BEACON, CRGB::Red}, + {STATUS_TEMP_WARNING, LED::FLASH, CRGB::Red}, + {STATUS_BATTERY_LOW, LED::BEACON, CRGB::Orange}, + {STATUS_ENABLING, LED::FLASH, CRGB::Blue}, + {STATUS_ENABLED, LED::BEACON, CRGB::Blue}, + {STATUS_IDLE, LED::BEACON, CRGB::Green}, // breathe instead? +}; -void LED::updateSolid() { - rgb(); -} +} // namespace -void LED::allOff() { - digitalWriteFast(LED_A_RED, HIGH); - digitalWriteFast(LED_A_GRN, HIGH); - digitalWriteFast(LED_A_BLU, HIGH); - digitalWriteFast(LED_B_RED, HIGH); - digitalWriteFast(LED_B_GRN, HIGH); - digitalWriteFast(LED_B_BLU, HIGH); +void LED::changeLights() { + for (const LightCase& s : INDICATIONS) + if (state->is(s.status)) { + use(s.pattern, s.colorRight, s.colorLeft, s.indicatorRed, s.indicatorGreen); + return; + } + use(LED::ALTERNATE, CRGB::Red, CRGB::Red, true, false); // No status bits set } void LED::indicatorRedOn() { - digitalWriteFast(RED_LED, HIGH); + digitalWriteFast(board::RED_LED, HIGH); } void LED::indicatorGreenOn() { - digitalWriteFast(GREEN_LED, HIGH); + digitalWriteFast(board::GREEN_LED, HIGH); } void LED::indicatorRedOff() { - digitalWriteFast(RED_LED, LOW); + digitalWriteFast(board::RED_LED, LOW); } void LED::indicatorGreenOff() { - digitalWriteFast(GREEN_LED, LOW); + digitalWriteFast(board::GREEN_LED, LOW); } diff --git a/led.h b/led.h index 8cb9cfb..1d488b5 100644 --- a/led.h +++ b/led.h @@ -13,13 +13,17 @@ #define led_h #include "Arduino.h" +#define FASTLED_INTERNAL +#include "FastLED.h" + +#include "board.h" class State; +extern void (*LEDFastUpdate)(); + class LED { public: - LED(State *state); - enum Pattern : uint8_t { NO_OVERRIDE = 0, FLASH = 1, @@ -29,55 +33,31 @@ class LED { SOLID = 5, }; - void set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator); - - void update(); // update using the state STATUS bitfield + explicit LED(State *state); - private: - State *state; - uint16_t oldStatus{0}; - uint8_t lightType{0}; - uint8_t red_a_{0}, green_a_{0}, blue_a_{0}; - uint8_t red_b_{0}, green_b_{0}, blue_b_{0}; + void update(); - uint16_t cycleIndex{0}; // incremented at 500Hz to support dithering, resets every ~8 seconds + void set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator); - bool override{false}; + void set(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indicator, bool green_indicator); + void set(Pattern pattern, CRGB color, bool red_indicator = false, bool green_indicator = false); - uint8_t getLightThreshold(); + void setWhite(board::led::Position lower_left = {-128, -128}, board::led::Position upper_right = {127, 127}, bool red_indicator = false, bool green_indicator = false); + private: + void use(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indicator, bool green_indicator); void changeLights(); - void updateBeacon(); // 2sec periodic double pulse - void updateFlash(); //~3Hz flasher - void updateBreathe(); // 2sec periodic breathe - void updateAlternate(); //~4Hz left/right alternating - void updateSolid(); // maintain constant light level - void rgb(); // dithered color - void rgb(uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b); - void use(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b); - void use(Pattern pattern, uint8_t red, uint8_t green, uint8_t blue); - - void allOff(); - void indicatorRedOn(); void indicatorGreenOn(); void indicatorRedOff(); void indicatorGreenOff(); -}; // class LED - -// pin definitions - -#define LED_B_BLU 30 // 56 -#define LED_B_GRN 26 // 2 -#define LED_B_RED 31 // 1 - -#define LED_A_BLU 11 // 51 -#define LED_A_GRN 12 // 52 -#define LED_A_RED 28 // 53 + State *state; + uint16_t oldStatus{0}; + bool override{false}; -#define GREEN_LED 13 // 50 -#define RED_LED 27 // 54 + CRGB colorRight{CRGB::Black}, colorLeft{CRGB::Black}; +}; #endif diff --git a/motors.cpp b/motors.cpp index c5b4d37..1395c26 100644 --- a/motors.cpp +++ b/motors.cpp @@ -5,56 +5,26 @@ */ #include "motors.h" +#include "board.h" #include "state.h" -Motors::Motors(State* __state) { - state = __state; - +Motors::Motors(State* state) : state(state) { // REFERENCE: https://www.pjrc.com/teensy/td_pulse.html analogWriteResolution(12); // actual resolution depends on frequency - // FTM2 - pinMode(PWM0, OUTPUT); - pinMode(PWM1, OUTPUT); - analogWriteFrequency(PWM0, 11718); // changes all pins on FTM2 + for (auto pin : board::PWM) + pinMode(pin, OUTPUT); - // FTM0 - pinMode(PWM2, OUTPUT); - pinMode(PWM3, OUTPUT); - pinMode(PWM4, OUTPUT); - pinMode(PWM5, OUTPUT); - pinMode(PWM6, OUTPUT); - pinMode(PWM7, OUTPUT); - analogWriteFrequency(PWM2, 11718); // changes all pins on FTM0 + for (auto ftm : board::FTM) // changes all pins on FTM2 and FTM0 + analogWriteFrequency(ftm, 11718); } void Motors::updateAllChannels() { - // 12 bit output + bool motors_enabled{state->is(STATUS_ENABLED) || state->is(STATUS_OVERRIDE)}; for (uint8_t motor = 0; motor < 8; motor++) { + // 12 bit output state->MotorOut[motor] = constrain(state->MotorOut[motor], 0, 4095); + analogWrite(board::PWM[motor], motors_enabled ? state->MotorOut[motor] : 0); } - - if (state->is(STATUS_ENABLED) || state->is(STATUS_OVERRIDE)) { - analogWrite(PWM0, state->MotorOut[0]); - analogWrite(PWM1, state->MotorOut[1]); - analogWrite(PWM2, state->MotorOut[2]); - analogWrite(PWM3, state->MotorOut[3]); - analogWrite(PWM4, state->MotorOut[4]); - analogWrite(PWM5, state->MotorOut[5]); - analogWrite(PWM6, state->MotorOut[6]); - analogWrite(PWM7, state->MotorOut[7]); - } - else - { - analogWrite(PWM0, 0); - analogWrite(PWM1, 0); - analogWrite(PWM2, 0); - analogWrite(PWM3, 0); - analogWrite(PWM4, 0); - analogWrite(PWM5, 0); - analogWrite(PWM6, 0); - analogWrite(PWM7, 0); - - } } diff --git a/motors.h b/motors.h index 6ffd8c9..62766f4 100644 --- a/motors.h +++ b/motors.h @@ -25,14 +25,4 @@ class Motors { State* state; }; -// pin definitions -#define PWM7 5 // 64 -#define PWM6 9 // 46 -#define PWM5 21 // 63 -#define PWM4 23 // 45 -#define PWM3 20 // 62 -#define PWM2 22 // 44 -#define PWM1 32 // 41 -#define PWM0 25 // 42 - #endif diff --git a/power.cpp b/power.cpp index 4c2d2fb..ea0d19c 100644 --- a/power.cpp +++ b/power.cpp @@ -5,15 +5,16 @@ */ #include "power.h" +#include "board.h" #include "state.h" PowerMonitor::PowerMonitor(State* __state) { state = __state; // REFERENCE: https://github.com/pedvide/ADC - pinMode(V0_DETECT, INPUT); - pinMode(I0_DETECT, INPUT); - pinMode(I1_DETECT, INPUT); + pinMode(board::V0_DETECT, INPUT); + pinMode(board::I0_DETECT, INPUT); + pinMode(board::I1_DETECT, INPUT); adc.setReference(ADC_REF_1V2, ADC_0); adc.setAveraging(1, ADC_0); // set number of averages @@ -21,7 +22,7 @@ PowerMonitor::PowerMonitor(State* __state) { adc.setConversionSpeed(ADC_HIGH_SPEED, ADC_0); // change the conversion speed adc.setSamplingSpeed(ADC_HIGH_SPEED, ADC_0); // change the sampling speed - adc.setReference(ADC_REF_1V2, ADC_1); + adc.setReference(BOARD_ADC_REF, ADC_1); adc.setAveraging(1, ADC_1); // set number of averages adc.setResolution(16, ADC_1); // set bits of resolution adc.setConversionSpeed(ADC_HIGH_SPEED, ADC_1); // change the conversion speed @@ -43,15 +44,15 @@ float PowerMonitor::getElectronicsPower(void) { } uint16_t PowerMonitor::getV0Raw(void) { - return (uint16_t)adc.analogRead(V0_DETECT, ADC_1); + return (uint16_t)adc.analogRead(board::V0_DETECT, ADC_1); } uint16_t PowerMonitor::getI0Raw(void) { - return (uint16_t)adc.analogRead(I0_DETECT, ADC_0); + return (uint16_t)adc.analogRead(board::I0_DETECT, ADC_0); } uint16_t PowerMonitor::getI1Raw(void) { - return (uint16_t)adc.analogRead(I1_DETECT, ADC_0); + return (uint16_t)adc.analogRead(board::I1_DETECT, ADC_0); } float PowerMonitor::getV0(void) { diff --git a/power.h b/power.h index cc8b9ca..d6afaf3 100644 --- a/power.h +++ b/power.h @@ -47,9 +47,4 @@ class PowerMonitor { }; // class PowerMonitor -// pin definitions -#define V0_DETECT A13 // ADC0_DM3 -#define I0_DETECT A10 // ADC0_DP0 -#define I1_DETECT A11 // ADC0_DM0 - #endif diff --git a/serial.cpp b/serial.cpp index 21c41d2..47d4639 100644 --- a/serial.cpp +++ b/serial.cpp @@ -5,8 +5,12 @@ */ #include "serial.h" + +#include "serialFork.h" #include "state.h" +#include "cardManagement.h" +#include "command.h" #include "config.h" //CONFIG variable #include "control.h" #include "led.h" @@ -21,11 +25,12 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP } template -inline void WriteToOutput(CobsPayload& payload, void (*f)(uint8_t*, size_t) = nullptr) { +inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { auto package = payload.Encode(); - Serial.write(package.data, package.length); - if (f) - f(package.data, package.length); + if (use_logger) + sdcard::write(package.data, package.length); + else + writeSerial(package.data, package.length); } template @@ -34,21 +39,20 @@ inline void WritePIDData(CobsPayload& payload, const PID& pid) { } } -SerialComm::SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, const CONFIG_union* config, LED* led) : state{state}, ppm{ppm}, control{control}, config{config}, led{led} { +SerialComm::SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, CONFIG_union* config, LED* led, PilotCommand* command) + : state{state}, ppm{ppm}, control{control}, config{config}, led{led}, command{command} { } -void SerialComm::ReadData() { - while (Serial.available()) { - data_input.AppendToBuffer(Serial.read()); - - if (!data_input.IsDone()) - continue; - - ProcessData(); +void SerialComm::Read() { + for (;;) { + CobsReaderBuffer* buffer{readSerial()}; + if (buffer == nullptr) + return; + ProcessData(*buffer); } } -void SerialComm::ProcessData() { +void SerialComm::ProcessData(CobsReaderBuffer& data_input) { MessageType code; uint32_t mask; @@ -59,13 +63,20 @@ void SerialComm::ProcessData() { uint32_t ack_data{0}; - if (mask & COM_SET_EEPROM_DATA && data_input.ParseInto(config->raw)) { - writeEEPROM(); // TODO: deal with side effect code - ack_data |= COM_SET_EEPROM_DATA; + if (mask & COM_SET_EEPROM_DATA) { + CONFIG_union tmp_config; + if (data_input.ParseInto(tmp_config.raw)) { + if (!config_verifier || config_verifier(tmp_config.data)) { + config->data = tmp_config.data; + writeEEPROM(); // TODO: deal with side effect code + ack_data |= COM_SET_EEPROM_DATA; + } + } } if (mask & COM_REINIT_EEPROM_DATA) { initializeEEPROM(); // TODO: deal with side effect code writeEEPROM(); // TODO: deal with side effect code + ack_data |= COM_REINIT_EEPROM_DATA; } if (mask & COM_REQ_EEPROM_DATA) { SendConfiguration(); @@ -124,12 +135,12 @@ void SerialComm::ProcessData() { ack_data |= COM_SET_STATE_DELAY; } } - if (mask & COM_REQ_HISTORY) { - String eeprom_data; - for (size_t i = EEPROM_LOG_START; i < EEPROM_LOG_END; ++i) - eeprom_data += char(EEPROM[i]); - SendDebugString(eeprom_data, MessageType::HistoryData); - ack_data |= COM_REQ_HISTORY; + if (mask & COM_SET_SD_WRITE_DELAY) { + uint16_t new_state_delay; + if (data_input.ParseInto(new_state_delay)) { + sd_card_state_delay = new_state_delay; + ack_data |= COM_SET_SD_WRITE_DELAY; + } } if (mask & COM_SET_LED) { uint8_t mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g; @@ -138,6 +149,25 @@ void SerialComm::ProcessData() { ack_data |= COM_SET_LED; } } + if (mask & COM_SET_SERIAL_RC) { + uint8_t enabled; + int16_t throttle, pitch, roll, yaw; + if (data_input.ParseInto(enabled, throttle, pitch, roll, yaw)) { + command->useSerialInput(enabled); + command->setRCValues(throttle, pitch, roll, yaw); + ack_data |= COM_SET_SERIAL_RC; + } + } + if (mask & COM_SET_CARD_RECORDING) { + bool shouldRecordToCard; + if (data_input.ParseInto(shouldRecordToCard)) { + if (shouldRecordToCard) + sdcard::openFile(); + else + sdcard::closeFile(); + ack_data |= COM_SET_CARD_RECORDING; + } + } if (mask & COM_REQ_RESPONSE) { SendResponse(mask, ack_data); @@ -157,6 +187,7 @@ void SerialComm::SendDebugString(const String& string, MessageType type) const { size_t str_len = string.length(); for (size_t i = 0; i < str_len; ++i) payload.Append(string.charAt(i)); + payload.Append(uint8_t(0)); WriteToOutput(payload); } @@ -219,7 +250,10 @@ uint16_t SerialComm::PacketSize(uint32_t mask) const { return sum; } -void SerialComm::SendState(uint32_t timestamp_us, void (*extra_handler)(uint8_t*, size_t), uint32_t mask) const { +void SerialComm::SendState(uint32_t timestamp_us, uint32_t mask, bool redirect_to_sd_card) const { + // No need to build the message if we are not writing to the card + if (redirect_to_sd_card && !sdcard::isOpen()) + return; if (!mask) mask = state_mask; // No need to publish empty state messages @@ -286,7 +320,7 @@ void SerialComm::SendState(uint32_t timestamp_us, void (*extra_handler)(uint8_t* payload.Append(state->kinematicsAltitude); if (mask & SerialComm::STATE_LOOP_COUNT) payload.Append(state->loopCount); - WriteToOutput(payload, extra_handler); + WriteToOutput(payload, redirect_to_sd_card); } void SerialComm::SendResponse(uint32_t mask, uint32_t response) const { @@ -300,6 +334,10 @@ uint16_t SerialComm::GetSendStateDelay() const { return send_state_delay; } +uint16_t SerialComm::GetSdCardStateDelay() const { + return sd_card_state_delay; +} + void SerialComm::SetStateMsg(uint32_t values) { state_mask = values; } diff --git a/serial.h b/serial.h index a959240..44147b3 100644 --- a/serial.h +++ b/serial.h @@ -15,6 +15,7 @@ #include "cobs.h" union CONFIG_union; +class PilotCommand; class Control; class LED; class State; @@ -44,14 +45,15 @@ class SerialComm { COM_MOTOR_OVERRIDE_SPEED_5 = 1 << 10, COM_MOTOR_OVERRIDE_SPEED_6 = 1 << 11, COM_MOTOR_OVERRIDE_SPEED_7 = 1 << 12, - COM_MOTOR_OVERRIDE_SPEED_ALL = - COM_MOTOR_OVERRIDE_SPEED_0 | COM_MOTOR_OVERRIDE_SPEED_1 | COM_MOTOR_OVERRIDE_SPEED_2 | COM_MOTOR_OVERRIDE_SPEED_3 | - COM_MOTOR_OVERRIDE_SPEED_4 | COM_MOTOR_OVERRIDE_SPEED_5 | COM_MOTOR_OVERRIDE_SPEED_6 | COM_MOTOR_OVERRIDE_SPEED_7 , + COM_MOTOR_OVERRIDE_SPEED_ALL = COM_MOTOR_OVERRIDE_SPEED_0 | COM_MOTOR_OVERRIDE_SPEED_1 | COM_MOTOR_OVERRIDE_SPEED_2 | COM_MOTOR_OVERRIDE_SPEED_3 | COM_MOTOR_OVERRIDE_SPEED_4 | + COM_MOTOR_OVERRIDE_SPEED_5 | COM_MOTOR_OVERRIDE_SPEED_6 | COM_MOTOR_OVERRIDE_SPEED_7, COM_SET_COMMAND_OVERRIDE = 1 << 13, COM_SET_STATE_MASK = 1 << 14, COM_SET_STATE_DELAY = 1 << 15, - COM_REQ_HISTORY = 1 << 16, + COM_SET_SD_WRITE_DELAY = 1 << 16, COM_SET_LED = 1 << 17, + COM_SET_SERIAL_RC = 1 << 18, + COM_SET_CARD_RECORDING = 1 << 19, }; enum StateFields : uint32_t { @@ -86,33 +88,35 @@ class SerialComm { STATE_LOOP_COUNT = 1 << 27, }; - explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, const CONFIG_union* config, LED* led); + explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, CONFIG_union* config, LED* led, PilotCommand* command); - void ReadData(); + void Read(); void SendConfiguration() const; void SendDebugString(const String& string, MessageType type = MessageType::DebugString) const; - void SendState(uint32_t timestamp_us, void (*extra_handler)(uint8_t*, size_t) = nullptr, uint32_t mask = 0) const; + void SendState(uint32_t timestamp_us, uint32_t mask = 0, bool redirect_to_sd_card = false) const; void SendResponse(uint32_t mask, uint32_t response) const; uint16_t GetSendStateDelay() const; + uint16_t GetSdCardStateDelay() const; void SetStateMsg(uint32_t values); void AddToStateMsg(uint32_t values); void RemoveFromStateMsg(uint32_t values); private: - void ProcessData(); + void ProcessData(CobsReaderBuffer& data_input); uint16_t PacketSize(uint32_t mask) const; State* state; const volatile uint16_t* ppm; const Control* control; - const CONFIG_union* config; + CONFIG_union* config; LED* led; - uint16_t send_state_delay{1001}; //anything over 1000 turns off state messages + PilotCommand* command; + uint16_t send_state_delay{1001}; // anything over 1000 turns off state messages + uint16_t sd_card_state_delay{2}; // write to SD at the highest rate by default uint32_t state_mask{0x7fffff}; - CobsReader<500> data_input; }; #endif diff --git a/serialFork.cpp b/serialFork.cpp new file mode 100644 index 0000000..764fa15 --- /dev/null +++ b/serialFork.cpp @@ -0,0 +1,154 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Interface for forking serial communication channels. +*/ + +#include "serialFork.h" +#include +#include "board.h" + +namespace { +struct USBComm { + USBComm() { + Serial.begin(9600); // USB is always 12 Mbit/sec + } + + bool read() { + while (Serial.available()) { + data_input.AppendToBuffer(Serial.read()); + if (data_input.IsDone()) + return true; + } + return false; + } + + void write(uint8_t* data, size_t length) { + Serial.write(data, length); + } + + CobsReaderBuffer& buffer() { + return data_input; + } + + private: + CobsReaderBuffer data_input; +}; + +USBComm usb_comm; + +#ifndef ALPHA +struct Bluetooth { + Bluetooth() { + pinMode(board::bluetooth::RESET, OUTPUT); + digitalWrite(board::bluetooth::RESET, HIGH); + Serial1.begin(57600); + } + + bool read() { + while (Serial1.available()) { + data_input.AppendToBuffer(Serial1.read()); + if (data_input.IsDone()) + return true; + } + return false; + } + + void write(uint8_t* data, size_t length) { + data_output.push(data, length); + } + + CobsReaderBuffer& buffer() { + return data_input; + } + + void flush() { + size_t l{data_output.hasData()}; + if (!l) + return; + Serial1.write(data_output.pop(), l); + } + + private: + struct BluetoothBuffer { + size_t hasData() { + size_t increment{(readerPointer <= writerPointer) ? (writerPointer - readerPointer) : (bufferSize - readerPointer)}; + if (increment > bufferChunk) + return bufferChunk; + return increment; + } + + bool canFit(size_t size) { + if (readerPointer <= writerPointer) + return bufferSize - (writerPointer - readerPointer) > size; + else + return readerPointer - writerPointer > size; + } + + const uint8_t* pop() { + size_t outputPointer{readerPointer}; + readerPointer += hasData(); + if (readerPointer >= bufferSize) + readerPointer -= bufferSize; + return data + outputPointer; + } + + void push(uint8_t* input_data, size_t length) { + if (!canFit(length)) // The buffer is too full + return; + size_t lengthSubtraction{0}; + if (bufferSize - writerPointer < length) + lengthSubtraction = length - (bufferSize - writerPointer); + length -= lengthSubtraction; + for (size_t pos = 0; pos < length; ++pos) + data[writerPointer++] = input_data[pos]; + if (writerPointer == bufferSize) { + writerPointer = 0; + push(input_data + length, lengthSubtraction); + } + } + + private: + static constexpr size_t bufferCount{40}; + static constexpr size_t bufferChunk{20}; + static constexpr size_t bufferSize{bufferCount * bufferChunk}; + uint8_t data[bufferSize]; + size_t writerPointer{0}; + size_t readerPointer{0}; + }; + + size_t writerPosition{0}; + BluetoothBuffer data_output; + CobsReaderBuffer data_input; +}; + +Bluetooth bluetooth; +#endif +} + +CobsReaderBuffer* readSerial() { + if (usb_comm.read()) + return &usb_comm.buffer(); +#ifndef ALPHA + if (bluetooth.read()) + return &bluetooth.buffer(); +#endif + return nullptr; +} + +void writeSerial(uint8_t* data, size_t length) { + usb_comm.write(data, length); +#ifndef ALPHA + bluetooth.write(data, length); +#endif +} + +void flushSerial() { +#ifndef ALPHA + bluetooth.flush(); +#endif +} diff --git a/serialFork.h b/serialFork.h new file mode 100644 index 0000000..1262d01 --- /dev/null +++ b/serialFork.h @@ -0,0 +1,21 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Interface for forking serial communication channels. +*/ + +#ifndef SERIAL_FORK_H +#define SERIAL_FORK_H + +#include +#include "cobs.h" + +CobsReaderBuffer* readSerial(); +void writeSerial(uint8_t* data, size_t length); +void flushSerial(); + +#endif diff --git a/stlFix.cpp b/stlFix.cpp new file mode 100644 index 0000000..83431ae --- /dev/null +++ b/stlFix.cpp @@ -0,0 +1,23 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Implements missing elements for using certain STL libraries in Teensyduino. + +*/ + +#include "stlFix.h" +#include "debug.h" + +void std::__throw_bad_alloc() { + DebugPrintf("SYSTEM ERROR: Unable to allocate memory"); + exit(1); +} + +void std::__throw_length_error(const char* e) { + DebugPrintf("SYSTEM ERROR: Length error: %s", e); + exit(1); +} diff --git a/stlFix.h b/stlFix.h new file mode 100644 index 0000000..95423db --- /dev/null +++ b/stlFix.h @@ -0,0 +1,20 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Implements missing elements for using certain STL libraries in Teensyduino. + +*/ + +#ifndef STL_FIX_H +#define STL_FIX_H + +namespace std { +void __throw_bad_alloc(); +void __throw_length_error(const char* e); +} + +#endif /* end of include guard: STL_FIX_H */ diff --git a/testMode.cpp b/testMode.cpp new file mode 100644 index 0000000..d3c69f8 --- /dev/null +++ b/testMode.cpp @@ -0,0 +1,37 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Test mode for LED and motor functionality + +*/ + +#include "Arduino.h" +#include "led.h" +#include "motors.h" +#include "state.h" + +void runTestRoutine(State& state, LED& led, Motors& motors, size_t led_id, size_t motor_id) { + for (auto& motorOut : state.MotorOut) { + motorOut = 0; + } + state.MotorOut[motor_id] = 4095; + motors.updateAllChannels(); + led.setWhite(board::led::POSITION[led_id], board::led::POSITION[led_id], led_id % 2 == 0, led_id % 2 == 1); + led.update(); +} + +void runTestMode(State& state, LED& led, Motors& motors) { + state.set(STATUS_ENABLED); + size_t led_id{0}; + size_t motor_id{0}; + while (true) { + runTestRoutine(state, led, motors, led_id, motor_id); + led_id = (led_id + 1) % 4; + motor_id = (motor_id + 1) % 8; + delay(500); + } +} diff --git a/testMode.h b/testMode.h new file mode 100644 index 0000000..41f257d --- /dev/null +++ b/testMode.h @@ -0,0 +1,16 @@ +/* + * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. + * + * License and other details available at: http://www.flybrix.com/firmware + + + + Test mode for LED and motor functionality + +*/ + +class LED; +class Motors; +class State; + +void runTestMode(State& state, LED& led, Motors& motors); diff --git a/version.h b/version.h index 0fa39c5..6cbb6bf 100644 --- a/version.h +++ b/version.h @@ -3,13 +3,9 @@ * * License and other details available at: http://www.flybrix.com/firmware - + - EEPROM based configuration data storage structure - - Nonvolatile parameters are being stored inside an CONFIG structure - that can be accesed as data union, for easier manipulation as a javascript - ArrayBuffer object over serial. + Definitions for the firmware version. */ @@ -18,6 +14,6 @@ #define FIRMWARE_VERSION_A 1 #define FIRMWARE_VERSION_B 2 -#define FIRMWARE_VERSION_C 0 +#define FIRMWARE_VERSION_C 1 #endif