Merge pull request #20 from Flybrix/development

Update to 1.5.0

command.cpp

@@ -12,30 +12,31 @@
 
 #include "cardManagement.h"
 
-PilotCommand::PilotCommand(State* __state, R415X* __receiver)
-    : state(__state), receiver(__receiver) {
+PilotCommand::PilotCommand(State* __state, R415X* __receiver) : state(__state), receiver(__receiver) {
 }
 
 void PilotCommand::processCommands(void) {
-
-    bool attempting_to_enable  = false;
+    bool attempting_to_enable = false;
     bool attempting_to_disable = false;
 
-    if (!(state->command_source_mask & COMMAND_READY_BTLE)){
+    if (state->is(STATUS_RX_FAIL)) {
+        state->command_throttle *= 0.99;
+    }
+
+    if (!(state->command_source_mask & COMMAND_READY_BTLE)) {
         if (bluetoothTolerance) {
             // we allow bluetooth a generous 1s before we give up
             --bluetoothTolerance;
         } else {
             // since we haven't seen bluetooth commands for more than 1 second, try the R415X
             receiver->getCommandData(state);
         }
-    }
-    else{
+    } else {
         // as soon as we start receiving bluetooth, reset the watchdog
         bluetoothTolerance = 40;
-    }    
-    
-    if (!(state->command_source_mask & (COMMAND_READY_R415X | COMMAND_READY_BTLE))){
+    }
+
+    if (!(state->command_source_mask & (COMMAND_READY_R415X | COMMAND_READY_BTLE))) {
         // we have no command data!
         state->command_invalid_count++;
         if (state->command_invalid_count > 80) {
@@ -48,27 +49,26 @@ void PilotCommand::processCommands(void) {
         if (state->command_invalid_count == 0) {
             state->clear(STATUS_RX_FAIL);
         }
-    }
-    else { 
+    } else {
         // use valid command data
-        attempting_to_enable  = state->command_AUX_mask & (1 << 0);  // AUX1 is low
+        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);    
+        // 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_OTHER);
     }
     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 | STATUS_FAIL_OTHER)) {
-            state->processMotorEnablingIteration(); //this can flip STATUS_ENABLED to true
+            state->processMotorEnablingIteration();  // this can flip STATUS_ENABLED to true
             recentlyEnabled = true;
             throttleHoldOff = 80;  // @40Hz -- hold for 2 sec
             if (state->is(STATUS_ENABLED))
@@ -78,23 +78,21 @@ void PilotCommand::processCommands(void) {
             state->disableMotors();
             sdcard::closeFile();
         }
-    }
-    else {
+    } 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;
-        state->command_roll     = 0;
-        state->command_yaw      = 0;
+        state->command_pitch = 0;
+        state->command_roll = 0;
+        state->command_yaw = 0;
 
         throttleHoldOff--;
         if (recentlyEnabled && (throttleHoldOff == 0)) {
             recentlyEnabled = false;
         }
     }
-
 }

config.h

@@ -102,7 +102,7 @@ static_assert(sizeof(Config) ==
                       sizeof(Control::PIDParameters) + sizeof(LED::States) + sizeof(DeviceName),
               "Data is not packed");
 
-static_assert(sizeof(Config) == 628, "Data does not have expected size");
+static_assert(sizeof(Config) == 644, "Data does not have expected size");
 
 Config readEEPROM();
 bool isEmptyEEPROM();

control.cpp

@@ -46,6 +46,10 @@ Control::PIDParameters::PIDParameters()
       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},
+      thrust_gain{4095.0},
+      pitch_gain{2047.0},
+      roll_gain{2047.0},
+      yaw_gain{2047.0},
       pid_bypass{BYPASS_THRUST_MASTER | BYPASS_THRUST_SLAVE | BYPASS_YAW_MASTER}  // AHRS/Horizon mode
 {
 }
@@ -113,10 +117,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], pid_parameters.thrust_gain));
+    pitch_pid.setSetpoint(state->command_pitch * (1.0f / 2047.0f) * pitch_pid.getScalingFactor(pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE], pid_parameters.pitch_gain));
+    roll_pid.setSetpoint(state->command_roll * (1.0f / 2047.0f) * roll_pid.getScalingFactor(pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE], pid_parameters.roll_gain));
+    yaw_pid.setSetpoint(state->command_yaw * (1.0f / 2047.0f) * yaw_pid.getScalingFactor(pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE], pid_parameters.yaw_gain));
 
     // compute new output levels for state
     uint32_t now = micros();

