From ff10a2e45787fa722f98afcaa810d787082f3b5e Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Tue, 23 Aug 2016 14:22:33 -0700 Subject: [PATCH 01/22] decide enabling based on valid data only --- command.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/command.cpp b/command.cpp index 4f6c4ef..35ee07a 100644 --- a/command.cpp +++ b/command.cpp @@ -33,6 +33,9 @@ void PilotCommand::processCommands(void) { bluetoothTolerance = 40; } + bool attempting_to_enable = false; + bool attempting_to_disable = false; + if (!(state->command_source_mask & (COMMAND_READY_R415X | COMMAND_READY_BTLE))){ // we have no command data! state->command_invalid_count += 2; @@ -50,16 +53,19 @@ void PilotCommand::processCommands(void) { state->clear(STATUS_RX_FAIL); } } - - bool attempting_to_enable = state->command_AUX_mask & (1 << 0); // AUX1 is low - bool attempting_to_disable = state->command_AUX_mask & (1 << 2); // AUX1 is high + else { // use valid command data + attempting_to_enable = state->command_AUX_mask & (1 << 0); // AUX1 is low + attempting_to_disable = state->command_AUX_mask & (1 << 2); // AUX1 is high + } if (blockEnabling && attempting_to_enable && !state->is(STATUS_OVERRIDE)) { // user attempted to enable, but we are blocking it state->clear(STATUS_IDLE); state->set(STATUS_FAIL_STABILITY); state->set(STATUS_FAIL_ANGLE); // set both flags as indication } + blockEnabling = false; // we block enable on the first run! + if (!state->is(STATUS_OVERRIDE)) { if (attempting_to_enable && !state->is(STATUS_ENABLED | STATUS_FAIL_STABILITY | STATUS_FAIL_ANGLE)) { state->processMotorEnablingIteration(); From 313854f091f58dccc583e5c01ef7da66fd00fe6b Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Wed, 14 Sep 2016 18:04:11 -0700 Subject: [PATCH 02/22] stop setting RX defaults to 1100 so we can see if the R415X is disconnected --- R415X.cpp | 8 -------- R415X.h | 2 -- 2 files changed, 10 deletions(-) diff --git a/R415X.cpp b/R415X.cpp index 03a1b2a..bfb6ac9 100644 --- a/R415X.cpp +++ b/R415X.cpp @@ -45,16 +45,8 @@ bool R415X::ChannelProperties::verify() const { } R415X::R415X() { - initialize_isr(); -} - -void R415X::initialize_isr(void) { 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; - } - // FLEX Timer1 input filter configuration // 4+4*val clock cycles, 48MHz = 4+4*7 = 32 clock cycles = 0.75us FTM1_FILTER = 0x07; diff --git a/R415X.h b/R415X.h index fdd3be8..08b368c 100644 --- a/R415X.h +++ b/R415X.h @@ -78,8 +78,6 @@ class R415X { static_assert(sizeof(ChannelProperties) == 6 + 1 + 6 * 2 * 2, "Data is not packed"); private: - void initialize_isr(void); - PPMchannel throttle; PPMchannel pitch; PPMchannel roll; From a8bd0e1e34bf8f46300039c13d6ec0d843c1e6ca Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Wed, 14 Sep 2016 18:53:10 -0700 Subject: [PATCH 03/22] use STATUS_FAIL_OTHER in fixed command logic --- command.cpp | 59 ++++++++++++++++++++++++++--------------------------- config.cpp | 14 ++++++------- state.cpp | 1 + state.h | 5 +++-- 4 files changed, 39 insertions(+), 40 deletions(-) diff --git a/command.cpp b/command.cpp index 35ee07a..522e3ef 100644 --- a/command.cpp +++ b/command.cpp @@ -17,71 +17,74 @@ PilotCommand::PilotCommand(State* __state, R415X* __receiver) } void PilotCommand::processCommands(void) { + + bool attempting_to_enable = false; + bool attempting_to_disable = false; if (!(state->command_source_mask & COMMAND_READY_BTLE)){ if (bluetoothTolerance) { - // we allow bluetooth a generous 1s before we give up on it + // we allow bluetooth a generous 1s before we give up --bluetoothTolerance; } else { - // if we aren't receiving bluetooth command data, try to get it from the R415X + // since we haven't seen bluetooth commands for more than 1 second, try the R415X receiver->getCommandData(state); } } else{ - //mark the BTLE data as used - state->command_source_mask &= ~(COMMAND_READY_BTLE); + // as soon as we start receiving bluetooth, reset the watchdog bluetoothTolerance = 40; - } - - bool attempting_to_enable = false; - bool attempting_to_disable = false; + } if (!(state->command_source_mask & (COMMAND_READY_R415X | COMMAND_READY_BTLE))){ // we have no command data! - state->command_invalid_count += 2; - if (state->command_invalid_count > 100) { - state->command_invalid_count = 100; + state->command_invalid_count++; + if (state->command_invalid_count > 80) { + // we haven't received data in two seconds + state->command_invalid_count = 80; state->set(STATUS_RX_FAIL); - } else if (state->command_invalid_count > 10) { - state->set(STATUS_UNPAIRED); } } else if (state->command_invalid_count > 0) { state->command_invalid_count--; if (state->command_invalid_count == 0) { - state->clear(STATUS_UNPAIRED); - } else if (state->command_invalid_count < 90) { state->clear(STATUS_RX_FAIL); } } - else { // use valid command data - attempting_to_enable = state->command_AUX_mask & (1 << 0); // AUX1 is low - attempting_to_disable = state->command_AUX_mask & (1 << 2); // AUX1 is high + else { + // use valid command data + attempting_to_enable = state->command_AUX_mask & (1 << 0); // AUX1 is low + attempting_to_disable = state->command_AUX_mask & (1 << 2); // AUX1 is high + + // in the future, this would be the place to look for other combination inputs or for AUX levels that mean something + + //mark the BTLE data as used so we don't use it again + state->command_source_mask &= ~(COMMAND_READY_BTLE); } if (blockEnabling && attempting_to_enable && !state->is(STATUS_OVERRIDE)) { // user attempted to enable, but we are blocking it state->clear(STATUS_IDLE); - state->set(STATUS_FAIL_STABILITY); - state->set(STATUS_FAIL_ANGLE); // set both flags as indication + state->set(STATUS_FAIL_OTHER); } - - blockEnabling = false; // we block enable on the first run! + blockEnabling = false; // we only block enabling if attempting_to_enable may have been accidentally set if (!state->is(STATUS_OVERRIDE)) { - if (attempting_to_enable && !state->is(STATUS_ENABLED | STATUS_FAIL_STABILITY | STATUS_FAIL_ANGLE)) { - state->processMotorEnablingIteration(); + if (attempting_to_enable && !state->is(STATUS_ENABLED | STATUS_FAIL_STABILITY | STATUS_FAIL_ANGLE | STATUS_FAIL_OTHER)) { + state->processMotorEnablingIteration(); //this can flip STATUS_ENABLED to true recentlyEnabled = true; throttleHoldOff = 80; // @40Hz -- hold for 2 sec if (state->is(STATUS_ENABLED)) sdcard::openFile(); } - if (attempting_to_disable && state->is(STATUS_ENABLED | STATUS_FAIL_STABILITY | STATUS_FAIL_ANGLE)) { + if (attempting_to_disable && state->is(STATUS_ENABLED | STATUS_FAIL_STABILITY | STATUS_FAIL_ANGLE | STATUS_FAIL_OTHER)) { state->disableMotors(); sdcard::closeFile(); } } + else { + blockEnabling = true; // block accidental enabling when we come out of pilot override + } bool throttle_is_low = (state->command_throttle == 0); - + if (recentlyEnabled || throttle_is_low) { state->command_throttle = 0; state->command_pitch = 0; @@ -94,8 +97,4 @@ void PilotCommand::processCommands(void) { } } - if (state->is(STATUS_OVERRIDE)) - blockEnabling = true; // block enabling when we come out of pilot override - - // in the future, this would be the place to look for other combination inputs or for AUX levels that mean something } diff --git a/config.cpp b/config.cpp index 5a3bfb1..b377c6a 100644 --- a/config.cpp +++ b/config.cpp @@ -186,16 +186,14 @@ CONFIG_struct::CONFIG_struct() { // Default Settings state_parameters.enable[0] = 0.001f; // max variance state_parameters.enable[1] = 30.0f; // max angle + //fading is in 256ths : https://github.com/FastLED/FastLED/wiki/Pixel-reference led_states = LED::States{{ - LED::StateCase(STATUS_MPU_FAIL, LED::SOLID, CRGB::Black, CRGB::Red, - true), - LED::StateCase(STATUS_BMP_FAIL, LED::SOLID, CRGB::Red, CRGB::Black, - true), + LED::StateCase(STATUS_MPU_FAIL, LED::SOLID, CRGB::Black, CRGB::Red,true), + LED::StateCase(STATUS_BMP_FAIL, LED::SOLID, CRGB::Red, CRGB::Black,true), LED::StateCase(STATUS_BOOT, LED::SOLID, CRGB::Green), - LED::StateCase(STATUS_UNPAIRED, LED::FLASH, CRGB::Orange, CRGB::Orange), - LED::StateCase(STATUS_RX_FAIL, LED::FLASH, CRGB::Red), - LED::StateCase(STATUS_FAIL_STABILITY, LED::FLASH, CRGB::Black, - CRGB::Blue), + LED::StateCase(STATUS_FAIL_OTHER, LED::ALTERNATE, CRGB::Blue, CRGB::Blue), + LED::StateCase(STATUS_RX_FAIL, LED::FLASH, CRGB::Orange, CRGB::Orange), + LED::StateCase(STATUS_FAIL_STABILITY, LED::FLASH, CRGB::Black,CRGB::Blue), LED::StateCase(STATUS_FAIL_ANGLE, LED::FLASH, CRGB::Blue, CRGB::Black), LED::StateCase(STATUS_OVERRIDE, LED::BEACON, CRGB::Red), LED::StateCase(STATUS_TEMP_WARNING, LED::FLASH, CRGB::Red), diff --git a/state.cpp b/state.cpp index 138aeb2..8c3395c 100644 --- a/state.cpp +++ b/state.cpp @@ -91,6 +91,7 @@ void State::disableMotors(void) { clear(STATUS_ENABLED); clear(STATUS_FAIL_STABILITY); clear(STATUS_FAIL_ANGLE); + clear(STATUS_FAIL_OTHER); set(STATUS_IDLE); } diff --git a/state.h b/state.h index 959c716..deec442 100644 --- a/state.h +++ b/state.h @@ -119,13 +119,14 @@ class State { #define STATUS_FAIL_STABILITY 0x0100 #define STATUS_FAIL_ANGLE 0x0200 +#define STATUS_FAIL_OTHER 0x4000 // flag other arming failures #define STATUS_ENABLED 0x0400 -#define STATUS_BATTERY_LOW 0x0800 +#define STATUS_BATTERY_LOW 0x0800 #define STATUS_TEMP_WARNING 0x1000 #define STATUS_LOG_FULL 0x2000 -#define STATUS_UNPAIRED 0x4000 + #define STATUS_OVERRIDE 0x8000 #endif From f8e75f3257d2d326a09dac37a83fef5cb089d838 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Thu, 15 Sep 2016 12:29:09 -0700 Subject: [PATCH 04/22] change the LEDs to be less blinding by default --- config.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/config.cpp b/config.cpp index b377c6a..54e4ce1 100644 --- a/config.cpp +++ b/config.cpp @@ -188,19 +188,19 @@ CONFIG_struct::CONFIG_struct() { // Default Settings //fading is in 256ths : https://github.com/FastLED/FastLED/wiki/Pixel-reference led_states = LED::States{{ - LED::StateCase(STATUS_MPU_FAIL, LED::SOLID, CRGB::Black, CRGB::Red,true), - LED::StateCase(STATUS_BMP_FAIL, LED::SOLID, CRGB::Red, CRGB::Black,true), - LED::StateCase(STATUS_BOOT, LED::SOLID, CRGB::Green), - LED::StateCase(STATUS_FAIL_OTHER, LED::ALTERNATE, CRGB::Blue, CRGB::Blue), - LED::StateCase(STATUS_RX_FAIL, LED::FLASH, CRGB::Orange, CRGB::Orange), - LED::StateCase(STATUS_FAIL_STABILITY, LED::FLASH, CRGB::Black,CRGB::Blue), - LED::StateCase(STATUS_FAIL_ANGLE, LED::FLASH, CRGB::Blue, CRGB::Black), - LED::StateCase(STATUS_OVERRIDE, LED::BEACON, CRGB::Red), - LED::StateCase(STATUS_TEMP_WARNING, LED::FLASH, CRGB::Red), - LED::StateCase(STATUS_BATTERY_LOW, LED::BEACON, CRGB::Orange), - LED::StateCase(STATUS_ENABLING, LED::FLASH, CRGB::Blue), - LED::StateCase(STATUS_ENABLED, LED::BEACON, CRGB::Blue), - LED::StateCase(STATUS_IDLE, LED::BEACON, CRGB::Green), + LED::StateCase(STATUS_MPU_FAIL, LED::SOLID, CRGB(CRGB::Black ).fadeLightBy(230), CRGB(CRGB::Red ).fadeLightBy(230), true), + LED::StateCase(STATUS_BMP_FAIL, LED::SOLID, CRGB(CRGB::Red ).fadeLightBy(230), CRGB(CRGB::Black ).fadeLightBy(230), true), + LED::StateCase(STATUS_BOOT, LED::SOLID, CRGB(CRGB::Green ).fadeLightBy(230)), + LED::StateCase(STATUS_RX_FAIL, LED::FLASH, CRGB(CRGB::Orange).fadeLightBy(230)), + LED::StateCase(STATUS_FAIL_OTHER, LED::ALTERNATE, CRGB(CRGB::Blue ).fadeLightBy(230)), + LED::StateCase(STATUS_FAIL_STABILITY, LED::FLASH, CRGB(CRGB::Black ).fadeLightBy(230), CRGB(CRGB::Blue ).fadeLightBy(230)), + LED::StateCase(STATUS_FAIL_ANGLE, LED::FLASH, CRGB(CRGB::Blue ).fadeLightBy(230), CRGB(CRGB::Black ).fadeLightBy(230)), + LED::StateCase(STATUS_OVERRIDE, LED::BEACON, CRGB(CRGB::Red ).fadeLightBy(230)), + LED::StateCase(STATUS_TEMP_WARNING, LED::FLASH, CRGB(CRGB::Red ).fadeLightBy(230)), + LED::StateCase(STATUS_BATTERY_LOW, LED::BEACON, CRGB(CRGB::Orange).fadeLightBy(230)), + LED::StateCase(STATUS_ENABLING, LED::FLASH, CRGB(CRGB::Blue ).fadeLightBy(230)), + LED::StateCase(STATUS_ENABLED, LED::BEACON, CRGB(CRGB::Blue ).fadeLightBy(230)), + LED::StateCase(STATUS_IDLE, LED::BEACON, CRGB(CRGB::Green ).fadeLightBy(230)), }}; // This function will only initialize data variables From 21471a47433887f85ff791b03f6491467573a4ec Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 11 Oct 2016 21:35:58 +0200 Subject: [PATCH 05/22] Add lock to SD card recording --- cardManagement.cpp | 9 +++++++++ cardManagement.h | 3 +++ serial.cpp | 8 ++++++-- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/cardManagement.cpp b/cardManagement.cpp index 5c5b138..1ebdc22 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -25,6 +25,7 @@ namespace sdcard { namespace { SdFat sd; +bool locked = false; bool openSDHardwarePort() { #ifdef SKIP_SD @@ -171,6 +172,8 @@ void startup() { } void openFile() { + if (locked) + return; if (!openSD()) return; openFile("st"); @@ -199,6 +202,8 @@ void write(const uint8_t* data, size_t length) { } void closeFile() { + if (locked) + return; if (!openSD()) return; if (!binFile.isOpen()) @@ -217,4 +222,8 @@ void closeFile() { binFile.close(); DebugPrint("File closing successful"); } + +void setLock(bool enable) { + locked = enable; +} } // namespace sdcard diff --git a/cardManagement.h b/cardManagement.h index 0b43171..f47c5d7 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -26,6 +26,9 @@ void write(const uint8_t* data, size_t length); // File closing (saving and truncating the file) takes a long time to perform void closeFile(); + +// We can set a lock to SD opening/closing controls, which can only be overriden with serial commands +void setLock(bool enable); } #endif diff --git a/serial.cpp b/serial.cpp index cbc2b91..5d17310 100644 --- a/serial.cpp +++ b/serial.cpp @@ -170,12 +170,16 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } } if (mask & COM_SET_CARD_RECORDING) { - bool shouldRecordToCard; - if (data_input.ParseInto(shouldRecordToCard)) { + uint8_t recording_flags; + if (data_input.ParseInto(recording_flags)) { + bool shouldRecordToCard = recording_flags & 1; + bool shouldLock = recording_flags & 2; + sdcard::setLock(false); if (shouldRecordToCard) sdcard::openFile(); else sdcard::closeFile(); + sdcard::setLock(shouldLock); ack_data |= COM_SET_CARD_RECORDING; } } From b289af94e12da07f64132722988b10022cea5777 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 11 Oct 2016 22:38:57 +0200 Subject: [PATCH 06/22] Add lock state query --- cardManagement.cpp | 4 ++++ cardManagement.h | 2 ++ 2 files changed, 6 insertions(+) diff --git a/cardManagement.cpp b/cardManagement.cpp index 1ebdc22..fc38b9d 100644 --- a/cardManagement.cpp +++ b/cardManagement.cpp @@ -226,4 +226,8 @@ void closeFile() { void setLock(bool enable) { locked = enable; } + +bool isLocked() { + return locked; +} } // namespace sdcard diff --git a/cardManagement.h b/cardManagement.h index f47c5d7..78a42c5 100644 --- a/cardManagement.h +++ b/cardManagement.h @@ -29,6 +29,8 @@ void closeFile(); // We can set a lock to SD opening/closing controls, which can only be overriden with serial commands void setLock(bool enable); + +bool isLocked(); } #endif From a1ac5bf7e7496c24a1a815113a53c910ed7edb64 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 11 Oct 2016 22:39:14 +0200 Subject: [PATCH 07/22] Add card recording state request message --- serial.cpp | 14 ++++++++++++++ serial.h | 1 + 2 files changed, 15 insertions(+) diff --git a/serial.cpp b/serial.cpp index 5d17310..584ebbc 100644 --- a/serial.cpp +++ b/serial.cpp @@ -279,6 +279,20 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } } } + if (mask & COM_REQ_CARD_RECORDING_STATE) { + CobsPayload<20> payload; + WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_CARD_RECORDING, payload); + uint8_t flags = 0; + if (sdcard::isOpen()) { + flags |= 1; + } + if (sdcard::isLocked()) { + flags |= 2; + } + payload.Append(flags); + WriteToOutput(payload); + ack_data |= COM_REQ_CARD_RECORDING_STATE; + } if (mask & COM_REQ_RESPONSE) { SendResponse(mask, ack_data); diff --git a/serial.h b/serial.h index ed28369..b033418 100644 --- a/serial.h +++ b/serial.h @@ -57,6 +57,7 @@ class SerialComm { COM_SET_PARTIAL_EEPROM_DATA = 1 << 20, COM_REINIT_PARTIAL_EEPROM_DATA = 1 << 21, COM_REQ_PARTIAL_EEPROM_DATA = 1 << 22, + COM_REQ_CARD_RECORDING_STATE = 1 << 23, }; enum StateFields : uint32_t { From 8cc27acbfbbdd7afeaf5878ed3a457333e4c01c8 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 17 Oct 2016 23:24:47 +0200 Subject: [PATCH 08/22] Send write delay as part of the recording state --- serial.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/serial.cpp b/serial.cpp index 584ebbc..8deab67 100644 --- a/serial.cpp +++ b/serial.cpp @@ -281,7 +281,8 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } if (mask & COM_REQ_CARD_RECORDING_STATE) { CobsPayload<20> payload; - WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_CARD_RECORDING, payload); + WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_SD_WRITE_DELAY | COM_SET_CARD_RECORDING, payload); + payload.Append(sd_card_state_delay); uint8_t flags = 0; if (sdcard::isOpen()) { flags |= 1; From a6e0bb1d00fee9d07c9534983aba4c987bc043f3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 21 Nov 2016 23:51:53 +0100 Subject: [PATCH 09/22] Fix compilation issues with variadic templates --- flybrix-firmware.ino | 61 +++++++++++++++++++++----------------------- 1 file changed, 29 insertions(+), 32 deletions(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 2c379fa..aecef9a 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -115,31 +115,9 @@ uint32_t low_battery_counter = 0; template uint32_t RunProcess(uint32_t start); -template -struct freqlist{}; - -void RunProcessesHelper(freqlist<>) { -} - -template -void RunProcessesHelper(freqlist) { - RunProcess(micros()); - sys.i2c.update(); - RunProcessesHelper(freqlist()); -} - -template -void RunProcesses() { - RunProcessesHelper(freqlist()); -} - -template -bool ProcessTask(); - bool skip_state_update = false; void loop() { - sys.state.loopCount++; sys.i2c.update(); // manages a queue of requests for mpu, mag, bmp @@ -167,11 +145,30 @@ void loop() { sys.motors.updateAllChannels(); } - RunProcesses<1000, 100, 40, 35, 30, 10, 1>(); + RunProcess<1000>(micros()); + sys.i2c.update(); + RunProcess<100>(micros()); + sys.i2c.update(); + RunProcess<40>(micros()); + sys.i2c.update(); + RunProcess<35>(micros()); + sys.i2c.update(); + RunProcess<30>(micros()); + sys.i2c.update(); + RunProcess<10>(micros()); + sys.i2c.update(); + RunProcess<1>(micros()); + sys.i2c.update(); } +template +class ProcessTask { + public: + static bool Run(); +}; + template <> -bool ProcessTask<1000>() { +bool ProcessTask<1000>::Run() { static uint16_t counterSerial{0}; static uint16_t counterSdCard{0}; if (++counterSerial > sys.conf.GetSendStateDelay() - 1) { @@ -190,13 +187,13 @@ bool ProcessTask<1000>() { } template <> -bool ProcessTask<30>() { +bool ProcessTask<30>::Run() { sys.led.update(); // update quickly to support color dithering return true; } template <> -bool ProcessTask<100>() { +bool ProcessTask<100>::Run() { if (sys.bmp.ready) { sys.state.updateStatePT(micros()); sys.bmp.startMeasurement(); @@ -219,13 +216,13 @@ bool ProcessTask<100>() { } template <> -bool ProcessTask<35>() { +bool ProcessTask<35>::Run() { flushSerial(); return true; } template <> -bool ProcessTask<40>() { +bool ProcessTask<40>::Run() { sys.pilot.processCommands(); sys.pwr.measureRawLevels(); // read all ADCs @@ -258,7 +255,7 @@ bool ProcessTask<40>() { } template <> -bool ProcessTask<10>() { +bool ProcessTask<10>::Run() { if (sys.mag.ready) { sys.mag.startMeasurement(); } else { @@ -269,7 +266,7 @@ bool ProcessTask<10>() { } template <> -bool ProcessTask<1>() { +bool ProcessTask<1>::Run() { return true; } @@ -279,11 +276,11 @@ uint32_t RunProcess(uint32_t start) { static uint32_t iterations{0}; while (start - previous_time > 1000000 / f) { - if (ProcessTask()) { + if (ProcessTask::Run()) { previous_time += 1000000 / f; ++iterations; } else { - break; + break; } } From cec1fa1da4a8deefd8f814a7140c0e51fb4c2e05 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 12 Dec 2016 21:48:14 +0100 Subject: [PATCH 10/22] Decentralize configuration setup --- AK8963.cpp | 2 + AK8963.h | 9 ++- R415X.cpp | 37 ++++++---- R415X.h | 1 + airframe.cpp | 24 ++++++ airframe.h | 1 + config.cpp | 205 +++------------------------------------------------ config.h | 12 +-- control.cpp | 52 +++++++++++-- control.h | 1 + led.cpp | 23 ++++++ led.h | 7 +- state.cpp | 5 +- state.h | 5 +- 14 files changed, 152 insertions(+), 232 deletions(-) diff --git a/AK8963.cpp b/AK8963.cpp index 517e585..f13c979 100644 --- a/AK8963.cpp +++ b/AK8963.cpp @@ -14,6 +14,8 @@ #include #include "state.h" +AK8963::MagBias::MagBias() : x{0.0}, y{0.0}, z{0.0} {} + // we have three coordinate systems here: // 1. REGISTER coordinates: native values as read // 2. IC/PCB coordinates: matches FLYER system if the pcb is in standard orientation diff --git a/AK8963.h b/AK8963.h index bbc5c96..dd696e2 100644 --- a/AK8963.h +++ b/AK8963.h @@ -31,17 +31,18 @@ class AK8963 : public CallbackProcessor { bool ready; bool startMeasurement(); // writes values to state (when data is ready) - void triggerCallback(); // handles return for getAccelGryo() + void triggerCallback(); // handles return for getAccelGryo() uint8_t getID(); struct __attribute__((packed)) MagBias { + MagBias(); bool verify() const { return true; } - float x; - float y; - float z; + float x; // Bx (milligauss) + float y; // By (milligauss) + float z; // Bz (milligauss) } mag_bias; static_assert(sizeof(MagBias) == 3 * 4, "Data is not packed"); diff --git a/R415X.cpp b/R415X.cpp index bfb6ac9..e3f3e8b 100644 --- a/R415X.cpp +++ b/R415X.cpp @@ -10,11 +10,23 @@ #include "state.h" #include "debug.h" -volatile uint16_t RX[RC_CHANNEL_COUNT]; // filled by the interrupt with valid data -volatile uint16_t RX_errors = 0; // count dropped frames -volatile uint16_t startPulse = 0; // keeps track of the last received pulse position +// RX -- PKZ3341 sends: RHS left/right, RHS up/down, LHS up/down, LHS +// left/right, RHS click (latch), LHS button(momentary) +// map throttle to LHS up/down +// map pitch to RHS up/down +// map roll to RHS left/righ +// map yaw to LHS up/down +// map AUX1 to RHS click +// map AUX2 to LHS click + +R415X::ChannelProperties::ChannelProperties() : assignment{2, 1, 0, 3, 4, 5}, inversion{6}, midpoint{1515, 1515, 1500, 1520, 1500, 1500}, deadzone{20, 20, 20, 40, 20, 20} { +} + +volatile uint16_t RX[RC_CHANNEL_COUNT]; // filled by the interrupt with valid data +volatile uint16_t RX_errors = 0; // count dropped frames +volatile uint16_t startPulse = 0; // keeps track of the last received pulse position volatile uint16_t RX_buffer[RC_CHANNEL_COUNT]; // buffer data in anticipation of a valid frame -volatile uint8_t RX_channel = 0; // we are collecting data for this channel +volatile uint8_t RX_channel = 0; // we are collecting data for this channel bool R415X::ChannelProperties::verify() const { bool ok{true}; @@ -95,7 +107,6 @@ extern "C" void ftm1_isr(void) { } void R415X::getCommandData(State* state) { - cli(); // disable interrupts // if R415X is working, we should never see anything less than 900! @@ -103,8 +114,8 @@ void R415X::getCommandData(State* state) { if (RX[i] < 900) { // tell state that R415X is not ready and return state->command_source_mask &= ~COMMAND_READY_R415X; - sei(); // enable interrupts - return; // don't load bad data into state + sei(); // enable interrupts + return; // don't load bad data into state } } @@ -157,13 +168,13 @@ void R415X::getCommandData(State* state) { // in some cases it is impossible to get a ppm channel to be close enought to the midpoint (~1500 usec) because the controller trim is too coarse to correct a small error // we get around this by creating a small dead zone around the midpoint of signed channel, specified in usec units pitch.val = pitch.applyDeadzone(); - roll.val = roll.applyDeadzone(); - yaw.val = yaw.applyDeadzone(); + roll.val = roll.applyDeadzone(); + yaw.val = yaw.applyDeadzone(); - uint16_t throttle_threshold = ((throttle.max - throttle.min) / 10) + throttle.min; // keep bottom 10% of throttle stick to mean 'off' + uint16_t throttle_threshold = ((throttle.max - throttle.min) / 10) + throttle.min; // keep bottom 10% of throttle stick to mean 'off' state->command_throttle = constrain((throttle.val - throttle_threshold) * 4095 / (throttle.max - throttle_threshold), 0, 4095); - state->command_pitch = constrain((1-2*((channel.inversion >> 1) & 1)) * (pitch.val - pitch.mid) * 4095 / (pitch.max - pitch.min), -2047, 2047); - state->command_roll = constrain((1-2*((channel.inversion >> 2) & 1)) * ( roll.val - roll.mid) * 4095 / ( roll.max - roll.min), -2047, 2047); - state->command_yaw = constrain((1-2*((channel.inversion >> 3) & 1)) * ( yaw.val - yaw.mid) * 4095 / ( yaw.max - yaw.min), -2047, 2047); + state->command_pitch = constrain((1 - 2 * ((channel.inversion >> 1) & 1)) * (pitch.val - pitch.mid) * 4095 / (pitch.max - pitch.min), -2047, 2047); + state->command_roll = constrain((1 - 2 * ((channel.inversion >> 2) & 1)) * (roll.val - roll.mid) * 4095 / (roll.max - roll.min), -2047, 2047); + state->command_yaw = constrain((1 - 2 * ((channel.inversion >> 3) & 1)) * (yaw.val - yaw.mid) * 4095 / (yaw.max - yaw.min), -2047, 2047); } diff --git a/R415X.h b/R415X.h index 08b368c..93c8156 100644 --- a/R415X.h +++ b/R415X.h @@ -68,6 +68,7 @@ class R415X { void getCommandData(State* state); struct __attribute__((packed)) ChannelProperties { + ChannelProperties(); bool verify() const; uint8_t assignment[6]; uint8_t inversion; // bitfield order is {throttle_channel, pitch_channel, roll_channel, yaw_channel, x, x, x, x} (LSB-->MSB) diff --git a/airframe.cpp b/airframe.cpp index a26031b..f0b19e6 100644 --- a/airframe.cpp +++ b/airframe.cpp @@ -20,3 +20,27 @@ void Airframe::updateMotorsMix() { for (size_t i = 0; i < 8; ++i) state->MotorOut[i] = mix(mix_table.fz[i], mix_table.tx[i], mix_table.ty[i], mix_table.tz[i]); } + +// 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 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 + +Airframe::MixTable::MixTable() : fz{1, 1, 1, 1, 1, 1, 1, 1}, tx{1, 1, 1, 1, -1, -1, -1, -1}, ty{-1, 1, -1, 1, -1, 1, -1, 1}, tz{1, -1, -1, 1, 1, -1, -1, 1} { +} diff --git a/airframe.h b/airframe.h index 6bcc6f0..1796daf 100644 --- a/airframe.h +++ b/airframe.h @@ -21,6 +21,7 @@ class Airframe { void updateMotorsMix(); struct __attribute__((packed)) MixTable { + MixTable(); bool verify() const { return true; } diff --git a/config.cpp b/config.cpp index 54e4ce1..4d7219b 100644 --- a/config.cpp +++ b/config.cpp @@ -8,201 +8,17 @@ #include "systems.h" -#define BYPASS_THRUST_MASTER 1 << 0 -#define BYPASS_PITCH_MASTER 1 << 1 -#define BYPASS_ROLL_MASTER 1 << 2 -#define BYPASS_YAW_MASTER 1 << 3 -#define BYPASS_THRUST_SLAVE 1 << 4 -#define BYPASS_PITCH_SLAVE 1 << 5 -#define BYPASS_ROLL_SLAVE 1 << 6 -#define BYPASS_YAW_SLAVE 1 << 7 - -CONFIG_struct::CONFIG_struct() { // Default Settings - pcb.orientation[0] = 0.0f; // pitch; applied first - pcb.orientation[1] = 0.0f; // roll; applied second - pcb.orientation[2] = 0.0f; // yaw; applied last - - pcb.translation[0] = 0.0f; // x (mm) - pcb.translation[1] = 0.0f; // y (mm) - pcb.translation[2] = 0.0f; // z (mm) - - id.id = 0; - - // 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 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 - // - mix_table.fz[0] = 1; - mix_table.tx[0] = 1; - mix_table.ty[0] = -1; - mix_table.tz[0] = 1; - mix_table.fz[1] = 1; - mix_table.tx[1] = 1; - mix_table.ty[1] = 1; - mix_table.tz[1] = -1; - mix_table.fz[2] = 1; - mix_table.tx[2] = 1; - mix_table.ty[2] = -1; - mix_table.tz[2] = -1; - mix_table.fz[3] = 1; - mix_table.tx[3] = 1; - mix_table.ty[3] = 1; - mix_table.tz[3] = 1; - mix_table.fz[4] = 1; - mix_table.tx[4] = -1; - mix_table.ty[4] = -1; - mix_table.tz[4] = 1; - mix_table.fz[5] = 1; - mix_table.tx[5] = -1; - mix_table.ty[5] = 1; - mix_table.tz[5] = -1; - mix_table.fz[6] = 1; - mix_table.tx[6] = -1; - mix_table.ty[6] = -1; - mix_table.tz[6] = -1; - mix_table.fz[7] = 1; - mix_table.tx[7] = -1; - mix_table.ty[7] = 1; - mix_table.tz[7] = 1; - - mag_bias.x = 0.0f; // Bx (milligauss) - mag_bias.y = 0.0f; // By (milligauss) - mag_bias.z = 0.0f; // Bz (milligauss) - - // RX -- PKZ3341 sends: RHS left/right, RHS up/down, LHS up/down, LHS - // left/right, RHS click (latch), LHS button(momentary) - channel.assignment[0] = 2; // map throttle to LHS up/down - channel.assignment[1] = 1; // map pitch to RHS up/down - channel.assignment[2] = 0; // map roll to RHS left/righ - channel.assignment[3] = 3; // map yaw to LHS up/down - channel.assignment[4] = 4; // map AUX1 to RHS click - channel.assignment[5] = 5; // map AUX2 to LHS click - - channel.inversion = 6; // invert both pitch and roll - - channel.midpoint[0] = 1515; - channel.midpoint[1] = 1515; - channel.midpoint[2] = 1500; - channel.midpoint[3] = 1520; - channel.midpoint[4] = 1500; - channel.midpoint[5] = 1500; - - channel.deadzone[0] = 20; - channel.deadzone[1] = 20; - channel.deadzone[2] = 20; - channel.deadzone[3] = 40; - channel.deadzone[4] = 20; - channel.deadzone[5] = 20; - - // PID Parameters - - pid_parameters.thrust_master[0] = 1.0f; // P - pid_parameters.thrust_master[1] = 0.0f; // I - pid_parameters.thrust_master[2] = 0.0f; // D - pid_parameters.thrust_master[3] = 0.0f; // Windup guard - pid_parameters.thrust_master[4] = 0.005f; // D filter usec (15Hz) - pid_parameters.thrust_master[5] = 0.005f; // setpoint filter usec (30Hz) - pid_parameters.thrust_master[6] = 1.0f; // (meters / full stick action) - - pid_parameters.pitch_master[0] = 5.0f; // P - pid_parameters.pitch_master[1] = 1.0f; // I - pid_parameters.pitch_master[2] = 0.0f; // D - pid_parameters.pitch_master[3] = 10.0f; // Windup guard - pid_parameters.pitch_master[4] = 0.005f; // D filter usec (15Hz) - pid_parameters.pitch_master[5] = 0.005f; // setpoint filter usec (30Hz) - pid_parameters.pitch_master[6] = 10.0f; // (degrees / full stick action) - - pid_parameters.roll_master[0] = 5.0f; // P - pid_parameters.roll_master[1] = 1.0f; // I - pid_parameters.roll_master[2] = 0.0f; // D - pid_parameters.roll_master[3] = 10.0f; // Windup guard - pid_parameters.roll_master[4] = 0.005f; // D filter usec (15Hz) - pid_parameters.roll_master[5] = 0.005f; // setpoint filter usec (30Hz) - pid_parameters.roll_master[6] = 10.0f; // (degrees / full stick action) - - pid_parameters.yaw_master[0] = 5.0f; // P - pid_parameters.yaw_master[1] = 1.0f; // I - pid_parameters.yaw_master[2] = 0.0f; // D - pid_parameters.yaw_master[3] = 10.0f; // Windup guard - pid_parameters.yaw_master[4] = 0.005f; // D filter usec (15Hz) - pid_parameters.yaw_master[5] = 0.005f; // setpoint filter usec (30Hz) - pid_parameters.yaw_master[6] = 180.0f; // (degrees / full stick action) - - pid_parameters.thrust_slave[0] = 1.0f; // P - pid_parameters.thrust_slave[1] = 0.0f; // I - pid_parameters.thrust_slave[2] = 0.0f; // D - pid_parameters.thrust_slave[3] = 10.0f; // Windup guard - pid_parameters.thrust_slave[4] = 0.001f; // D filter usec (150Hz) - pid_parameters.thrust_slave[5] = 0.001f; // setpoint filter usec (300Hz) - pid_parameters.thrust_slave[6] = 0.3f; // (meters/sec / full stick action) - - pid_parameters.pitch_slave[0] = 20.0f; // P - pid_parameters.pitch_slave[1] = 8.0f; // I - pid_parameters.pitch_slave[2] = 0.0f; // D - pid_parameters.pitch_slave[3] = 30.0f; // Windup guard - pid_parameters.pitch_slave[4] = 0.001f; // D filter usec (150Hz) - pid_parameters.pitch_slave[5] = 0.001f; // setpoint filter usec (300Hz) - pid_parameters.pitch_slave[6] = 30.0f; // (deg/sec / full stick action) - - pid_parameters.roll_slave[0] = 20.0f; // P - pid_parameters.roll_slave[1] = 8.0f; // I - pid_parameters.roll_slave[2] = 0.0f; // D - pid_parameters.roll_slave[3] = 30.0f; // Windup guard - pid_parameters.roll_slave[4] = 0.001f; // D filter usec (150Hz) - pid_parameters.roll_slave[5] = 0.001f; // setpoint filter usec (300Hz) - pid_parameters.roll_slave[6] = 30.0f; // (deg/sec / full stick action) - - pid_parameters.yaw_slave[0] = 30.0f; // P - pid_parameters.yaw_slave[1] = 5.0f; // I - pid_parameters.yaw_slave[2] = 0.0f; // D - pid_parameters.yaw_slave[3] = 20.0f; // Windup guard - pid_parameters.yaw_slave[4] = 0.001f; // D filter usec (150Hz) - pid_parameters.yaw_slave[5] = 0.001f; // setpoint filter usec (300Hz) - pid_parameters.yaw_slave[6] = 240.0f; // (deg/sec / full stick action) - - pid_parameters.pid_bypass = BYPASS_THRUST_MASTER | BYPASS_THRUST_SLAVE | - BYPASS_YAW_MASTER; // AHRS/Horizon mode - - state_parameters.state_estimation[0] = 1.00f; // 2*kp or BETA - state_parameters.state_estimation[1] = 0.01f; // 2*ki - - state_parameters.enable[0] = 0.001f; // max variance - state_parameters.enable[1] = 30.0f; // max angle +PcbTransform::PcbTransform() // Default settings + : orientation{0.0f, 0.0f, 0.0f}, // pitch, roll, yaw; applied in that order + translation{0.0f, 0.0f, 0.0f} // x, y, z in mm +{ +} - //fading is in 256ths : https://github.com/FastLED/FastLED/wiki/Pixel-reference - led_states = LED::States{{ - LED::StateCase(STATUS_MPU_FAIL, LED::SOLID, CRGB(CRGB::Black ).fadeLightBy(230), CRGB(CRGB::Red ).fadeLightBy(230), true), - LED::StateCase(STATUS_BMP_FAIL, LED::SOLID, CRGB(CRGB::Red ).fadeLightBy(230), CRGB(CRGB::Black ).fadeLightBy(230), true), - LED::StateCase(STATUS_BOOT, LED::SOLID, CRGB(CRGB::Green ).fadeLightBy(230)), - LED::StateCase(STATUS_RX_FAIL, LED::FLASH, CRGB(CRGB::Orange).fadeLightBy(230)), - LED::StateCase(STATUS_FAIL_OTHER, LED::ALTERNATE, CRGB(CRGB::Blue ).fadeLightBy(230)), - LED::StateCase(STATUS_FAIL_STABILITY, LED::FLASH, CRGB(CRGB::Black ).fadeLightBy(230), CRGB(CRGB::Blue ).fadeLightBy(230)), - LED::StateCase(STATUS_FAIL_ANGLE, LED::FLASH, CRGB(CRGB::Blue ).fadeLightBy(230), CRGB(CRGB::Black ).fadeLightBy(230)), - LED::StateCase(STATUS_OVERRIDE, LED::BEACON, CRGB(CRGB::Red ).fadeLightBy(230)), - LED::StateCase(STATUS_TEMP_WARNING, LED::FLASH, CRGB(CRGB::Red ).fadeLightBy(230)), - LED::StateCase(STATUS_BATTERY_LOW, LED::BEACON, CRGB(CRGB::Orange).fadeLightBy(230)), - LED::StateCase(STATUS_ENABLING, LED::FLASH, CRGB(CRGB::Blue ).fadeLightBy(230)), - LED::StateCase(STATUS_ENABLED, LED::BEACON, CRGB(CRGB::Blue ).fadeLightBy(230)), - LED::StateCase(STATUS_IDLE, LED::BEACON, CRGB(CRGB::Green ).fadeLightBy(230)), - }}; +// Default Config ID +ConfigID::ConfigID() : ConfigID{0} { +} +CONFIG_struct::CONFIG_struct() { // Default Settings // This function will only initialize data variables // writeEEPROM() needs to be called manually to store this data in EEPROM } @@ -240,8 +56,7 @@ bool verifyArgs(T& var, TArgs&... varArgs) { } bool CONFIG_struct::verify() const { - return verifyArgs(version, pcb, mix_table, mag_bias, channel, - pid_parameters, state_parameters, led_states, id); + return verifyArgs(version, pcb, mix_table, mag_bias, channel, pid_parameters, state_parameters, led_states, id); } void writeEEPROM(const CONFIG_union& CONFIG) { diff --git a/config.h b/config.h index 91e3158..cfaf652 100644 --- a/config.h +++ b/config.h @@ -30,8 +30,7 @@ struct Systems; struct __attribute__((packed)) ConfigID { - ConfigID() : ConfigID{0} { - } + ConfigID(); explicit ConfigID(uint32_t id) : id{id} { } bool verify() const { @@ -43,9 +42,7 @@ struct __attribute__((packed)) ConfigID { static_assert(sizeof(ConfigID) == 4, "Data is not packed"); struct __attribute__((packed)) PcbTransform { - PcbTransform() - : orientation{0.0f, 0.0f, 0.0f}, translation{0.0f, 0.0f, 0.0f} { - } + PcbTransform(); bool verify() const { return true; } @@ -86,10 +83,7 @@ struct __attribute__((packed)) CONFIG_struct { }; static_assert(sizeof(CONFIG_struct) == - sizeof(Version) + sizeof(ConfigID) + sizeof(PcbTransform) + - sizeof(Airframe::MixTable) + sizeof(AK8963::MagBias) + - sizeof(R415X::ChannelProperties) + - sizeof(State::Parameters) + + sizeof(Version) + sizeof(ConfigID) + sizeof(PcbTransform) + sizeof(Airframe::MixTable) + sizeof(AK8963::MagBias) + sizeof(R415X::ChannelProperties) + sizeof(State::Parameters) + sizeof(Control::PIDParameters) + sizeof(LED::States), "Data is not packed"); diff --git a/control.cpp b/control.cpp index 20d7a04..cbd3bcd 100644 --- a/control.cpp +++ b/control.cpp @@ -8,6 +8,48 @@ #include "debug.h" #include "state.h" +#define BYPASS_THRUST_MASTER 1 << 0 +#define BYPASS_PITCH_MASTER 1 << 1 +#define BYPASS_ROLL_MASTER 1 << 2 +#define BYPASS_YAW_MASTER 1 << 3 +#define BYPASS_THRUST_SLAVE 1 << 4 +#define BYPASS_PITCH_SLAVE 1 << 5 +#define BYPASS_ROLL_SLAVE 1 << 6 +#define BYPASS_YAW_SLAVE 1 << 7 + +// PID parameters: + +// MASTER: +// P +// I +// D +// Windup guard +// D filter usec (15Hz) +// setpoint filter usec (30Hz) +// (meters / full stick action) + +// SLAVE: +// P +// I +// D +// Windup guard +// D filter usec (150Hz) +// setpoint filter usec (300Hz) +// (deg/sec / full stick action) + +Control::PIDParameters::PIDParameters() + : thrust_master{1.0, 0.0, 0.0, 0.0, 0.005, 0.005, 1.0}, + pitch_master{5.0, 1.0, 0.0, 10.0, 0.005, 0.005, 10.0}, + roll_master{5.0, 1.0, 0.0, 10.0, 0.005, 0.005, 10.0}, + yaw_master{5.0, 1.0, 0.0, 10.0, 0.005, 0.005, 180.0}, + thrust_slave{1.0, 0.0, 0.0, 10.0, 0.001, 0.001, 0.3}, + pitch_slave{20.0, 8.0, 0.0, 30.0, 0.001, 0.001, 30.0}, + roll_slave{20.0, 8.0, 0.0, 30.0, 0.001, 0.001, 30.0}, + yaw_slave{30.0, 5.0, 0.0, 20.0, 0.001, 0.001, 240.0}, + pid_bypass{BYPASS_THRUST_MASTER | BYPASS_THRUST_SLAVE | BYPASS_YAW_MASTER} // AHRS/Horizon mode +{ +} + namespace { enum PID_ID { THRUST_MASTER = 0, @@ -65,7 +107,7 @@ void Control::parseConfig(const PIDParameters& config) { void Control::calculateControlVectors() { thrust_pid.setMasterInput(state->kinematicsAltitude); - thrust_pid.setSlaveInput(0.0f); //state->kinematicsClimbRate + thrust_pid.setSlaveInput(0.0f); // state->kinematicsClimbRate pitch_pid.setMasterInput(state->kinematicsAngle[0] * 57.2957795f); pitch_pid.setSlaveInput(state->kinematicsRate[0] * 57.2957795f); roll_pid.setMasterInput(state->kinematicsAngle[1] * 57.2957795f); @@ -73,10 +115,10 @@ void Control::calculateControlVectors() { yaw_pid.setMasterInput(state->kinematicsAngle[2] * 57.2957795f); yaw_pid.setSlaveInput(state->kinematicsRate[2] * 57.2957795f); - thrust_pid.setSetpoint(state->command_throttle * (1.0f/4095.0f) * thrust_pid.getScalingFactor(pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE], 4095.0f)); - pitch_pid.setSetpoint(state->command_pitch * (1.0f/2047.0f) * pitch_pid.getScalingFactor(pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE], 2047.0f)); - roll_pid.setSetpoint(state->command_roll * (1.0f/2047.0f) * roll_pid.getScalingFactor(pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE], 2047.0f)); - yaw_pid.setSetpoint(state->command_yaw * (1.0f/2047.0f) * yaw_pid.getScalingFactor(pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE], 2047.0f)); + thrust_pid.setSetpoint(state->command_throttle * (1.0f / 4095.0f) * thrust_pid.getScalingFactor(pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE], 4095.0f)); + pitch_pid.setSetpoint(state->command_pitch * (1.0f / 2047.0f) * pitch_pid.getScalingFactor(pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE], 2047.0f)); + roll_pid.setSetpoint(state->command_roll * (1.0f / 2047.0f) * roll_pid.getScalingFactor(pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE], 2047.0f)); + yaw_pid.setSetpoint(state->command_yaw * (1.0f / 2047.0f) * yaw_pid.getScalingFactor(pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE], 2047.0f)); // compute new output levels for state uint32_t now = micros(); diff --git a/control.h b/control.h index 261cce3..fd4cd5a 100644 --- a/control.h +++ b/control.h @@ -31,6 +31,7 @@ class Control { State* state; struct __attribute__((packed)) PIDParameters { + PIDParameters(); bool verify() const; float thrust_master[7]; // parameters are {P,I,D,integral windup guard, D filter delay sec, setpoint filter delay sec, command scaling factor} diff --git a/led.cpp b/led.cpp index 69e39bf..16110ef 100644 --- a/led.cpp +++ b/led.cpp @@ -7,6 +7,29 @@ #include "led.h" #include "state.h" +CRGB fade(CRGB color) { + return color.fadeLightBy(230); +} + +// fading is in 256ths : https://github.com/FastLED/FastLED/wiki/Pixel-reference +LED::States::States() + : states{ + LED::StateCase(STATUS_MPU_FAIL, LED::SOLID, fade(CRGB::Black), fade(CRGB::Red), true), + LED::StateCase(STATUS_BMP_FAIL, LED::SOLID, fade(CRGB::Red), fade(CRGB::Black), true), + LED::StateCase(STATUS_BOOT, LED::SOLID, fade(CRGB::Green)), + LED::StateCase(STATUS_RX_FAIL, LED::FLASH, fade(CRGB::Orange)), + LED::StateCase(STATUS_FAIL_OTHER, LED::ALTERNATE, fade(CRGB::Blue)), + LED::StateCase(STATUS_FAIL_STABILITY, LED::FLASH, fade(CRGB::Black), fade(CRGB::Blue)), + LED::StateCase(STATUS_FAIL_ANGLE, LED::FLASH, fade(CRGB::Blue), fade(CRGB::Black)), + LED::StateCase(STATUS_OVERRIDE, LED::BEACON, fade(CRGB::Red)), + LED::StateCase(STATUS_TEMP_WARNING, LED::FLASH, fade(CRGB::Red)), + LED::StateCase(STATUS_BATTERY_LOW, LED::BEACON, fade(CRGB::Orange)), + LED::StateCase(STATUS_ENABLING, LED::FLASH, fade(CRGB::Blue)), + LED::StateCase(STATUS_ENABLED, LED::BEACON, fade(CRGB::Blue)), + LED::StateCase(STATUS_IDLE, LED::BEACON, fade(CRGB::Green)), + } { +} + void (*LEDFastUpdate)(){nullptr}; namespace { diff --git a/led.h b/led.h index 49de79d..b506d77 100644 --- a/led.h +++ b/led.h @@ -33,7 +33,7 @@ class LED { SOLID = 5, }; - explicit LED(State *state); + explicit LED(State* state); void update(); @@ -62,7 +62,7 @@ class LED { struct __attribute__((packed)) StateCase { StateCase(uint16_t status, Pattern pattern, CRGB color_right_front, CRGB color_right_back, CRGB color_left_front, CRGB color_left_back, bool indicator_red = false, - bool indicator_green = false) + bool indicator_green = false) : status{status}, pattern{pattern}, color_right_front{color_right_front}, @@ -95,6 +95,7 @@ class LED { static_assert(sizeof(StateCase) == 2 + 1 + 4 * sizeof(Color) + 2, "Data is not packed"); struct __attribute__((packed)) States { + States(); bool verify() const { return true; } @@ -114,7 +115,7 @@ class LED { void indicatorRedOff(); void indicatorGreenOff(); - State *state; + State* state; uint16_t oldStatus{0}; bool override{false}; diff --git a/state.cpp b/state.cpp index 8c3395c..3e27488 100644 --- a/state.cpp +++ b/state.cpp @@ -17,6 +17,9 @@ #define STATE_T_SCALE 0.01f #define STATE_P_SCALE 0.000039063f +State::Parameters::Parameters() : state_estimation{1.0, 0.01}, enable{0.001, 30.0} { +} + State::State() : localization(0.0f, 1.0f, 0.0f, 0.0f, STATE_EXPECTED_TIME_STEP, FilterType::Madgwick, parameters.state_estimation, STATE_BARO_VARIANCE) { } @@ -43,7 +46,7 @@ bool State::upright(void) { for (uint8_t i = 0; i < 3; i++) { a_dot_a += accel_filter[i] * accel_filter[i]; } - return (accel_filter[2]*accel_filter[2] > a_dot_a*cos_test_angle*cos_test_angle ); + return (accel_filter[2] * accel_filter[2] > a_dot_a * cos_test_angle * cos_test_angle); } void State::processMotorEnablingIteration(void) { diff --git a/state.h b/state.h index deec442..927e916 100644 --- a/state.h +++ b/state.h @@ -75,11 +75,12 @@ class State { float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; struct __attribute__((packed)) Parameters { + Parameters(); bool verify() const { return true; } // state estimation parameters for tuning - float state_estimation[2]; // Madwick 2Kp, 2Ki + float state_estimation[2]; // Madwick 2Kp, 2Ki, Mahony Beta // limits for enabling motors float enable[2]; // variance and gravity angle @@ -119,7 +120,7 @@ class State { #define STATUS_FAIL_STABILITY 0x0100 #define STATUS_FAIL_ANGLE 0x0200 -#define STATUS_FAIL_OTHER 0x4000 // flag other arming failures +#define STATUS_FAIL_OTHER 0x4000 // flag other arming failures #define STATUS_ENABLED 0x0400 From 4530531bad19889773c4224b613aec8f2d00ed83 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 12 Dec 2016 22:31:19 +0100 Subject: [PATCH 11/22] Make config interface more streamlined --- config.cpp | 33 ++++++++----------- config.h | 13 ++------ eepromcursor.h | 51 +++++++++++++++++++++++++++++ flybrix-firmware.ino | 2 +- serial.cpp | 76 ++++++++++++++++++++++---------------------- 5 files changed, 106 insertions(+), 69 deletions(-) create mode 100644 eepromcursor.h diff --git a/config.cpp b/config.cpp index 4d7219b..0b6a8ff 100644 --- a/config.cpp +++ b/config.cpp @@ -7,6 +7,7 @@ #include "config.h" #include "systems.h" +#include "eepromcursor.h" PcbTransform::PcbTransform() // Default settings : orientation{0.0f, 0.0f, 0.0f}, // pitch, roll, yaw; applied in that order @@ -59,37 +60,31 @@ bool CONFIG_struct::verify() const { return verifyArgs(version, pcb, mix_table, mag_bias, channel, pid_parameters, state_parameters, led_states, id); } -void writeEEPROM(const CONFIG_union& CONFIG) { - for (size_t i = 0; i < sizeof(CONFIG_struct); i++) { - if (CONFIG.raw[i] != EEPROM.read(i)) { // Only re-write new data - EEPROM.write(i, CONFIG.raw[i]); - } - } +void writeEEPROM(const CONFIG_struct& CONFIG) { + EEPROMCursor cursor; + cursor.Append(CONFIG); } bool isEmptyEEPROM() { return EEPROM.read(0) == 255; } -CONFIG_union readEEPROM() { +CONFIG_struct readEEPROM() { + CONFIG_struct CONFIG; if (isEmptyEEPROM()) { // No EEPROM values detected, re-initialize to default values - writeEEPROM(CONFIG_union()); // store the default values - return readEEPROM(); + writeEEPROM(CONFIG_struct()); // store the default values + CONFIG = readEEPROM(); } else { - CONFIG_union CONFIG; - // There "is" data in the EEPROM, read it all - for (uint16_t i = 0; i < sizeof(struct CONFIG_struct); i++) { - CONFIG.raw[i] = EEPROM.read(i); - } + EEPROMCursor cursor; + cursor.ParseInto(CONFIG); // Verify version and general settings - if (!CONFIG.data.verify()) { + if (!CONFIG.verify()) { // If the stored configuration isn't legal in any way, report it // via debug and reset it - writeEEPROM(CONFIG_union()); // store the default values - return readEEPROM(); - } else { - return CONFIG; + writeEEPROM(CONFIG_struct()); // store the default values + CONFIG = readEEPROM(); } } + return CONFIG; } diff --git a/config.h b/config.h index cfaf652..9717217 100644 --- a/config.h +++ b/config.h @@ -89,17 +89,8 @@ static_assert(sizeof(CONFIG_struct) == static_assert(sizeof(CONFIG_struct) == 619, "Data does not have expected size"); -union CONFIG_union { - CONFIG_union() : data{CONFIG_struct()} { - } - explicit CONFIG_union(Systems& systems) : data{CONFIG_struct(systems)} { - } - struct CONFIG_struct data; - uint8_t raw[sizeof(CONFIG_struct)]; -}; - -void writeEEPROM(const CONFIG_union& CONFIG); -CONFIG_union readEEPROM(); +void writeEEPROM(const CONFIG_struct& CONFIG); +CONFIG_struct readEEPROM(); bool isEmptyEEPROM(); #endif diff --git a/eepromcursor.h b/eepromcursor.h new file mode 100644 index 0000000..a7cca48 --- /dev/null +++ b/eepromcursor.h @@ -0,0 +1,51 @@ +#ifndef EEPROMCURSOR_H +#define EEPROMCURSOR_H + +#include + +class EEPROMCursor final { + public: + template + void Append(T&& v) { + uint8_t* v_begin{(uint8_t*)&v}; + uint8_t* v_end{v_begin + sizeof(T)}; + while (v_begin != v_end) { + uint8_t value{*v_begin++}; + if (value != EEPROM.read(l)) { + EEPROM.write(l, value); + } + ++l; + } + } + + template + void Append(T&& v, Targs&&... vargs) { + Append(v); + Append(vargs...); + } + + template + void Skip(T&& v) { + l += sizeof(T); + } + + template + void ParseInto(T& data) { + uint8_t* v_begin{(uint8_t*)&data}; + uint8_t* v_end{v_begin + sizeof(T)}; + while (v_begin < v_end) { + *v_begin++ = EEPROM.read(l++); + } + } + + template + void ParseInto(T& data, Targs&... data_args) { + ParseInto(data); + ParseInto(data_args...); + } + + private: + std::size_t l{0}; +}; + +#endif /* EEPROMCURSOR_H */ diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index aecef9a..f3bd263 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -60,7 +60,7 @@ void setup() { setBluetoothUart(); // load stored settings (this will reinitialize if there is no data in the EEPROM! - readEEPROM().data.applyTo(sys); + readEEPROM().applyTo(sys); sys.state.resetState(); sys.state.set(STATUS_BMP_FAIL); diff --git a/serial.cpp b/serial.cpp index 8deab67..aaa3ed7 100644 --- a/serial.cpp +++ b/serial.cpp @@ -65,18 +65,18 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { uint32_t ack_data{0}; if (mask & COM_SET_EEPROM_DATA) { - CONFIG_union tmp_config; - if (data_input.ParseInto(tmp_config.raw)) { - if (tmp_config.data.verify()) { - tmp_config.data.applyTo(*systems); + CONFIG_struct tmp_config; + if (data_input.ParseInto(tmp_config)) { + if (tmp_config.verify()) { + tmp_config.applyTo(*systems); writeEEPROM(tmp_config); // TODO: deal with side effect code ack_data |= COM_SET_EEPROM_DATA; } } } if (mask & COM_REINIT_EEPROM_DATA) { - CONFIG_union tmp_config; - tmp_config.data.applyTo(*systems); + CONFIG_struct tmp_config; + tmp_config.applyTo(*systems); writeEEPROM(tmp_config); // TODO: deal with side effect code ack_data |= COM_REINIT_EEPROM_DATA; } @@ -186,28 +186,28 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { if (mask & COM_SET_PARTIAL_EEPROM_DATA) { uint16_t submask; if (data_input.ParseInto(submask)) { - CONFIG_union tmp_config(*systems); + CONFIG_struct tmp_config(*systems); bool success{true}; if (success && (submask & CONFIG_struct::VERSION)) { - success = data_input.ParseInto(tmp_config.data.version); + success = data_input.ParseInto(tmp_config.version); } if (success && (submask & CONFIG_struct::PCB)) { - success = data_input.ParseInto(tmp_config.data.pcb); + success = data_input.ParseInto(tmp_config.pcb); } if (success && (submask & CONFIG_struct::MIX_TABLE)) { - success = data_input.ParseInto(tmp_config.data.mix_table); + success = data_input.ParseInto(tmp_config.mix_table); } if (success && (submask & CONFIG_struct::MAG_BIAS)) { - success = data_input.ParseInto(tmp_config.data.mag_bias); + success = data_input.ParseInto(tmp_config.mag_bias); } if (success && (submask & CONFIG_struct::CHANNEL)) { - success = data_input.ParseInto(tmp_config.data.channel); + success = data_input.ParseInto(tmp_config.channel); } if (success && (submask & CONFIG_struct::PID_PARAMETERS)) { - success = data_input.ParseInto(tmp_config.data.pid_parameters); + success = data_input.ParseInto(tmp_config.pid_parameters); } if (success && (submask & CONFIG_struct::STATE_PARAMETERS)) { - success = data_input.ParseInto(tmp_config.data.state_parameters); + success = data_input.ParseInto(tmp_config.state_parameters); } if (success && (submask & CONFIG_struct::LED_STATES)) { // split up LED states further, since the variable is giant @@ -215,12 +215,12 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { success = data_input.ParseInto(led_mask); for (size_t led_code = 0; success && (led_code < 16); ++led_code) { if (led_mask & (1 << led_code)) { - success = data_input.ParseInto(tmp_config.data.led_states.states[led_code]); + success = data_input.ParseInto(tmp_config.led_states.states[led_code]); } } } - if (success && tmp_config.data.verify()) { - tmp_config.data.applyTo(*systems); + if (success && tmp_config.verify()) { + tmp_config.applyTo(*systems); writeEEPROM(tmp_config); // TODO: deal with side effect code ack_data |= COM_SET_PARTIAL_EEPROM_DATA; } @@ -229,41 +229,41 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { if (mask & COM_REINIT_PARTIAL_EEPROM_DATA) { uint16_t submask; if (data_input.ParseInto(submask)) { - CONFIG_union tmp_config(*systems); - CONFIG_union default_config; + CONFIG_struct tmp_config(*systems); + CONFIG_struct default_config; bool success{true}; if (submask & CONFIG_struct::VERSION) { - tmp_config.data.version = default_config.data.version; + tmp_config.version = default_config.version; } if (submask & CONFIG_struct::PCB) { - tmp_config.data.pcb = default_config.data.pcb; + tmp_config.pcb = default_config.pcb; } if (submask & CONFIG_struct::MIX_TABLE) { - tmp_config.data.mix_table = default_config.data.mix_table; + tmp_config.mix_table = default_config.mix_table; } if (submask & CONFIG_struct::MAG_BIAS) { - tmp_config.data.mag_bias = default_config.data.mag_bias; + tmp_config.mag_bias = default_config.mag_bias; } if (submask & CONFIG_struct::CHANNEL) { - tmp_config.data.channel = default_config.data.channel; + tmp_config.channel = default_config.channel; } if (submask & CONFIG_struct::PID_PARAMETERS) { - tmp_config.data.pid_parameters = default_config.data.pid_parameters; + tmp_config.pid_parameters = default_config.pid_parameters; } if (submask & CONFIG_struct::STATE_PARAMETERS) { - tmp_config.data.state_parameters = default_config.data.state_parameters; + tmp_config.state_parameters = default_config.state_parameters; } if (submask & CONFIG_struct::LED_STATES) { uint16_t led_mask; success = data_input.ParseInto(led_mask); for (size_t led_code = 0; success && (led_code < 16); ++led_code) { if (led_mask & (1 << led_code)) { - tmp_config.data.led_states.states[led_code] = default_config.data.led_states.states[led_code]; + tmp_config.led_states.states[led_code] = default_config.led_states.states[led_code]; } } } - if (success && tmp_config.data.verify()) { - tmp_config.data.applyTo(*systems); + if (success && tmp_config.verify()) { + tmp_config.applyTo(*systems); writeEEPROM(tmp_config); // TODO: deal with side effect code ack_data |= COM_REINIT_PARTIAL_EEPROM_DATA; } @@ -311,34 +311,34 @@ void SerialComm::SendPartialConfiguration(uint16_t submask, uint16_t led_mask) c CobsPayloadGeneric payload; WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_PARTIAL_EEPROM_DATA, payload); - CONFIG_union tmp_config(*systems); + CONFIG_struct tmp_config(*systems); payload.Append(submask); if (submask & CONFIG_struct::VERSION) { - payload.Append(tmp_config.data.version); + payload.Append(tmp_config.version); } if (submask & CONFIG_struct::PCB) { - payload.Append(tmp_config.data.pcb); + payload.Append(tmp_config.pcb); } if (submask & CONFIG_struct::MIX_TABLE) { - payload.Append(tmp_config.data.mix_table); + payload.Append(tmp_config.mix_table); } if (submask & CONFIG_struct::MAG_BIAS) { - payload.Append(tmp_config.data.mag_bias); + payload.Append(tmp_config.mag_bias); } if (submask & CONFIG_struct::CHANNEL) { - payload.Append(tmp_config.data.channel); + payload.Append(tmp_config.channel); } if (submask & CONFIG_struct::PID_PARAMETERS) { - payload.Append(tmp_config.data.pid_parameters); + payload.Append(tmp_config.pid_parameters); } if (submask & CONFIG_struct::STATE_PARAMETERS) { - payload.Append(tmp_config.data.state_parameters); + payload.Append(tmp_config.state_parameters); } if (submask & CONFIG_struct::LED_STATES) { payload.Append(led_mask); for (size_t led_code = 0; led_code < 16; ++led_code) { if (led_mask & (1 << led_code)) { - payload.Append(tmp_config.data.led_states.states[led_code]); + payload.Append(tmp_config.led_states.states[led_code]); } } } From a36da82f6dff536a8e35aadb049ab2c0a879693c Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 12 Dec 2016 23:47:09 +0100 Subject: [PATCH 12/22] Rewrite EEPROM's serial messages --- config.cpp | 48 +++++++++++++---- config.h | 20 ++++++- config_impl.h | 105 +++++++++++++++++++++++++++++++++++++ eepromcursor.h | 6 ++- serial.cpp | 138 +++++++------------------------------------------ 5 files changed, 187 insertions(+), 130 deletions(-) create mode 100644 config_impl.h diff --git a/config.cpp b/config.cpp index 0b6a8ff..9b62d1f 100644 --- a/config.cpp +++ b/config.cpp @@ -21,7 +21,7 @@ ConfigID::ConfigID() : ConfigID{0} { CONFIG_struct::CONFIG_struct() { // Default Settings // This function will only initialize data variables - // writeEEPROM() needs to be called manually to store this data in EEPROM + // data needs to be written manually to the EEPROM } CONFIG_struct::CONFIG_struct(Systems& sys) @@ -34,6 +34,41 @@ CONFIG_struct::CONFIG_struct(Systems& sys) led_states(sys.led.states) { } +void CONFIG_struct::resetPartial(uint16_t submask, uint16_t led_mask) { + if (submask & CONFIG_struct::VERSION) { + version = Version(); + } + if (submask & CONFIG_struct::ID) { + id = ConfigID(); + } + if (submask & CONFIG_struct::PCB) { + pcb = PcbTransform(); + } + if (submask & CONFIG_struct::MIX_TABLE) { + mix_table = Airframe::MixTable(); + } + if (submask & CONFIG_struct::MAG_BIAS) { + mag_bias = AK8963::MagBias(); + } + if (submask & CONFIG_struct::CHANNEL) { + channel = R415X::ChannelProperties(); + } + if (submask & CONFIG_struct::PID_PARAMETERS) { + pid_parameters = Control::PIDParameters(); + } + if (submask & CONFIG_struct::STATE_PARAMETERS) { + state_parameters = State::Parameters(); + } + if (submask & CONFIG_struct::LED_STATES) { + LED::States default_states; + for (size_t led_code = 0; led_code < 16; ++led_code) { + if (led_mask & (1 << led_code)) { + led_states.states[led_code] = default_states.states[led_code]; + } + } + } +} + void CONFIG_struct::applyTo(Systems& systems) const { systems.airframe.mix_table = mix_table; systems.mag.mag_bias = mag_bias; @@ -57,12 +92,7 @@ bool verifyArgs(T& var, TArgs&... varArgs) { } bool CONFIG_struct::verify() const { - return verifyArgs(version, pcb, mix_table, mag_bias, channel, pid_parameters, state_parameters, led_states, id); -} - -void writeEEPROM(const CONFIG_struct& CONFIG) { - EEPROMCursor cursor; - cursor.Append(CONFIG); + return verifyArgs(version, id, pcb, mix_table, mag_bias, channel, pid_parameters, state_parameters, led_states); } bool isEmptyEEPROM() { @@ -73,7 +103,7 @@ CONFIG_struct readEEPROM() { CONFIG_struct CONFIG; if (isEmptyEEPROM()) { // No EEPROM values detected, re-initialize to default values - writeEEPROM(CONFIG_struct()); // store the default values + CONFIG_struct().writeTo(EEPROMCursor()); // store the default values CONFIG = readEEPROM(); } else { EEPROMCursor cursor; @@ -82,7 +112,7 @@ CONFIG_struct readEEPROM() { if (!CONFIG.verify()) { // If the stored configuration isn't legal in any way, report it // via debug and reset it - writeEEPROM(CONFIG_struct()); // store the default values + CONFIG_struct().writeTo(EEPROMCursor()); // store the default values CONFIG = readEEPROM(); } } diff --git a/config.h b/config.h index 9717217..1c36588 100644 --- a/config.h +++ b/config.h @@ -71,6 +71,23 @@ struct __attribute__((packed)) CONFIG_struct { void applyTo(Systems& systems) const; bool verify() const; + void resetPartial(uint16_t submask, uint16_t led_mask); + + template + void writeTo(Cursor&& cursor) const; + + template + void writePartialTo(Cursor&& cursor, uint16_t submask, uint16_t led_mask) const; + + template + bool readFrom(Cursor&& cursor); + + template + bool readPartialFrom(Cursor&& cursor); + + template + static bool readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_mask); + Version version; ConfigID id; PcbTransform pcb; @@ -89,8 +106,9 @@ static_assert(sizeof(CONFIG_struct) == static_assert(sizeof(CONFIG_struct) == 619, "Data does not have expected size"); -void writeEEPROM(const CONFIG_struct& CONFIG); CONFIG_struct readEEPROM(); bool isEmptyEEPROM(); +#include "config_impl.h" + #endif diff --git a/config_impl.h b/config_impl.h new file mode 100644 index 0000000..2c81c26 --- /dev/null +++ b/config_impl.h @@ -0,0 +1,105 @@ +#ifndef CONFIG_IMPL_H +#define CONFIG_IMPL_H + +template +void CONFIG_struct::writeTo(Cursor&& cursor) const { + cursor.Append(*this); +} + +template +bool CONFIG_struct::readFrom(Cursor&& cursor) { + return cursor.ParseInto(*this); +} + +template +bool CONFIG_struct::readPartialFrom(Cursor&& cursor) { + uint16_t submask; + if (!cursor.ParseInto(submask)) { + return false; + } + if ((submask & CONFIG_struct::VERSION) && !cursor.ParseInto(version)) { + return false; + } + if ((submask & CONFIG_struct::ID) && !cursor.ParseInto(id)) { + return false; + } + if ((submask & CONFIG_struct::PCB) && !cursor.ParseInto(pcb)) { + return false; + } + if ((submask & CONFIG_struct::MIX_TABLE) && !cursor.ParseInto(mix_table)) { + return false; + } + if ((submask & CONFIG_struct::MAG_BIAS) && !cursor.ParseInto(mag_bias)) { + return false; + } + if ((submask & CONFIG_struct::CHANNEL) && !cursor.ParseInto(channel)) { + return false; + } + if ((submask & CONFIG_struct::PID_PARAMETERS) && !cursor.ParseInto(pid_parameters)) { + return false; + } + if ((submask & CONFIG_struct::STATE_PARAMETERS) && !cursor.ParseInto(state_parameters)) { + return false; + } + if (submask & CONFIG_struct::LED_STATES) { + // split up LED states further, since the variable is giant + uint16_t led_mask; + if (!cursor.ParseInto(led_mask)) { + return false; + } + for (size_t led_code = 0; led_code < 16; ++led_code) { + if ((led_mask & (1 << led_code)) && !cursor.ParseInto(led_states.states[led_code])) { + return false; + } + } + } + + return true; +} + +template +void CONFIG_struct::writePartialTo(Cursor&& cursor, uint16_t submask, uint16_t led_mask) const { + cursor.Append(submask); + if (submask & CONFIG_struct::VERSION) { + cursor.Append(version); + } + if (submask & CONFIG_struct::PCB) { + cursor.Append(pcb); + } + if (submask & CONFIG_struct::MIX_TABLE) { + cursor.Append(mix_table); + } + if (submask & CONFIG_struct::MAG_BIAS) { + cursor.Append(mag_bias); + } + if (submask & CONFIG_struct::CHANNEL) { + cursor.Append(channel); + } + if (submask & CONFIG_struct::PID_PARAMETERS) { + cursor.Append(pid_parameters); + } + if (submask & CONFIG_struct::STATE_PARAMETERS) { + cursor.Append(state_parameters); + } + if (submask & CONFIG_struct::LED_STATES) { + cursor.Append(led_mask); + for (size_t led_code = 0; led_code < 16; ++led_code) { + if (led_mask & (1 << led_code)) { + cursor.Append(led_states.states[led_code]); + } + } + } +} + +template +bool CONFIG_struct::readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_mask) { + if (!cursor.ParseInto(submask)) { + return false; + } + if (!(submask & CONFIG_struct::LED_STATES)) { + return true; + } + return cursor.ParseInto(led_mask); +} + +#endif /* CONFIG_IMPL_H */ diff --git a/eepromcursor.h b/eepromcursor.h index a7cca48..e832aaa 100644 --- a/eepromcursor.h +++ b/eepromcursor.h @@ -30,18 +30,20 @@ class EEPROMCursor final { } template - void ParseInto(T& data) { + bool ParseInto(T& data) { uint8_t* v_begin{(uint8_t*)&data}; uint8_t* v_end{v_begin + sizeof(T)}; while (v_begin < v_end) { *v_begin++ = EEPROM.read(l++); } + return true; } template - void ParseInto(T& data, Targs&... data_args) { + bool ParseInto(T& data, Targs&... data_args) { ParseInto(data); ParseInto(data_args...); + return true; } private: diff --git a/serial.cpp b/serial.cpp index aaa3ed7..b955688 100644 --- a/serial.cpp +++ b/serial.cpp @@ -15,6 +15,7 @@ #include "control.h" #include "led.h" #include "systems.h" +#include "eepromcursor.h" namespace { using CobsPayloadGeneric = CobsPayload<1000>; // impacts memory use only; packet size should be <= client packet size @@ -66,18 +67,18 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { if (mask & COM_SET_EEPROM_DATA) { CONFIG_struct tmp_config; - if (data_input.ParseInto(tmp_config)) { + if (tmp_config.readFrom(data_input)) { if (tmp_config.verify()) { tmp_config.applyTo(*systems); - writeEEPROM(tmp_config); // TODO: deal with side effect code + tmp_config.writeTo(EEPROMCursor()); ack_data |= COM_SET_EEPROM_DATA; } } } if (mask & COM_REINIT_EEPROM_DATA) { - CONFIG_struct tmp_config; + const CONFIG_struct tmp_config; tmp_config.applyTo(*systems); - writeEEPROM(tmp_config); // TODO: deal with side effect code + tmp_config.writeTo(EEPROMCursor()); ack_data |= COM_REINIT_EEPROM_DATA; } if (mask & COM_REQ_EEPROM_DATA) { @@ -184,99 +185,32 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } } if (mask & COM_SET_PARTIAL_EEPROM_DATA) { - uint16_t submask; - if (data_input.ParseInto(submask)) { - CONFIG_struct tmp_config(*systems); - bool success{true}; - if (success && (submask & CONFIG_struct::VERSION)) { - success = data_input.ParseInto(tmp_config.version); - } - if (success && (submask & CONFIG_struct::PCB)) { - success = data_input.ParseInto(tmp_config.pcb); - } - if (success && (submask & CONFIG_struct::MIX_TABLE)) { - success = data_input.ParseInto(tmp_config.mix_table); - } - if (success && (submask & CONFIG_struct::MAG_BIAS)) { - success = data_input.ParseInto(tmp_config.mag_bias); - } - if (success && (submask & CONFIG_struct::CHANNEL)) { - success = data_input.ParseInto(tmp_config.channel); - } - if (success && (submask & CONFIG_struct::PID_PARAMETERS)) { - success = data_input.ParseInto(tmp_config.pid_parameters); - } - if (success && (submask & CONFIG_struct::STATE_PARAMETERS)) { - success = data_input.ParseInto(tmp_config.state_parameters); - } - if (success && (submask & CONFIG_struct::LED_STATES)) { - // split up LED states further, since the variable is giant - uint16_t led_mask; - success = data_input.ParseInto(led_mask); - for (size_t led_code = 0; success && (led_code < 16); ++led_code) { - if (led_mask & (1 << led_code)) { - success = data_input.ParseInto(tmp_config.led_states.states[led_code]); - } - } - } - if (success && tmp_config.verify()) { + CONFIG_struct tmp_config(*systems); + if (tmp_config.readPartialFrom(data_input)) { + if (tmp_config.verify()) { tmp_config.applyTo(*systems); - writeEEPROM(tmp_config); // TODO: deal with side effect code + tmp_config.writeTo(EEPROMCursor()); ack_data |= COM_SET_PARTIAL_EEPROM_DATA; } } } if (mask & COM_REINIT_PARTIAL_EEPROM_DATA) { - uint16_t submask; - if (data_input.ParseInto(submask)) { + uint16_t submask, led_mask; + if (CONFIG_struct::readMasks(data_input, submask, led_mask)) { CONFIG_struct tmp_config(*systems); - CONFIG_struct default_config; - bool success{true}; - if (submask & CONFIG_struct::VERSION) { - tmp_config.version = default_config.version; - } - if (submask & CONFIG_struct::PCB) { - tmp_config.pcb = default_config.pcb; - } - if (submask & CONFIG_struct::MIX_TABLE) { - tmp_config.mix_table = default_config.mix_table; - } - if (submask & CONFIG_struct::MAG_BIAS) { - tmp_config.mag_bias = default_config.mag_bias; - } - if (submask & CONFIG_struct::CHANNEL) { - tmp_config.channel = default_config.channel; - } - if (submask & CONFIG_struct::PID_PARAMETERS) { - tmp_config.pid_parameters = default_config.pid_parameters; - } - if (submask & CONFIG_struct::STATE_PARAMETERS) { - tmp_config.state_parameters = default_config.state_parameters; - } - if (submask & CONFIG_struct::LED_STATES) { - uint16_t led_mask; - success = data_input.ParseInto(led_mask); - for (size_t led_code = 0; success && (led_code < 16); ++led_code) { - if (led_mask & (1 << led_code)) { - tmp_config.led_states.states[led_code] = default_config.led_states.states[led_code]; - } - } - } - if (success && tmp_config.verify()) { + tmp_config.resetPartial(submask, led_mask); + if (tmp_config.verify()) { tmp_config.applyTo(*systems); - writeEEPROM(tmp_config); // TODO: deal with side effect code + tmp_config.writeTo(EEPROMCursor()); ack_data |= COM_REINIT_PARTIAL_EEPROM_DATA; } } } if (mask & COM_REQ_PARTIAL_EEPROM_DATA) { - uint16_t submask; - if (data_input.ParseInto(submask)) { - uint16_t led_mask{0}; - if (!(submask & CONFIG_struct::LED_STATES) || data_input.ParseInto(led_mask)) { - SendPartialConfiguration(submask, led_mask); - ack_data |= COM_REQ_PARTIAL_EEPROM_DATA; - } + uint16_t submask, led_mask; + if (CONFIG_struct::readMasks(data_input, submask, led_mask)) { + SendPartialConfiguration(submask, led_mask); + ack_data |= COM_REQ_PARTIAL_EEPROM_DATA; } } if (mask & COM_REQ_CARD_RECORDING_STATE) { @@ -303,46 +237,14 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { void SerialComm::SendConfiguration() const { CobsPayloadGeneric payload; WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_EEPROM_DATA, payload); - payload.Append(CONFIG_struct(*systems)); + CONFIG_struct(*systems).writeTo(payload); WriteToOutput(payload); } void SerialComm::SendPartialConfiguration(uint16_t submask, uint16_t led_mask) const { CobsPayloadGeneric payload; WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_PARTIAL_EEPROM_DATA, payload); - - CONFIG_struct tmp_config(*systems); - payload.Append(submask); - if (submask & CONFIG_struct::VERSION) { - payload.Append(tmp_config.version); - } - if (submask & CONFIG_struct::PCB) { - payload.Append(tmp_config.pcb); - } - if (submask & CONFIG_struct::MIX_TABLE) { - payload.Append(tmp_config.mix_table); - } - if (submask & CONFIG_struct::MAG_BIAS) { - payload.Append(tmp_config.mag_bias); - } - if (submask & CONFIG_struct::CHANNEL) { - payload.Append(tmp_config.channel); - } - if (submask & CONFIG_struct::PID_PARAMETERS) { - payload.Append(tmp_config.pid_parameters); - } - if (submask & CONFIG_struct::STATE_PARAMETERS) { - payload.Append(tmp_config.state_parameters); - } - if (submask & CONFIG_struct::LED_STATES) { - payload.Append(led_mask); - for (size_t led_code = 0; led_code < 16; ++led_code) { - if (led_mask & (1 << led_code)) { - payload.Append(tmp_config.led_states.states[led_code]); - } - } - } - + CONFIG_struct(*systems).writePartialTo(payload, submask, led_mask); WriteToOutput(payload); } From 5137e3b4abd464e1dda2faff5ac4bf215c9b07b3 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 13 Dec 2016 14:54:48 +0100 Subject: [PATCH 13/22] Add reflection to configuration structure --- config.cpp | 158 ++++++++++++++++++++++--------------- config.h | 34 ++++---- config_impl.h | 210 +++++++++++++++++++++++++++++++++++--------------- control.cpp | 6 +- control.h | 3 +- led.cpp | 3 +- led.h | 2 +- systems.cpp | 5 ++ systems.h | 2 + 9 files changed, 273 insertions(+), 150 deletions(-) diff --git a/config.cpp b/config.cpp index 9b62d1f..bdf3ec7 100644 --- a/config.cpp +++ b/config.cpp @@ -24,75 +24,59 @@ CONFIG_struct::CONFIG_struct() { // Default Settings // data needs to be written manually to the EEPROM } -CONFIG_struct::CONFIG_struct(Systems& sys) - : id(sys.id), - mix_table(sys.airframe.mix_table), - mag_bias(sys.mag.mag_bias), - channel(sys.receiver.channel), - pid_parameters(sys.control.pid_parameters), - state_parameters(sys.state.parameters), - led_states(sys.led.states) { +Version version; +PcbTransform pcb_transform; + +template +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys); + +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + // TODO: add this to the systems + version = Version(); + return version; } -void CONFIG_struct::resetPartial(uint16_t submask, uint16_t led_mask) { - if (submask & CONFIG_struct::VERSION) { - version = Version(); - } - if (submask & CONFIG_struct::ID) { - id = ConfigID(); - } - if (submask & CONFIG_struct::PCB) { - pcb = PcbTransform(); - } - if (submask & CONFIG_struct::MIX_TABLE) { - mix_table = Airframe::MixTable(); - } - if (submask & CONFIG_struct::MAG_BIAS) { - mag_bias = AK8963::MagBias(); - } - if (submask & CONFIG_struct::CHANNEL) { - channel = R415X::ChannelProperties(); - } - if (submask & CONFIG_struct::PID_PARAMETERS) { - pid_parameters = Control::PIDParameters(); - } - if (submask & CONFIG_struct::STATE_PARAMETERS) { - state_parameters = State::Parameters(); - } - if (submask & CONFIG_struct::LED_STATES) { - LED::States default_states; - for (size_t led_code = 0; led_code < 16; ++led_code) { - if (led_mask & (1 << led_code)) { - led_states.states[led_code] = default_states.states[led_code]; - } - } - } +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.id; } -void CONFIG_struct::applyTo(Systems& systems) const { - systems.airframe.mix_table = mix_table; - systems.mag.mag_bias = mag_bias; - systems.receiver.channel = channel; - systems.state.parameters = state_parameters; +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + // TODO: add this to the systems + pcb_transform = PcbTransform(); + return pcb_transform; +} - systems.control.parseConfig(pid_parameters); - systems.led.parseConfig(led_states); - systems.id = id; +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.airframe.mix_table; } -template -bool verifyArgs(T& var) { - return var.verify(); +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.mag.mag_bias; } -template -bool verifyArgs(T& var, TArgs&... varArgs) { - bool ok{verifyArgs(var)}; - return verifyArgs(varArgs...) && ok; +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.receiver.channel; } -bool CONFIG_struct::verify() const { - return verifyArgs(version, id, pcb, mix_table, mag_bias, channel, pid_parameters, state_parameters, led_states); +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.control.pid_parameters; +} + +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.state.parameters; +} + +template <> +inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { + return sys.led.states; } bool isEmptyEEPROM() { @@ -106,8 +90,7 @@ CONFIG_struct readEEPROM() { CONFIG_struct().writeTo(EEPROMCursor()); // store the default values CONFIG = readEEPROM(); } else { - EEPROMCursor cursor; - cursor.ParseInto(CONFIG); + CONFIG.readFrom(EEPROMCursor()); // Verify version and general settings if (!CONFIG.verify()) { // If the stored configuration isn't legal in any way, report it @@ -118,3 +101,58 @@ CONFIG_struct readEEPROM() { } return CONFIG; } + +// No need to modify anything below this line + +template +inline typename std::enable_if::type applyFieldsTo(const std::tuple& t, Systems& systems) { +} + +template + inline typename std::enable_if < I::type applyFieldsTo(const std::tuple& t, Systems& systems) { + systemMapping(systems) = std::get(t); + applyFieldsTo(t, systems); + systems.parseConfig(); +} + +template +inline typename std::enable_if::type setFieldsFrom(std::tuple& t, Systems& systems) { +} + +template + inline typename std::enable_if < I::type setFieldsFrom(std::tuple& t, Systems& systems) { + std::get(t) = systemMapping(systems); + setFieldsFrom(t, systems); +} + +CONFIG_struct::CONFIG_struct(Systems& sys) { + setFieldsFrom(data, sys); +} + +void CONFIG_struct::resetPartial(uint16_t submask, uint16_t led_mask) { + resetFields(data, submask, led_mask); +} + +void CONFIG_struct::applyTo(Systems& systems) const { + applyFieldsTo(data, systems); +} + +template +inline bool verifyArg(T& var) { + return var.verify(); +} + +template +inline typename std::enable_if::type verifyArgs(const std::tuple& t) { + return true; +} + +template + inline typename std::enable_if < I::type verifyArgs(const std::tuple& t) { + bool ok{verifyArg(std::get(t))}; + return verifyArgs(t) && ok; +} + +bool CONFIG_struct::verify() const { + return verifyArgs(data); +} diff --git a/config.h b/config.h index 1c36588..4efd674 100644 --- a/config.h +++ b/config.h @@ -27,6 +27,8 @@ #include "state.h" #include "version.h" +#include + struct Systems; struct __attribute__((packed)) ConfigID { @@ -53,19 +55,21 @@ struct __attribute__((packed)) PcbTransform { static_assert(sizeof(PcbTransform) == 3 * 2 * 4, "Data is not packed"); -struct __attribute__((packed)) CONFIG_struct { +struct CONFIG_struct { enum Field : uint16_t { - VERSION = 1 << 0, - ID = 1 << 1, - PCB = 1 << 2, - MIX_TABLE = 1 << 3, - MAG_BIAS = 1 << 4, - CHANNEL = 1 << 5, - PID_PARAMETERS = 1 << 6, - STATE_PARAMETERS = 1 << 7, - LED_STATES = 1 << 8, + VERSION = 0, + ID = 1, + PCB = 2, + MIX_TABLE = 3, + MAG_BIAS = 4, + CHANNEL = 5, + PID_PARAMETERS = 6, + STATE_PARAMETERS = 7, + LED_STATES = 8, }; + using Data = std::tuple; + CONFIG_struct(); explicit CONFIG_struct(Systems& sys); void applyTo(Systems& systems) const; @@ -88,15 +92,7 @@ struct __attribute__((packed)) CONFIG_struct { template static bool readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_mask); - Version version; - ConfigID id; - PcbTransform pcb; - Airframe::MixTable mix_table; - AK8963::MagBias mag_bias; - R415X::ChannelProperties channel; - Control::PIDParameters pid_parameters; - State::Parameters state_parameters; - LED::States led_states; + Data data; }; static_assert(sizeof(CONFIG_struct) == diff --git a/config_impl.h b/config_impl.h index 2c81c26..0fe06d8 100644 --- a/config_impl.h +++ b/config_impl.h @@ -1,94 +1,170 @@ #ifndef CONFIG_IMPL_H #define CONFIG_IMPL_H -template -void CONFIG_struct::writeTo(Cursor&& cursor) const { - cursor.Append(*this); +constexpr uint16_t fieldToMask(std::size_t field) { + return 1 << field; } -template -bool CONFIG_struct::readFrom(Cursor&& cursor) { - return cursor.ParseInto(*this); -} +static_assert(4 == fieldToMask(CONFIG_struct::PCB), "Mask generation isn't static"); -template -bool CONFIG_struct::readPartialFrom(Cursor&& cursor) { - uint16_t submask; - if (!cursor.ParseInto(submask)) { - return false; - } - if ((submask & CONFIG_struct::VERSION) && !cursor.ParseInto(version)) { - return false; - } - if ((submask & CONFIG_struct::ID) && !cursor.ParseInto(id)) { - return false; +template +struct FieldFunctor { + static void Reset(T& data, uint16_t submask, uint16_t led_mask) { + using FieldType = typename std::tuple_element::type; + if (submask & fieldToMask(field)) { + std::get(data) = FieldType(); + } } - if ((submask & CONFIG_struct::PCB) && !cursor.ParseInto(pcb)) { - return false; + + template + static bool ReadAll(T& data, Cursor&& cursor) { + return cursor.ParseInto(std::get(data)); } - if ((submask & CONFIG_struct::MIX_TABLE) && !cursor.ParseInto(mix_table)) { - return false; + + template + static bool Read(T& data, Cursor&& cursor, uint16_t submask) { + return (!(submask & fieldToMask(field))) || cursor.ParseInto(std::get(data)); } - if ((submask & CONFIG_struct::MAG_BIAS) && !cursor.ParseInto(mag_bias)) { - return false; + + template + static void WriteAll(const T& data, Cursor&& cursor) { + cursor.Append(std::get(data)); } - if ((submask & CONFIG_struct::CHANNEL) && !cursor.ParseInto(channel)) { - return false; + + template + static void Write(const T& data, Cursor&& cursor, uint16_t submask, uint16_t led_mask) { + if (submask & fieldToMask(field)) { + cursor.Append(std::get(data)); + } } - if ((submask & CONFIG_struct::PID_PARAMETERS) && !cursor.ParseInto(pid_parameters)) { - return false; +}; + +template +struct FieldFunctor { + static constexpr std::size_t FIELD = CONFIG_struct::LED_STATES; + + static void Reset(T& data, uint16_t submask, uint16_t led_mask) { + if (!(submask & fieldToMask(FIELD))) { + return; + } + LED::States default_states; + for (size_t led_code = 0; led_code < 16; ++led_code) { + if (led_mask & (1 << led_code)) { + std::get(data).states[led_code] = default_states.states[led_code]; + } + } } - if ((submask & CONFIG_struct::STATE_PARAMETERS) && !cursor.ParseInto(state_parameters)) { - return false; + + template + static bool ReadAll(T& data, Cursor&& cursor) { + return cursor.ParseInto(std::get(data)); } - if (submask & CONFIG_struct::LED_STATES) { + + template + static bool Read(T& data, Cursor&& cursor, uint16_t submask) { + if (!(submask & fieldToMask(FIELD))) { + return false; + } // split up LED states further, since the variable is giant uint16_t led_mask; if (!cursor.ParseInto(led_mask)) { return false; } for (size_t led_code = 0; led_code < 16; ++led_code) { - if ((led_mask & (1 << led_code)) && !cursor.ParseInto(led_states.states[led_code])) { + if ((led_mask & (1 << led_code)) && !cursor.ParseInto(std::get(data).states[led_code])) { return false; } } + return true; + } + + template + static void WriteAll(const T& data, Cursor&& cursor) { + cursor.Append(std::get(data)); + } + + template + static void Write(const T& data, Cursor&& cursor, uint16_t submask, uint16_t led_mask) { + if (submask & fieldToMask(FIELD)) { + cursor.Append(led_mask); + for (size_t led_code = 0; led_code < 16; ++led_code) { + if (led_mask & (1 << led_code)) { + cursor.Append(std::get(data).states[led_code]); + } + } + } } +}; + +template +inline typename std::enable_if::type resetFields(std::tuple& t, uint16_t submask, uint16_t led_mask) { +} + +template + inline typename std::enable_if < I::type resetFields(std::tuple& t, uint16_t submask, uint16_t led_mask) { + FieldFunctor, I>::Reset(t, submask, led_mask); + resetFields(t, submask, led_mask); +} +template +inline typename std::enable_if::type readFieldsFrom(std::tuple& t, Cursor&& cursor, uint16_t submask) { return true; } -template -void CONFIG_struct::writePartialTo(Cursor&& cursor, uint16_t submask, uint16_t led_mask) const { - cursor.Append(submask); - if (submask & CONFIG_struct::VERSION) { - cursor.Append(version); - } - if (submask & CONFIG_struct::PCB) { - cursor.Append(pcb); - } - if (submask & CONFIG_struct::MIX_TABLE) { - cursor.Append(mix_table); - } - if (submask & CONFIG_struct::MAG_BIAS) { - cursor.Append(mag_bias); - } - if (submask & CONFIG_struct::CHANNEL) { - cursor.Append(channel); - } - if (submask & CONFIG_struct::PID_PARAMETERS) { - cursor.Append(pid_parameters); +template + inline typename std::enable_if < I::type readFieldsFrom(std::tuple& t, Cursor&& cursor, uint16_t submask) { + if (!FieldFunctor, I>::Read(t, cursor, submask)) { + return false; } - if (submask & CONFIG_struct::STATE_PARAMETERS) { - cursor.Append(state_parameters); + return readFieldsFrom(t, cursor, submask); +} + +template +inline typename std::enable_if::type readAllFieldsFrom(std::tuple& t, Cursor&& cursor) { + return true; +} + +template + inline typename std::enable_if < I::type readAllFieldsFrom(std::tuple& t, Cursor&& cursor) { + if (!FieldFunctor, I>::ReadAll(t, cursor)) { + return false; } - if (submask & CONFIG_struct::LED_STATES) { - cursor.Append(led_mask); - for (size_t led_code = 0; led_code < 16; ++led_code) { - if (led_mask & (1 << led_code)) { - cursor.Append(led_states.states[led_code]); - } - } + return readAllFieldsFrom(t, cursor); +} + +template +inline typename std::enable_if::type writeFieldsTo(const std::tuple& t, Cursor&& cursor, uint16_t submask, uint16_t led_mask) { +} + +template + inline typename std::enable_if < I::type writeFieldsTo(const std::tuple& t, Cursor&& cursor, uint16_t submask, uint16_t led_mask) { + FieldFunctor, I>::Write(t, cursor, submask, led_mask); + writeFieldsTo(t, cursor, submask, led_mask); +} + +template +inline typename std::enable_if::type writeAllFieldsTo(const std::tuple& t, Cursor&& cursor) { +} + +template + inline typename std::enable_if < I::type writeAllFieldsTo(const std::tuple& t, Cursor&& cursor) { + FieldFunctor, I>::WriteAll(t, cursor); + writeAllFieldsTo(t, cursor); +} + +template +bool CONFIG_struct::readPartialFrom(Cursor&& cursor) { + uint16_t submask; + if (!cursor.ParseInto(submask)) { + return false; } + return readFieldsFrom(data, cursor, submask); +} + +template +void CONFIG_struct::writePartialTo(Cursor&& cursor, uint16_t submask, uint16_t led_mask) const { + cursor.Append(submask); + writeFieldsTo(data, cursor, submask, led_mask); } template @@ -96,10 +172,20 @@ bool CONFIG_struct::readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_ if (!cursor.ParseInto(submask)) { return false; } - if (!(submask & CONFIG_struct::LED_STATES)) { + if (!(submask & fieldToMask(LED_STATES))) { return true; } return cursor.ParseInto(led_mask); } +template +void CONFIG_struct::writeTo(Cursor&& cursor) const { + writeAllFieldsTo(data, cursor); +} + +template +bool CONFIG_struct::readFrom(Cursor&& cursor) { + return readAllFieldsFrom(data, cursor); +} + #endif /* CONFIG_IMPL_H */ diff --git a/control.cpp b/control.cpp index cbd3bcd..6c67795 100644 --- a/control.cpp +++ b/control.cpp @@ -70,7 +70,7 @@ Control::Control(State* __state, const PIDParameters& config) pitch_pid{config.pitch_master, config.pitch_slave}, roll_pid{config.roll_master, config.roll_slave}, yaw_pid{config.yaw_master, config.yaw_slave} { - parseConfig(pid_parameters); + parseConfig(); } bool Control::PIDParameters::verify() const { @@ -83,9 +83,7 @@ bool Control::PIDParameters::verify() const { return retval; } -void Control::parseConfig(const PIDParameters& config) { - pid_parameters = config; - +void Control::parseConfig() { thrust_pid = {pid_parameters.thrust_master, pid_parameters.thrust_slave}; pitch_pid = {pid_parameters.pitch_master, pid_parameters.pitch_slave}; roll_pid = {pid_parameters.roll_master, pid_parameters.roll_slave}; diff --git a/control.h b/control.h index fd4cd5a..c856728 100644 --- a/control.h +++ b/control.h @@ -16,7 +16,6 @@ #include "cascadedPID.h" class PID; -class CONFIG_struct; class State; class Control { @@ -24,7 +23,7 @@ class Control { struct PIDParameters; Control(State* state, const PIDParameters& config); - void parseConfig(const PIDParameters& config); + void parseConfig(); void calculateControlVectors(); diff --git a/led.cpp b/led.cpp index 16110ef..9db0dbd 100644 --- a/led.cpp +++ b/led.cpp @@ -289,8 +289,7 @@ void LED::changeLights() { use(LED::ALTERNATE, CRGB::Red, CRGB::Red, CRGB::Red, CRGB::Red, true, false); // No status bits set } -void LED::parseConfig(const States& states) { - this->states = states; +void LED::parseConfig() { oldStatus = ~state->status; update(); } diff --git a/led.h b/led.h index b506d77..2d0dc7c 100644 --- a/led.h +++ b/led.h @@ -104,7 +104,7 @@ class LED { static_assert(sizeof(States) == 16 * sizeof(StateCase), "Data is not packed"); - void parseConfig(const States& states); + void parseConfig(); private: void use(Pattern pattern, CRGB color_right_front, CRGB color_right_back, CRGB color_left_front, CRGB color_left_back, bool red_indicator, bool green_indicator); diff --git a/systems.cpp b/systems.cpp index 6698b61..a4e5719 100644 --- a/systems.cpp +++ b/systems.cpp @@ -29,3 +29,8 @@ Systems::Systems() id{0} { CONFIG_struct().applyTo(*this); } + +void Systems::parseConfig() { + led.parseConfig(); + control.parseConfig(); +} diff --git a/systems.h b/systems.h index 46714c0..62f69e8 100644 --- a/systems.h +++ b/systems.h @@ -47,6 +47,8 @@ struct Systems { SerialComm conf; ConfigID id; + + void parseConfig(); }; #endif /* end of include guard: SYSTEMS_H */ From 1380c43789480e35674ab93a78573f44539567ce Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 13 Dec 2016 22:35:31 +0100 Subject: [PATCH 14/22] Use explicit datatypes --- config.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/config.cpp b/config.cpp index bdf3ec7..c7a5a35 100644 --- a/config.cpp +++ b/config.cpp @@ -31,51 +31,51 @@ template inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys); template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline Version& systemMapping(Systems& sys) { // TODO: add this to the systems version = Version(); return version; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline ConfigID& systemMapping(Systems& sys) { return sys.id; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline PcbTransform& systemMapping(Systems& sys) { // TODO: add this to the systems pcb_transform = PcbTransform(); return pcb_transform; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline Airframe::MixTable& systemMapping(Systems& sys) { return sys.airframe.mix_table; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline AK8963::MagBias& systemMapping(Systems& sys) { return sys.mag.mag_bias; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline R415X::ChannelProperties& systemMapping(Systems& sys) { return sys.receiver.channel; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline Control::PIDParameters& systemMapping(Systems& sys) { return sys.control.pid_parameters; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline State::Parameters& systemMapping(Systems& sys) { return sys.state.parameters; } template <> -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys) { +inline LED::States& systemMapping(Systems& sys) { return sys.led.states; } From c295a1687422e4089640176b10539a66b7726da5 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Tue, 13 Dec 2016 22:42:10 +0100 Subject: [PATCH 15/22] Use nicer config variable name --- config.cpp | 38 +++++++++++++++++++------------------- config.h | 30 +++++++++++++++--------------- config_impl.h | 16 ++++++++-------- serial.cpp | 16 ++++++++-------- systems.cpp | 2 +- 5 files changed, 51 insertions(+), 51 deletions(-) diff --git a/config.cpp b/config.cpp index c7a5a35..1e94e92 100644 --- a/config.cpp +++ b/config.cpp @@ -19,7 +19,7 @@ PcbTransform::PcbTransform() // Default settings ConfigID::ConfigID() : ConfigID{0} { } -CONFIG_struct::CONFIG_struct() { // Default Settings +Config::Config() { // Default Settings // This function will only initialize data variables // data needs to be written manually to the EEPROM } @@ -28,54 +28,54 @@ Version version; PcbTransform pcb_transform; template -inline decltype(std::get(CONFIG_struct::Data())) & systemMapping(Systems& sys); +inline decltype(std::get(Config::Data())) & systemMapping(Systems& sys); template <> -inline Version& systemMapping(Systems& sys) { +inline Version& systemMapping(Systems& sys) { // TODO: add this to the systems version = Version(); return version; } template <> -inline ConfigID& systemMapping(Systems& sys) { +inline ConfigID& systemMapping(Systems& sys) { return sys.id; } template <> -inline PcbTransform& systemMapping(Systems& sys) { +inline PcbTransform& systemMapping(Systems& sys) { // TODO: add this to the systems pcb_transform = PcbTransform(); return pcb_transform; } template <> -inline Airframe::MixTable& systemMapping(Systems& sys) { +inline Airframe::MixTable& systemMapping(Systems& sys) { return sys.airframe.mix_table; } template <> -inline AK8963::MagBias& systemMapping(Systems& sys) { +inline AK8963::MagBias& systemMapping(Systems& sys) { return sys.mag.mag_bias; } template <> -inline R415X::ChannelProperties& systemMapping(Systems& sys) { +inline R415X::ChannelProperties& systemMapping(Systems& sys) { return sys.receiver.channel; } template <> -inline Control::PIDParameters& systemMapping(Systems& sys) { +inline Control::PIDParameters& systemMapping(Systems& sys) { return sys.control.pid_parameters; } template <> -inline State::Parameters& systemMapping(Systems& sys) { +inline State::Parameters& systemMapping(Systems& sys) { return sys.state.parameters; } template <> -inline LED::States& systemMapping(Systems& sys) { +inline LED::States& systemMapping(Systems& sys) { return sys.led.states; } @@ -83,11 +83,11 @@ bool isEmptyEEPROM() { return EEPROM.read(0) == 255; } -CONFIG_struct readEEPROM() { - CONFIG_struct CONFIG; +Config readEEPROM() { + Config CONFIG; if (isEmptyEEPROM()) { // No EEPROM values detected, re-initialize to default values - CONFIG_struct().writeTo(EEPROMCursor()); // store the default values + Config().writeTo(EEPROMCursor()); // store the default values CONFIG = readEEPROM(); } else { CONFIG.readFrom(EEPROMCursor()); @@ -95,7 +95,7 @@ CONFIG_struct readEEPROM() { if (!CONFIG.verify()) { // If the stored configuration isn't legal in any way, report it // via debug and reset it - CONFIG_struct().writeTo(EEPROMCursor()); // store the default values + Config().writeTo(EEPROMCursor()); // store the default values CONFIG = readEEPROM(); } } @@ -125,15 +125,15 @@ template setFieldsFrom(t, systems); } -CONFIG_struct::CONFIG_struct(Systems& sys) { +Config::Config(Systems& sys) { setFieldsFrom(data, sys); } -void CONFIG_struct::resetPartial(uint16_t submask, uint16_t led_mask) { +void Config::resetPartial(uint16_t submask, uint16_t led_mask) { resetFields(data, submask, led_mask); } -void CONFIG_struct::applyTo(Systems& systems) const { +void Config::applyTo(Systems& systems) const { applyFieldsTo(data, systems); } @@ -153,6 +153,6 @@ template return verifyArgs(t) && ok; } -bool CONFIG_struct::verify() const { +bool Config::verify() const { return verifyArgs(data); } diff --git a/config.h b/config.h index 4efd674..1b0b14b 100644 --- a/config.h +++ b/config.h @@ -55,23 +55,23 @@ struct __attribute__((packed)) PcbTransform { static_assert(sizeof(PcbTransform) == 3 * 2 * 4, "Data is not packed"); -struct CONFIG_struct { +struct Config { enum Field : uint16_t { - VERSION = 0, - ID = 1, - PCB = 2, - MIX_TABLE = 3, - MAG_BIAS = 4, - CHANNEL = 5, - PID_PARAMETERS = 6, - STATE_PARAMETERS = 7, - LED_STATES = 8, + VERSION, + ID, + PCB, + MIX_TABLE, + MAG_BIAS, + CHANNEL, + PID_PARAMETERS, + STATE_PARAMETERS, + LED_STATES, }; using Data = std::tuple; - CONFIG_struct(); - explicit CONFIG_struct(Systems& sys); + Config(); + explicit Config(Systems& sys); void applyTo(Systems& systems) const; bool verify() const; @@ -95,14 +95,14 @@ struct CONFIG_struct { Data data; }; -static_assert(sizeof(CONFIG_struct) == +static_assert(sizeof(Config) == sizeof(Version) + sizeof(ConfigID) + sizeof(PcbTransform) + sizeof(Airframe::MixTable) + sizeof(AK8963::MagBias) + sizeof(R415X::ChannelProperties) + sizeof(State::Parameters) + sizeof(Control::PIDParameters) + sizeof(LED::States), "Data is not packed"); -static_assert(sizeof(CONFIG_struct) == 619, "Data does not have expected size"); +static_assert(sizeof(Config) == 619, "Data does not have expected size"); -CONFIG_struct readEEPROM(); +Config readEEPROM(); bool isEmptyEEPROM(); #include "config_impl.h" diff --git a/config_impl.h b/config_impl.h index 0fe06d8..e42f43b 100644 --- a/config_impl.h +++ b/config_impl.h @@ -5,7 +5,7 @@ constexpr uint16_t fieldToMask(std::size_t field) { return 1 << field; } -static_assert(4 == fieldToMask(CONFIG_struct::PCB), "Mask generation isn't static"); +static_assert(4 == fieldToMask(Config::PCB), "Mask generation isn't static"); template struct FieldFunctor { @@ -40,8 +40,8 @@ struct FieldFunctor { }; template -struct FieldFunctor { - static constexpr std::size_t FIELD = CONFIG_struct::LED_STATES; +struct FieldFunctor { + static constexpr std::size_t FIELD = Config::LED_STATES; static void Reset(T& data, uint16_t submask, uint16_t led_mask) { if (!(submask & fieldToMask(FIELD))) { @@ -153,7 +153,7 @@ template } template -bool CONFIG_struct::readPartialFrom(Cursor&& cursor) { +bool Config::readPartialFrom(Cursor&& cursor) { uint16_t submask; if (!cursor.ParseInto(submask)) { return false; @@ -162,13 +162,13 @@ bool CONFIG_struct::readPartialFrom(Cursor&& cursor) { } template -void CONFIG_struct::writePartialTo(Cursor&& cursor, uint16_t submask, uint16_t led_mask) const { +void Config::writePartialTo(Cursor&& cursor, uint16_t submask, uint16_t led_mask) const { cursor.Append(submask); writeFieldsTo(data, cursor, submask, led_mask); } template -bool CONFIG_struct::readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_mask) { +bool Config::readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_mask) { if (!cursor.ParseInto(submask)) { return false; } @@ -179,12 +179,12 @@ bool CONFIG_struct::readMasks(Cursor&& cursor, uint16_t& submask, uint16_t& led_ } template -void CONFIG_struct::writeTo(Cursor&& cursor) const { +void Config::writeTo(Cursor&& cursor) const { writeAllFieldsTo(data, cursor); } template -bool CONFIG_struct::readFrom(Cursor&& cursor) { +bool Config::readFrom(Cursor&& cursor) { return readAllFieldsFrom(data, cursor); } diff --git a/serial.cpp b/serial.cpp index b955688..9380efe 100644 --- a/serial.cpp +++ b/serial.cpp @@ -66,7 +66,7 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { uint32_t ack_data{0}; if (mask & COM_SET_EEPROM_DATA) { - CONFIG_struct tmp_config; + Config tmp_config; if (tmp_config.readFrom(data_input)) { if (tmp_config.verify()) { tmp_config.applyTo(*systems); @@ -76,7 +76,7 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } } if (mask & COM_REINIT_EEPROM_DATA) { - const CONFIG_struct tmp_config; + const Config tmp_config; tmp_config.applyTo(*systems); tmp_config.writeTo(EEPROMCursor()); ack_data |= COM_REINIT_EEPROM_DATA; @@ -185,7 +185,7 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } } if (mask & COM_SET_PARTIAL_EEPROM_DATA) { - CONFIG_struct tmp_config(*systems); + Config tmp_config(*systems); if (tmp_config.readPartialFrom(data_input)) { if (tmp_config.verify()) { tmp_config.applyTo(*systems); @@ -196,8 +196,8 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } if (mask & COM_REINIT_PARTIAL_EEPROM_DATA) { uint16_t submask, led_mask; - if (CONFIG_struct::readMasks(data_input, submask, led_mask)) { - CONFIG_struct tmp_config(*systems); + if (Config::readMasks(data_input, submask, led_mask)) { + Config tmp_config(*systems); tmp_config.resetPartial(submask, led_mask); if (tmp_config.verify()) { tmp_config.applyTo(*systems); @@ -208,7 +208,7 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { } if (mask & COM_REQ_PARTIAL_EEPROM_DATA) { uint16_t submask, led_mask; - if (CONFIG_struct::readMasks(data_input, submask, led_mask)) { + if (Config::readMasks(data_input, submask, led_mask)) { SendPartialConfiguration(submask, led_mask); ack_data |= COM_REQ_PARTIAL_EEPROM_DATA; } @@ -237,14 +237,14 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { void SerialComm::SendConfiguration() const { CobsPayloadGeneric payload; WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_EEPROM_DATA, payload); - CONFIG_struct(*systems).writeTo(payload); + Config(*systems).writeTo(payload); WriteToOutput(payload); } void SerialComm::SendPartialConfiguration(uint16_t submask, uint16_t led_mask) const { CobsPayloadGeneric payload; WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_PARTIAL_EEPROM_DATA, payload); - CONFIG_struct(*systems).writePartialTo(payload, submask, led_mask); + Config(*systems).writePartialTo(payload, submask, led_mask); WriteToOutput(payload); } diff --git a/systems.cpp b/systems.cpp index a4e5719..24f8c9f 100644 --- a/systems.cpp +++ b/systems.cpp @@ -27,7 +27,7 @@ Systems::Systems() // listen for configuration inputs conf{&state, RX, &control, this, &led, &pilot}, id{0} { - CONFIG_struct().applyTo(*this); + Config().applyTo(*this); } void Systems::parseConfig() { From 48e343813f154155a316a50e0be2cbb9e3836640 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 14 Dec 2016 22:19:32 +0100 Subject: [PATCH 16/22] Rewrite serial communication --- debug.cpp | 8 + debug.h | 11 +- flybrix-firmware.ino | 31 ++-- serial.cpp | 348 +++++-------------------------------------- serial.h | 127 ++++++++-------- serial_impl.h | 35 +++++ serial_subcommands.h | 243 ++++++++++++++++++++++++++++++ serial_substates.h | 142 ++++++++++++++++++ 8 files changed, 557 insertions(+), 388 deletions(-) create mode 100644 serial_impl.h create mode 100644 serial_subcommands.h create mode 100644 serial_substates.h diff --git a/debug.cpp b/debug.cpp index 07c8033..db93b7d 100644 --- a/debug.cpp +++ b/debug.cpp @@ -10,4 +10,12 @@ #include "debug.h" +#include "serial.h" + SerialComm* debug_serial_comm{nullptr}; + +void DebugSendString(const String& string) { + if (!debug_serial_comm) + return; + debug_serial_comm->SendDebugString(string); +} diff --git a/debug.h b/debug.h index f1a66b8..be39a44 100644 --- a/debug.h +++ b/debug.h @@ -11,17 +11,16 @@ #ifndef debug_h #define debug_h -#include - -#include "serial.h" +#include "Arduino.h" +class SerialComm; extern SerialComm* debug_serial_comm; +void DebugSendString(const String& string); + template void DebugPrint(T... args) { - if (!debug_serial_comm) - return; - debug_serial_comm->SendDebugString(String(args...)); + DebugSendString(String(args...)); } template diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index f3bd263..a4e65d2 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -69,10 +69,10 @@ void setup() { if (sys.bmp.getID() == 0x58) { sys.state.clear(STATUS_BMP_FAIL); // state is unhappy without an initial pressure - sys.bmp.startMeasurement(); // important; otherwise we'll never set ready! - sys.i2c.update(); // write data - delay(2); // wait for data to arrive - sys.i2c.update(); // read data + sys.bmp.startMeasurement(); // important; otherwise we'll never set ready! + sys.i2c.update(); // write data + delay(2); // wait for data to arrive + sys.i2c.update(); // read data sys.state.p0 = sys.state.pressure; // initialize reference pressure } else { sys.led.update(); @@ -173,11 +173,11 @@ bool ProcessTask<1000>::Run() { static uint16_t counterSdCard{0}; if (++counterSerial > sys.conf.GetSendStateDelay() - 1) { counterSerial = 0; - sys.conf.SendState(micros()); + sys.conf.SendState(); } if (++counterSdCard > sys.conf.GetSdCardStateDelay() - 1) { counterSdCard = 0; - sys.conf.SendState(micros(), 0xFFFFFFFF, true); + sys.conf.SendState(0xFFFFFFFF, true); } counterSerial %= 1000; counterSdCard %= 1000; @@ -228,25 +228,22 @@ bool ProcessTask<40>::Run() { sys.pwr.measureRawLevels(); // read all ADCs // check for low voltage condition - if ( ((1/50)/0.003*1.2/65536 * sys.state.I1_raw ) > 1.0f ){ //if total battery current > 1A - if ( ((20.5+226)/20.5*1.2/65536 * sys.state.V0_raw) < 2.8f ) { + if (((1 / 50) / 0.003 * 1.2 / 65536 * sys.state.I1_raw) > 1.0f) { // if total battery current > 1A + if (((20.5 + 226) / 20.5 * 1.2 / 65536 * sys.state.V0_raw) < 2.8f) { low_battery_counter++; - if ( low_battery_counter > 40 ){ + if (low_battery_counter > 40) { sys.state.set(STATUS_BATTERY_LOW); } - } - else { + } else { low_battery_counter = 0; } - } - else { - if ( ((20.5+226)/20.5*1.2/65536 * sys.state.V0_raw) < 3.63f ) { + } else { + if (((20.5 + 226) / 20.5 * 1.2 / 65536 * sys.state.V0_raw) < 3.63f) { low_battery_counter++; - if ( low_battery_counter > 40 ){ + if (low_battery_counter > 40) { sys.state.set(STATUS_BATTERY_LOW); } - } - else { + } else { low_battery_counter = 0; } } diff --git a/serial.cpp b/serial.cpp index 9380efe..4428999 100644 --- a/serial.cpp +++ b/serial.cpp @@ -6,38 +6,48 @@ #include "serial.h" -#include "serialFork.h" -#include "state.h" - -#include "cardManagement.h" #include "command.h" -#include "config.h" //CONFIG variable #include "control.h" #include "led.h" +#include "state.h" #include "systems.h" -#include "eepromcursor.h" namespace { -using CobsPayloadGeneric = CobsPayload<1000>; // impacts memory use only; packet size should be <= client packet size -template -inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsPayload& payload) { - payload.Append(type); - payload.Append(mask); +static_assert(0x4000000 == FLAG(26), "Mask is not calculated at compile time"); + +template +inline void doSubcommandWrap(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { + if ((mask & FLAG(I)) && serial.doSubcommand(input)) { + ack |= FLAG(I); + } +} + +template +inline typename std::enable_if::type doSubcommands(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { +} + +template + inline typename std::enable_if < I::type doSubcommands(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { + doSubcommandWrap(serial, input, mask, ack); + doSubcommands(serial, input, mask, ack); } -template -inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { - auto package = payload.Encode(); - if (use_logger) - sdcard::write(package.data, package.length); - else - writeSerial(package.data, package.length); +template +inline void readSubstateWrap(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { + if (mask & FLAG(I)) { + serial.readSubstate(payload); + } +} + +template +inline typename std::enable_if::type readSubstates(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { } -template -inline void WritePIDData(CobsPayload& payload, const PID& pid) { - payload.Append(pid.lastTime(), pid.input(), pid.setpoint(), pid.pTerm(), pid.iTerm(), pid.dTerm()); +template + inline typename std::enable_if < I::type readSubstates(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { + readSubstateWrap(serial, payload, mask); + readSubstates(serial, payload, mask); } } @@ -58,192 +68,31 @@ void SerialComm::ProcessData(CobsReaderBuffer& data_input) { MessageType code; uint32_t mask; - if (!data_input.ParseInto(code, mask)) + if (!data_input.ParseInto(code, mask)) { return; - if (code != MessageType::Command) + } + if (code != MessageType::Command) { return; + } uint32_t ack_data{0}; + doSubcommands(*this, data_input, mask, ack_data); - if (mask & COM_SET_EEPROM_DATA) { - Config tmp_config; - if (tmp_config.readFrom(data_input)) { - if (tmp_config.verify()) { - tmp_config.applyTo(*systems); - tmp_config.writeTo(EEPROMCursor()); - ack_data |= COM_SET_EEPROM_DATA; - } - } - } - if (mask & COM_REINIT_EEPROM_DATA) { - const Config tmp_config; - tmp_config.applyTo(*systems); - tmp_config.writeTo(EEPROMCursor()); - ack_data |= COM_REINIT_EEPROM_DATA; - } - if (mask & COM_REQ_EEPROM_DATA) { - SendConfiguration(); - ack_data |= COM_REQ_EEPROM_DATA; - } - if (mask & COM_REQ_ENABLE_ITERATION) { - uint8_t flag; - if (data_input.ParseInto(flag)) { - if (flag == 1) - state->processMotorEnablingIteration(); - else - state->disableMotors(); - ack_data |= COM_REQ_ENABLE_ITERATION; - } - } - // This should pass if any motor speed is set - if (mask & COM_MOTOR_OVERRIDE_SPEED_ALL) { - if (mask & COM_MOTOR_OVERRIDE_SPEED_0 && data_input.ParseInto(state->MotorOut[0])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_0; - if (mask & COM_MOTOR_OVERRIDE_SPEED_1 && data_input.ParseInto(state->MotorOut[1])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_1; - if (mask & COM_MOTOR_OVERRIDE_SPEED_2 && data_input.ParseInto(state->MotorOut[2])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_2; - if (mask & COM_MOTOR_OVERRIDE_SPEED_3 && data_input.ParseInto(state->MotorOut[3])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_3; - if (mask & COM_MOTOR_OVERRIDE_SPEED_4 && data_input.ParseInto(state->MotorOut[4])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_4; - if (mask & COM_MOTOR_OVERRIDE_SPEED_5 && data_input.ParseInto(state->MotorOut[5])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_5; - if (mask & COM_MOTOR_OVERRIDE_SPEED_6 && data_input.ParseInto(state->MotorOut[6])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_6; - if (mask & COM_MOTOR_OVERRIDE_SPEED_7 && data_input.ParseInto(state->MotorOut[7])) - ack_data |= COM_MOTOR_OVERRIDE_SPEED_7; - } - if (mask & COM_SET_COMMAND_OVERRIDE) { - uint8_t flag; - if (data_input.ParseInto(flag)) { - if (flag == 1) - state->set(STATUS_OVERRIDE); - else - state->clear(STATUS_OVERRIDE); - ack_data |= COM_SET_COMMAND_OVERRIDE; - } - } - if (mask & COM_SET_STATE_MASK) { - uint32_t new_state_mask; - if (data_input.ParseInto(new_state_mask)) { - SetStateMsg(new_state_mask); - ack_data |= COM_SET_STATE_MASK; - } - } - if (mask & COM_SET_STATE_DELAY) { - uint16_t new_state_delay; - if (data_input.ParseInto(new_state_delay)) { - send_state_delay = new_state_delay; - ack_data |= COM_SET_STATE_DELAY; - } - } - 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; - if (data_input.ParseInto(mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g)) { - led->set(LED::Pattern(mode), r1, g1, b1, r2, g2, b2, ind_r, ind_g); - ack_data |= COM_SET_LED; - } - } - if (mask & COM_SET_SERIAL_RC) { - uint8_t enabled; - int16_t throttle, pitch, roll, yaw; - uint8_t auxmask; - if (data_input.ParseInto(enabled, throttle, pitch, roll, yaw, auxmask)) { - if (enabled) { - state->command_source_mask |= COMMAND_READY_BTLE; - state->command_AUX_mask = auxmask; - state->command_throttle = throttle; - state->command_pitch = pitch; - state->command_roll = roll; - state->command_yaw = yaw; - } else { - state->command_source_mask &= ~COMMAND_READY_BTLE; - } - ack_data |= COM_SET_SERIAL_RC; - } - } - if (mask & COM_SET_CARD_RECORDING) { - uint8_t recording_flags; - if (data_input.ParseInto(recording_flags)) { - bool shouldRecordToCard = recording_flags & 1; - bool shouldLock = recording_flags & 2; - sdcard::setLock(false); - if (shouldRecordToCard) - sdcard::openFile(); - else - sdcard::closeFile(); - sdcard::setLock(shouldLock); - ack_data |= COM_SET_CARD_RECORDING; - } - } - if (mask & COM_SET_PARTIAL_EEPROM_DATA) { - Config tmp_config(*systems); - if (tmp_config.readPartialFrom(data_input)) { - if (tmp_config.verify()) { - tmp_config.applyTo(*systems); - tmp_config.writeTo(EEPROMCursor()); - ack_data |= COM_SET_PARTIAL_EEPROM_DATA; - } - } - } - if (mask & COM_REINIT_PARTIAL_EEPROM_DATA) { - uint16_t submask, led_mask; - if (Config::readMasks(data_input, submask, led_mask)) { - Config tmp_config(*systems); - tmp_config.resetPartial(submask, led_mask); - if (tmp_config.verify()) { - tmp_config.applyTo(*systems); - tmp_config.writeTo(EEPROMCursor()); - ack_data |= COM_REINIT_PARTIAL_EEPROM_DATA; - } - } - } - if (mask & COM_REQ_PARTIAL_EEPROM_DATA) { - uint16_t submask, led_mask; - if (Config::readMasks(data_input, submask, led_mask)) { - SendPartialConfiguration(submask, led_mask); - ack_data |= COM_REQ_PARTIAL_EEPROM_DATA; - } - } - if (mask & COM_REQ_CARD_RECORDING_STATE) { - CobsPayload<20> payload; - WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_SD_WRITE_DELAY | COM_SET_CARD_RECORDING, payload); - payload.Append(sd_card_state_delay); - uint8_t flags = 0; - if (sdcard::isOpen()) { - flags |= 1; - } - if (sdcard::isLocked()) { - flags |= 2; - } - payload.Append(flags); - WriteToOutput(payload); - ack_data |= COM_REQ_CARD_RECORDING_STATE; - } - - if (mask & COM_REQ_RESPONSE) { + if (mask & FLAG(REQ_RESPONSE)) { SendResponse(mask, ack_data); } } void SerialComm::SendConfiguration() const { CobsPayloadGeneric payload; - WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_EEPROM_DATA, payload); + WriteProtocolHead(SerialComm::MessageType::Command, FLAG(SET_EEPROM_DATA), payload); Config(*systems).writeTo(payload); WriteToOutput(payload); } void SerialComm::SendPartialConfiguration(uint16_t submask, uint16_t led_mask) const { CobsPayloadGeneric payload; - WriteProtocolHead(SerialComm::MessageType::Command, COM_SET_PARTIAL_EEPROM_DATA, payload); + WriteProtocolHead(SerialComm::MessageType::Command, FLAG(SET_PARTIAL_EEPROM_DATA), payload); Config(*systems).writePartialTo(payload, submask, led_mask); WriteToOutput(payload); } @@ -258,66 +107,7 @@ void SerialComm::SendDebugString(const String& string, MessageType type) const { WriteToOutput(payload); } -uint16_t SerialComm::PacketSize(uint32_t mask) const { - uint16_t sum = 0; - if (mask & SerialComm::STATE_MICROS) - sum += 4; - if (mask & SerialComm::STATE_STATUS) - sum += 2; - if (mask & SerialComm::STATE_V0) - sum += 2; - if (mask & SerialComm::STATE_I0) - sum += 2; - if (mask & SerialComm::STATE_I1) - sum += 2; - if (mask & SerialComm::STATE_ACCEL) - sum += 3 * 4; - if (mask & SerialComm::STATE_GYRO) - sum += 3 * 4; - if (mask & SerialComm::STATE_MAG) - sum += 3 * 4; - if (mask & SerialComm::STATE_TEMPERATURE) - sum += 2; - if (mask & SerialComm::STATE_PRESSURE) - sum += 4; - if (mask & SerialComm::STATE_RX_PPM) - sum += 6 * 2; - if (mask & SerialComm::STATE_AUX_CHAN_MASK) - sum += 1; - if (mask & SerialComm::STATE_COMMANDS) - sum += 4 * 2; - if (mask & SerialComm::STATE_F_AND_T) - sum += 4 * 4; - if (mask & SerialComm::STATE_PID_FZ_MASTER) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_TX_MASTER) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_TY_MASTER) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_TZ_MASTER) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_FZ_SLAVE) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_TX_SLAVE) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_TY_SLAVE) - sum += 7 * 4; - if (mask & SerialComm::STATE_PID_TZ_SLAVE) - sum += 7 * 4; - if (mask & SerialComm::STATE_MOTOR_OUT) - sum += 8 * 2; - if (mask & SerialComm::STATE_KINE_ANGLE) - sum += 3 * 4; - if (mask & SerialComm::STATE_KINE_RATE) - sum += 3 * 4; - if (mask & SerialComm::STATE_KINE_ALTITUDE) - sum += 4; - if (mask & SerialComm::STATE_LOOP_COUNT) - sum += 4; - return sum; -} - -void SerialComm::SendState(uint32_t timestamp_us, uint32_t mask, bool redirect_to_sd_card) const { +void SerialComm::SendState(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; @@ -330,63 +120,7 @@ void SerialComm::SendState(uint32_t timestamp_us, uint32_t mask, bool redirect_t CobsPayloadGeneric payload; WriteProtocolHead(SerialComm::MessageType::State, mask, payload); - - if (mask & SerialComm::STATE_MICROS) - payload.Append(timestamp_us); - if (mask & SerialComm::STATE_STATUS) - payload.Append(state->status); - if (mask & SerialComm::STATE_V0) - payload.Append(state->V0_raw); - if (mask & SerialComm::STATE_I0) - payload.Append(state->I0_raw); - if (mask & SerialComm::STATE_I1) - payload.Append(state->I1_raw); - if (mask & SerialComm::STATE_ACCEL) - payload.Append(state->accel); - if (mask & SerialComm::STATE_GYRO) - payload.Append(state->gyro); - if (mask & SerialComm::STATE_MAG) - payload.Append(state->mag); - if (mask & SerialComm::STATE_TEMPERATURE) - payload.Append(state->temperature); - if (mask & SerialComm::STATE_PRESSURE) - payload.Append(state->pressure); - if (mask & SerialComm::STATE_RX_PPM) { - for (int i = 0; i < 6; ++i) - payload.Append(ppm[i]); - } - if (mask & SerialComm::STATE_AUX_CHAN_MASK) - payload.Append(state->command_AUX_mask); - if (mask & SerialComm::STATE_COMMANDS) - payload.Append(state->command_throttle, state->command_pitch, state->command_roll, state->command_yaw); - if (mask & SerialComm::STATE_F_AND_T) - payload.Append(state->Fz, state->Tx, state->Ty, state->Tz); - if (mask & SerialComm::STATE_PID_FZ_MASTER) - WritePIDData(payload, control->thrust_pid.master()); - if (mask & SerialComm::STATE_PID_TX_MASTER) - WritePIDData(payload, control->pitch_pid.master()); - if (mask & SerialComm::STATE_PID_TY_MASTER) - WritePIDData(payload, control->roll_pid.master()); - if (mask & SerialComm::STATE_PID_TZ_MASTER) - WritePIDData(payload, control->yaw_pid.master()); - if (mask & SerialComm::STATE_PID_FZ_SLAVE) - WritePIDData(payload, control->thrust_pid.slave()); - if (mask & SerialComm::STATE_PID_TX_SLAVE) - WritePIDData(payload, control->pitch_pid.slave()); - if (mask & SerialComm::STATE_PID_TY_SLAVE) - WritePIDData(payload, control->roll_pid.slave()); - if (mask & SerialComm::STATE_PID_TZ_SLAVE) - WritePIDData(payload, control->yaw_pid.slave()); - if (mask & SerialComm::STATE_MOTOR_OUT) - payload.Append(state->MotorOut); - if (mask & SerialComm::STATE_KINE_ANGLE) - payload.Append(state->kinematicsAngle); - if (mask & SerialComm::STATE_KINE_RATE) - payload.Append(state->kinematicsRate); - if (mask & SerialComm::STATE_KINE_ALTITUDE) - payload.Append(state->kinematicsAltitude); - if (mask & SerialComm::STATE_LOOP_COUNT) - payload.Append(state->loopCount); + readSubstates(*this, payload, mask); WriteToOutput(payload, redirect_to_sd_card); } diff --git a/serial.h b/serial.h index b033418..d525614 100644 --- a/serial.h +++ b/serial.h @@ -20,6 +20,12 @@ class LED; class State; struct Systems; +using CobsPayloadGeneric = CobsPayload<1000>; // impacts memory use only; packet size should be <= client packet size + +static constexpr uint32_t FLAG(uint8_t field) { + return uint32_t{1} << field; +} + class SerialComm { public: enum class MessageType : uint8_t { @@ -31,65 +37,63 @@ class SerialComm { HistoryData = 4, }; - enum CommandFields : uint32_t { - COM_REQ_RESPONSE = 1 << 0, - COM_SET_EEPROM_DATA = 1 << 1, - COM_REINIT_EEPROM_DATA = 1 << 2, - COM_REQ_EEPROM_DATA = 1 << 3, - COM_REQ_ENABLE_ITERATION = 1 << 4, - COM_MOTOR_OVERRIDE_SPEED_0 = 1 << 5, - COM_MOTOR_OVERRIDE_SPEED_1 = 1 << 6, - COM_MOTOR_OVERRIDE_SPEED_2 = 1 << 7, - COM_MOTOR_OVERRIDE_SPEED_3 = 1 << 8, - COM_MOTOR_OVERRIDE_SPEED_4 = 1 << 9, - 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_SET_COMMAND_OVERRIDE = 1 << 13, - COM_SET_STATE_MASK = 1 << 14, - COM_SET_STATE_DELAY = 1 << 15, - COM_SET_SD_WRITE_DELAY = 1 << 16, - COM_SET_LED = 1 << 17, - COM_SET_SERIAL_RC = 1 << 18, - COM_SET_CARD_RECORDING = 1 << 19, - COM_SET_PARTIAL_EEPROM_DATA = 1 << 20, - COM_REINIT_PARTIAL_EEPROM_DATA = 1 << 21, - COM_REQ_PARTIAL_EEPROM_DATA = 1 << 22, - COM_REQ_CARD_RECORDING_STATE = 1 << 23, + enum Command : uint8_t { + REQ_RESPONSE, + SET_EEPROM_DATA, + REINIT_EEPROM_DATA, + REQ_EEPROM_DATA, + REQ_ENABLE_ITERATION, + MOTOR_OVERRIDE_SPEED_0, + MOTOR_OVERRIDE_SPEED_1, + MOTOR_OVERRIDE_SPEED_2, + MOTOR_OVERRIDE_SPEED_3, + MOTOR_OVERRIDE_SPEED_4, + MOTOR_OVERRIDE_SPEED_5, + MOTOR_OVERRIDE_SPEED_6, + MOTOR_OVERRIDE_SPEED_7, + SET_COMMAND_OVERRIDE, + SET_STATE_MASK, + SET_STATE_DELAY, + SET_SD_WRITE_DELAY, + SET_LED, + SET_SERIAL_RC, + SET_CARD_RECORDING, + SET_PARTIAL_EEPROM_DATA, + REINIT_PARTIAL_EEPROM_DATA, + REQ_PARTIAL_EEPROM_DATA, + REQ_CARD_RECORDING_STATE, + END_OF_COMMANDS, }; - enum StateFields : uint32_t { - STATE_ALL = 0xFFFFFFFF, - STATE_NONE = 0, - STATE_MICROS = 1 << 0, - STATE_STATUS = 1 << 1, - STATE_V0 = 1 << 2, - STATE_I0 = 1 << 3, - STATE_I1 = 1 << 4, - STATE_ACCEL = 1 << 5, - STATE_GYRO = 1 << 6, - STATE_MAG = 1 << 7, - STATE_TEMPERATURE = 1 << 8, - STATE_PRESSURE = 1 << 9, - STATE_RX_PPM = 1 << 10, - STATE_AUX_CHAN_MASK = 1 << 11, - STATE_COMMANDS = 1 << 12, - STATE_F_AND_T = 1 << 13, - STATE_PID_FZ_MASTER = 1 << 15, - STATE_PID_TX_MASTER = 1 << 16, - STATE_PID_TY_MASTER = 1 << 17, - STATE_PID_TZ_MASTER = 1 << 18, - STATE_PID_FZ_SLAVE = 1 << 19, - STATE_PID_TX_SLAVE = 1 << 20, - STATE_PID_TY_SLAVE = 1 << 21, - STATE_PID_TZ_SLAVE = 1 << 22, - STATE_MOTOR_OUT = 1 << 23, - STATE_KINE_ANGLE = 1 << 24, - STATE_KINE_RATE = 1 << 25, - STATE_KINE_ALTITUDE = 1 << 26, - STATE_LOOP_COUNT = 1 << 27, + enum States : uint8_t { + MICROS, + STATUS, + V0, + I0, + I1, + ACCEL, + GYRO, + MAG, + TEMPERATURE, + PRESSURE, + RX_PPM, + AUX_CHAN_MASK, + COMMANDS, + F_AND_T, + PID_FZ_MASTER, + PID_TX_MASTER, + PID_TY_MASTER, + PID_TZ_MASTER, + PID_FZ_SLAVE, + PID_TX_SLAVE, + PID_TY_SLAVE, + PID_TZ_SLAVE, + MOTOR_OUT, + KINE_ANGLE, + KINE_RATE, + KINE_ALTITUDE, + LOOP_COUNT, + END_OF_STATES, }; explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, Systems* systems, LED* led, PilotCommand* command); @@ -99,7 +103,7 @@ class SerialComm { void SendConfiguration() const; void SendPartialConfiguration(uint16_t submask, uint16_t led_mask) const; void SendDebugString(const String& string, MessageType type = MessageType::DebugString) const; - void SendState(uint32_t timestamp_us, uint32_t mask = 0, bool redirect_to_sd_card = false) const; + void SendState(uint32_t mask = 0, bool redirect_to_sd_card = false) const; void SendResponse(uint32_t mask, uint32_t response) const; uint16_t GetSendStateDelay() const; @@ -108,6 +112,11 @@ class SerialComm { void AddToStateMsg(uint32_t values); void RemoveFromStateMsg(uint32_t values); + template + inline bool doSubcommand(CobsReaderBuffer& input); + template + inline void readSubstate(CobsPayloadGeneric& payload) const; + private: void ProcessData(CobsReaderBuffer& data_input); @@ -124,4 +133,6 @@ class SerialComm { uint32_t state_mask{0x7fffff}; }; +#include "serial_impl.h" + #endif diff --git a/serial_impl.h b/serial_impl.h new file mode 100644 index 0000000..196f8f1 --- /dev/null +++ b/serial_impl.h @@ -0,0 +1,35 @@ +#ifndef SERIAL_IMPL_H +#define SERIAL_IMPL_H + +#include "serial.h" + +#include "cardManagement.h" +#include "config.h" +#include "eepromcursor.h" +#include "serialFork.h" + +template +inline void WriteProtocolHead(SerialComm::MessageType type, uint32_t mask, CobsPayload& payload) { + payload.Append(type); + payload.Append(mask); +} + +template +inline void WriteToOutput(CobsPayload& payload, bool use_logger = false) { + auto package = payload.Encode(); + if (use_logger) { + sdcard::write(package.data, package.length); + } else { + writeSerial(package.data, package.length); + } +} + +template +inline void WritePIDData(CobsPayload& payload, const PID& pid) { + payload.Append(pid.lastTime(), pid.input(), pid.setpoint(), pid.pTerm(), pid.iTerm(), pid.dTerm()); +} + +#include "serial_subcommands.h" +#include "serial_substates.h" + +#endif /* SERIAL_IMPL_H */ diff --git a/serial_subcommands.h b/serial_subcommands.h new file mode 100644 index 0000000..e3cfccb --- /dev/null +++ b/serial_subcommands.h @@ -0,0 +1,243 @@ +#ifndef SERIAL_SUBCOMMANDS_H +#define SERIAL_SUBCOMMANDS_H + +#include "serial_impl.h" + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return false; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + Config tmp_config; + if (!tmp_config.readFrom(input)) { + return false; + } + if (!tmp_config.verify()) { + return false; + } + tmp_config.applyTo(*systems); + tmp_config.writeTo(EEPROMCursor()); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + const Config tmp_config; + tmp_config.applyTo(*systems); + tmp_config.writeTo(EEPROMCursor()); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + SendConfiguration(); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint8_t flag; + if (!input.ParseInto(flag)) { + return false; + } + if (flag == 1) { + state->processMotorEnablingIteration(); + } else { + state->disableMotors(); + } + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[0]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[1]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[2]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[3]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[4]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[5]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[6]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + return input.ParseInto(state->MotorOut[7]); +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint8_t flag; + if (!input.ParseInto(flag)) { + return false; + } + if (flag == 1) { + state->set(STATUS_OVERRIDE); + } else { + state->clear(STATUS_OVERRIDE); + } + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint32_t new_state_mask; + if (!input.ParseInto(new_state_mask)) { + return false; + } + SetStateMsg(new_state_mask); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint16_t new_state_delay; + if (!input.ParseInto(new_state_delay)) { + return false; + } + send_state_delay = new_state_delay; + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint16_t new_state_delay; + if (!input.ParseInto(new_state_delay)) { + return false; + } + sd_card_state_delay = new_state_delay; + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint8_t mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g; + if (!input.ParseInto(mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g)) { + return false; + } + led->set(LED::Pattern(mode), r1, g1, b1, r2, g2, b2, ind_r, ind_g); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint8_t enabled; + int16_t throttle, pitch, roll, yaw; + uint8_t auxmask; + if (!input.ParseInto(enabled, throttle, pitch, roll, yaw, auxmask)) { + return false; + } + if (enabled) { + state->command_source_mask |= COMMAND_READY_BTLE; + state->command_AUX_mask = auxmask; + state->command_throttle = throttle; + state->command_pitch = pitch; + state->command_roll = roll; + state->command_yaw = yaw; + } else { + state->command_source_mask &= ~COMMAND_READY_BTLE; + } + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint8_t recording_flags; + if (!input.ParseInto(recording_flags)) { + return false; + } + bool shouldRecordToCard = recording_flags & 1; + bool shouldLock = recording_flags & 2; + sdcard::setLock(false); + if (shouldRecordToCard) { + sdcard::openFile(); + } else { + sdcard::closeFile(); + } + sdcard::setLock(shouldLock); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + Config tmp_config(*systems); + if (!tmp_config.readPartialFrom(input)) { + return false; + } + if (tmp_config.verify()) { + return false; + } + tmp_config.applyTo(*systems); + tmp_config.writeTo(EEPROMCursor()); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint16_t submask, led_mask; + if (!Config::readMasks(input, submask, led_mask)) { + return false; + } + Config tmp_config(*systems); + tmp_config.resetPartial(submask, led_mask); + if (!tmp_config.verify()) { + return false; + } + tmp_config.applyTo(*systems); + tmp_config.writeTo(EEPROMCursor()); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + uint16_t submask, led_mask; + if (!Config::readMasks(input, submask, led_mask)) { + return false; + } + SendPartialConfiguration(submask, led_mask); + return true; +} + +template <> +inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { + CobsPayload<20> payload; + WriteProtocolHead(SerialComm::MessageType::Command, FLAG(SET_SD_WRITE_DELAY) | FLAG(SET_CARD_RECORDING), payload); + payload.Append(sd_card_state_delay); + uint8_t flags = 0; + if (sdcard::isOpen()) { + flags |= 1; + } + if (sdcard::isLocked()) { + flags |= 2; + } + payload.Append(flags); + WriteToOutput(payload); + return true; +} + +#endif /* SERIAL_SUBCOMMANDS_H */ diff --git a/serial_substates.h b/serial_substates.h new file mode 100644 index 0000000..be010ec --- /dev/null +++ b/serial_substates.h @@ -0,0 +1,142 @@ +#ifndef SERIAL_SUBSTATES_H +#define SERIAL_SUBSTATES_H + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + uint32_t timestamp_us{micros()}; + payload.Append(timestamp_us); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->status); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->V0_raw); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->I0_raw); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->I1_raw); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->accel); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->gyro); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->mag); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->temperature); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->pressure); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + for (std::size_t i = 0; i < 6; ++i) { + payload.Append(ppm[i]); + } +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->command_AUX_mask); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->command_throttle, state->command_pitch, state->command_roll, state->command_yaw); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->Fz, state->Tx, state->Ty, state->Tz); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->thrust_pid.master()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->pitch_pid.master()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->roll_pid.master()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->yaw_pid.master()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->thrust_pid.slave()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->pitch_pid.slave()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->roll_pid.slave()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + WritePIDData(payload, control->yaw_pid.slave()); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->MotorOut); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->kinematicsAngle); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->kinematicsRate); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->kinematicsAltitude); +} + +template <> +inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { + payload.Append(state->loopCount); +} + +#endif /* SERIAL_SUBSTATES_H */ From 39afb9ad741e93b22f7989b64d1a29225bf9df37 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 14 Dec 2016 23:04:42 +0100 Subject: [PATCH 17/22] Make template specializations more readable --- config.cpp | 47 +++++---------- serial_subcommands.h | 79 ++++++++++-------------- serial_substates.h | 141 ++++++++++++------------------------------- 3 files changed, 86 insertions(+), 181 deletions(-) diff --git a/config.cpp b/config.cpp index 1e94e92..3b423be 100644 --- a/config.cpp +++ b/config.cpp @@ -30,6 +30,13 @@ PcbTransform pcb_transform; template inline decltype(std::get(Config::Data())) & systemMapping(Systems& sys); +// Maps a subsystem configuration to a flag, whereby we state the subsystem we're mapping to +#define MAP_SYSTEM(name, type, variable) \ + template <> \ + inline type& systemMapping(Systems & sys) { \ + return sys.variable; \ + } + template <> inline Version& systemMapping(Systems& sys) { // TODO: add this to the systems @@ -37,10 +44,7 @@ inline Version& systemMapping(Systems& sys) { return version; } -template <> -inline ConfigID& systemMapping(Systems& sys) { - return sys.id; -} +MAP_SYSTEM(ID, ConfigID, id); template <> inline PcbTransform& systemMapping(Systems& sys) { @@ -49,35 +53,14 @@ inline PcbTransform& systemMapping(Systems& sys) { return pcb_transform; } -template <> -inline Airframe::MixTable& systemMapping(Systems& sys) { - return sys.airframe.mix_table; -} - -template <> -inline AK8963::MagBias& systemMapping(Systems& sys) { - return sys.mag.mag_bias; -} - -template <> -inline R415X::ChannelProperties& systemMapping(Systems& sys) { - return sys.receiver.channel; -} - -template <> -inline Control::PIDParameters& systemMapping(Systems& sys) { - return sys.control.pid_parameters; -} +MAP_SYSTEM(MIX_TABLE, Airframe::MixTable, airframe.mix_table) +MAP_SYSTEM(MAG_BIAS, AK8963::MagBias, mag.mag_bias) +MAP_SYSTEM(CHANNEL, R415X::ChannelProperties, receiver.channel) +MAP_SYSTEM(PID_PARAMETERS, Control::PIDParameters, control.pid_parameters) +MAP_SYSTEM(STATE_PARAMETERS, State::Parameters, state.parameters) +MAP_SYSTEM(LED_STATES, LED::States, led.states) -template <> -inline State::Parameters& systemMapping(Systems& sys) { - return sys.state.parameters; -} - -template <> -inline LED::States& systemMapping(Systems& sys) { - return sys.led.states; -} +#undef MAP_SYSTEM bool isEmptyEEPROM() { return EEPROM.read(0) == 255; diff --git a/serial_subcommands.h b/serial_subcommands.h index e3cfccb..b87d02e 100644 --- a/serial_subcommands.h +++ b/serial_subcommands.h @@ -3,13 +3,16 @@ #include "serial_impl.h" -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +// Assigns a subcommand to a flag, whereby the input is named "input" +#define DO_SUBCOMMAND(name) \ + template <> \ + inline bool SerialComm::doSubcommand(CobsReaderBuffer & input) + +DO_SUBCOMMAND(REQ_RESPONSE) { return false; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_EEPROM_DATA) { Config tmp_config; if (!tmp_config.readFrom(input)) { return false; @@ -22,22 +25,19 @@ inline bool SerialComm::doSubcommand(CobsReaderBuff return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(REINIT_EEPROM_DATA) { const Config tmp_config; tmp_config.applyTo(*systems); tmp_config.writeTo(EEPROMCursor()); return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(REQ_EEPROM_DATA) { SendConfiguration(); return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(REQ_ENABLE_ITERATION) { uint8_t flag; if (!input.ParseInto(flag)) { return false; @@ -50,48 +50,39 @@ inline bool SerialComm::doSubcommand(CobsReade return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_0) { return input.ParseInto(state->MotorOut[0]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_1) { return input.ParseInto(state->MotorOut[1]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_2) { return input.ParseInto(state->MotorOut[2]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_3) { return input.ParseInto(state->MotorOut[3]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_4) { return input.ParseInto(state->MotorOut[4]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_5) { return input.ParseInto(state->MotorOut[5]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_6) { return input.ParseInto(state->MotorOut[6]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(MOTOR_OVERRIDE_SPEED_7) { return input.ParseInto(state->MotorOut[7]); } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_COMMAND_OVERRIDE) { uint8_t flag; if (!input.ParseInto(flag)) { return false; @@ -104,8 +95,7 @@ inline bool SerialComm::doSubcommand(CobsReade return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_STATE_MASK) { uint32_t new_state_mask; if (!input.ParseInto(new_state_mask)) { return false; @@ -114,8 +104,7 @@ inline bool SerialComm::doSubcommand(CobsReaderBuffe return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_STATE_DELAY) { uint16_t new_state_delay; if (!input.ParseInto(new_state_delay)) { return false; @@ -124,8 +113,7 @@ inline bool SerialComm::doSubcommand(CobsReaderBuff return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_SD_WRITE_DELAY) { uint16_t new_state_delay; if (!input.ParseInto(new_state_delay)) { return false; @@ -134,8 +122,7 @@ inline bool SerialComm::doSubcommand(CobsReaderB return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_LED) { uint8_t mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g; if (!input.ParseInto(mode, r1, g1, b1, r2, g2, b2, ind_r, ind_g)) { return false; @@ -144,8 +131,7 @@ inline bool SerialComm::doSubcommand(CobsReaderBuffer& inpu return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_SERIAL_RC) { uint8_t enabled; int16_t throttle, pitch, roll, yaw; uint8_t auxmask; @@ -165,8 +151,7 @@ inline bool SerialComm::doSubcommand(CobsReaderBuffer return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_CARD_RECORDING) { uint8_t recording_flags; if (!input.ParseInto(recording_flags)) { return false; @@ -183,8 +168,7 @@ inline bool SerialComm::doSubcommand(CobsReaderB return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(SET_PARTIAL_EEPROM_DATA) { Config tmp_config(*systems); if (!tmp_config.readPartialFrom(input)) { return false; @@ -197,8 +181,7 @@ inline bool SerialComm::doSubcommand(CobsRe return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(REINIT_PARTIAL_EEPROM_DATA) { uint16_t submask, led_mask; if (!Config::readMasks(input, submask, led_mask)) { return false; @@ -213,8 +196,7 @@ inline bool SerialComm::doSubcommand(Cob return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(REQ_PARTIAL_EEPROM_DATA) { uint16_t submask, led_mask; if (!Config::readMasks(input, submask, led_mask)) { return false; @@ -223,8 +205,7 @@ inline bool SerialComm::doSubcommand(CobsRe return true; } -template <> -inline bool SerialComm::doSubcommand(CobsReaderBuffer& input) { +DO_SUBCOMMAND(REQ_CARD_RECORDING_STATE) { CobsPayload<20> payload; WriteProtocolHead(SerialComm::MessageType::Command, FLAG(SET_SD_WRITE_DELAY) | FLAG(SET_CARD_RECORDING), payload); payload.Append(sd_card_state_delay); @@ -240,4 +221,6 @@ inline bool SerialComm::doSubcommand(CobsR return true; } +#undef DO_SUBCOMMAND + #endif /* SERIAL_SUBCOMMANDS_H */ diff --git a/serial_substates.h b/serial_substates.h index be010ec..890c894 100644 --- a/serial_substates.h +++ b/serial_substates.h @@ -1,142 +1,81 @@ #ifndef SERIAL_SUBSTATES_H #define SERIAL_SUBSTATES_H -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +// Reads part of the state into a COBS payload, named "payload" +#define READ_SUBSTATE(name) \ + template <> \ + inline void SerialComm::readSubstate(CobsPayloadGeneric & payload) const + +// A shorthand for a common case where all we do is append some data to a payload +#define READ_SUBSTATE_PAYLOAD(name, ...) \ + READ_SUBSTATE(name) { \ + payload.Append(__VA_ARGS__); \ + } + +READ_SUBSTATE(MICROS) { uint32_t timestamp_us{micros()}; payload.Append(timestamp_us); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->status); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->V0_raw); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->I0_raw); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->I1_raw); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->accel); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->gyro); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->mag); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->temperature); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->pressure); -} +READ_SUBSTATE_PAYLOAD(STATUS, state->status) +READ_SUBSTATE_PAYLOAD(V0, state->V0_raw) +READ_SUBSTATE_PAYLOAD(I0, state->I0_raw) +READ_SUBSTATE_PAYLOAD(I1, state->I1_raw) +READ_SUBSTATE_PAYLOAD(ACCEL, state->accel) +READ_SUBSTATE_PAYLOAD(GYRO, state->gyro) +READ_SUBSTATE_PAYLOAD(MAG, state->mag) +READ_SUBSTATE_PAYLOAD(TEMPERATURE, state->temperature) +READ_SUBSTATE_PAYLOAD(PRESSURE, state->pressure) -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(RX_PPM) { for (std::size_t i = 0; i < 6; ++i) { payload.Append(ppm[i]); } } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->command_AUX_mask); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->command_throttle, state->command_pitch, state->command_roll, state->command_yaw); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->Fz, state->Tx, state->Ty, state->Tz); -} +READ_SUBSTATE_PAYLOAD(AUX_CHAN_MASK, state->command_AUX_mask) +READ_SUBSTATE_PAYLOAD(COMMANDS, state->command_throttle, state->command_pitch, state->command_roll, state->command_yaw) +READ_SUBSTATE_PAYLOAD(F_AND_T, state->Fz, state->Tx, state->Ty, state->Tz) -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_FZ_MASTER) { WritePIDData(payload, control->thrust_pid.master()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_TX_MASTER) { WritePIDData(payload, control->pitch_pid.master()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_TY_MASTER) { WritePIDData(payload, control->roll_pid.master()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_TZ_MASTER) { WritePIDData(payload, control->yaw_pid.master()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_FZ_SLAVE) { WritePIDData(payload, control->thrust_pid.slave()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_TX_SLAVE) { WritePIDData(payload, control->pitch_pid.slave()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_TY_SLAVE) { WritePIDData(payload, control->roll_pid.slave()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { +READ_SUBSTATE(PID_TZ_SLAVE) { WritePIDData(payload, control->yaw_pid.slave()); } -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->MotorOut); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->kinematicsAngle); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->kinematicsRate); -} - -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->kinematicsAltitude); -} +READ_SUBSTATE_PAYLOAD(MOTOR_OUT, state->MotorOut) +READ_SUBSTATE_PAYLOAD(KINE_ANGLE, state->kinematicsAngle) +READ_SUBSTATE_PAYLOAD(KINE_RATE, state->kinematicsRate) +READ_SUBSTATE_PAYLOAD(KINE_ALTITUDE, state->kinematicsAltitude) +READ_SUBSTATE_PAYLOAD(LOOP_COUNT, state->loopCount) -template <> -inline void SerialComm::readSubstate(CobsPayloadGeneric& payload) const { - payload.Append(state->loopCount); -} +#undef READ_SUBSTATE +#undef READ_SUBSTATE_PAYLOAD #endif /* SERIAL_SUBSTATES_H */ From 60b677102dd8f71694bf1cd28b20a100c3d236e9 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 14 Dec 2016 23:38:34 +0100 Subject: [PATCH 18/22] Move enums closer to their only uses --- serial.cpp | 8 +++--- serial.h | 60 ++------------------------------------------ serial_subcommands.h | 30 +++++++++++++++++++++- serial_substates.h | 35 +++++++++++++++++++++++++- 4 files changed, 69 insertions(+), 64 deletions(-) diff --git a/serial.cpp b/serial.cpp index 4428999..0979f53 100644 --- a/serial.cpp +++ b/serial.cpp @@ -24,11 +24,11 @@ inline void doSubcommandWrap(SerialComm& serial, CobsReaderBuffer& input, uint32 } template -inline typename std::enable_if::type doSubcommands(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { +inline typename std::enable_if::type doSubcommands(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { } template - inline typename std::enable_if < I::type doSubcommands(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { + inline typename std::enable_if < I::type doSubcommands(SerialComm& serial, CobsReaderBuffer& input, uint32_t mask, uint32_t& ack) { doSubcommandWrap(serial, input, mask, ack); doSubcommands(serial, input, mask, ack); } @@ -41,11 +41,11 @@ inline void readSubstateWrap(const SerialComm& serial, CobsPayloadGeneric& paylo } template -inline typename std::enable_if::type readSubstates(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { +inline typename std::enable_if::type readSubstates(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { } template - inline typename std::enable_if < I::type readSubstates(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { + inline typename std::enable_if < I::type readSubstates(const SerialComm& serial, CobsPayloadGeneric& payload, uint32_t mask) { readSubstateWrap(serial, payload, mask); readSubstates(serial, payload, mask); } diff --git a/serial.h b/serial.h index d525614..eda8f0f 100644 --- a/serial.h +++ b/serial.h @@ -37,64 +37,8 @@ class SerialComm { HistoryData = 4, }; - enum Command : uint8_t { - REQ_RESPONSE, - SET_EEPROM_DATA, - REINIT_EEPROM_DATA, - REQ_EEPROM_DATA, - REQ_ENABLE_ITERATION, - MOTOR_OVERRIDE_SPEED_0, - MOTOR_OVERRIDE_SPEED_1, - MOTOR_OVERRIDE_SPEED_2, - MOTOR_OVERRIDE_SPEED_3, - MOTOR_OVERRIDE_SPEED_4, - MOTOR_OVERRIDE_SPEED_5, - MOTOR_OVERRIDE_SPEED_6, - MOTOR_OVERRIDE_SPEED_7, - SET_COMMAND_OVERRIDE, - SET_STATE_MASK, - SET_STATE_DELAY, - SET_SD_WRITE_DELAY, - SET_LED, - SET_SERIAL_RC, - SET_CARD_RECORDING, - SET_PARTIAL_EEPROM_DATA, - REINIT_PARTIAL_EEPROM_DATA, - REQ_PARTIAL_EEPROM_DATA, - REQ_CARD_RECORDING_STATE, - END_OF_COMMANDS, - }; - - enum States : uint8_t { - MICROS, - STATUS, - V0, - I0, - I1, - ACCEL, - GYRO, - MAG, - TEMPERATURE, - PRESSURE, - RX_PPM, - AUX_CHAN_MASK, - COMMANDS, - F_AND_T, - PID_FZ_MASTER, - PID_TX_MASTER, - PID_TY_MASTER, - PID_TZ_MASTER, - PID_FZ_SLAVE, - PID_TX_SLAVE, - PID_TY_SLAVE, - PID_TZ_SLAVE, - MOTOR_OUT, - KINE_ANGLE, - KINE_RATE, - KINE_ALTITUDE, - LOOP_COUNT, - END_OF_STATES, - }; + enum Commands : uint8_t; + enum States : uint8_t; explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, Systems* systems, LED* led, PilotCommand* command); diff --git a/serial_subcommands.h b/serial_subcommands.h index b87d02e..b173c50 100644 --- a/serial_subcommands.h +++ b/serial_subcommands.h @@ -3,10 +3,38 @@ #include "serial_impl.h" +enum SerialComm::Commands : uint8_t { + REQ_RESPONSE, + SET_EEPROM_DATA, + REINIT_EEPROM_DATA, + REQ_EEPROM_DATA, + REQ_ENABLE_ITERATION, + MOTOR_OVERRIDE_SPEED_0, + MOTOR_OVERRIDE_SPEED_1, + MOTOR_OVERRIDE_SPEED_2, + MOTOR_OVERRIDE_SPEED_3, + MOTOR_OVERRIDE_SPEED_4, + MOTOR_OVERRIDE_SPEED_5, + MOTOR_OVERRIDE_SPEED_6, + MOTOR_OVERRIDE_SPEED_7, + SET_COMMAND_OVERRIDE, + SET_STATE_MASK, + SET_STATE_DELAY, + SET_SD_WRITE_DELAY, + SET_LED, + SET_SERIAL_RC, + SET_CARD_RECORDING, + SET_PARTIAL_EEPROM_DATA, + REINIT_PARTIAL_EEPROM_DATA, + REQ_PARTIAL_EEPROM_DATA, + REQ_CARD_RECORDING_STATE, + END_OF_COMMANDS, +}; + // Assigns a subcommand to a flag, whereby the input is named "input" #define DO_SUBCOMMAND(name) \ template <> \ - inline bool SerialComm::doSubcommand(CobsReaderBuffer & input) + inline bool SerialComm::doSubcommand(CobsReaderBuffer & input) DO_SUBCOMMAND(REQ_RESPONSE) { return false; diff --git a/serial_substates.h b/serial_substates.h index 890c894..f9ea996 100644 --- a/serial_substates.h +++ b/serial_substates.h @@ -1,10 +1,43 @@ #ifndef SERIAL_SUBSTATES_H #define SERIAL_SUBSTATES_H +#include "serial_impl.h" + +enum SerialComm::States : uint8_t { + MICROS, + STATUS, + V0, + I0, + I1, + ACCEL, + GYRO, + MAG, + TEMPERATURE, + PRESSURE, + RX_PPM, + AUX_CHAN_MASK, + COMMANDS, + F_AND_T, + PID_FZ_MASTER, + PID_TX_MASTER, + PID_TY_MASTER, + PID_TZ_MASTER, + PID_FZ_SLAVE, + PID_TX_SLAVE, + PID_TY_SLAVE, + PID_TZ_SLAVE, + MOTOR_OUT, + KINE_ANGLE, + KINE_RATE, + KINE_ALTITUDE, + LOOP_COUNT, + END_OF_STATES, +}; + // Reads part of the state into a COBS payload, named "payload" #define READ_SUBSTATE(name) \ template <> \ - inline void SerialComm::readSubstate(CobsPayloadGeneric & payload) const + inline void SerialComm::readSubstate(CobsPayloadGeneric & payload) const // A shorthand for a common case where all we do is append some data to a payload #define READ_SUBSTATE_PAYLOAD(name, ...) \ From d3bf30e5eb09245d727a847da2dcd57b292ee411 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 15 Dec 2016 21:58:20 +0100 Subject: [PATCH 19/22] Add naming to config --- config.cpp | 1 + config.h | 8 +++--- devicename.cpp | 64 ++++++++++++++++++++++++++++++++++++++++++++ devicename.h | 21 +++++++++++++++ flybrix-firmware.ino | 4 +-- serialFork.cpp | 44 +++++++----------------------- serialFork.h | 4 ++- systems.h | 3 +++ 8 files changed, 109 insertions(+), 40 deletions(-) create mode 100644 devicename.cpp create mode 100644 devicename.h diff --git a/config.cpp b/config.cpp index 3b423be..b8e9994 100644 --- a/config.cpp +++ b/config.cpp @@ -59,6 +59,7 @@ MAP_SYSTEM(CHANNEL, R415X::ChannelProperties, receiver.channel) MAP_SYSTEM(PID_PARAMETERS, Control::PIDParameters, control.pid_parameters) MAP_SYSTEM(STATE_PARAMETERS, State::Parameters, state.parameters) MAP_SYSTEM(LED_STATES, LED::States, led.states) +MAP_SYSTEM(DEVICE_NAME, DeviceName, name) #undef MAP_SYSTEM diff --git a/config.h b/config.h index 1b0b14b..7c8cb65 100644 --- a/config.h +++ b/config.h @@ -26,6 +26,7 @@ #include "led.h" #include "state.h" #include "version.h" +#include "devicename.h" #include @@ -66,9 +67,10 @@ struct Config { PID_PARAMETERS, STATE_PARAMETERS, LED_STATES, + DEVICE_NAME, }; - using Data = std::tuple; + using Data = std::tuple; Config(); explicit Config(Systems& sys); @@ -97,10 +99,10 @@ struct Config { static_assert(sizeof(Config) == sizeof(Version) + sizeof(ConfigID) + sizeof(PcbTransform) + sizeof(Airframe::MixTable) + sizeof(AK8963::MagBias) + sizeof(R415X::ChannelProperties) + sizeof(State::Parameters) + - sizeof(Control::PIDParameters) + sizeof(LED::States), + sizeof(Control::PIDParameters) + sizeof(LED::States) + sizeof(DeviceName), "Data is not packed"); -static_assert(sizeof(Config) == 619, "Data does not have expected size"); +static_assert(sizeof(Config) == 628, "Data does not have expected size"); Config readEEPROM(); bool isEmptyEEPROM(); diff --git a/devicename.cpp b/devicename.cpp new file mode 100644 index 0000000..6ea810e --- /dev/null +++ b/devicename.cpp @@ -0,0 +1,64 @@ +#include "devicename.h" + +#include "Arduino.h" + +#include "debug.h" + +DeviceName::DeviceName() : DeviceName("FLYBRIX") { +} + +DeviceName::DeviceName(const String& name) { + std::size_t l{name.length()}; + if (l > 8) { + l = 8; + } + for (std::size_t i{0}; i < l; ++i) { + value[i] = name.charAt(i); + } + for (std::size_t i{l}; i < MAX_NAME_LENGTH + 1; ++i) { + value[i] = 0; + } +} + +bool badChar(char c); + +bool DeviceName::verify() const { + if (!value[0]) { + DebugPrint("Name cannot be empty"); + } + for (char c : value) { + if (badChar(c)) { + DebugPrint( + "Illegal character in name!" + " " + "Names are limited to 0-9, a-z, A-Z, '_', '-'!"); + return false; + } + if (!c) { + return true; + } + } + DebugPrint("Given device name is too long (max 8 characters)!"); + return false; +} + +bool badChar(char c) { + if (!c) { + return false; + } + if (c >= 'a' && c <= 'z') { + return false; + } + if (c >= 'A' && c <= 'Z') { + return false; + } + if (c >= '0' && c <= '9') { + return false; + } + for (char c_legal : "_-") { + if (c == c_legal) { + return false; + } + } + return true; +} diff --git a/devicename.h b/devicename.h new file mode 100644 index 0000000..ab892f5 --- /dev/null +++ b/devicename.h @@ -0,0 +1,21 @@ +#ifndef DEVICENAME_H +#define DEVICENAME_H + +#include + +class String; + +constexpr uint8_t MAX_NAME_LENGTH = 8; + +struct __attribute__((packed)) DeviceName { + DeviceName(); + DeviceName(const String& name); + + bool verify() const; + + char value[MAX_NAME_LENGTH + 1]; +}; + +static_assert(sizeof(DeviceName) == sizeof(char) * 9, "Data is not packed"); + +#endif /* DEVICENAME_H */ diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index a4e65d2..a185fb0 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -57,12 +57,12 @@ void setup() { bool go_to_test_mode{isEmptyEEPROM()}; - setBluetoothUart(); - // load stored settings (this will reinitialize if there is no data in the EEPROM! readEEPROM().applyTo(sys); sys.state.resetState(); + setBluetoothUart(sys.name); + sys.state.set(STATUS_BMP_FAIL); sys.led.update(); sys.bmp.restart(); diff --git a/serialFork.cpp b/serialFork.cpp index e6ce77f..a1268e3 100644 --- a/serialFork.cpp +++ b/serialFork.cpp @@ -11,6 +11,7 @@ #include "serialFork.h" #include #include "board.h" +#include "devicename.h" namespace { struct USBComm { @@ -49,7 +50,7 @@ struct Bluetooth { Serial1.begin(57600); } - void setBluetoothUart(); + void setBluetoothUart(const DeviceName& name); bool read() { while (Serial1.available()) { @@ -139,7 +140,7 @@ void flushATmodeResponse() { } } -void Bluetooth::setBluetoothUart() { +void Bluetooth::setBluetoothUart(const DeviceName& name) { // PIN 12 of teensy is BMD (P0.13) // PIN 30 of teensy is BMD (PO.14) AT Mode // PIN 28 of teensy is BMD RST @@ -153,39 +154,14 @@ void Bluetooth::setBluetoothUart() { digitalWriteFast(board::bluetooth::RESET, LOW); // reset BMD delay(100); digitalWriteFast(board::bluetooth::RESET, HIGH); // reset BMD complete, now in AT mode - delay(2500); // time needed initialization of AT mode - uint8_t data[18]; - - data[0] = 'a'; - data[1] = 't'; - data[2] = '$'; - - data[3] = 'u'; - data[4] = 'e'; - data[5] = 'n'; - data[6] = ' '; - data[7] = '0'; - data[8] = '1'; - data[9] = '\n'; - Serial1.write(data, 10); + delay(2500); // time needed initialization of AT mode + Serial1.print("at$uen 01\n"); flushATmodeResponse(); - data[3] = 'n'; - data[4] = 'a'; - data[5] = 'm'; - data[6] = 'e'; - data[7] = ' '; - data[8] = 'F'; - data[9] = 'L'; - data[10] = 'Y'; - data[11] = 'B'; - data[12] = 'R'; - data[13] = 'I'; - data[14] = 'X'; - data[15] = '\n'; - Serial1.write(data, 16); - + Serial1.print("at$name "); + Serial1.print(name.value); + Serial1.print("\n"); flushATmodeResponse(); digitalWriteFast(board::bluetooth::MODE, HIGH); @@ -197,9 +173,9 @@ void Bluetooth::setBluetoothUart() { #endif } -void setBluetoothUart() { +void setBluetoothUart(const DeviceName& name) { #ifndef ALPHA - bluetooth.setBluetoothUart(); + bluetooth.setBluetoothUart(name); #endif } diff --git a/serialFork.h b/serialFork.h index 80f05f4..505a3b4 100644 --- a/serialFork.h +++ b/serialFork.h @@ -14,8 +14,10 @@ #include #include "cobs.h" +class DeviceName; + CobsReaderBuffer* readSerial(); void writeSerial(uint8_t* data, size_t length); void flushSerial(); -void setBluetoothUart(); +void setBluetoothUart(const DeviceName& name); #endif diff --git a/systems.h b/systems.h index 62f69e8..e8e03b9 100644 --- a/systems.h +++ b/systems.h @@ -25,6 +25,7 @@ #include "serial.h" #include "state.h" #include "version.h" +#include "devicename.h" struct Systems { Systems(); @@ -48,6 +49,8 @@ struct Systems { ConfigID id; + DeviceName name; + void parseConfig(); }; From bda0d59a061249e0fcd7fac3c6e48f86bacdddda Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 15 Dec 2016 22:06:45 +0100 Subject: [PATCH 20/22] Add everything configured to systems --- config.cpp | 20 ++------------------ systems.h | 4 ++++ 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/config.cpp b/config.cpp index b8e9994..72f3a7a 100644 --- a/config.cpp +++ b/config.cpp @@ -24,9 +24,6 @@ Config::Config() { // Default Settings // data needs to be written manually to the EEPROM } -Version version; -PcbTransform pcb_transform; - template inline decltype(std::get(Config::Data())) & systemMapping(Systems& sys); @@ -37,22 +34,9 @@ inline decltype(std::get(Config::Data())) & systemMapping(Systems& sys); return sys.variable; \ } -template <> -inline Version& systemMapping(Systems& sys) { - // TODO: add this to the systems - version = Version(); - return version; -} - +MAP_SYSTEM(VERSION, Version, version); MAP_SYSTEM(ID, ConfigID, id); - -template <> -inline PcbTransform& systemMapping(Systems& sys) { - // TODO: add this to the systems - pcb_transform = PcbTransform(); - return pcb_transform; -} - +MAP_SYSTEM(PCB, PcbTransform, pcb_transform); MAP_SYSTEM(MIX_TABLE, Airframe::MixTable, airframe.mix_table) MAP_SYSTEM(MAG_BIAS, AK8963::MagBias, mag.mag_bias) MAP_SYSTEM(CHANNEL, R415X::ChannelProperties, receiver.channel) diff --git a/systems.h b/systems.h index e8e03b9..d94f288 100644 --- a/systems.h +++ b/systems.h @@ -26,6 +26,7 @@ #include "state.h" #include "version.h" #include "devicename.h" +#include "config.h" struct Systems { Systems(); @@ -51,6 +52,9 @@ struct Systems { DeviceName name; + Version version; + PcbTransform pcb_transform; + void parseConfig(); }; From d4de8c8ffb0833eb7c821b8094be57531cf71efb Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Mon, 19 Dec 2016 22:20:59 +0100 Subject: [PATCH 21/22] Fix configuration verification typos --- config_impl.h | 2 +- serial_subcommands.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/config_impl.h b/config_impl.h index e42f43b..231e261 100644 --- a/config_impl.h +++ b/config_impl.h @@ -63,7 +63,7 @@ struct FieldFunctor { template static bool Read(T& data, Cursor&& cursor, uint16_t submask) { if (!(submask & fieldToMask(FIELD))) { - return false; + return true; } // split up LED states further, since the variable is giant uint16_t led_mask; diff --git a/serial_subcommands.h b/serial_subcommands.h index b173c50..222a1a9 100644 --- a/serial_subcommands.h +++ b/serial_subcommands.h @@ -201,7 +201,7 @@ DO_SUBCOMMAND(SET_PARTIAL_EEPROM_DATA) { if (!tmp_config.readPartialFrom(input)) { return false; } - if (tmp_config.verify()) { + if (!tmp_config.verify()) { return false; } tmp_config.applyTo(*systems); From 447194cdef829dfa12ee16d140b293d109f7598a Mon Sep 17 00:00:00 2001 From: Amir Hirsch Date: Thu, 22 Dec 2016 13:33:11 -0800 Subject: [PATCH 22/22] updated default PID, update 1.4.0 --- control.cpp | 8 ++++---- version.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/control.cpp b/control.cpp index 6c67795..00a4a67 100644 --- a/control.cpp +++ b/control.cpp @@ -39,12 +39,12 @@ Control::PIDParameters::PIDParameters() : thrust_master{1.0, 0.0, 0.0, 0.0, 0.005, 0.005, 1.0}, - pitch_master{5.0, 1.0, 0.0, 10.0, 0.005, 0.005, 10.0}, - roll_master{5.0, 1.0, 0.0, 10.0, 0.005, 0.005, 10.0}, + pitch_master{10.0, 1.0, 0.0, 10.0, 0.005, 0.005, 10.0}, + roll_master{10.0, 1.0, 0.0, 10.0, 0.005, 0.005, 10.0}, yaw_master{5.0, 1.0, 0.0, 10.0, 0.005, 0.005, 180.0}, thrust_slave{1.0, 0.0, 0.0, 10.0, 0.001, 0.001, 0.3}, - pitch_slave{20.0, 8.0, 0.0, 30.0, 0.001, 0.001, 30.0}, - roll_slave{20.0, 8.0, 0.0, 30.0, 0.001, 0.001, 30.0}, + pitch_slave{10.0, 4.0, 0.0, 30.0, 0.001, 0.001, 30.0}, + roll_slave{10.0, 4.0, 0.0, 30.0, 0.001, 0.001, 30.0}, yaw_slave{30.0, 5.0, 0.0, 20.0, 0.001, 0.001, 240.0}, pid_bypass{BYPASS_THRUST_MASTER | BYPASS_THRUST_SLAVE | BYPASS_YAW_MASTER} // AHRS/Horizon mode { diff --git a/version.h b/version.h index b8ff404..556cd0c 100644 --- a/version.h +++ b/version.h @@ -15,7 +15,7 @@ #include "debug.h" #define FIRMWARE_VERSION_A 1 -#define FIRMWARE_VERSION_B 3 +#define FIRMWARE_VERSION_B 4 #define FIRMWARE_VERSION_C 0 struct __attribute__((packed)) Version {