Merge branch 'development'

AK8963.cpp

@@ -14,6 +14,8 @@
 #include <math.h>
 #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

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");

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};
@@ -45,16 +57,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;
@@ -103,16 +107,15 @@ 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!
     for (uint8_t i = 0; i < RC_CHANNEL_COUNT; i++) {
         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
         }
     }
 
@@ -165,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);
 }

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)
@@ -78,8 +79,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;

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} {
+}

airframe.h

@@ -21,6 +21,7 @@ class Airframe {
     void updateMotorsMix();
 
     struct __attribute__((packed)) MixTable {
+        MixTable();
         bool verify() const {
             return true;
         }

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,12 @@ void closeFile() {
     binFile.close();
     DebugPrint("File closing successful");
 }
+
+void setLock(bool enable) {
+    locked = enable;
+}
+
+bool isLocked() {
+    return locked;
+}
 }  // namespace sdcard

cardManagement.h

@@ -26,6 +26,11 @@ 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);
+
+bool isLocked();
 }
 
 #endif

command.cpp

@@ -17,65 +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;
-    }
-
+    }    
+    
     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
 
-    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
+        // 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;
@@ -88,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
 }