control.h

@@ -43,10 +43,15 @@ class Control {
         float roll_slave[7];
         float yaw_slave[7];
 
+        float thrust_gain;
+        float pitch_gain;
+        float roll_gain;
+        float yaw_gain;
+
         uint8_t pid_bypass;  // bitfield order for bypass: {thrustMaster, pitchMaster, rollMaster, yawMaster, thrustSlave, pitchSlave, rollSlave, yawSlave} (LSB-->MSB)
     } pid_parameters;
 
-    static_assert(sizeof(PIDParameters) == 4 * 8 * 7 + 1, "Data is not packed");
+    static_assert(sizeof(PIDParameters) == 4 * 8 * 7 + 4 * 4 + 1, "Data is not packed");
 
     uint32_t lastUpdateMicros = 0;  // 1.2 hrs should be enough
 

flybrix-firmware.ino

@@ -61,6 +61,8 @@ void setup() {
     readEEPROM().applyTo(sys);
     sys.state.resetState();
 
+    sys.version.display(sys.led);
+
     setBluetoothUart(sys.name);
 
     sys.state.set(STATUS_BMP_FAIL);

serial_subcommands.h

@@ -28,6 +28,7 @@ enum SerialComm::Commands : uint8_t {
     REINIT_PARTIAL_EEPROM_DATA,
     REQ_PARTIAL_EEPROM_DATA,
     REQ_CARD_RECORDING_STATE,
+    SET_PARTIAL_TEMPORARY_CONFIG,
     END_OF_COMMANDS,
 };
 
@@ -249,6 +250,18 @@ DO_SUBCOMMAND(REQ_CARD_RECORDING_STATE) {
     return true;
 }
 
+DO_SUBCOMMAND(SET_PARTIAL_TEMPORARY_CONFIG) {
+    Config tmp_config(*systems);
+    if (!tmp_config.readPartialFrom(input)) {
+        return false;
+    }
+    if (!tmp_config.verify()) {
+        return false;
+    }
+    tmp_config.applyTo(*systems);
+    return true;
+}
+
 #undef DO_SUBCOMMAND
 
 #endif /* SERIAL_SUBCOMMANDS_H */

serial_substates.h

@@ -34,6 +34,8 @@ enum SerialComm::States : uint8_t {
     END_OF_STATES,
 };
 
+static_assert(SerialComm::States::END_OF_STATES == 27, "Added/removed states not acknowledged");
+
 // Reads part of the state into a COBS payload, named "payload"
 #define READ_SUBSTATE(name) \
     template <>             \

version.cpp

@@ -12,6 +12,7 @@
 #include "version.h"
 
 #include "debug.h"
+#include "led.h"
 
 bool Version::verify() const {
     if (major == FIRMWARE_VERSION_A && minor == FIRMWARE_VERSION_B && patch == FIRMWARE_VERSION_C) {
@@ -20,3 +21,16 @@ bool Version::verify() const {
     DebugPrint("Configuration versions do not match");
     return false;
 }
+
+uint32_t encode(uint32_t color) {
+    bool lighter{color & 8};
+    uint32_t strength{lighter ? 0xcc : 0x11};
+    uint32_t base{lighter ? 0x010101 : 0x000000};
+    return (((color & 4) >> 2) + ((color & 2) << 7) + ((color & 1) << 16)) * strength + base;
+}
+
+void Version::display(LED& led) const {
+    led.set(LED::SOLID, encode(minor), 0, encode(major), encode(patch), false, false);
+    led.update();
+    led.set(LED::NO_OVERRIDE, 0, false, false);
+}

version.h

@@ -15,12 +15,15 @@
 #include "debug.h"
 
 #define FIRMWARE_VERSION_A 1
-#define FIRMWARE_VERSION_B 4
+#define FIRMWARE_VERSION_B 5
 #define FIRMWARE_VERSION_C 0
 
+class LED;
+
 struct __attribute__((packed)) Version {
     Version() : major(FIRMWARE_VERSION_A), minor(FIRMWARE_VERSION_B), patch(FIRMWARE_VERSION_C) {
     }
+    void display(LED& led) const;
     bool verify() const;
     uint8_t major;
     uint8_t minor;