From 9de94c6c96e138776a7f2817494806d4f4e46fbd Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 11 Mar 2016 21:09:42 +0100 Subject: [PATCH 01/57] Fix #3 Add verification to config Configurations are now loaded into a temporary variable, prior to being written into the actual configuration location. Between those two steps, the configuration is verified for any illegal settings, and failing at it prevents writing the actual variable. The verification syntax seems overly complex at the moment, but it allows us a simple implementation of verification by each subsystem when the need arises. With this syntax, failure of verification for one system doesn't stop the verification process, but continues it, so we can detect all issues, and not only the first one detected. --- config.cpp | 1 + config.h | 1 + control.cpp | 11 +++++++++++ control.h | 3 ++- flybrix-firmware.ino | 6 ++++++ serial.cpp | 15 +++++++++++---- serial.h | 4 ++-- 7 files changed, 34 insertions(+), 7 deletions(-) diff --git a/config.cpp b/config.cpp index 1794373..ff2f8f1 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 diff --git a/config.h b/config.h index 85c1c79..dd98149 100644 --- a/config.h +++ b/config.h @@ -68,6 +68,7 @@ union CONFIG_union { #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); diff --git a/control.cpp b/control.cpp index e912668..4e42ddc 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/flybrix-firmware.ino b/flybrix-firmware.ino index f9f8cf9..b0edf0f 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -79,6 +79,12 @@ void setup() { sys.control.parseConfig(config); }; + config_verifier = [&](const CONFIG_struct& config) { + bool retval{true}; + retval &= sys.control.verifyConfig(config); + return retval; + }; + debug_serial_comm = &sys.conf; // setup USB debug serial diff --git a/serial.cpp b/serial.cpp index 21c41d2..baa8909 100644 --- a/serial.cpp +++ b/serial.cpp @@ -34,7 +34,7 @@ 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) : state{state}, ppm{ppm}, control{control}, config{config}, led{led} { } void SerialComm::ReadData() { @@ -59,9 +59,15 @@ 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 @@ -157,6 +163,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); } diff --git a/serial.h b/serial.h index a959240..18373f0 100644 --- a/serial.h +++ b/serial.h @@ -86,7 +86,7 @@ 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); void ReadData(); @@ -108,7 +108,7 @@ class SerialComm { 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 uint32_t state_mask{0x7fffff}; From 8dd1205bba1d64db3336dff4348140e3169c47a7 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 19:27:25 +0100 Subject: [PATCH 02/57] Move the most obvious pin bindings --- R415X.cpp | 1 + R415X.h | 2 -- board.h | 43 +++++++++++++++++++++++++++++++++++++++++++ led.cpp | 3 +-- led.h | 13 ------------- motors.cpp | 3 ++- motors.h | 10 ---------- power.cpp | 1 + power.h | 5 ----- 9 files changed, 48 insertions(+), 33 deletions(-) create mode 100644 board.h diff --git a/R415X.cpp b/R415X.cpp index b0e4317..681cc47 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 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/board.h b/board.h new file mode 100644 index 0000000..8a06d52 --- /dev/null +++ b/board.h @@ -0,0 +1,43 @@ +/* + * 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 + +#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 + +#define GREEN_LED 13 // 50 +#define RED_LED 27 // 54 + + +#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 + + +#define V0_DETECT A13 // ADC0_DM3 +#define I0_DETECT A10 // ADC0_DP0 +#define I1_DETECT A11 // ADC0_DM0 + + +#define RX_DAT 3 // 28 --- MUST BE PIN 3 + +#endif diff --git a/led.cpp b/led.cpp index 5c15945..277e1f0 100644 --- a/led.cpp +++ b/led.cpp @@ -4,9 +4,8 @@ * License and other details available at: http://www.flybrix.com/firmware */ -//#define LED_SERIAL_DEBUG - #include "led.h" +#include "board.h" #include "state.h" LED::LED(State* __state) : state(__state) { diff --git a/led.h b/led.h index 8cb9cfb..44d887e 100644 --- a/led.h +++ b/led.h @@ -67,17 +67,4 @@ class LED { }; // 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 - -#define GREEN_LED 13 // 50 -#define RED_LED 27 // 54 - #endif diff --git a/motors.cpp b/motors.cpp index c5b4d37..6f124c3 100644 --- a/motors.cpp +++ b/motors.cpp @@ -5,6 +5,7 @@ */ #include "motors.h" +#include "board.h" #include "state.h" Motors::Motors(State* __state) { @@ -55,6 +56,6 @@ void Motors::updateAllChannels() { 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..da42029 100644 --- a/power.cpp +++ b/power.cpp @@ -5,6 +5,7 @@ */ #include "power.h" +#include "board.h" #include "state.h" PowerMonitor::PowerMonitor(State* __state) { 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 From a9f2f0533bb9c06f93888ef5ec45aa088240ea9a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 20:19:41 +0100 Subject: [PATCH 03/57] Turn defines into one enum --- board.h | 61 +++++++++++++++++++++++++++++++-------------------------- 1 file changed, 33 insertions(+), 28 deletions(-) diff --git a/board.h b/board.h index 8a06d52..23ea997 100644 --- a/board.h +++ b/board.h @@ -11,33 +11,38 @@ #ifndef BOARD_H #define BOARD_H -#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 - -#define GREEN_LED 13 // 50 -#define RED_LED 27 // 54 - - -#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 - - -#define V0_DETECT A13 // ADC0_DM3 -#define I0_DETECT A10 // ADC0_DP0 -#define I1_DETECT A11 // ADC0_DM0 - - -#define RX_DAT 3 // 28 --- MUST BE PIN 3 +#include + +inline namespace board { +inline 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 + + PWM7 = 5, // 64 + PWM6 = 9, // 46 + PWM5 = 21, // 63 + PWM4 = 23, // 45 + PWM3 = 20, // 62 + PWM2 = 22, // 44 + PWM1 = 32, // 41 + PWM0 = 25, // 42 + + V0_DETECT = A13, // ADC0_DM3 + I0_DETECT = A10, // ADC0_DP0 + I1_DETECT = A11, // ADC0_DM0 + + RX_DAT = 3, // 28 --- MUST BE PIN 3 +}; +} +} #endif From 763d5eb469f3ba243e2d0432eb67db9d709a3e4b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 20:25:04 +0100 Subject: [PATCH 04/57] Prefix pin names with the board namespace --- R415X.cpp | 8 ++++---- board.h | 2 +- led.cpp | 56 +++++++++++++++++++++++++++--------------------------- motors.cpp | 52 +++++++++++++++++++++++++------------------------- power.cpp | 12 ++++++------ 5 files changed, 65 insertions(+), 65 deletions(-) diff --git a/R415X.cpp b/R415X.cpp index 681cc47..de66f11 100644 --- a/R415X.cpp +++ b/R415X.cpp @@ -19,7 +19,7 @@ R415X::R415X() { } 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; @@ -74,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/board.h b/board.h index 23ea997..f05a516 100644 --- a/board.h +++ b/board.h @@ -13,7 +13,7 @@ #include -inline namespace board { +namespace board { inline namespace alpha { enum Pins : uint8_t { LED_B_BLU = 30, // 56 diff --git a/led.cpp b/led.cpp index 277e1f0..f1e1a6a 100644 --- a/led.cpp +++ b/led.cpp @@ -10,16 +10,16 @@ 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); + 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); // indicator leds are not inverted - pinMode(GREEN_LED, OUTPUT); - pinMode(RED_LED, OUTPUT); + pinMode(board::GREEN_LED, OUTPUT); + pinMode(board::RED_LED, OUTPUT); } 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) { @@ -79,16 +79,16 @@ void LED::changeLights() { if (state->is(STATUS_MPU_FAIL)) { allOff(); // no dithering during setup! - digitalWriteFast(LED_A_RED, LOW); + digitalWriteFast(board::LED_A_RED, LOW); indicatorRedOn(); } else if (state->is(STATUS_BMP_FAIL)) { allOff(); // no dithering during setup! - digitalWriteFast(LED_B_RED, LOW); + digitalWriteFast(board::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); + digitalWriteFast(board::LED_A_GRN, LOW); + digitalWriteFast(board::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 @@ -132,12 +132,12 @@ void LED::rgb() { 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); + digitalWriteFast(board::LED_A_RED, (red_a > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_B_RED, (red_b > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_A_GRN, (green_a > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_B_GRN, (green_b > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_A_BLU, (blue_a > threshold) ? LOW : HIGH); + digitalWriteFast(board::LED_B_BLU, (blue_b > threshold) ? LOW : HIGH); } void LED::updateFlash() { @@ -190,26 +190,26 @@ void LED::updateSolid() { } 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); + digitalWriteFast(board::LED_A_RED, HIGH); + digitalWriteFast(board::LED_A_GRN, HIGH); + digitalWriteFast(board::LED_A_BLU, HIGH); + digitalWriteFast(board::LED_B_RED, HIGH); + digitalWriteFast(board::LED_B_GRN, HIGH); + digitalWriteFast(board::LED_B_BLU, HIGH); } 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/motors.cpp b/motors.cpp index 6f124c3..cade6c8 100644 --- a/motors.cpp +++ b/motors.cpp @@ -15,18 +15,18 @@ Motors::Motors(State* __state) { analogWriteResolution(12); // actual resolution depends on frequency // FTM2 - pinMode(PWM0, OUTPUT); - pinMode(PWM1, OUTPUT); - analogWriteFrequency(PWM0, 11718); // changes all pins on FTM2 + pinMode(board::PWM0, OUTPUT); + pinMode(board::PWM1, OUTPUT); + analogWriteFrequency(board::PWM0, 11718); // changes all pins on FTM2 // 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 + pinMode(board::PWM2, OUTPUT); + pinMode(board::PWM3, OUTPUT); + pinMode(board::PWM4, OUTPUT); + pinMode(board::PWM5, OUTPUT); + pinMode(board::PWM6, OUTPUT); + pinMode(board::PWM7, OUTPUT); + analogWriteFrequency(board::PWM2, 11718); // changes all pins on FTM0 } void Motors::updateAllChannels() { @@ -37,25 +37,25 @@ void Motors::updateAllChannels() { } 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]); + analogWrite(board::PWM0, state->MotorOut[0]); + analogWrite(board::PWM1, state->MotorOut[1]); + analogWrite(board::PWM2, state->MotorOut[2]); + analogWrite(board::PWM3, state->MotorOut[3]); + analogWrite(board::PWM4, state->MotorOut[4]); + analogWrite(board::PWM5, state->MotorOut[5]); + analogWrite(board::PWM6, state->MotorOut[6]); + analogWrite(board::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); + analogWrite(board::PWM0, 0); + analogWrite(board::PWM1, 0); + analogWrite(board::PWM2, 0); + analogWrite(board::PWM3, 0); + analogWrite(board::PWM4, 0); + analogWrite(board::PWM5, 0); + analogWrite(board::PWM6, 0); + analogWrite(board::PWM7, 0); } } diff --git a/power.cpp b/power.cpp index da42029..38e6cc2 100644 --- a/power.cpp +++ b/power.cpp @@ -12,9 +12,9 @@ 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 @@ -44,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) { From f0ab0a76e7605b203b9f861ba3189de699a243fa Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 20:39:21 +0100 Subject: [PATCH 05/57] Index PWM pins --- board.h | 19 ++++++++++--------- motors.cpp | 52 ++++++++++++++++++++++++++-------------------------- 2 files changed, 36 insertions(+), 35 deletions(-) diff --git a/board.h b/board.h index f05a516..8fb91c0 100644 --- a/board.h +++ b/board.h @@ -27,21 +27,22 @@ enum Pins : uint8_t { GREEN_LED = 13, // 50 RED_LED = 27, // 54 - PWM7 = 5, // 64 - PWM6 = 9, // 46 - PWM5 = 21, // 63 - PWM4 = 23, // 45 - PWM3 = 20, // 62 - PWM2 = 22, // 44 - PWM1 = 32, // 41 - PWM0 = 25, // 42 - V0_DETECT = A13, // ADC0_DM3 I0_DETECT = A10, // ADC0_DP0 I1_DETECT = A11, // ADC0_DM0 RX_DAT = 3, // 28 --- MUST BE PIN 3 }; +constexpr uint8_t PWM[]{ + 25, // 42 + 32, // 41 + 22, // 44 + 20, // 62 + 23, // 45 + 21, // 63 + 9, // 46 + 5, // 64 +}; } } diff --git a/motors.cpp b/motors.cpp index cade6c8..91c4cb0 100644 --- a/motors.cpp +++ b/motors.cpp @@ -15,18 +15,18 @@ Motors::Motors(State* __state) { analogWriteResolution(12); // actual resolution depends on frequency // FTM2 - pinMode(board::PWM0, OUTPUT); - pinMode(board::PWM1, OUTPUT); - analogWriteFrequency(board::PWM0, 11718); // changes all pins on FTM2 + pinMode(board::PWM[0], OUTPUT); + pinMode(board::PWM[1], OUTPUT); + analogWriteFrequency(board::PWM[0], 11718); // changes all pins on FTM2 // FTM0 - pinMode(board::PWM2, OUTPUT); - pinMode(board::PWM3, OUTPUT); - pinMode(board::PWM4, OUTPUT); - pinMode(board::PWM5, OUTPUT); - pinMode(board::PWM6, OUTPUT); - pinMode(board::PWM7, OUTPUT); - analogWriteFrequency(board::PWM2, 11718); // changes all pins on FTM0 + pinMode(board::PWM[2], OUTPUT); + pinMode(board::PWM[3], OUTPUT); + pinMode(board::PWM[4], OUTPUT); + pinMode(board::PWM[5], OUTPUT); + pinMode(board::PWM[6], OUTPUT); + pinMode(board::PWM[7], OUTPUT); + analogWriteFrequency(board::PWM[2], 11718); // changes all pins on FTM0 } void Motors::updateAllChannels() { @@ -37,25 +37,25 @@ void Motors::updateAllChannels() { } if (state->is(STATUS_ENABLED) || state->is(STATUS_OVERRIDE)) { - analogWrite(board::PWM0, state->MotorOut[0]); - analogWrite(board::PWM1, state->MotorOut[1]); - analogWrite(board::PWM2, state->MotorOut[2]); - analogWrite(board::PWM3, state->MotorOut[3]); - analogWrite(board::PWM4, state->MotorOut[4]); - analogWrite(board::PWM5, state->MotorOut[5]); - analogWrite(board::PWM6, state->MotorOut[6]); - analogWrite(board::PWM7, state->MotorOut[7]); + analogWrite(board::PWM[0], state->MotorOut[0]); + analogWrite(board::PWM[1], state->MotorOut[1]); + analogWrite(board::PWM[2], state->MotorOut[2]); + analogWrite(board::PWM[3], state->MotorOut[3]); + analogWrite(board::PWM[4], state->MotorOut[4]); + analogWrite(board::PWM[5], state->MotorOut[5]); + analogWrite(board::PWM[6], state->MotorOut[6]); + analogWrite(board::PWM[7], state->MotorOut[7]); } else { - analogWrite(board::PWM0, 0); - analogWrite(board::PWM1, 0); - analogWrite(board::PWM2, 0); - analogWrite(board::PWM3, 0); - analogWrite(board::PWM4, 0); - analogWrite(board::PWM5, 0); - analogWrite(board::PWM6, 0); - analogWrite(board::PWM7, 0); + analogWrite(board::PWM[0], 0); + analogWrite(board::PWM[1], 0); + analogWrite(board::PWM[2], 0); + analogWrite(board::PWM[3], 0); + analogWrite(board::PWM[4], 0); + analogWrite(board::PWM[5], 0); + analogWrite(board::PWM[6], 0); + analogWrite(board::PWM[7], 0); } } From c491d91c04d181487b852b3e717ff7cf353ecaca Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 21:04:04 +0100 Subject: [PATCH 06/57] Extract I2C and interrupt pins --- MPU9250.cpp | 5 +++-- MPU9250.h | 2 -- board.h | 7 +++++++ flybrix-firmware.ino | 3 ++- 4 files changed, 12 insertions(+), 5 deletions(-) diff --git a/MPU9250.cpp b/MPU9250.cpp index a2e151a..0949e5a 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() { diff --git a/MPU9250.h b/MPU9250.h index e41125a..ffe0ed2 100644 --- a/MPU9250.h +++ b/MPU9250.h @@ -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/board.h b/board.h index 8fb91c0..c896cc1 100644 --- a/board.h +++ b/board.h @@ -12,6 +12,7 @@ #define BOARD_H #include +#include namespace board { inline namespace alpha { @@ -32,7 +33,13 @@ enum Pins : uint8_t { 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 diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index b0edf0f..fdfe310 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -35,6 +35,7 @@ #include "localization.h" #include "debug.h" #include "version.h" +#include "board.h" struct Systems { // subsystem objects initialize pins when created @@ -91,7 +92,7 @@ void setup() { Serial.begin(9600); // USB is always 12 Mbit/sec // 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(); From add6955c20c0af2fb903a1363e8a1bd9df188538 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 23:15:17 +0100 Subject: [PATCH 07/57] Compress PWM code --- board.h | 6 ++++++ motors.cpp | 47 ++++++++--------------------------------------- 2 files changed, 14 insertions(+), 39 deletions(-) diff --git a/board.h b/board.h index c896cc1..5f68012 100644 --- a/board.h +++ b/board.h @@ -50,6 +50,12 @@ constexpr uint8_t PWM[]{ 9, // 46 5, // 64 }; + +constexpr uint8_t FTM[]{ + // TODO: properly consider right FTM pins + 25, // 42 | PWM[0] + 22, // 44 | PWM[2] +}; } } diff --git a/motors.cpp b/motors.cpp index 91c4cb0..1395c26 100644 --- a/motors.cpp +++ b/motors.cpp @@ -8,54 +8,23 @@ #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(board::PWM[0], OUTPUT); - pinMode(board::PWM[1], OUTPUT); - analogWriteFrequency(board::PWM[0], 11718); // changes all pins on FTM2 + for (auto pin : board::PWM) + pinMode(pin, OUTPUT); - // FTM0 - pinMode(board::PWM[2], OUTPUT); - pinMode(board::PWM[3], OUTPUT); - pinMode(board::PWM[4], OUTPUT); - pinMode(board::PWM[5], OUTPUT); - pinMode(board::PWM[6], OUTPUT); - pinMode(board::PWM[7], OUTPUT); - analogWriteFrequency(board::PWM[2], 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(board::PWM[0], state->MotorOut[0]); - analogWrite(board::PWM[1], state->MotorOut[1]); - analogWrite(board::PWM[2], state->MotorOut[2]); - analogWrite(board::PWM[3], state->MotorOut[3]); - analogWrite(board::PWM[4], state->MotorOut[4]); - analogWrite(board::PWM[5], state->MotorOut[5]); - analogWrite(board::PWM[6], state->MotorOut[6]); - analogWrite(board::PWM[7], state->MotorOut[7]); - } - else - { - analogWrite(board::PWM[0], 0); - analogWrite(board::PWM[1], 0); - analogWrite(board::PWM[2], 0); - analogWrite(board::PWM[3], 0); - analogWrite(board::PWM[4], 0); - analogWrite(board::PWM[5], 0); - analogWrite(board::PWM[6], 0); - analogWrite(board::PWM[7], 0); - - } } From eb189531e00d01bcf877f0d1f266592685282229 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 15 Mar 2016 23:26:37 +0100 Subject: [PATCH 08/57] Compress the airframe code --- airframe.cpp | 16 ++++------------ airframe.h | 2 +- 2 files changed, 5 insertions(+), 13 deletions(-) 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; From b06c3650b228c929f0b46537542002f39136b5c2 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 17 Mar 2016 22:12:40 +0100 Subject: [PATCH 09/57] Apply first iteration of the new LED interface The LEDs work at a 30Hz rate Individual updates of lights eat up 100us. Worst case: LED::BREATHE during the second in which things update - eating up 3ms over the span of a second. Other cases eat up 0-600us per second. --- board.h | 62 ++++++++- flybrix-firmware.ino | 7 +- led.cpp | 300 +++++++++++++++++++++++-------------------- led.h | 40 ++---- 4 files changed, 238 insertions(+), 171 deletions(-) diff --git a/board.h b/board.h index 5f68012..5ac5f36 100644 --- a/board.h +++ b/board.h @@ -15,7 +15,10 @@ #include namespace board { -inline namespace alpha { +#ifdef ALPHA +inline +#endif + namespace alpha { enum Pins : uint8_t { LED_B_BLU = 30, // 56 LED_B_GRN = 26, // 2 @@ -57,6 +60,63 @@ constexpr uint8_t FTM[]{ 22, // 44 | PWM[2] }; } + +#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 + 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 { + 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, FRONT}, {LEFT, BACK}, {RIGHT, BACK}, {RIGHT, FRONT}, +}; +} +} } #endif diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index fdfe310..03d6a58 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -158,9 +158,9 @@ uint32_t pwr_reads = 0; sys.i2c.update(); DEF_PROCESS_VARIABLES(1000) -DEF_PROCESS_VARIABLES(500) DEF_PROCESS_VARIABLES(100) DEF_PROCESS_VARIABLES(40) +DEF_PROCESS_VARIABLES(30) DEF_PROCESS_VARIABLES(10) DEF_PROCESS_VARIABLES(1) @@ -210,10 +210,9 @@ void loop() { } RUN_PROCESS(1000) - RUN_PROCESS(500) - RUN_PROCESS(500) RUN_PROCESS(100) RUN_PROCESS(40) + RUN_PROCESS(30) RUN_PROCESS(10) RUN_PROCESS(1) } @@ -239,7 +238,7 @@ bool ProcessTask<1000>() { } template <> -bool ProcessTask<500>() { +bool ProcessTask<30>() { sys.led.update(); // update quickly to support color dithering return true; } diff --git a/led.cpp b/led.cpp index f1e1a6a..44e8c32 100644 --- a/led.cpp +++ b/led.cpp @@ -8,40 +8,134 @@ #include "board.h" #include "state.h" -LED::LED(State* __state) : state(__state) { - // RGB leds A and B are inverted - 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); +#define NO_LEDS // Instead of looking at actual LEDs, we look at the serial debug for feedback +#ifdef NO_LEDS +#include "debug.h" +#endif + +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 + + private: + void writeToDisplay(); + void updateBeacon(); // 2sec periodic double pulse + void updateFlash(); //~3Hz flasher + void updateBreathe(); // 4sec periodic breathe + void updateAlternate(); // 4sec on - 4sec off alternating TODO: use the proper left-right alternating + void updateSolid(); // maintain constant light level + + uint8_t cycleIndex{0}; + uint8_t pattern; + uint8_t scale{0}; + CRGB leds[board::led::COUNT]; + CRGB leds_display[board::led::COUNT]; + bool hasChanges{true}; +} LED_driver; + +LEDDriver::LEDDriver() { + setColor(CRGB::Black); + setPattern(LED::SOLID); + FastLED.addLeds(leds_display, board::led::COUNT); +} + +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) { + 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; + } +} - // indicator leds are not inverted - pinMode(board::GREEN_LED, OUTPUT); - pinMode(board::RED_LED, OUTPUT); +void LEDDriver::setPattern(LED::Pattern pattern) { + if (pattern == this->pattern) + return; + this->pattern = pattern; + hasChanges = true; + cycleIndex = 255; } -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) +void LEDDriver::set(LED::Pattern pattern, CRGB color) { + setColor(color); + setPattern(pattern); +} + +void LEDDriver::update() { + ++cycleIndex; + writeToDisplay(); + if (!hasChanges) 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); + FastLED.show(scale); +#ifdef NO_LEDS + DebugPrintf("%d %d %d | %d %d %d | %d %d %d | %d %d %d | %d", leds[0].red, leds[0].green, leds[0].blue, leds[1].red, leds[1].green, leds[1].blue, leds[2].red, leds[2].green, leds[2].blue, + leds[3].red, leds[3].green, leds[3].blue, scale); +#endif + hasChanges = false; } -void LED::update() { - cycleIndex++; // 500Hz update - cycleIndex &= 4095; // ~8 second period +void LEDDriver::updateFlash() { + if (cycleIndex & 3) + return; + scale = (cycleIndex & 4) ? 0 : 255; + hasChanges = true; +} - if (!override && oldStatus != state->status) { - oldStatus = state->status; - changeLights(); +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; +} - switch (lightType) { +void LEDDriver::updateAlternate() { + if (cycleIndex & 127) + return; + scale = (cycleIndex & 128) ? 0 : 255; + hasChanges = true; +} + +void LEDDriver::updateSolid() { + if (scale == 255) + return; + scale = 255; + hasChanges = true; +} + +void LEDDriver::writeToDisplay() { + switch (pattern) { case LED::FLASH: updateFlash(); break; @@ -59,145 +153,75 @@ void LED::update() { 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::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; + oldStatus = 0; + if (!override) + return; + red_indicator ? indicatorRedOn() : indicatorRedOff(); + green_indicator ? indicatorGreenOn() : indicatorGreenOff(); + LED_driver.setPattern(pattern); + LED_driver.setColor({red_a, green_a, blue_a}, {0, -128}, {127, 127}); + LED_driver.setColor({red_b, green_b, blue_b}, {-128, -128}, {0, 127}); } -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::update() { + if (!override && oldStatus != state->status) { + oldStatus = state->status; + changeLights(); + } + LED_driver.update(); } void LED::changeLights() { - lightType = 0; - if (state->is(STATUS_MPU_FAIL)) { - allOff(); // no dithering during setup! - digitalWriteFast(board::LED_A_RED, LOW); + LED_driver.setPattern(LED::SOLID); + LED_driver.setColor(CRGB::Black); + LED_driver.setColor(CRGB::Red, {-128, -128}, {0, 127}); indicatorRedOn(); } else if (state->is(STATUS_BMP_FAIL)) { - allOff(); // no dithering during setup! - digitalWriteFast(board::LED_B_RED, LOW); + LED_driver.setPattern(LED::SOLID); + LED_driver.setColor(CRGB::Black); + LED_driver.setColor(CRGB::Red, {0, -128}, {127, 127}); indicatorRedOn(); } else if (state->is(STATUS_BOOT)) { - allOff(); // no dithering during setup! - digitalWriteFast(board::LED_A_GRN, LOW); - digitalWriteFast(board::LED_B_GRN, LOW); + LED_driver.set(LED::SOLID, CRGB::Green); } 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 + LED_driver.set(LED::FLASH, CRGB::White); // 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 + LED_driver.set(LED::FLASH, CRGB::Red); // red } else if (state->is(STATUS_FAIL_STABILITY) || state->is(STATUS_FAIL_ANGLE)) { - use(LED::FLASH, 100, 110, 250); // yellow + LED_driver.set(LED::FLASH, CRGB::Yellow); // yellow } else if (state->is(STATUS_OVERRIDE)) { - use(LED::BEACON, 255, 0, 0); // red + LED_driver.set(LED::BEACON, CRGB::Red); // red } else if (state->is(STATUS_ENABLING)) { - use(LED::FLASH, 0, 0, 255); // blue + LED_driver.set(LED::FLASH, CRGB::Blue); // blue } else if (state->is(STATUS_ENABLED)) { - use(LED::BEACON, 0, 0, 255); // blue for enable - } - - else if (state->is(STATUS_TEMP_WARNING)) { - use(LED::FLASH, 100, 150, 250); // yellow + LED_driver.set(LED::BEACON, CRGB::Blue); // blue for enable + } else if (state->is(STATUS_TEMP_WARNING)) { + LED_driver.set(LED::FLASH, CRGB::Yellow); // yellow } else if (state->is(STATUS_LOG_FULL)) { - use(LED::FLASH, 0, 0, 250); + LED_driver.set(LED::FLASH, CRGB::Blue); } else if (state->is(STATUS_BATTERY_LOW)) { - use(LED::BEACON, 255, 180, 20); + LED_driver.set(LED::BEACON, CRGB::Orange); } else if (state->is(STATUS_IDLE)) { - indicatorRedOff(); // clear boot test - use(LED::BEACON, 0, 255, 0); // breathe instead? + indicatorRedOff(); // clear boot test + LED_driver.set(LED::BEACON, CRGB::Green); // breathe instead? } else { // ERROR: ("NO STATUS BITS SET???"); } } -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::rgb() { - rgb(red_a_, green_a_, blue_a_, red_b_, green_b_, blue_b_); -} - -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(board::LED_A_RED, (red_a > threshold) ? LOW : HIGH); - digitalWriteFast(board::LED_B_RED, (red_b > threshold) ? LOW : HIGH); - digitalWriteFast(board::LED_A_GRN, (green_a > threshold) ? LOW : HIGH); - digitalWriteFast(board::LED_B_GRN, (green_b > threshold) ? LOW : HIGH); - digitalWriteFast(board::LED_A_BLU, (blue_a > threshold) ? LOW : HIGH); - digitalWriteFast(board::LED_B_BLU, (blue_b > threshold) ? LOW : HIGH); -} - -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::updateBeacon() { - switch ((cycleIndex & 1023) >> 6) // two second period - { - case 1: - case 4: - rgb(); - break; - - case 2: - case 5: - allOff(); - break; - - default: - break; - } -} - -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); -} - -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_); -} - -void LED::updateSolid() { - rgb(); -} - -void LED::allOff() { - digitalWriteFast(board::LED_A_RED, HIGH); - digitalWriteFast(board::LED_A_GRN, HIGH); - digitalWriteFast(board::LED_A_BLU, HIGH); - digitalWriteFast(board::LED_B_RED, HIGH); - digitalWriteFast(board::LED_B_GRN, HIGH); - digitalWriteFast(board::LED_B_BLU, HIGH); -} - void LED::indicatorRedOn() { digitalWriteFast(board::RED_LED, HIGH); } diff --git a/led.h b/led.h index 44d887e..0390ac2 100644 --- a/led.h +++ b/led.h @@ -13,13 +13,13 @@ #define led_h #include "Arduino.h" +#define FASTLED_INTERNAL +#include "FastLED.h" class State; class LED { public: - LED(State *state); - enum Pattern : uint8_t { NO_OVERRIDE = 0, FLASH = 1, @@ -29,42 +29,26 @@ 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 - - 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}; + explicit LED(State *state); - uint16_t cycleIndex{0}; // incremented at 500Hz to support dithering, resets every ~8 seconds + void update(); - bool override{false}; + 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); - uint8_t getLightThreshold(); + 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); + private: 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 + State *state; + uint16_t oldStatus{0}; + bool override{false}; +}; #endif From bd875fdadd7ebcc4a35c9889fc54f135c558eb55 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 18 Mar 2016 18:28:56 +0100 Subject: [PATCH 10/57] Propagate LED values directly to FastLED The alternating mode will require different intensities for different LEDs. This is a different logic from any other LED mode, so we'll use a different method for it. --- led.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/led.cpp b/led.cpp index 44e8c32..28f36ce 100644 --- a/led.cpp +++ b/led.cpp @@ -34,14 +34,13 @@ class LEDDriver { uint8_t pattern; uint8_t scale{0}; CRGB leds[board::led::COUNT]; - CRGB leds_display[board::led::COUNT]; bool hasChanges{true}; } LED_driver; LEDDriver::LEDDriver() { setColor(CRGB::Black); setPattern(LED::SOLID); - FastLED.addLeds(leds_display, board::led::COUNT); + FastLED.addLeds(leds, board::led::COUNT); } inline bool isInside(const board::led::Position& p, const board::led::Position& p_min, const board::led::Position& p_max) { From 0f1bfbb9f91f74895f21ba8f76a3527fbdf54f4e Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 18 Mar 2016 19:37:26 +0100 Subject: [PATCH 11/57] Implement alternating mode --- led.cpp | 54 ++++++++++++++++++++++++++++++++++++++---------------- led.h | 2 ++ 2 files changed, 40 insertions(+), 16 deletions(-) diff --git a/led.cpp b/led.cpp index 28f36ce..eafc1fe 100644 --- a/led.cpp +++ b/led.cpp @@ -22,13 +22,15 @@ class LEDDriver { 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 updateAlternate(); // 4sec on - 4sec off alternating TODO: use the proper left-right alternating - void updateSolid(); // maintain constant light level + 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; @@ -43,6 +45,14 @@ LEDDriver::LEDDriver() { FastLED.addLeds(leds, board::led::COUNT); } +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; } @@ -119,13 +129,6 @@ void LEDDriver::updateBreathe() { hasChanges = true; } -void LEDDriver::updateAlternate() { - if (cycleIndex & 127) - return; - scale = (cycleIndex & 128) ? 0 : 255; - hasChanges = true; -} - void LEDDriver::updateSolid() { if (scale == 255) return; @@ -145,8 +148,8 @@ void LEDDriver::writeToDisplay() { 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; @@ -161,6 +164,12 @@ LED::LED(State* __state) : state(__state) { } 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::set(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indicator, bool green_indicator) { + colorRight = color_right; + colorLeft = color_left; override = pattern != LED::NO_OVERRIDE; oldStatus = 0; if (!override) @@ -168,8 +177,12 @@ void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, u red_indicator ? indicatorRedOn() : indicatorRedOff(); green_indicator ? indicatorGreenOn() : indicatorGreenOff(); LED_driver.setPattern(pattern); - LED_driver.setColor({red_a, green_a, blue_a}, {0, -128}, {127, 127}); - LED_driver.setColor({red_b, green_b, blue_b}, {-128, -128}, {0, 127}); + LED_driver.setColor(color_right, {0, -128}, {127, 127}); + LED_driver.setColor(color_left, {-128, -128}, {0, 127}); +} + +void LED::set(Pattern pattern, CRGB color, bool red_indicator, bool green_indicator) { + set(pattern, color, color, red_indicator, green_indicator); } void LED::update() { @@ -177,6 +190,15 @@ void LED::update() { 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(); } diff --git a/led.h b/led.h index 0390ac2..e534eed 100644 --- a/led.h +++ b/led.h @@ -49,6 +49,8 @@ class LED { State *state; uint16_t oldStatus{0}; bool override{false}; + + CRGB colorRight{CRGB::Black}, colorLeft{CRGB::Black}; }; #endif From 6cc0c8dd57b14c873740953de1b67b5aefe0092a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 18 Mar 2016 20:17:56 +0100 Subject: [PATCH 12/57] Make indicator code less verbose --- led.cpp | 104 +++++++++++++++++++++++++++++++------------------------- led.h | 1 + 2 files changed, 58 insertions(+), 47 deletions(-) diff --git a/led.cpp b/led.cpp index eafc1fe..c119ec9 100644 --- a/led.cpp +++ b/led.cpp @@ -148,8 +148,8 @@ void LEDDriver::writeToDisplay() { updateBreathe(); break; case LED::ALTERNATE: - // Alternate is handled outside of the driver - // and here it's just a solid light + // Alternate is handled outside of the driver + // and here it's just a solid light case LED::SOLID: updateSolid(); break; @@ -168,17 +168,11 @@ void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, u } void LED::set(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indicator, bool green_indicator) { - colorRight = color_right; - colorLeft = color_left; override = pattern != LED::NO_OVERRIDE; oldStatus = 0; if (!override) return; - 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}); + use(pattern, color_right, color_left, red_indicator, green_indicator); } void LED::set(Pattern pattern, CRGB color, bool red_indicator, bool green_indicator) { @@ -202,45 +196,61 @@ void LED::update() { LED_driver.update(); } +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}); +} + +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; +}; + +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} { +} + +LightCase::LightCase(uint16_t status, LED::Pattern pattern, CRGB color) : LightCase{status, pattern, color, color, false, false} { +} + +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::White, CRGB::White, false, true}, // for pcba testing purposes -- a "good" board will end up in this state + {STATUS_RX_FAIL, LED::FLASH, CRGB::Red}, + {STATUS_FAIL_STABILITY, LED::FLASH, CRGB::Yellow}, + {STATUS_FAIL_ANGLE, LED::FLASH, CRGB::Yellow}, + {STATUS_OVERRIDE, LED::BEACON, CRGB::Red}, + {STATUS_ENABLING, LED::FLASH, CRGB::Blue}, + {STATUS_ENABLED, LED::BEACON, CRGB::Blue}, + {STATUS_TEMP_WARNING, LED::FLASH, CRGB::Yellow}, + {STATUS_LOG_FULL, LED::FLASH, CRGB::Blue}, + {STATUS_BATTERY_LOW, LED::BEACON, CRGB::Orange}, + {STATUS_IDLE, LED::BEACON, CRGB::Green}, // breathe instead? +}; + +} // namespace + void LED::changeLights() { - if (state->is(STATUS_MPU_FAIL)) { - LED_driver.setPattern(LED::SOLID); - LED_driver.setColor(CRGB::Black); - LED_driver.setColor(CRGB::Red, {-128, -128}, {0, 127}); - indicatorRedOn(); - } else if (state->is(STATUS_BMP_FAIL)) { - LED_driver.setPattern(LED::SOLID); - LED_driver.setColor(CRGB::Black); - LED_driver.setColor(CRGB::Red, {0, -128}, {127, 127}); - indicatorRedOn(); - } else if (state->is(STATUS_BOOT)) { - LED_driver.set(LED::SOLID, CRGB::Green); - } else if (state->is(STATUS_UNPAIRED)) { - // use(LED::FLASH, 255,180,20); //orange - LED_driver.set(LED::FLASH, CRGB::White); // 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)) { - LED_driver.set(LED::FLASH, CRGB::Red); // red - } else if (state->is(STATUS_FAIL_STABILITY) || state->is(STATUS_FAIL_ANGLE)) { - LED_driver.set(LED::FLASH, CRGB::Yellow); // yellow - } else if (state->is(STATUS_OVERRIDE)) { - LED_driver.set(LED::BEACON, CRGB::Red); // red - } else if (state->is(STATUS_ENABLING)) { - LED_driver.set(LED::FLASH, CRGB::Blue); // blue - } else if (state->is(STATUS_ENABLED)) { - LED_driver.set(LED::BEACON, CRGB::Blue); // blue for enable - } else if (state->is(STATUS_TEMP_WARNING)) { - LED_driver.set(LED::FLASH, CRGB::Yellow); // yellow - } else if (state->is(STATUS_LOG_FULL)) { - LED_driver.set(LED::FLASH, CRGB::Blue); - } else if (state->is(STATUS_BATTERY_LOW)) { - LED_driver.set(LED::BEACON, CRGB::Orange); - } else if (state->is(STATUS_IDLE)) { - indicatorRedOff(); // clear boot test - LED_driver.set(LED::BEACON, CRGB::Green); // breathe instead? - } else { - // ERROR: ("NO STATUS BITS SET???"); - } + 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() { diff --git a/led.h b/led.h index e534eed..a2155f8 100644 --- a/led.h +++ b/led.h @@ -39,6 +39,7 @@ class LED { void set(Pattern pattern, CRGB color, 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 indicatorRedOn(); From 5f2138162f9fd77cdb8f25015aca6bf4a0ec7143 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 29 Mar 2016 17:17:00 +0200 Subject: [PATCH 13/57] Create initial SD card logging --- board.h | 7 ++++++ cardManagement.cpp | 60 ++++++++++++++++++++++++++++++++++++++++++++ cardManagement.h | 28 +++++++++++++++++++++ config.h | 3 --- flybrix-firmware.ino | 11 +------- serial.cpp | 21 ++++++++-------- serial.h | 2 +- 7 files changed, 108 insertions(+), 24 deletions(-) create mode 100644 cardManagement.cpp create mode 100644 cardManagement.h diff --git a/board.h b/board.h index 5ac5f36..b9f8eb3 100644 --- a/board.h +++ b/board.h @@ -98,6 +98,13 @@ constexpr uint8_t FTM[]{ 22, // 44 | PWM[2] }; +namespace spi { +enum ChipSelect : uint8_t { + SD_CARD = 15, // 43 + EXT = 2, // 57 +}; +} + namespace led { enum PositionSimpleName : int8_t { FRONT = 1, diff --git a/cardManagement.cpp b/cardManagement.cpp new file mode 100644 index 0000000..84e6fd5 --- /dev/null +++ b/cardManagement.cpp @@ -0,0 +1,60 @@ +/* + * 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 "board.h" +#include "debug.h" +#include "version.h" + +namespace { +bool sd_open{false}; + +bool openSD() { + if (sd_open) + return true; + sd_open = SD.begin(board::spi::SD_CARD); + if (sd_open) + return true; + DebugPrint("Failed to open connection to SD card!"); + return false; +} +} // namespace + +Logger::Logger(const char* base_name) { + if (!openSD()) + return; + char file_dir[64]; + sprintf(file_dir, "/%d_%d_%d", FIRMWARE_VERSION_A, FIRMWARE_VERSION_B, FIRMWARE_VERSION_C); + if (!SD.mkdir(file_dir)) { + DebugPrintf("Failed to create directory %s on SD card!", file_dir); + return; + } + for (int idx = 0; true; ++idx) { + sprintf(filename, "%s/%s_%d.bin", file_dir, base_name, idx); + if (!SD.exists(filename)) + return; + } +} + +void Logger::write(const uint8_t* data, size_t length) { + if (!openSD()) + return; + File file{SD.open(filename, FILE_WRITE)}; + if (!file) { + DebugPrintf("Failed to access file %s on SD card!", filename); + return; + } + for (size_t idx = 0; idx < length; ++idx) + file.print(char(data[idx])); + file.close(); +} diff --git a/cardManagement.h b/cardManagement.h new file mode 100644 index 0000000..a5d2fb4 --- /dev/null +++ b/cardManagement.h @@ -0,0 +1,28 @@ +/* + * 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 + +class String; + +class Logger { + public: + explicit Logger(const char* base_name); + + void write(const uint8_t* data, size_t length); + + private: + char filename[64]{'\0'}; +}; + +#endif diff --git a/config.h b/config.h index a34511f..ecf8716 100644 --- a/config.h +++ b/config.h @@ -66,9 +66,6 @@ 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&); diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index c51d12a..68102db 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -218,21 +218,12 @@ void loop() { RUN_PROCESS(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]); - }); + sys.conf.SendState(micros()); } counter %= 1000; return true; diff --git a/serial.cpp b/serial.cpp index baa8909..6a18e2b 100644 --- a/serial.cpp +++ b/serial.cpp @@ -7,6 +7,7 @@ #include "serial.h" #include "state.h" +#include "cardManagement.h" #include "config.h" //CONFIG variable #include "control.h" #include "led.h" @@ -20,12 +21,14 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP payload.Append(mask); } +Logger state_logger{"states"}; + 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) + state_logger.write(package.data, package.length); } template @@ -131,11 +134,9 @@ void SerialComm::ProcessData() { } } 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; + // TODO: should we respond to this with SD data, or just deprecate it? + SendDebugString("", MessageType::HistoryData); + // ack_data |= COM_REQ_HISTORY; } if (mask & COM_SET_LED) { uint8_t mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g; @@ -226,7 +227,7 @@ 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) const { if (!mask) mask = state_mask; // No need to publish empty state messages @@ -293,7 +294,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, true); } void SerialComm::SendResponse(uint32_t mask, uint32_t response) const { diff --git a/serial.h b/serial.h index 18373f0..f8b7447 100644 --- a/serial.h +++ b/serial.h @@ -92,7 +92,7 @@ class SerialComm { 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) const; void SendResponse(uint32_t mask, uint32_t response) const; uint16_t GetSendStateDelay() const; From 7cd8e4530310a990259741c79d478394a82fe96a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 4 Apr 2016 17:05:00 +0200 Subject: [PATCH 14/57] Comment on filenames --- cardManagement.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cardManagement.cpp b/cardManagement.cpp index 84e6fd5..7363154 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -34,6 +34,7 @@ Logger::Logger(const char* base_name) { if (!openSD()) return; char file_dir[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.mkdir(file_dir)) { DebugPrintf("Failed to create directory %s on SD card!", file_dir); @@ -41,6 +42,7 @@ Logger::Logger(const char* base_name) { } 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)) return; } From 933af03aeea1dbd18c4026841045844d2dbe3615 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 4 Apr 2016 18:45:19 +0200 Subject: [PATCH 15/57] Add support for reading messages from SD cards --- cardManagement.cpp | 30 ++++++++++++++++++++++++++++++ cardManagement.h | 15 +++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/cardManagement.cpp b/cardManagement.cpp index 7363154..ca52f12 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -60,3 +60,33 @@ void Logger::write(const uint8_t* data, size_t length) { file.print(char(data[idx])); file.close(); } + +Messenger::Messenger(const char* base_name) { + if (!openSD()) + return; + // Construct the filename /__/commands/.bin + sprintf(filename, "/%d_%d_%d/commands/%s.bin", FIRMWARE_VERSION_A, FIRMWARE_VERSION_B, FIRMWARE_VERSION_C, base_name); +} + +bool Messenger::read() { + File file{SD.open(filename, FILE_READ)}; + if (!file) { + DebugPrintf("Failed to access file %s on SD card!", filename); + return false; + } + if (!file.seek(cursor)) { + DebugPrintf("Failed to reach the byte number %lu in file %s", cursor, filename); + return false; + } + while (file.available()) { + data_input.AppendToBuffer(file.read()); + ++cursor; + if (data_input.IsDone()) + return true; + } + return false; +} + +CobsReaderBuffer& Messenger::buffer() { + return data_input; +} diff --git a/cardManagement.h b/cardManagement.h index a5d2fb4..e46d80b 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -12,8 +12,10 @@ #define CARD_MANAGEMENT_H #include +#include "cobs.h" class String; +using CobsReaderBuffer = CobsReader<500>; class Logger { public: @@ -25,4 +27,17 @@ class Logger { char filename[64]{'\0'}; }; +class Messenger { + public: + explicit Messenger(const char* base_name); + + bool read(); + CobsReaderBuffer& buffer(); + + private: + char filename[64]{'\0'}; + size_t cursor{0}; + CobsReaderBuffer data_input; +}; + #endif From dc39a3523761904c1f58caeeae3f8642b584ef26 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 13 Apr 2016 22:07:32 +0200 Subject: [PATCH 16/57] Fix missing ACK flag for EEPROM reinitialization --- serial.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/serial.cpp b/serial.cpp index baa8909..b908a09 100644 --- a/serial.cpp +++ b/serial.cpp @@ -72,6 +72,7 @@ void SerialComm::ProcessData() { 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(); From 4123faea1621f053ae1b7dbf355e16e723cbfe65 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 21 Apr 2016 22:15:29 +0200 Subject: [PATCH 17/57] Implement serial override of RC and close #14 --- command.cpp | 24 ++++++++++++++++++++---- command.h | 5 +++++ flybrix-firmware.ino | 2 +- serial.cpp | 13 ++++++++++++- serial.h | 12 +++++++----- 5 files changed, 45 insertions(+), 11 deletions(-) diff --git a/command.cpp b/command.cpp index 986701f..22b564e 100644 --- a/command.cpp +++ b/command.cpp @@ -53,14 +53,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()) { @@ -111,6 +111,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 +128,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 +144,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/flybrix-firmware.ino b/flybrix-firmware.ino index a87a3c3..4ef03c6 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -71,7 +71,7 @@ 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 { } diff --git a/serial.cpp b/serial.cpp index b908a09..51d99f0 100644 --- a/serial.cpp +++ b/serial.cpp @@ -7,6 +7,7 @@ #include "serial.h" #include "state.h" +#include "command.h" #include "config.h" //CONFIG variable #include "control.h" #include "led.h" @@ -34,7 +35,8 @@ inline void WritePIDData(CobsPayload& payload, const PID& pid) { } } -SerialComm::SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, 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() { @@ -145,6 +147,15 @@ 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_REQ_RESPONSE) { SendResponse(mask, ack_data); diff --git a/serial.h b/serial.h index 18373f0..2a18f01 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,14 @@ 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_LED = 1 << 17, + COM_SET_SERIAL_RC = 1 << 18, }; enum StateFields : uint32_t { @@ -86,7 +87,7 @@ class SerialComm { STATE_LOOP_COUNT = 1 << 27, }; - explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, 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(); @@ -110,7 +111,8 @@ class SerialComm { const Control* control; 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 uint32_t state_mask{0x7fffff}; CobsReader<500> data_input; }; From b77e4afc14c3d64b854b18a5d779679befb2eff5 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 22 Apr 2016 20:48:31 +0200 Subject: [PATCH 18/57] Tidy up the I2C Manager --- AK8963.cpp | 14 +++++------ AK8963.h | 2 +- BMP280.cpp | 9 +++---- BMP280.h | 2 +- MPU9250.cpp | 20 +++++++-------- MPU9250.h | 2 +- i2cManager.cpp | 68 +++++++++++--------------------------------------- i2cManager.h | 16 +++++------- stlFix.cpp | 23 +++++++++++++++++ stlFix.h | 20 +++++++++++++++ 10 files changed, 86 insertions(+), 90 deletions(-) create mode 100644 stlFix.cpp create mode 100644 stlFix.h 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 0949e5a..09a1826 100644 --- a/MPU9250.cpp +++ b/MPU9250.cpp @@ -166,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 ffe0ed2..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; diff --git a/i2cManager.cpp b/i2cManager.cpp index 55a13e7..af3b7ec 100644 --- a/i2cManager.cpp +++ b/i2cManager.cpp @@ -10,52 +10,26 @@ #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; - } + 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 } @@ -73,13 +47,6 @@ uint8_t I2CManager::readByte(uint8_t address, uint8_t subAddress) { 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 +54,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..4bb3870 100644 --- a/i2cManager.h +++ b/i2cManager.h @@ -13,11 +13,13 @@ #define i2cManager_h #include "Arduino.h" -#include +#include "stlFix.h" + +#include class CallbackProcessor { public: - virtual void processCallback(uint8_t count, uint8_t *data); + virtual void triggerCallback(); }; struct I2CTransfer { @@ -27,16 +29,10 @@ struct I2CTransfer { uint8_t receive_count; uint8_t *receive_data; CallbackProcessor *cb_object; - I2CTransfer *next; }; 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); @@ -45,8 +41,8 @@ class I2CManager { uint8_t writeByte(uint8_t address, uint8_t subAddress, uint8_t data); private: - I2CTransfer *transfer; - bool waiting_for_data; + std::queue transfers; + bool waiting_for_data{false}; }; // class I2CManager 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 */ From 75699ee7be6ce03fb02d44727a559b34d17534f3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 22 Apr 2016 22:46:29 +0200 Subject: [PATCH 19/57] Implement lightweight queue The usage of an STL queue increases the program size by ~13kB. Until we decide to use STL for more than that, we do not need that amount of boilerplate. This queue doesn't exhibit the same behavior, but it is the same for our uses. --- i2cManager.cpp | 26 ++++++++++++++++++++++++-- i2cManager.h | 30 +++++++++++++++++++++--------- 2 files changed, 45 insertions(+), 11 deletions(-) diff --git a/i2cManager.cpp b/i2cManager.cpp index af3b7ec..ed86b8f 100644 --- a/i2cManager.cpp +++ b/i2cManager.cpp @@ -9,7 +9,29 @@ #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& 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}); } @@ -44,7 +66,7 @@ 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; Wire.beginTransmission(address); diff --git a/i2cManager.h b/i2cManager.h index 4bb3870..66f96a7 100644 --- a/i2cManager.h +++ b/i2cManager.h @@ -12,10 +12,8 @@ #ifndef i2cManager_h #define i2cManager_h +#include #include "Arduino.h" -#include "stlFix.h" - -#include class CallbackProcessor { public: @@ -25,23 +23,37 @@ class CallbackProcessor { 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; + uint8_t* receive_data; + CallbackProcessor* cb_object; }; class I2CManager { public: 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: - std::queue transfers; + 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 From 9c987b99741b63450d3b3722b7ca04148f74bd3f Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Apr 2016 19:51:04 +0200 Subject: [PATCH 20/57] Create initial compatibility support --- board.h | 18 ++++++++++++++++++ cardManagement.cpp | 4 ++++ led.cpp | 8 +++++--- 3 files changed, 27 insertions(+), 3 deletions(-) diff --git a/board.h b/board.h index b9f8eb3..31a860e 100644 --- a/board.h +++ b/board.h @@ -14,6 +14,8 @@ #include #include +#define ALPHA + namespace board { #ifdef ALPHA inline @@ -59,6 +61,22 @@ constexpr uint8_t FTM[]{ 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[]{ + {LEFT, 0}, {RIGHT, 0}, +}; +} } #ifndef ALPHA diff --git a/cardManagement.cpp b/cardManagement.cpp index ca52f12..0947a34 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -22,7 +22,11 @@ bool sd_open{false}; bool openSD() { if (sd_open) return true; +#ifdef ALPHA + return false; +#else sd_open = SD.begin(board::spi::SD_CARD); +#endif if (sd_open) return true; DebugPrint("Failed to open connection to SD card!"); diff --git a/led.cpp b/led.cpp index 767293f..e197497 100644 --- a/led.cpp +++ b/led.cpp @@ -8,8 +8,7 @@ #include "board.h" #include "state.h" -#define NO_LEDS // Instead of looking at actual LEDs, we look at the serial debug for feedback -#ifdef NO_LEDS +#ifdef ALPHA #include "debug.h" #endif @@ -42,7 +41,9 @@ class LEDDriver { LEDDriver::LEDDriver() { setColor(CRGB::Black); setPattern(LED::SOLID); +#ifndef ALPHA FastLED.addLeds(leds, board::led::COUNT); +#endif } uint8_t LEDDriver::getCycleIndex() const { @@ -86,8 +87,9 @@ void LEDDriver::update() { writeToDisplay(); if (!hasChanges) return; +#ifndef ALPHA FastLED.show(scale); -#ifdef NO_LEDS +#else DebugPrintf("%d %d %d | %d %d %d | %d %d %d | %d %d %d | %d", leds[0].red, leds[0].green, leds[0].blue, leds[1].red, leds[1].green, leds[1].blue, leds[2].red, leds[2].green, leds[2].blue, leds[3].red, leds[3].green, leds[3].blue, scale); #endif From 0102399167464befa74c42f8293d53e83bdc7560 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Apr 2016 20:29:55 +0200 Subject: [PATCH 21/57] Support old LEDs --- flybrix-firmware.ino | 2 ++ led.cpp | 34 ++++++++++++++++++++++++++++------ led.h | 2 ++ 3 files changed, 32 insertions(+), 6 deletions(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index e41ff33..7653b7e 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -226,6 +226,8 @@ bool ProcessTask<1000>() { sys.conf.SendState(micros()); } counter %= 1000; + if (LEDFastUpdate) + LEDFastUpdate(); return true; } diff --git a/led.cpp b/led.cpp index e197497..09f02df 100644 --- a/led.cpp +++ b/led.cpp @@ -8,9 +8,7 @@ #include "board.h" #include "state.h" -#ifdef ALPHA -#include "debug.h" -#endif +void (*LEDFastUpdate)(){nullptr}; namespace { class LEDDriver { @@ -38,11 +36,38 @@ class LEDDriver { 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); +} + +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 } @@ -89,9 +114,6 @@ void LEDDriver::update() { return; #ifndef ALPHA FastLED.show(scale); -#else - DebugPrintf("%d %d %d | %d %d %d | %d %d %d | %d %d %d | %d", leds[0].red, leds[0].green, leds[0].blue, leds[1].red, leds[1].green, leds[1].blue, leds[2].red, leds[2].green, leds[2].blue, - leds[3].red, leds[3].green, leds[3].blue, scale); #endif hasChanges = false; } diff --git a/led.h b/led.h index a2155f8..7b42ceb 100644 --- a/led.h +++ b/led.h @@ -18,6 +18,8 @@ class State; +extern void (*LEDFastUpdate)(); + class LED { public: enum Pattern : uint8_t { From d7e8c9059b6968db79d004c75cb81155c65a8316 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Apr 2016 20:51:03 +0200 Subject: [PATCH 22/57] Fix startup lights --- led.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/led.cpp b/led.cpp index 09f02df..89bb920 100644 --- a/led.cpp +++ b/led.cpp @@ -114,6 +114,9 @@ void LEDDriver::update() { return; #ifndef ALPHA FastLED.show(scale); +#else + if (LEDFastUpdate) + LEDFastUpdate(); #endif hasChanges = false; } From b08e89bf65a7d5d46afd7d40fa8641d345d0413b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 26 Apr 2016 20:54:21 +0200 Subject: [PATCH 23/57] Fix LED positions --- board.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board.h b/board.h index 31a860e..23c1818 100644 --- a/board.h +++ b/board.h @@ -74,7 +74,7 @@ struct Position { constexpr uint8_t COUNT{2}; constexpr Position POSITION[]{ - {LEFT, 0}, {RIGHT, 0}, + {RIGHT, 0}, {LEFT, 0}, }; } } From 870c154b700c0c7ae61a86aabef0621241595958 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 29 Apr 2016 14:51:35 +0200 Subject: [PATCH 24/57] Skip SD card in the default build This should be a temporary solution to get the board working while an SD card is missing --- board.h | 2 +- cardManagement.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/board.h b/board.h index 23c1818..782543e 100644 --- a/board.h +++ b/board.h @@ -14,7 +14,7 @@ #include #include -#define ALPHA +#define BETA namespace board { #ifdef ALPHA diff --git a/cardManagement.cpp b/cardManagement.cpp index 0947a34..afc64f3 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -16,13 +16,19 @@ #include "debug.h" #include "version.h" +#define SKIP_SD + +#ifdef ALPHA +#define SKIP_SD +#endif + namespace { bool sd_open{false}; bool openSD() { if (sd_open) return true; -#ifdef ALPHA +#ifdef SKIP_SD return false; #else sd_open = SD.begin(board::spi::SD_CARD); From b9648f713aeacabdde343b44169dcd6b70cb9bb2 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 29 Apr 2016 14:59:01 +0200 Subject: [PATCH 25/57] Fix the LED position order --- board.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board.h b/board.h index 782543e..1228f56 100644 --- a/board.h +++ b/board.h @@ -138,7 +138,7 @@ struct Position { constexpr uint8_t DATA_PIN{11}; // 51 constexpr uint8_t COUNT{4}; constexpr Position POSITION[]{ - {LEFT, FRONT}, {LEFT, BACK}, {RIGHT, BACK}, {RIGHT, FRONT}, + {LEFT, BACK}, {LEFT, FRONT}, {RIGHT, FRONT}, {RIGHT, BACK}, }; } } From 4d0cd4332817a225a65a3dd0b2e82cff134b9ad8 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 29 Apr 2016 17:21:37 +0200 Subject: [PATCH 26/57] Swap LED color channels to reflect desired values --- led.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/led.cpp b/led.cpp index 89bb920..0cb008c 100644 --- a/led.cpp +++ b/led.cpp @@ -84,6 +84,9 @@ inline bool isInside(const board::led::Position& p, const board::led::Position& } 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; From 54bb382d2b76e1395238dbe645d7d3084bf53ab6 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 4 May 2016 19:54:34 +0200 Subject: [PATCH 27/57] Implement recording state to the SD card --- board.h | 5 +++++ cardManagement.cpp | 26 ++++++++++++++------------ serial.cpp | 3 +-- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/board.h b/board.h index 1228f56..279feaa 100644 --- a/board.h +++ b/board.h @@ -117,6 +117,11 @@ constexpr uint8_t FTM[]{ }; namespace spi { +enum SdCardPins : uint8_t { + MOSI = 7, + MISO = 8, + SCK = 14, +}; enum ChipSelect : uint8_t { SD_CARD = 15, // 43 EXT = 2, // 57 diff --git a/cardManagement.cpp b/cardManagement.cpp index afc64f3..7cdf20c 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -10,33 +10,35 @@ #include "cardManagement.h" +#include #include #include #include "board.h" #include "debug.h" #include "version.h" -#define SKIP_SD - #ifdef ALPHA #define SKIP_SD #endif namespace { -bool sd_open{false}; - -bool openSD() { - if (sd_open) - return true; +bool openSDHardwarePort() { #ifdef SKIP_SD return false; #else - sd_open = SD.begin(board::spi::SD_CARD); + SPI.setMOSI(board::spi::MOSI); + SPI.setMISO(board::spi::MISO); + SPI.setSCK(board::spi::SCK); + bool opened = SD.begin(board::spi::SD_CARD); + if (!opened) + DebugPrint("Failed to open connection to SD card!"); + return opened; #endif - if (sd_open) - return true; - DebugPrint("Failed to open connection to SD card!"); - return false; +} + +bool openSD() { + static bool sd_open{openSDHardwarePort()}; + return sd_open; } } // namespace diff --git a/serial.cpp b/serial.cpp index c9bbc64..e38d55d 100644 --- a/serial.cpp +++ b/serial.cpp @@ -22,10 +22,9 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP payload.Append(mask); } -Logger state_logger{"states"}; - template inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { + static Logger state_logger("states"); auto package = payload.Encode(); Serial.write(package.data, package.length); if (use_logger) From 40fa17c2bd4d77259a1aef0d8be6ad29f15da919 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 4 May 2016 23:01:01 +0200 Subject: [PATCH 28/57] Reopen files only when necessary --- cardManagement.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 7cdf20c..3743e3c 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -10,8 +10,8 @@ #include "cardManagement.h" -#include #include +#include #include #include "board.h" #include "debug.h" @@ -40,6 +40,9 @@ bool openSD() { static bool sd_open{openSDHardwarePort()}; return sd_open; } + +static void* last_file_user{nullptr}; +static File file; } // namespace Logger::Logger(const char* base_name) { @@ -63,14 +66,19 @@ Logger::Logger(const char* base_name) { void Logger::write(const uint8_t* data, size_t length) { if (!openSD()) return; - File file{SD.open(filename, FILE_WRITE)}; + if (last_file_user != this) { + if (last_file_user != nullptr) + file.close(); + last_file_user = this; + file = SD.open(filename, FILE_WRITE); + file.seek(file.size()); + } if (!file) { DebugPrintf("Failed to access file %s on SD card!", filename); return; } for (size_t idx = 0; idx < length; ++idx) file.print(char(data[idx])); - file.close(); } Messenger::Messenger(const char* base_name) { @@ -81,7 +89,12 @@ Messenger::Messenger(const char* base_name) { } bool Messenger::read() { - File file{SD.open(filename, FILE_READ)}; + if (last_file_user != this) { + if (last_file_user != nullptr) + file.close(); + last_file_user = this; + file = SD.open(filename, FILE_READ); + } if (!file) { DebugPrintf("Failed to access file %s on SD card!", filename); return false; From d3b579964cd86964c621466ba5017ebab72249ab Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 4 May 2016 23:01:35 +0200 Subject: [PATCH 29/57] Shorten the logging filenames The filesystem has limited filename lengths, so we are shortening the name base --- serial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/serial.cpp b/serial.cpp index e38d55d..3916274 100644 --- a/serial.cpp +++ b/serial.cpp @@ -24,7 +24,7 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP template inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { - static Logger state_logger("states"); + static Logger state_logger("st"); auto package = payload.Encode(); Serial.write(package.data, package.length); if (use_logger) From 52f75127936b371bf201688ef1f586c20b3971b7 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 4 May 2016 23:53:15 +0200 Subject: [PATCH 30/57] Remove extra SD card functionality The SD card handling is too slow to allow handling of multiple files at once. As a result, we are cutting that functionality until we manage to potentially increase the speed. --- cardManagement.cpp | 70 ++++++++++------------------------------------ cardManagement.h | 27 +----------------- serial.cpp | 3 +- 3 files changed, 17 insertions(+), 83 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 3743e3c..913eb15 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -10,8 +10,10 @@ #include "cardManagement.h" -#include #include + +#include + #include #include "board.h" #include "debug.h" @@ -41,77 +43,35 @@ bool openSD() { return sd_open; } -static void* last_file_user{nullptr}; -static File file; -} // namespace - -Logger::Logger(const char* base_name) { +File openFile(const char* base_name) { if (!openSD()) - return; + return File(); 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.mkdir(file_dir)) { DebugPrintf("Failed to create directory %s on SD card!", file_dir); - return; + return File(); } 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)) - return; + return SD.open(filename, FILE_WRITE); } + return File(); } +} // namespace -void Logger::write(const uint8_t* data, size_t length) { - if (!openSD()) - return; - if (last_file_user != this) { - if (last_file_user != nullptr) - file.close(); - last_file_user = this; - file = SD.open(filename, FILE_WRITE); - file.seek(file.size()); - } +void writeToCard(const uint8_t* data, size_t length) { + static File file{openFile("st")}; if (!file) { - DebugPrintf("Failed to access file %s on SD card!", filename); + if (openSD()) + DebugPrintf("Failed to access file on SD card!"); return; } for (size_t idx = 0; idx < length; ++idx) file.print(char(data[idx])); -} - -Messenger::Messenger(const char* base_name) { - if (!openSD()) - return; - // Construct the filename /__/commands/.bin - sprintf(filename, "/%d_%d_%d/commands/%s.bin", FIRMWARE_VERSION_A, FIRMWARE_VERSION_B, FIRMWARE_VERSION_C, base_name); -} - -bool Messenger::read() { - if (last_file_user != this) { - if (last_file_user != nullptr) - file.close(); - last_file_user = this; - file = SD.open(filename, FILE_READ); - } - if (!file) { - DebugPrintf("Failed to access file %s on SD card!", filename); - return false; - } - if (!file.seek(cursor)) { - DebugPrintf("Failed to reach the byte number %lu in file %s", cursor, filename); - return false; - } - while (file.available()) { - data_input.AppendToBuffer(file.read()); - ++cursor; - if (data_input.IsDone()) - return true; - } - return false; -} - -CobsReaderBuffer& Messenger::buffer() { - return data_input; + file.flush(); } diff --git a/cardManagement.h b/cardManagement.h index e46d80b..b24ef24 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -12,32 +12,7 @@ #define CARD_MANAGEMENT_H #include -#include "cobs.h" -class String; -using CobsReaderBuffer = CobsReader<500>; - -class Logger { - public: - explicit Logger(const char* base_name); - - void write(const uint8_t* data, size_t length); - - private: - char filename[64]{'\0'}; -}; - -class Messenger { - public: - explicit Messenger(const char* base_name); - - bool read(); - CobsReaderBuffer& buffer(); - - private: - char filename[64]{'\0'}; - size_t cursor{0}; - CobsReaderBuffer data_input; -}; +void writeToCard(const uint8_t* data, size_t length); #endif diff --git a/serial.cpp b/serial.cpp index 3916274..2d7e6a5 100644 --- a/serial.cpp +++ b/serial.cpp @@ -24,11 +24,10 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP template inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { - static Logger state_logger("st"); auto package = payload.Encode(); Serial.write(package.data, package.length); if (use_logger) - state_logger.write(package.data, package.length); + writeToCard(package.data, package.length); } template From 794ed40874c083128451da80f920303f2e7d02d0 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 5 May 2016 09:58:38 +0200 Subject: [PATCH 31/57] Fix PWM pins --- board.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/board.h b/board.h index 279feaa..1ec4681 100644 --- a/board.h +++ b/board.h @@ -103,7 +103,7 @@ constexpr uint8_t PWM[]{ 25, // 42 32, // 41 22, // 44 - 20, // 62 + 10, // 49 23, // 45 21, // 63 9, // 46 From 5757e751438c0b3c51e61f0cedd4fdc9a9edc427 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 5 May 2016 11:56:24 +0200 Subject: [PATCH 32/57] Switch to SdFat for SD card handling The usage of SdFat requires applying a patch (SdSpiTeensy3.cpp.diff) that has been added to the repository. --- SdSpiTeensy3.cpp.diff | 8 ++++++++ cardManagement.cpp | 48 +++++++++++++++++++++++-------------------- cardManagement.h | 2 ++ flybrix-firmware.ino | 2 ++ 4 files changed, 38 insertions(+), 22 deletions(-) create mode 100644 SdSpiTeensy3.cpp.diff 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/cardManagement.cpp b/cardManagement.cpp index 913eb15..2fd2ce8 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -10,10 +10,7 @@ #include "cardManagement.h" -#include - -#include - +#include #include #include "board.h" #include "debug.h" @@ -24,14 +21,13 @@ #endif namespace { +SdFat sd; + bool openSDHardwarePort() { #ifdef SKIP_SD return false; #else - SPI.setMOSI(board::spi::MOSI); - SPI.setMISO(board::spi::MISO); - SPI.setSCK(board::spi::SCK); - bool opened = SD.begin(board::spi::SD_CARD); + bool opened = sd.begin(board::spi::SD_CARD, SPI_FULL_SPEED); if (!opened) DebugPrint("Failed to open connection to SD card!"); return opened; @@ -43,35 +39,43 @@ bool openSD() { return sd_open; } -File openFile(const char* base_name) { +SdFile file; + +bool openFile(const char* base_name) { if (!openSD()) - return File(); + 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.mkdir(file_dir)) { + 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 File(); + 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)) - return SD.open(filename, FILE_WRITE); + if (!sd.exists(filename)) { + bool success = file.open(filename, O_CREAT | O_WRITE); + if (!success) + DebugPrintf("Failed to open file %s on SD card!", filename); + return success; + } } - return File(); + return false; } } // namespace void writeToCard(const uint8_t* data, size_t length) { - static File file{openFile("st")}; - if (!file) { + static bool openedFile{openFile("st")}; + if (!(openedFile)) { if (openSD()) - DebugPrintf("Failed to access file on SD card!"); + DebugPrint("Failed to access file on SD card!"); return; } - for (size_t idx = 0; idx < length; ++idx) - file.print(char(data[idx])); - file.flush(); + file.write(data, length); +} + +void commitWriteToCard() { + file.sync(); } diff --git a/cardManagement.h b/cardManagement.h index b24ef24..017bf98 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -15,4 +15,6 @@ void writeToCard(const uint8_t* data, size_t length); +void commitWriteToCard(); + #endif diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 7653b7e..57a3261 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -36,6 +36,7 @@ #include "debug.h" #include "version.h" #include "board.h" +#include "cardManagement.h" struct Systems { // subsystem objects initialize pins when created @@ -309,6 +310,7 @@ bool ProcessTask<10>() { template <> bool ProcessTask<1>() { + commitWriteToCard(); #ifdef DEBUG float elapsed_seconds = (micros() - start_time) / 1000000.0; Serial.print("DEBUG: elapsed time (seconds) = "); From f6507b17aaa21b3484cad0029212a38f1b62c27b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 10 May 2016 16:58:22 +0200 Subject: [PATCH 33/57] Check for SD card at startup --- cardManagement.cpp | 7 ++++++- cardManagement.h | 4 ++++ flybrix-firmware.ino | 4 ++++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 2fd2ce8..5394e1a 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -66,6 +66,10 @@ bool openFile(const char* base_name) { } } // namespace +void startupCard() { + writeToCard(nullptr, 0); +} + void writeToCard(const uint8_t* data, size_t length) { static bool openedFile{openFile("st")}; if (!(openedFile)) { @@ -73,7 +77,8 @@ void writeToCard(const uint8_t* data, size_t length) { DebugPrint("Failed to access file on SD card!"); return; } - file.write(data, length); + if (length > 0) + file.write(data, length); } void commitWriteToCard() { diff --git a/cardManagement.h b/cardManagement.h index 017bf98..b405cf8 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -13,6 +13,10 @@ #include +// Having a missing card causes great latency at the first attempt of access +// We can use this command to call it prior to the flight starting +void startupCard(); + void writeToCard(const uint8_t* data, size_t length); void commitWriteToCard(); diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 57a3261..f34bad6 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -134,6 +134,10 @@ void setup() { ; } + sys.led.update(); + // Perform intial check for an SD card + startupCard(); + sys.state.clear(STATUS_BOOT); sys.state.set(STATUS_IDLE); sys.led.update(); From cd73f36ac8f22d9a54d8f096870277db36dec392 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 10 May 2016 17:33:12 +0200 Subject: [PATCH 34/57] Perform some code cleanup and serial refactoring --- cobs.h | 2 ++ debug.cpp | 26 --------------- debug.h | 41 ++---------------------- flybrix-firmware.ino | 75 +++++++++++--------------------------------- serial.cpp | 20 ++++++------ serial.h | 5 ++- serialFork.cpp | 66 ++++++++++++++++++++++++++++++++++++++ serialFork.h | 20 ++++++++++++ 8 files changed, 121 insertions(+), 134 deletions(-) create mode 100644 serialFork.cpp create mode 100644 serialFork.h 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/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 f34bad6..f412779 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -89,9 +89,6 @@ void setup() { debug_serial_comm = &sys.conf; - // setup USB debug serial - Serial.begin(9600); // USB is always 12 Mbit/sec - // MPU9250 is limited to 400kHz bus speed. 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); @@ -156,22 +153,26 @@ uint32_t pwr_reads = 0; uint32_t low_battery_counter = 0; -#define DEF_PROCESS_VARIABLES(F) uint32_t iterations_at_##F##Hz = 0; +template +uint32_t RunProcess(uint32_t start); -/* F is target frequency, D is a tuning factor */ -#define RUN_PROCESS(F) \ - iterations_at_##F##Hz = RunProcess(micros()); \ - sys.i2c.update(); +template +struct freqlist{}; -DEF_PROCESS_VARIABLES(1000) -DEF_PROCESS_VARIABLES(100) -DEF_PROCESS_VARIABLES(40) -DEF_PROCESS_VARIABLES(30) -DEF_PROCESS_VARIABLES(10) -DEF_PROCESS_VARIABLES(1) +void RunProcessesHelper(freqlist<>) { +} -template -uint32_t RunProcess(uint32_t start); +template +void RunProcessesHelper(freqlist) { + RunProcess(micros()); + sys.i2c.update(); + RunProcessesHelper(freqlist()); +} + +template +void RunProcesses() { + RunProcessesHelper(freqlist()); +} template bool ProcessTask(); @@ -215,12 +216,7 @@ void loop() { sys.motors.updateAllChannels(); } - RUN_PROCESS(1000) - RUN_PROCESS(100) - RUN_PROCESS(40) - RUN_PROCESS(30) - RUN_PROCESS(10) - RUN_PROCESS(1) + RunProcesses<1000, 100, 40, 30, 10, 1>(); } template <> @@ -261,7 +257,7 @@ 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; } @@ -315,39 +311,6 @@ bool ProcessTask<10>() { template <> bool ProcessTask<1>() { commitWriteToCard(); -#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/serial.cpp b/serial.cpp index 2d7e6a5..127847b 100644 --- a/serial.cpp +++ b/serial.cpp @@ -5,6 +5,8 @@ */ #include "serial.h" + +#include "serialFork.h" #include "state.h" #include "cardManagement.h" @@ -25,7 +27,7 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP template inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { auto package = payload.Encode(); - Serial.write(package.data, package.length); + writeSerial(package.data, package.length); if (use_logger) writeToCard(package.data, package.length); } @@ -40,18 +42,16 @@ SerialComm::SerialComm(State* state, const volatile uint16_t* ppm, const Control : 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; diff --git a/serial.h b/serial.h index aada8db..4d37a2c 100644 --- a/serial.h +++ b/serial.h @@ -89,7 +89,7 @@ class SerialComm { 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; @@ -102,7 +102,7 @@ class SerialComm { void RemoveFromStateMsg(uint32_t values); private: - void ProcessData(); + void ProcessData(CobsReaderBuffer& data_input); uint16_t PacketSize(uint32_t mask) const; @@ -114,7 +114,6 @@ class SerialComm { PilotCommand* command; uint16_t send_state_delay{1001}; // anything over 1000 turns off state messages uint32_t state_mask{0x7fffff}; - CobsReader<500> data_input; }; #endif diff --git a/serialFork.cpp b/serialFork.cpp new file mode 100644 index 0000000..30045a9 --- /dev/null +++ b/serialFork.cpp @@ -0,0 +1,66 @@ +/* + * 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 +// TODO: Create a Bluetooth interface +// Bluetooth bluetooth{115200}; +#endif +} + +CobsReaderBuffer* readSerial() { + if (usb_comm.read()) + return &usb_comm.buffer(); +#ifndef ALPHA +// TODO: Handle bluetooth the same way +// if (bluetooth.read()) +// return &bluetooth.buffer(); +#endif + return nullptr; +} + +void writeSerial(uint8_t* data, size_t length) { + usb_comm.write(data, length); +#ifndef ALPHA +// TODO: Handle bluetooth the same way +// bluetooth.write(data, length); +#endif +} diff --git a/serialFork.h b/serialFork.h new file mode 100644 index 0000000..3f51c96 --- /dev/null +++ b/serialFork.h @@ -0,0 +1,20 @@ +/* + * 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); + +#endif From 5abb248aecce531a85242dae17ac1f64f9bfd659 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 10 May 2016 19:04:43 +0200 Subject: [PATCH 35/57] Fix the version.h comment --- version.h | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/version.h b/version.h index 0fa39c5..6a3ad68 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. */ From 3cfbe6a69f693338f8c58e2754698109fec6dff9 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 11 May 2016 21:39:50 +0200 Subject: [PATCH 36/57] Improve logging speed for the SD card Saving full state messages to the buffer takes ~60us, and sending 512B chunks to the SD card takes additional ~200us, resulting in state messages needing ~253us in ~45% of the cases, and ~453us in the rest of the cases. That includes ~173us needed to process the packages and send them via serial. --- cardManagement.cpp | 133 +++++++++++++++++++++++++++++++++++++++---- cardManagement.h | 9 ++- flybrix-firmware.ino | 1 - 3 files changed, 128 insertions(+), 15 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 5394e1a..d46df7d 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -10,7 +10,9 @@ #include "cardManagement.h" +#include #include +#include #include #include "board.h" #include "debug.h" @@ -39,7 +41,88 @@ bool openSD() { return sd_open; } -SdFile file; +SdBaseFile binFile; +uint32_t bgnBlock, endBlock; +uint8_t* cache; + +uint32_t block_number = 0; +constexpr uint32_t FILE_BLOCK_COUNT = 256000; +constexpr uint32_t ERASE_SIZE = 262144L; +constexpr uint32_t BUFFER_BLOCK_COUNT = 4; + +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; + currentBlock = (currentBlock + 1) % BUFFER_BLOCK_COUNT; + 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]}; + startBlock = (startBlock + 1) % BUFFER_BLOCK_COUNT; + 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()) @@ -56,7 +139,7 @@ bool openFile(const char* base_name) { 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 = file.open(filename, O_CREAT | O_WRITE); + bool success = openFileHelper(filename); if (!success) DebugPrintf("Failed to open file %s on SD card!", filename); return success; @@ -67,20 +150,48 @@ bool openFile(const char* base_name) { } // namespace void startupCard() { - writeToCard(nullptr, 0); + openSD(); +} + +void openFileOnCard() { + if (!openSD()) + return; + openFile("st"); } void writeToCard(const uint8_t* data, size_t length) { - static bool openedFile{openFile("st")}; - if (!(openedFile)) { - if (openSD()) - DebugPrint("Failed to access file on SD card!"); + if (!openSD()) + return; + if (!binFile.isOpen()) + return; + if (block_number == FILE_BLOCK_COUNT) return; - } if (length > 0) - file.write(data, length); + 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 commitWriteToCard() { - file.sync(); +void closeFileOnCard() { + 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; + } + } + binFile.close(); + DebugPrint("File closing successful"); } diff --git a/cardManagement.h b/cardManagement.h index b405cf8..cb1e0d5 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -13,12 +13,15 @@ #include -// Having a missing card causes great latency at the first attempt of access -// We can use this command to call it prior to the flight starting +// Card startup takes a long time to perform void startupCard(); +// Opening and clearing the file takes a long time to perform +void openFileOnCard(); + void writeToCard(const uint8_t* data, size_t length); -void commitWriteToCard(); +// File closing (saving and truncating the file) takes a long time to perform +void closeFileOnCard(); #endif diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index f412779..c17dc02 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -310,7 +310,6 @@ bool ProcessTask<10>() { template <> bool ProcessTask<1>() { - commitWriteToCard(); return true; } From a4f4e8efc8e4ef7a13e5d964b817aec4dfb8bb43 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 11 May 2016 21:57:33 +0200 Subject: [PATCH 37/57] Take hardware considerations for the SD card --- cardManagement.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index d46df7d..9f0ce03 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -46,9 +46,21 @@ 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: @@ -72,7 +84,8 @@ void WritingBuffer::write(const uint8_t* data, size_t length) { if (currentPointer < 512) continue; currentPointer = 0; - currentBlock = (currentBlock + 1) % BUFFER_BLOCK_COUNT; + if (++currentBlock >= BUFFER_BLOCK_COUNT) + currentBlock = 0; if (currentBlock == startBlock) { overbuffered = true; DebugPrint("SD card buffer is full"); @@ -89,7 +102,8 @@ uint8_t* WritingBuffer::popBlock() { if (!hasBlock()) return nullptr; uint8_t* retval{block[startBlock]}; - startBlock = (startBlock + 1) % BUFFER_BLOCK_COUNT; + if (++startBlock >= BUFFER_BLOCK_COUNT) + startBlock = 0; overbuffered = false; return retval; } From 27931de42b7996a4356e51a47e65dd8f5cd082e2 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 12 May 2016 01:10:59 +0200 Subject: [PATCH 38/57] Allow toggling SD card logging via serial --- cardManagement.cpp | 2 ++ serial.cpp | 10 ++++++++++ serial.h | 1 + 3 files changed, 13 insertions(+) diff --git a/cardManagement.cpp b/cardManagement.cpp index 9f0ce03..4a23f3d 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -141,6 +141,8 @@ bool openFileHelper(const char* filename) { 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 diff --git a/serial.cpp b/serial.cpp index 127847b..80feebc 100644 --- a/serial.cpp +++ b/serial.cpp @@ -155,6 +155,16 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { ack_data |= COM_SET_SERIAL_RC; } } + if (mask & COM_SET_CARD_RECORDING) { + bool shouldRecordToCard; + if (data_input.ParseInto(shouldRecordToCard)) { + if (shouldRecordToCard) + openFileOnCard(); + else + closeFileOnCard(); + ack_data |= COM_SET_CARD_RECORDING; + } + } if (mask & COM_REQ_RESPONSE) { SendResponse(mask, ack_data); diff --git a/serial.h b/serial.h index 4d37a2c..9ffd04f 100644 --- a/serial.h +++ b/serial.h @@ -53,6 +53,7 @@ class SerialComm { COM_REQ_HISTORY = 1 << 16, COM_SET_LED = 1 << 17, COM_SET_SERIAL_RC = 1 << 18, + COM_SET_CARD_RECORDING = 1 << 19, }; enum StateFields : uint32_t { From cd23ef4e1d0228efb776c43c061152610922e0fc Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 12 May 2016 21:37:05 +0200 Subject: [PATCH 39/57] Toggle SD card logging on RC arm/disarm --- command.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/command.cpp b/command.cpp index 22b564e..9025dd4 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} { } @@ -94,10 +96,13 @@ void PilotCommand::processCommands(void) { state->processMotorEnablingIteration(); recentlyEnabled = true; throttleHoldOff = 80; // @40Hz -- hold for 2 sec + if (state->is(STATUS_ENABLED)) + openFileOnCard(); } } 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(); + closeFileOnCard(); } } From 9c663d2d66f2d71f49ec73b8d2a592130d3e9821 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 23 May 2016 17:03:07 +0200 Subject: [PATCH 40/57] Improve SD card function naming --- cardManagement.cpp | 10 ++++++---- cardManagement.h | 10 ++++++---- command.cpp | 4 ++-- serial.cpp | 6 +++--- 4 files changed, 17 insertions(+), 13 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 4a23f3d..3106dca 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -22,6 +22,7 @@ #define SKIP_SD #endif +namespace sdcard { namespace { SdFat sd; @@ -165,17 +166,17 @@ bool openFile(const char* base_name) { } } // namespace -void startupCard() { +void startup() { openSD(); } -void openFileOnCard() { +void openFile() { if (!openSD()) return; openFile("st"); } -void writeToCard(const uint8_t* data, size_t length) { +void write(const uint8_t* data, size_t length) { if (!openSD()) return; if (!binFile.isOpen()) @@ -193,7 +194,7 @@ void writeToCard(const uint8_t* data, size_t length) { block_number++; } -void closeFileOnCard() { +void closeFile() { if (!openSD()) return; if (!binFile.isOpen()) @@ -211,3 +212,4 @@ void closeFileOnCard() { binFile.close(); DebugPrint("File closing successful"); } +} // namespace sdcard diff --git a/cardManagement.h b/cardManagement.h index cb1e0d5..7409ae6 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -13,15 +13,17 @@ #include +namespace sdcard { // Card startup takes a long time to perform -void startupCard(); +void startup(); // Opening and clearing the file takes a long time to perform -void openFileOnCard(); +void openFile(); -void writeToCard(const uint8_t* data, size_t length); +void write(const uint8_t* data, size_t length); // File closing (saving and truncating the file) takes a long time to perform -void closeFileOnCard(); +void closeFile(); +} #endif diff --git a/command.cpp b/command.cpp index 9025dd4..fab3468 100644 --- a/command.cpp +++ b/command.cpp @@ -97,12 +97,12 @@ void PilotCommand::processCommands(void) { recentlyEnabled = true; throttleHoldOff = 80; // @40Hz -- hold for 2 sec if (state->is(STATUS_ENABLED)) - openFileOnCard(); + 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(); - closeFileOnCard(); + sdcard::closeFile(); } } diff --git a/serial.cpp b/serial.cpp index 80feebc..9b08e93 100644 --- a/serial.cpp +++ b/serial.cpp @@ -29,7 +29,7 @@ inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { auto package = payload.Encode(); writeSerial(package.data, package.length); if (use_logger) - writeToCard(package.data, package.length); + sdcard::write(package.data, package.length); } template @@ -159,9 +159,9 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { bool shouldRecordToCard; if (data_input.ParseInto(shouldRecordToCard)) { if (shouldRecordToCard) - openFileOnCard(); + sdcard::openFile(); else - closeFileOnCard(); + sdcard::closeFile(); ack_data |= COM_SET_CARD_RECORDING; } } From 53a08473cb0396579550f8b35ac7cec0f56d6e73 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 23 May 2016 17:18:54 +0200 Subject: [PATCH 41/57] Fix SD card interface change --- flybrix-firmware.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index c17dc02..6a83065 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -133,7 +133,7 @@ void setup() { sys.led.update(); // Perform intial check for an SD card - startupCard(); + sdcard::startup(); sys.state.clear(STATUS_BOOT); sys.state.set(STATUS_IDLE); From d9ec906b617379b51011f6f71a3fefedc029a9c3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 24 May 2016 20:55:48 +0200 Subject: [PATCH 42/57] Remove unused variables With the removal of old 1Hz logging, some variables can be removed as well. --- flybrix-firmware.ino | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 6a83065..f40ff45 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -140,17 +140,6 @@ void setup() { sys.led.update(); } -// Main loop variables - -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; - uint32_t low_battery_counter = 0; template @@ -183,21 +172,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 @@ -210,7 +192,6 @@ void loop() { sys.motors.updateAllChannels(); } else { sys.control.calculateControlVectors(); - control_updates++; sys.airframe.updateMotorsMix(); sys.motors.updateAllChannels(); @@ -243,7 +224,6 @@ bool ProcessTask<100>() { if (sys.bmp.ready) { sys.state.updateStatePT(micros()); sys.bmp.startMeasurement(); - bmp_reads++; } else { return false; } @@ -267,7 +247,6 @@ 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 +279,6 @@ template <> bool ProcessTask<10>() { if (sys.mag.ready) { sys.mag.startMeasurement(); - mag_reads++; } else { return false; } From ef9ee652083f619a6a333c8f3d97970d2e27374e Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 30 May 2016 19:05:13 +0200 Subject: [PATCH 43/57] Create Bluetooth serial interface --- board.h | 4 ++++ serialFork.cpp | 40 +++++++++++++++++++++++++++++++++------- 2 files changed, 37 insertions(+), 7 deletions(-) diff --git a/board.h b/board.h index 1ec4681..1ee7d07 100644 --- a/board.h +++ b/board.h @@ -146,6 +146,10 @@ constexpr Position POSITION[]{ {LEFT, BACK}, {LEFT, FRONT}, {RIGHT, FRONT}, {RIGHT, BACK}, }; } + +namespace bluetooth { +constexpr uint8_t RESET{28}; +} } } diff --git a/serialFork.cpp b/serialFork.cpp index 30045a9..e8f79cd 100644 --- a/serialFork.cpp +++ b/serialFork.cpp @@ -40,9 +40,37 @@ struct USBComm { }; USBComm usb_comm; + #ifndef ALPHA -// TODO: Create a Bluetooth interface -// Bluetooth bluetooth{115200}; +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) { + Serial1.write(data, length); + } + + CobsReaderBuffer& buffer() { + return data_input; + } + + private: + CobsReaderBuffer data_input; +}; + +Bluetooth bluetooth; #endif } @@ -50,9 +78,8 @@ CobsReaderBuffer* readSerial() { if (usb_comm.read()) return &usb_comm.buffer(); #ifndef ALPHA -// TODO: Handle bluetooth the same way -// if (bluetooth.read()) -// return &bluetooth.buffer(); + if (bluetooth.read()) + return &bluetooth.buffer(); #endif return nullptr; } @@ -60,7 +87,6 @@ CobsReaderBuffer* readSerial() { void writeSerial(uint8_t* data, size_t length) { usb_comm.write(data, length); #ifndef ALPHA -// TODO: Handle bluetooth the same way -// bluetooth.write(data, length); + bluetooth.write(data, length); #endif } From b833beced30cea3956ccf4fc2df9a09614acfcef Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 10 Jun 2016 23:39:41 +0200 Subject: [PATCH 44/57] Create a software buffer for Bluetooth Writing to Bluetooth arbitrarily can cause delays of multiple milliseconds. By adding a software buffer, that can overflow instead of causing delays, we prevent those issues, at the cost of ultimately losing data that the Bluetooth serial connection couldn't handle anyway. --- flybrix-firmware.ino | 9 ++++++- serialFork.cpp | 63 +++++++++++++++++++++++++++++++++++++++++++- serialFork.h | 1 + 3 files changed, 71 insertions(+), 2 deletions(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index f40ff45..9064e20 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -37,6 +37,7 @@ #include "version.h" #include "board.h" #include "cardManagement.h" +#include "serialFork.h" struct Systems { // subsystem objects initialize pins when created @@ -197,7 +198,7 @@ void loop() { sys.motors.updateAllChannels(); } - RunProcesses<1000, 100, 40, 30, 10, 1>(); + RunProcesses<1000, 100, 70, 40, 30, 10, 1>(); } template <> @@ -242,6 +243,12 @@ bool ProcessTask<100>() { return true; } +template <> +bool ProcessTask<70>() { + flushSerial(); + return true; +} + template <> bool ProcessTask<40>() { sys.pilot.processCommands(); diff --git a/serialFork.cpp b/serialFork.cpp index e8f79cd..570a096 100644 --- a/serialFork.cpp +++ b/serialFork.cpp @@ -59,14 +59,69 @@ struct Bluetooth { } void write(uint8_t* data, size_t length) { - Serial1.write(data, 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 > 70) + return 70; + 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{5}; + static constexpr size_t bufferSize{bufferCount * 70}; + uint8_t data[bufferSize]; + size_t writerPointer{0}; + size_t readerPointer{0}; + }; + + size_t writerPosition{0}; + BluetoothBuffer data_output; CobsReaderBuffer data_input; }; @@ -90,3 +145,9 @@ void writeSerial(uint8_t* data, size_t length) { bluetooth.write(data, length); #endif } + +void flushSerial() { +#ifndef ALPHA + bluetooth.flush(); +#endif +} diff --git a/serialFork.h b/serialFork.h index 3f51c96..1262d01 100644 --- a/serialFork.h +++ b/serialFork.h @@ -16,5 +16,6 @@ CobsReaderBuffer* readSerial(); void writeSerial(uint8_t* data, size_t length); +void flushSerial(); #endif From a96fb6f37b9215bf0b1d26ee31efb9d467a1f47b Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 13 Jun 2016 21:07:09 +0200 Subject: [PATCH 45/57] Reduce maximum data rate for BLE --- flybrix-firmware.ino | 4 ++-- serialFork.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 9064e20..7c51c5b 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -198,7 +198,7 @@ void loop() { sys.motors.updateAllChannels(); } - RunProcesses<1000, 100, 70, 40, 30, 10, 1>(); + RunProcesses<1000, 100, 40, 35, 30, 10, 1>(); } template <> @@ -244,7 +244,7 @@ bool ProcessTask<100>() { } template <> -bool ProcessTask<70>() { +bool ProcessTask<35>() { flushSerial(); return true; } diff --git a/serialFork.cpp b/serialFork.cpp index 570a096..7d15cff 100644 --- a/serialFork.cpp +++ b/serialFork.cpp @@ -77,8 +77,8 @@ struct Bluetooth { struct BluetoothBuffer { size_t hasData() { size_t increment{(readerPointer <= writerPointer) ? (writerPointer - readerPointer) : (bufferSize - readerPointer)}; - if (increment > 70) - return 70; + if (increment > bufferChunk) + return bufferChunk; return increment; } @@ -113,8 +113,9 @@ struct Bluetooth { } private: - static constexpr size_t bufferCount{5}; - static constexpr size_t bufferSize{bufferCount * 70}; + static constexpr size_t bufferCount{15}; + 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}; From 13be76ed69f1fd21ef36ca3570dabec3bb4cd3cf Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Fri, 17 Jun 2016 17:27:24 +0200 Subject: [PATCH 46/57] Increase Bluetooth buffer size Previously the buffer was 300B, and we increased it to 800B, since the previous one couldn't fit even one large messaga --- serialFork.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/serialFork.cpp b/serialFork.cpp index 7d15cff..764fa15 100644 --- a/serialFork.cpp +++ b/serialFork.cpp @@ -113,7 +113,7 @@ struct Bluetooth { } private: - static constexpr size_t bufferCount{15}; + static constexpr size_t bufferCount{40}; static constexpr size_t bufferChunk{20}; static constexpr size_t bufferSize{bufferCount * bufferChunk}; uint8_t data[bufferSize]; From 9d45f203ef2a0eada55b073415a53ed267df65a4 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 21 Jun 2016 17:13:56 +0200 Subject: [PATCH 47/57] Detach Serial logging from SD Card logging --- cardManagement.cpp | 4 ++++ cardManagement.h | 2 ++ flybrix-firmware.ino | 14 ++++++++++---- serial.cpp | 24 +++++++++++++++++------- serial.h | 6 ++++-- 5 files changed, 37 insertions(+), 13 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 3106dca..5209966 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -176,6 +176,10 @@ void openFile() { openFile("st"); } +bool isOpen() { + return binFile.isOpen(); +} + void write(const uint8_t* data, size_t length) { if (!openSD()) return; diff --git a/cardManagement.h b/cardManagement.h index 7409ae6..0b43171 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -20,6 +20,8 @@ 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 diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 7c51c5b..0551296 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -203,12 +203,18 @@ void loop() { template <> bool ProcessTask<1000>() { - static uint16_t counter{0}; - if (++counter > sys.conf.GetSendStateDelay() - 1) { - counter = 0; + static uint16_t counterSerial{0}; + static uint16_t counterSdCard{0}; + if (++counterSerial > sys.conf.GetSendStateDelay() - 1) { + counterSerial = 0; sys.conf.SendState(micros()); } - counter %= 1000; + if (++counterSdCard > sys.conf.GetSdCardStateDelay() - 1) { + counterSdCard = 0; + sys.conf.SendState(micros(), 0xFFFFFFFF, true); + } + counterSerial %= 1000; + counterSdCard %= 1000; if (LEDFastUpdate) LEDFastUpdate(); return true; diff --git a/serial.cpp b/serial.cpp index 9b08e93..47d4639 100644 --- a/serial.cpp +++ b/serial.cpp @@ -27,9 +27,10 @@ inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsP template inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { auto package = payload.Encode(); - writeSerial(package.data, package.length); if (use_logger) sdcard::write(package.data, package.length); + else + writeSerial(package.data, package.length); } template @@ -134,10 +135,12 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { ack_data |= COM_SET_STATE_DELAY; } } - if (mask & COM_REQ_HISTORY) { - // TODO: should we respond to this with SD data, or just deprecate it? - SendDebugString("", 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; @@ -247,7 +250,10 @@ uint16_t SerialComm::PacketSize(uint32_t mask) const { return sum; } -void SerialComm::SendState(uint32_t timestamp_us, 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 @@ -314,7 +320,7 @@ void SerialComm::SendState(uint32_t timestamp_us, uint32_t mask) const { payload.Append(state->kinematicsAltitude); if (mask & SerialComm::STATE_LOOP_COUNT) payload.Append(state->loopCount); - WriteToOutput(payload, true); + WriteToOutput(payload, redirect_to_sd_card); } void SerialComm::SendResponse(uint32_t mask, uint32_t response) const { @@ -328,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 9ffd04f..44147b3 100644 --- a/serial.h +++ b/serial.h @@ -50,7 +50,7 @@ class SerialComm { 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, @@ -94,10 +94,11 @@ class SerialComm { void SendConfiguration() const; void SendDebugString(const String& string, MessageType type = MessageType::DebugString) const; - void SendState(uint32_t timestamp_us, 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); @@ -114,6 +115,7 @@ class SerialComm { LED* led; 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}; }; From d654fca9bd87e91aefb642bd9dc5fe6d228bf427 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 21 Jun 2016 18:52:12 +0200 Subject: [PATCH 48/57] Fix issues at writing multiple consecutive files --- cardManagement.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/cardManagement.cpp b/cardManagement.cpp index 5209966..5c5b138 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -213,6 +213,7 @@ void closeFile() { return; } } + block_number = 0; binFile.close(); DebugPrint("File closing successful"); } From 9ea09e391057d1e5ed8f4af6ae550a85de53196a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 22 Jun 2016 18:30:02 +0200 Subject: [PATCH 49/57] Add a test mode --- config.cpp | 10 +++++++--- config.h | 1 + flybrix-firmware.ino | 6 ++++++ led.cpp | 13 ++++++++++++- led.h | 4 ++++ testMode.cpp | 37 +++++++++++++++++++++++++++++++++++++ testMode.h | 16 ++++++++++++++++ 7 files changed, 83 insertions(+), 4 deletions(-) create mode 100644 testMode.cpp create mode 100644 testMode.h diff --git a/config.cpp b/config.cpp index 20e2d9b..22a4d16 100644 --- a/config.cpp +++ b/config.cpp @@ -67,7 +67,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; @@ -147,7 +147,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 @@ -170,8 +170,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 ecf8716..2c1134a 100644 --- a/config.h +++ b/config.h @@ -73,5 +73,6 @@ extern CONFIG_union CONFIG; extern void initializeEEPROM(void); extern void writeEEPROM(void); extern void readEEPROM(void); +bool isEmptyEEPROM(); #endif diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 0551296..c4a9ccd 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -38,6 +38,7 @@ #include "board.h" #include "cardManagement.h" #include "serialFork.h" +#include "testMode.h" struct Systems { // subsystem objects initialize pins when created @@ -95,6 +96,8 @@ void setup() { 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(); @@ -133,6 +136,9 @@ void setup() { } sys.led.update(); + + if (go_to_test_mode) + // Perform intial check for an SD card sdcard::startup(); diff --git a/led.cpp b/led.cpp index 0cb008c..1860ada 100644 --- a/led.cpp +++ b/led.cpp @@ -5,7 +5,6 @@ */ #include "led.h" -#include "board.h" #include "state.h" void (*LEDFastUpdate)(){nullptr}; @@ -236,6 +235,18 @@ void LED::use(Pattern pattern, CRGB color_right, CRGB color_left, bool red_indic LED_driver.setColor(color_left, {-128, -128}, {0, 127}); } +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); +} + namespace { struct LightCase { LightCase(uint16_t status, LED::Pattern pattern, CRGB colorRight, CRGB colorLeft, bool indicatorRed = false, bool indicatorGreen = false); diff --git a/led.h b/led.h index 7b42ceb..1d488b5 100644 --- a/led.h +++ b/led.h @@ -16,6 +16,8 @@ #define FASTLED_INTERNAL #include "FastLED.h" +#include "board.h" + class State; extern void (*LEDFastUpdate)(); @@ -40,6 +42,8 @@ class LED { 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); + 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(); 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); From 476f5b561662f9262f769c33d144958a477fe5ce Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 22 Jun 2016 18:30:26 +0200 Subject: [PATCH 50/57] Fix missing line --- flybrix-firmware.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index c4a9ccd..b8db6f5 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -138,6 +138,7 @@ void setup() { sys.led.update(); if (go_to_test_mode) + runTestMode(sys.state, sys.led, sys.motors); // Perform intial check for an SD card sdcard::startup(); From 95bea397164d468b9ff50e1b573f303e42632fc2 Mon Sep 17 00:00:00 2001 From: Amir Hirsch Date: Fri, 24 Jun 2016 14:27:27 -0700 Subject: [PATCH 51/57] new boards adc 3.3V ref --- board.h | 1 + 1 file changed, 1 insertion(+) diff --git a/board.h b/board.h index 1ee7d07..5fc305d 100644 --- a/board.h +++ b/board.h @@ -15,6 +15,7 @@ #include #define BETA +#define BOARD_ADC_REF ADC_REF_3V3 //ADC_REF_1V2 namespace board { #ifdef ALPHA From 2929c6fd00729a469294097c58ed334578730961 Mon Sep 17 00:00:00 2001 From: Amir Hirsch Date: Fri, 24 Jun 2016 14:27:35 -0700 Subject: [PATCH 52/57] compiler complains --- flybrix-firmware.ino | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 7c51c5b..b04f2e9 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -1,3 +1,6 @@ +#include +#include +#include /* * Flybrix Flight Controller -- Copyright 2015 Flying Selfie Inc. * From b07f46666124bb44d458a178012a3f21227e528d Mon Sep 17 00:00:00 2001 From: Amir Hirsch Date: Fri, 24 Jun 2016 14:27:43 -0700 Subject: [PATCH 53/57] adc ref --- power.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/power.cpp b/power.cpp index 38e6cc2..ea0d19c 100644 --- a/power.cpp +++ b/power.cpp @@ -22,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 From cfd87e27ec35596ea4a6fb5c10bb024b117baeec Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Mon, 27 Jun 2016 15:12:57 -0700 Subject: [PATCH 54/57] update readme --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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. From d2eebee294a90d2b65abe35b0c29cd436f7e068b Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Mon, 27 Jun 2016 18:47:14 -0700 Subject: [PATCH 55/57] attemptToBind already calls initialize_isr --- R415X.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/R415X.cpp b/R415X.cpp index de66f11..21c0b66 100644 --- a/R415X.cpp +++ b/R415X.cpp @@ -14,8 +14,8 @@ 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) { From 6d6d6f82eadad02b4ff4ecf93c34666cd8f9930d Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Mon, 27 Jun 2016 18:47:36 -0700 Subject: [PATCH 56/57] changes to status led colors --- led.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/led.cpp b/led.cpp index 1860ada..0efb072 100644 --- a/led.cpp +++ b/led.cpp @@ -270,12 +270,12 @@ 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::White, CRGB::White, false, true}, // for pcba testing purposes -- a "good" board will end up in this state + {STATUS_UNPAIRED, LED::FLASH, CRGB::Orange, CRGB::Orange}, {STATUS_RX_FAIL, LED::FLASH, CRGB::Red}, - {STATUS_FAIL_STABILITY, LED::FLASH, CRGB::Yellow}, - {STATUS_FAIL_ANGLE, LED::FLASH, CRGB::Yellow}, + {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::Yellow}, + {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}, From f118a16f14164065ed475efa4081529fbba4ff86 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Mon, 27 Jun 2016 18:47:51 -0700 Subject: [PATCH 57/57] roll default config to v1.2.1 --- config.cpp | 37 +++++++++++++++++++++---------------- flybrix-firmware.ino | 7 +++++-- version.h | 2 +- 3 files changed, 27 insertions(+), 19 deletions(-) diff --git a/config.cpp b/config.cpp index 22a4d16..5235f19 100644 --- a/config.cpp +++ b/config.cpp @@ -35,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) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 77cea5e..788a528 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -2,12 +2,13 @@ #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") */ @@ -82,6 +83,7 @@ Systems::Systems() } void setup() { + config_handler = [&](CONFIG_struct& config){ sys.control.parseConfig(config); }; @@ -140,6 +142,7 @@ void setup() { sys.led.update(); + // factory test pattern runs only once if (go_to_test_mode) runTestMode(sys.state, sys.led, sys.motors); diff --git a/version.h b/version.h index 6a3ad68..6cbb6bf 100644 --- a/version.h +++ b/version.h @@ -14,6 +14,6 @@ #define FIRMWARE_VERSION_A 1 #define FIRMWARE_VERSION_B 2 -#define FIRMWARE_VERSION_C 0 +#define FIRMWARE_VERSION_C 1 #endif