From b3d34c03a918c66141208b80472bdf010dbc41b1 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 3 Mar 2016 22:12:37 +0100 Subject: [PATCH 01/11] Provide interface for overriding LED lights The interface operates by giving a 7 byte message for the command COM_SET_LED, giving the mode and red/green/blue signal lights for each LED. --- flybrix-firmware.ino | 2 +- led.cpp | 136 ++++++++++++++++++++++++++----------------- led.h | 33 ++++++++--- serial.cpp | 10 +++- serial.h | 5 +- 5 files changed, 123 insertions(+), 63 deletions(-) diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index 66f6665..f9f8cf9 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -70,7 +70,7 @@ Systems::Systems() airframe{&state}, pilot{&state}, control{&state, CONFIG.data}, - conf{&state, RX, &control, &CONFIG} // listen for configuration inputs + conf{&state, RX, &control, &CONFIG, &led} // listen for configuration inputs { } diff --git a/led.cpp b/led.cpp index ced9ce5..413209f 100644 --- a/led.cpp +++ b/led.cpp @@ -23,34 +23,55 @@ LED::LED(State* __state) : state(__state) { pinMode(RED_LED, OUTPUT); } +void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) { + override = pattern != LED::NO_OVERRIDE; + if (!override) + return; + indicatorRedOff(); + indicatorGreenOff(); + use(pattern, red_a, green_a, blue_a, red_b, green_b, blue_b); +} + void LED::update() { - cycleIndex++; // 500Hz update + cycleIndex++; // 500Hz update cycleIndex &= 4095; // ~8 second period - if (oldStatus != state->status) { + if (!override && oldStatus != state->status) { oldStatus = state->status; changeLights(); } - if (lightType == 1) { - updateFlash(); - } else if (lightType == 2) { - updateBeacon(); + switch (lightType) { + case LED::FLASH: + updateFlash(); + break; + case LED::BEACON: + updateBeacon(); + break; + case LED::BREATHE: + updateBreathe(); + break; + case LED::ALTERNATE: + updateAlternate(); + break; + case LED::SOLID: + updateSolid(); + break; } } -void LED::beacon(uint8_t red, uint8_t green, uint8_t blue) { - lightType = 2; - this->red = red; - this->green = green; - this->blue = blue; +void LED::use(Pattern pattern, uint8_t red, uint8_t green, uint8_t blue) { + use(pattern, red, green, blue, red, green, blue); } -void LED::flash(uint8_t red, uint8_t green, uint8_t blue) { - lightType = 1; - this->red = red; - this->green = green; - this->blue = blue; +void LED::use(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) { + lightType = pattern; + this->red_a_ = red_a; + this->green_a_ = green_a; + this->blue_a_ = blue_a; + this->red_b_ = red_b; + this->green_b_ = green_b; + this->blue_b_ = blue_b; } void LED::changeLights() { @@ -69,30 +90,30 @@ void LED::changeLights() { digitalWriteFast(LED_A_GRN, LOW); digitalWriteFast(LED_B_GRN, LOW); } else if (state->is(STATUS_UNPAIRED)) { - // flash(255,180,20); //orange - flash(255, 255, 255); // for pcba testing purposes -- a "good" board will end up in this state - indicatorGreenOn(); // for pcba testing purposes -- a "good" board will end up in this state + // use(LED::FLASH, 255,180,20); //orange + use(LED::FLASH, 255, 255, 255); // for pcba testing purposes -- a "good" board will end up in this state + indicatorGreenOn(); // for pcba testing purposes -- a "good" board will end up in this state } else if (state->is(STATUS_RX_FAIL)) { - flash(255, 0, 0); // red + use(LED::FLASH, 255, 0, 0); // red } else if (state->is(STATUS_FAIL_STABILITY) || state->is(STATUS_FAIL_ANGLE)) { - flash(100, 110, 250); // yellow + use(LED::FLASH, 100, 110, 250); // yellow } else if (state->is(STATUS_OVERRIDE)) { - beacon(255, 0, 0); // red + use(LED::BEACON, 255, 0, 0); // red } else if (state->is(STATUS_ENABLING)) { - flash(0, 0, 255); // blue + use(LED::FLASH, 0, 0, 255); // blue } else if (state->is(STATUS_ENABLED)) { - beacon(0, 0, 255); // blue for enable + use(LED::BEACON, 0, 0, 255); // blue for enable } else if (state->is(STATUS_TEMP_WARNING)) { - flash(100, 150, 250); // yellow + use(LED::FLASH, 100, 150, 250); // yellow } else if (state->is(STATUS_LOG_FULL)) { - flash(0, 0, 250); + use(LED::FLASH, 0, 0, 250); } else if (state->is(STATUS_BATTERY_LOW)) { - beacon(255, 180, 20); + use(LED::BEACON, 255, 180, 20); } else if (state->is(STATUS_IDLE)) { - indicatorRedOff(); // clear boot test - beacon(0, 255, 0); // breathe instead? + indicatorRedOff(); // clear boot test + use(LED::BEACON, 0, 255, 0); // breathe instead? } else { // ERROR: ("NO STATUS BITS SET???"); } @@ -106,31 +127,17 @@ uint8_t LED::getLightThreshold() { } void LED::rgb() { - uint8_t threshold = getLightThreshold(); - - if (red > threshold) { - digitalWriteFast(LED_A_RED, LOW); - digitalWriteFast(LED_B_RED, LOW); - } else { - digitalWriteFast(LED_A_RED, HIGH); - digitalWriteFast(LED_B_RED, HIGH); - } - - if (green > threshold) { - digitalWriteFast(LED_A_GRN, LOW); - digitalWriteFast(LED_B_GRN, LOW); - } else { - digitalWriteFast(LED_A_GRN, HIGH); - digitalWriteFast(LED_B_GRN, HIGH); - } + rgb(red_a_, green_a_, blue_a_, red_b_, green_b_, blue_b_); +} - if (blue > threshold) { - digitalWriteFast(LED_A_BLU, LOW); - digitalWriteFast(LED_B_BLU, LOW); - } else { - digitalWriteFast(LED_A_BLU, HIGH); - digitalWriteFast(LED_B_BLU, HIGH); - } +void LED::rgb(uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) { + uint8_t threshold = getLightThreshold(); + digitalWriteFast(LED_A_RED, (red_a > threshold) ? LOW : HIGH); + digitalWriteFast(LED_B_RED, (red_b > threshold) ? LOW : HIGH); + digitalWriteFast(LED_A_GRN, (green_a > threshold) ? LOW : HIGH); + digitalWriteFast(LED_B_GRN, (green_b > threshold) ? LOW : HIGH); + digitalWriteFast(LED_A_BLU, (blue_a > threshold) ? LOW : HIGH); + digitalWriteFast(LED_B_BLU, (blue_b > threshold) ? LOW : HIGH); } void LED::updateFlash() { @@ -159,6 +166,29 @@ void LED::updateBeacon() { } } +void LED::updateBreathe() { + uint16_t multiplier = cycleIndex & 2047; + if (cycleIndex > 511) { + allOff(); + return; + } + if (multiplier > 255) + multiplier = 512 - multiplier; + + rgb((multiplier * red_a_) >> 8, (multiplier * green_a_) >> 8, (multiplier * blue_a_) >> 8, (multiplier * red_b_) >> 8, (multiplier * green_b_) >> 8, (multiplier * blue_b_) >> 8); +} + +void LED::updateAlternate() { + if (cycleIndex & 128) + rgb(red_a_, green_a_, blue_a_, 0, 0, 0); + else + rgb(0, 0, 0, red_b_, green_b_, blue_b_); +} + +void LED::updateSolid() { + rgb(); +} + void LED::allOff() { digitalWriteFast(LED_A_RED, HIGH); digitalWriteFast(LED_A_GRN, HIGH); diff --git a/led.h b/led.h index 71f8b7c..59c1814 100644 --- a/led.h +++ b/led.h @@ -20,24 +20,43 @@ class LED { public: LED(State *state); + enum Pattern : uint8_t { + NO_OVERRIDE = 0, + FLASH = 1, + BEACON = 2, + BREATHE = 3, + ALTERNATE = 4, + SOLID = 5, + }; + + void set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b); + void update(); // update using the state STATUS bitfield private: State *state; uint16_t oldStatus{0}; uint8_t lightType{0}; - uint8_t red{0}, green{0}, blue{0}; + uint8_t red_a_{0}, green_a_{0}, blue_a_{0}; + uint8_t red_b_{0}, green_b_{0}, blue_b_{0}; uint16_t cycleIndex{0}; // incremented at 500Hz to support dithering, resets every ~8 seconds + + bool override{false}; + uint8_t getLightThreshold(); void changeLights(); - void updateBeacon(); - void updateFlash(); - void rgb(); // dithered color - void beacon(uint8_t red, uint8_t green, uint8_t blue); // 2sec periodic double pulse - void flash(uint8_t red, uint8_t green, uint8_t blue); //~3Hz flasher + void updateBeacon(); // 2sec periodic double pulse + void updateFlash(); //~3Hz flasher + void updateBreathe(); // 2sec periodic breathe + void updateAlternate(); //~4Hz left/right alternating + void updateSolid(); // maintain constant light level + void rgb(); // dithered color + void rgb(uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b); + void use(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b); + void use(Pattern pattern, uint8_t red, uint8_t green, uint8_t blue); void allOff(); @@ -59,6 +78,6 @@ class LED { #define LED_A_RED 28 // 53 #define GREEN_LED 13 // 50 -#define RED_LED 27 // 54 +#define RED_LED 27 // 54 #endif diff --git a/serial.cpp b/serial.cpp index 5abd3a6..9583ce7 100644 --- a/serial.cpp +++ b/serial.cpp @@ -9,6 +9,7 @@ #include "config.h" //CONFIG variable #include "control.h" +#include "led.h" namespace { using CobsPayloadGeneric = CobsPayload<500>; // impacts memory use only; packet size should be <= client packet size @@ -33,7 +34,7 @@ inline void WritePIDData(CobsPayload& payload, const PID& pid) { } } -SerialComm::SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, const CONFIG_union* config) : state{state}, ppm{ppm}, control{control}, config{config} { +SerialComm::SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, const CONFIG_union* config, LED* led) : state{state}, ppm{ppm}, control{control}, config{config}, led{led} { } void SerialComm::ReadData() { @@ -130,6 +131,13 @@ void SerialComm::ProcessData() { SendDebugString(eeprom_data, MessageType::HistoryData); ack_data |= COM_REQ_HISTORY; } + if (mask & COM_SET_LED) { + uint8_t mode, r1, g1, b1, r2, g2, b2; + if (data_input.ParseInto(mode, r1, g1, b1, r2, g2, b2)) { + led->set(LED::Pattern(mode), r1, g1, b1, r2, g2, b2); + ack_data |= COM_SET_LED; + } + } if (mask & COM_REQ_RESPONSE) { SendResponse(mask, ack_data); diff --git a/serial.h b/serial.h index 9fc1066..03aaa76 100644 --- a/serial.h +++ b/serial.h @@ -16,6 +16,7 @@ union CONFIG_union; class Control; +class LED; class State; class SerialComm { @@ -50,6 +51,7 @@ class SerialComm { COM_SET_STATE_MASK = 1 << 14, COM_SET_STATE_DELAY = 1 << 15, COM_REQ_HISTORY = 1 << 16, + COM_SET_LED = 1 << 17, }; enum StateFields : uint32_t { @@ -85,7 +87,7 @@ class SerialComm { STATE_LOOP_COUNT = 1 << 27, }; - explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, const CONFIG_union* config); + explicit SerialComm(State* state, const volatile uint16_t* ppm, const Control* control, const CONFIG_union* config, LED* led); void ReadData(); @@ -108,6 +110,7 @@ class SerialComm { const volatile uint16_t* ppm; const Control* control; const CONFIG_union* config; + LED* led; uint16_t send_state_delay{1001}; //anything over 1000 turns off state messages uint32_t state_mask{0x7fffff}; CobsReader<500> data_input; From 02f0c6c47f8d26cc5db2ae08b2b33a4db072864f Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Thu, 3 Mar 2016 22:18:13 +0100 Subject: [PATCH 02/11] Add indicators to the LED command interface Now the interface sends 9 bytes, with the last 2 being bools for the red and green indicator lights, respectively. --- led.cpp | 6 +++--- led.h | 2 +- serial.cpp | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/led.cpp b/led.cpp index 413209f..f1b0797 100644 --- a/led.cpp +++ b/led.cpp @@ -23,12 +23,12 @@ LED::LED(State* __state) : state(__state) { pinMode(RED_LED, OUTPUT); } -void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) { +void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator) { override = pattern != LED::NO_OVERRIDE; if (!override) return; - indicatorRedOff(); - indicatorGreenOff(); + red_indicator ? indicatorRedOn() : indicatorRedOff(); + green_indicator ? indicatorGreenOn() : indicatorGreenOff(); use(pattern, red_a, green_a, blue_a, red_b, green_b, blue_b); } diff --git a/led.h b/led.h index 59c1814..8cb9cfb 100644 --- a/led.h +++ b/led.h @@ -29,7 +29,7 @@ class LED { SOLID = 5, }; - void set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b); + void set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator); void update(); // update using the state STATUS bitfield diff --git a/serial.cpp b/serial.cpp index 9583ce7..1093d26 100644 --- a/serial.cpp +++ b/serial.cpp @@ -132,9 +132,9 @@ void SerialComm::ProcessData() { ack_data |= COM_REQ_HISTORY; } if (mask & COM_SET_LED) { - uint8_t mode, r1, g1, b1, r2, g2, b2; - if (data_input.ParseInto(mode, r1, g1, b1, r2, g2, b2)) { - led->set(LED::Pattern(mode), r1, g1, b1, r2, g2, b2); + 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; } } From ff362ebc516e4791b8a36f202eadfabb42901d12 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Fri, 4 Mar 2016 15:16:25 -0800 Subject: [PATCH 03/11] update README with ADC and i2c_t3 libraries --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 5d9e4c5..8c84b38 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,9 @@ Dependencies: * arduino 1.6.7 from https://www.arduino.cc/en/Main/Software * teensyduino libraries from https://www.pjrc.com/teensy/teensyduino.html +* (arduino library) ADC from https://github.com/pedvide/ADC +* (arduino library) i2c_t3 from https://github.com/nox771/i2c_t3 * teensy loader from https://www.pjrc.com/teensy/loader.html + If you run into problems, send us a note! From 1561008f3eb7c04f6240906518887b6e72082c20 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Fri, 4 Mar 2016 15:30:51 -0800 Subject: [PATCH 04/11] ADC and i2c_t3 are in teensyduino collection --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 8c84b38..a1c03a6 100644 --- a/README.md +++ b/README.md @@ -6,9 +6,9 @@ Dependencies: * arduino 1.6.7 from https://www.arduino.cc/en/Main/Software * teensyduino libraries from https://www.pjrc.com/teensy/teensyduino.html -* (arduino library) ADC from https://github.com/pedvide/ADC -* (arduino library) i2c_t3 from https://github.com/nox771/i2c_t3 * teensy loader from https://www.pjrc.com/teensy/loader.html +If you can't compile because it appears that you're missing the ADC or i2c_t3 libraries, +be sure that you have set up the Arduino IDE to target the correct board ("Teensy 3.2 / 3.1")! If you run into problems, send us a note! From 7c99016fc13a442066309c6a896a219cde12ce50 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Fri, 4 Mar 2016 17:47:34 -0800 Subject: [PATCH 05/11] add comment about tab size in arduino ide --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index a1c03a6..4003d3f 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,13 @@ Dependencies: * teensyduino libraries from https://www.pjrc.com/teensy/teensyduino.html * teensy loader from https://www.pjrc.com/teensy/loader.html + +Tips: + If you can't compile because it appears that you're missing the ADC or i2c_t3 libraries, be sure that you have set up the Arduino IDE to target the correct board ("Teensy 3.2 / 3.1")! +The Arduino IDE defaults to expand tabs with 2 spaces. To change that edit your preferences file. +https://www.arduino.cc/en/Hacking/Preferences -- Change “editor.tabs.size=” to 4 and restart arduino + If you run into problems, send us a note! From c23ae46571f416669aa64b4c352db3b13d2f1249 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Fri, 4 Mar 2016 17:47:48 -0800 Subject: [PATCH 06/11] remove "feed forward" trim stuff --- command.cpp | 15 --------------- command.h | 3 +-- control.cpp | 19 ++++++++----------- serial.cpp | 25 +++++++++---------------- serial.h | 1 - state.h | 2 -- 6 files changed, 18 insertions(+), 47 deletions(-) diff --git a/command.cpp b/command.cpp index 7b18c85..5b17d98 100644 --- a/command.cpp +++ b/command.cpp @@ -93,21 +93,6 @@ void PilotCommand::processCommands(void) { } } - // look for trim adjustment - if (throttle.isExtraLow()) { - static const int16_t trimGain = 3; //consider exposing in CONFIG - state->Tx_trim = trimGain * (1-2*((CONFIG.data.commandInversion >> 0) & 1)) * ((int16_t)pitch.untrimmed_mid - (int16_t)pitch.val); - state->Ty_trim = trimGain * (1-2*((CONFIG.data.commandInversion >> 1) & 1)) * ((int16_t)roll.untrimmed_mid - (int16_t)roll.val); - state->Tz_trim = trimGain * (1-2*((CONFIG.data.commandInversion >> 2) & 1)) * ((int16_t)yaw.untrimmed_mid - (int16_t)yaw.val); - pitch.mid = pitch.val; - roll.mid = roll.val; - yaw.mid = yaw.val; - //copy the midpoint values to state for logging purposes - state->PPMchannel_pitch_mid = pitch.mid; - state->PPMchannel_roll_mid = roll.mid; - state->PPMchannel_yaw_mid = yaw.mid; - } - if (recentlyEnabled || throttle.isLow()) { *throttle_command = 0; *pitch_command = 0; diff --git a/command.h b/command.h index 4a79beb..70278b6 100644 --- a/command.h +++ b/command.h @@ -23,8 +23,7 @@ class PPMchannel { PPMchannel(){}; uint16_t val = 1500; - uint16_t mid = 1500; // we allow the midpoint to be overwritten using trim - static const uint16_t untrimmed_mid = 1500; + static const uint16_t mid = 1500; static const uint16_t min = 1100; static const uint16_t max = 1900; diff --git a/control.cpp b/control.cpp index 114d536..ced30a6 100644 --- a/control.cpp +++ b/control.cpp @@ -53,7 +53,7 @@ void Control::parseConfig(CONFIG_struct& config) { void Control::calculateControlVectors() { thrust_pid.setMasterInput(state->kinematicsAltitude); - thrust_pid.setSlaveInput(0.0f); // TODO: where would be get this data from? + thrust_pid.setSlaveInput(0.0f); //state->kinematicsClimbRate pitch_pid.setMasterInput(state->kinematicsAngle[0] * 57.2957795); pitch_pid.setSlaveInput(state->kinematicsRate[0] * 57.2957795); roll_pid.setMasterInput(state->kinematicsAngle[1] * 57.2957795); @@ -66,24 +66,21 @@ void Control::calculateControlVectors() { roll_pid.setSetpoint(state->command_roll * roll_pid.getScalingFactor(nonzero_input_[ROLL_MASTER], nonzero_input_[ROLL_SLAVE])); yaw_pid.setSetpoint(state->command_yaw * yaw_pid.getScalingFactor(nonzero_input_[YAW_MASTER], nonzero_input_[YAW_SLAVE])); + // compute new output levels for state uint32_t now = micros(); - - // compute new slave setpoints in the master PIDs - // and compute state control torques state->Fz = thrust_pid.Compute(now, nonzero_input_[THRUST_MASTER], nonzero_input_[THRUST_SLAVE]); state->Tx = pitch_pid.Compute(now, nonzero_input_[PITCH_MASTER], nonzero_input_[PITCH_SLAVE]); state->Ty = roll_pid.Compute(now, nonzero_input_[ROLL_MASTER], nonzero_input_[ROLL_SLAVE]); state->Tz = yaw_pid.Compute(now, nonzero_input_[YAW_MASTER], nonzero_input_[YAW_SLAVE]); - - // add in the feedforward torques - state->Tx += state->Tx_trim; - state->Ty += state->Ty_trim; - state->Tz += state->Tz_trim; - - // use the updated pid outputs to calculate the motor drive terms + if (state->Fz == 0) { // throttle is in low condition state->Tx = 0; state->Ty = 0; state->Tz = 0; + + thrust_pid.IntegralReset(); + pitch_pid.IntegralReset(); + roll_pid.IntegralReset(); + yaw_pid.IntegralReset(); } } diff --git a/serial.cpp b/serial.cpp index 1093d26..21c41d2 100644 --- a/serial.cpp +++ b/serial.cpp @@ -183,31 +183,29 @@ uint16_t SerialComm::PacketSize(uint32_t mask) const { if (mask & SerialComm::STATE_PRESSURE) sum += 4; if (mask & SerialComm::STATE_RX_PPM) - sum += 9 * 2; // six channels and three midpoints + 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_T_TRIM) - sum += 3 * 4; if (mask & SerialComm::STATE_PID_FZ_MASTER) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_TX_MASTER) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_TY_MASTER) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_TZ_MASTER) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_FZ_SLAVE) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_TX_SLAVE) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_TY_SLAVE) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_PID_TZ_SLAVE) - sum += 6 * 4; + sum += 7 * 4; if (mask & SerialComm::STATE_MOTOR_OUT) sum += 8 * 2; if (mask & SerialComm::STATE_KINE_ANGLE) @@ -255,9 +253,6 @@ void SerialComm::SendState(uint32_t timestamp_us, void (*extra_handler)(uint8_t* if (mask & SerialComm::STATE_RX_PPM) { for (int i = 0; i < 6; ++i) payload.Append(ppm[i]); - payload.Append(state->PPMchannel_pitch_mid); - payload.Append(state->PPMchannel_roll_mid); - payload.Append(state->PPMchannel_yaw_mid); } if (mask & SerialComm::STATE_AUX_CHAN_MASK) payload.Append(state->AUX_chan_mask); @@ -265,8 +260,6 @@ void SerialComm::SendState(uint32_t timestamp_us, void (*extra_handler)(uint8_t* 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_T_TRIM) - payload.Append(state->Tx_trim, state->Ty_trim, state->Tz_trim); if (mask & SerialComm::STATE_PID_FZ_MASTER) WritePIDData(payload, control->thrust_pid.master()); if (mask & SerialComm::STATE_PID_TX_MASTER) diff --git a/serial.h b/serial.h index 03aaa76..a959240 100644 --- a/serial.h +++ b/serial.h @@ -71,7 +71,6 @@ class SerialComm { STATE_AUX_CHAN_MASK = 1 << 11, STATE_COMMANDS = 1 << 12, STATE_F_AND_T = 1 << 13, - STATE_T_TRIM = 1 << 14, STATE_PID_FZ_MASTER = 1 << 15, STATE_PID_TX_MASTER = 1 << 16, STATE_PID_TY_MASTER = 1 << 17, diff --git a/state.h b/state.h index 57c6907..a16fc6a 100644 --- a/state.h +++ b/state.h @@ -47,12 +47,10 @@ class State { // Command uint8_t AUX_chan_mask = 0; // bitfield order is {AUX1_low, AUX1_mid, AUX1_high, AUX2_low, AUX2_mid, AUX2_high, x, x} (LSB-->MSB) int16_t invalidRXcount = 0; - int16_t PPMchannel_pitch_mid, PPMchannel_roll_mid, PPMchannel_yaw_mid; //copies of PPMchannel variables we would like to expose for logging int16_t command_throttle = 0, command_pitch = 0, command_roll = 0, command_yaw = 0; // Control float Fz = 0, Tx = 0, Ty = 0, Tz = 0; // control vectors (forces/torques) - float Tx_trim = 0, Ty_trim = 0, Tz_trim = 0; // feed-forward control vectors (offset torques) // Airframe uint16_t MotorOut[8] = {0, 0, 0, 0, 0, 0, 0, 0}; From cad0734838822e992e9448c5d21a07e36cd015bd Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Fri, 4 Mar 2016 17:47:57 -0800 Subject: [PATCH 07/11] roll version to 1.1.0 --- version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/version.h b/version.h index 1e70bea..e01e2ca 100644 --- a/version.h +++ b/version.h @@ -17,7 +17,7 @@ #define version_h #define FIRMWARE_VERSION_A 1 -#define FIRMWARE_VERSION_B 0 +#define FIRMWARE_VERSION_B 1 #define FIRMWARE_VERSION_C 0 #endif From e3553ff67d21ad0d5e712fe2ab64505c524ea320 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Fri, 4 Mar 2016 17:52:00 -0800 Subject: [PATCH 08/11] drop unused "extraLow" test --- command.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/command.h b/command.h index 70278b6..32bcf6e 100644 --- a/command.h +++ b/command.h @@ -31,9 +31,6 @@ class PPMchannel { val = newVal; }; - boolean isExtraLow() { - return ((val - min) < (max - min) / 5); - }; boolean isLow() { return ((val - min) < (max - min) / 10); }; From c626804559d91f845d3de80bc68553f2d9499f56 Mon Sep 17 00:00:00 2001 From: Robb Walters Date: Mon, 7 Mar 2016 23:41:32 -0800 Subject: [PATCH 09/11] create deadzone on pitch, roll, and yaw --- command.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/command.cpp b/command.cpp index 5b17d98..f4448b4 100644 --- a/command.cpp +++ b/command.cpp @@ -115,6 +115,16 @@ void PilotCommand::processCommands(void) { *pitch_command = constrain((1-2*((CONFIG.data.commandInversion >> 0) & 1))*(pitch.val - pitch.mid) * 4095 / (pitch.max - pitch.min), -2047, 2047); *roll_command = constrain((1-2*((CONFIG.data.commandInversion >> 1) & 1))*( roll.val - roll.mid ) * 4095 / (roll.max - roll.min), -2047, 2047); *yaw_command = constrain((1-2*((CONFIG.data.commandInversion >> 2) & 1))*( yaw.val - yaw.mid ) * 4095 / (yaw.max - yaw.min), -2047, 2047); + + // + // in some cases it is impossible to get a ppm channel to be exactly 1500 usec because the controller trim is too coarse to correct a small error + // we can get around by creating a small dead zone on the commands that are potentially effected + + int16_t dead_zone_half_width = 30; + *pitch_command = *pitch_command > 0 ? max(0, *pitch_command - dead_zone_half_width) : min(*pitch_command + dead_zone_half_width, 0); + *roll_command = *roll_command > 0 ? max(0, *roll_command - dead_zone_half_width) : min(*roll_command + dead_zone_half_width, 0); + *yaw_command = *yaw_command > 0 ? max(0, *yaw_command - dead_zone_half_width) : min(*yaw_command + dead_zone_half_width, 0); + } if (state->is(STATUS_OVERRIDE)) From d9e4155ece984975638077f8a615b5f98c61be2a Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 9 Mar 2016 15:10:58 +0100 Subject: [PATCH 10/11] Force reverts from the LED override mode to work --- led.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/led.cpp b/led.cpp index f1b0797..5c15945 100644 --- a/led.cpp +++ b/led.cpp @@ -27,6 +27,7 @@ void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, u override = pattern != LED::NO_OVERRIDE; if (!override) return; + oldStatus = 0; red_indicator ? indicatorRedOn() : indicatorRedOff(); green_indicator ? indicatorGreenOn() : indicatorGreenOff(); use(pattern, red_a, green_a, blue_a, red_b, green_b, blue_b); From e251bc258a92e2e7f6455795b8ad596b0e4f7035 Mon Sep 17 00:00:00 2001 From: Adnan Ademovic Date: Wed, 9 Mar 2016 19:39:59 +0100 Subject: [PATCH 11/11] Roll version to 1.1.0 --- version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/version.h b/version.h index 1e70bea..e01e2ca 100644 --- a/version.h +++ b/version.h @@ -17,7 +17,7 @@ #define version_h #define FIRMWARE_VERSION_A 1 -#define FIRMWARE_VERSION_B 0 +#define FIRMWARE_VERSION_B 1 #define FIRMWARE_VERSION_C 0 #endif