Merge pull request #15 from Flybrix/development

Release 1.3.0

AK8963.cpp

@@ -13,7 +13,6 @@
 #include <stdio.h>
 #include <math.h>
 #include "state.h"
-#include "config.h"  //CONFIG variable
 
 // we have three coordinate systems here:
 // 1. REGISTER coordinates: native values as read
@@ -77,9 +76,9 @@ void AK8963::triggerCallback() {
         magCount[0] = (int16_t)((float)magCount[0] * magCalibration[0]);
         magCount[1] = (int16_t)((float)magCount[1] * magCalibration[1]);
         magCount[2] = (int16_t)((float)magCount[2] * magCalibration[2]);
-        state->mag[0] = (float)magCount[0] * mRes - CONFIG.data.magBias[0];
-        state->mag[1] = (float)magCount[1] * mRes - CONFIG.data.magBias[1];
-        state->mag[2] = (float)magCount[2] * mRes - CONFIG.data.magBias[2];
+        state->mag[0] = (float)magCount[0] * mRes - mag_bias.x;
+        state->mag[1] = (float)magCount[1] * mRes - mag_bias.y;
+        state->mag[2] = (float)magCount[2] * mRes - mag_bias.z;
         rotate(state->R, state->mag);  // rotate to FLYER coords
         state->updateStateMag();
     } else {

AK8963.h

@@ -35,6 +35,17 @@ class AK8963 : public CallbackProcessor {
 
     uint8_t getID();
 
+    struct __attribute__((packed)) MagBias {
+        bool verify() const {
+            return true;
+        }
+        float x;
+        float y;
+        float z;
+    } mag_bias;
+
+    static_assert(sizeof(MagBias) == 3 * 4, "Data is not packed");
+
    private:
     State *state;
     I2CManager *i2c;
@@ -51,7 +62,7 @@ class AK8963 : public CallbackProcessor {
 
     // 16-bit raw values, bias correction, factory calibration
     int16_t magCount[3] = {0, 0, 0};
-    // bias is stored in CONFIG
+    // bias is stored in MagBias
     float magCalibration[3] = {0.0, 0.0, 0.0};
 
     // buffers for processCallback

PID.h

@@ -29,7 +29,7 @@ class IIRfilter {
 
 class PID {
    public:
-    explicit PID(float* terms)
+    explicit PID(const float* terms)
         : Kp{terms[0]},
           Ki{terms[1]},
           Kd{terms[2]},

R415X.cpp

@@ -5,17 +5,47 @@
 */
 
 #include "R415X.h"
+
 #include "board.h"
+#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
 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
 
+bool R415X::ChannelProperties::verify() const {
+    bool ok{true};
+    bool assigned[] = {false, false, false, false, false, false};
+    for (size_t idx = 0; idx < 6; ++idx) {
+        uint8_t assig = assignment[idx];
+        if (assig < 0 || assig > 5) {
+            ok = false;
+            DebugPrint("All channel assignments must be within the [0, 5] range");
+        } else if (assigned[assig]) {
+            ok = false;
+            DebugPrint("Duplicate channel assignment detected");
+        } else {
+            assigned[assig] = true;
+        }
+    }
+    for (size_t idx = 0; idx < 6; ++idx) {
+        if (midpoint[idx] < PPMchannel::min || midpoint[idx] > PPMchannel::max) {
+            ok = false;
+            DebugPrint("Channel midpoints must be within the channel range");
+        }
+        if (midpoint[idx] < deadzone[idx]) {
+            ok = false;
+            DebugPrint("Channel deadzone cannot be larger than the midpoint value");
+        }
+    }
+    return ok;
+}
+
 R415X::R415X() {
-    attemptToBind(50); //this calls initialize_isr
-    //initialize_isr();
+    initialize_isr();
 }
 
 void R415X::initialize_isr(void) {
@@ -54,7 +84,6 @@ extern "C" void ftm1_isr(void) {
     // too long : pulseWidth > RX_PPM_SYNCPULSE_MAX;
     if (pulseWidth < 2700 || (pulseWidth > 6300 && pulseWidth < RX_PPM_SYNCPULSE_MIN) || pulseWidth > RX_PPM_SYNCPULSE_MAX) {
         RX_errors++;
-
         RX_channel = RC_CHANNEL_COUNT + 1;           // set RX_channel out of range to drop frame
     } else if (pulseWidth > RX_PPM_SYNCPULSE_MIN) {  // this is the sync pulse
         if (RX_channel <= RC_CHANNEL_COUNT) {        // valid frame = push from our buffer
@@ -73,12 +102,76 @@ extern "C" void ftm1_isr(void) {
     startPulse = stopPulse;  // Save time at pulse start
 }
 
-void R415X::attemptToBind(uint16_t milliseconds) {
-    pinMode(board::RX_DAT, OUTPUT);
-    digitalWrite(board::RX_DAT, LOW);
-    delay(milliseconds);
-    pinMode(board::RX_DAT, INPUT);  // WE ARE ASSUMING RX_DAT IS PIN 3 IN FTM1 SETUP!
+void R415X::getCommandData(State* state) {
 
-    // after we bind, we must setup our timer again.
-    initialize_isr();
+    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
+        }
+    }
+
+    // read data into PPMchannel objects using receiver channels assigned from configuration
+    throttle.val = RX[channel.assignment[0]];
+    pitch.val = RX[channel.assignment[1]];
+    roll.val = RX[channel.assignment[2]];
+    yaw.val = RX[channel.assignment[3]];
+    AUX1.val = RX[channel.assignment[4]];
+    AUX2.val = RX[channel.assignment[5]];
+
+    // update midpoints from config
+    throttle.mid = channel.midpoint[channel.assignment[0]];
+    pitch.mid = channel.midpoint[channel.assignment[1]];
+    roll.mid = channel.midpoint[channel.assignment[2]];
+    yaw.mid = channel.midpoint[channel.assignment[3]];
+    AUX1.mid = channel.midpoint[channel.assignment[4]];
+    AUX2.mid = channel.midpoint[channel.assignment[5]];
+
+    // update deadzones from config
+    throttle.deadzone = channel.deadzone[channel.assignment[0]];
+    pitch.deadzone = channel.deadzone[channel.assignment[1]];
+    roll.deadzone = channel.deadzone[channel.assignment[2]];
+    yaw.deadzone = channel.deadzone[channel.assignment[3]];
+    AUX1.deadzone = channel.deadzone[channel.assignment[4]];
+    AUX2.deadzone = channel.deadzone[channel.assignment[5]];
+
+    sei();  // enable interrupts
+
+    // tell state R415X is working and translate PPMChannel data into the four command level and aux mask
+    state->command_source_mask |= COMMAND_READY_R415X;
+
+    state->command_AUX_mask = 0x00;  // reset the AUX mode bitmask
+    // bitfield order is {AUX1_low, AUX1_mid, AUX1_high, AUX2_low, AUX2_mid, AUX2_high, x, x} (LSB-->MSB)
+    if (AUX1.isLow()) {
+        state->command_AUX_mask |= (1 << 0);
+    } else if (AUX1.isMid()) {
+        state->command_AUX_mask |= (1 << 1);
+    } else if (AUX1.isHigh()) {
+        state->command_AUX_mask |= (1 << 2);
+    }
+    if (AUX2.isLow()) {
+        state->command_AUX_mask |= (1 << 3);
+    } else if (AUX2.isMid()) {
+        state->command_AUX_mask |= (1 << 4);
+    } else if (AUX2.isHigh()) {
+        state->command_AUX_mask |= (1 << 5);
+    }
+
+    // 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();
+
+    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);
 }

R415X.h

@@ -14,23 +14,88 @@
 
 #include "Arduino.h"
 
+class State;
+
+class PPMchannel {
+   public:
+    PPMchannel(){};
+
+    uint16_t val = 1500;
+
+    uint16_t mid = 1500;
+    uint16_t deadzone = 0;
+    static const uint16_t min = 1100;
+    static const uint16_t max = 1900;
+
+    uint16_t applyDeadzone() {
+        if (val > mid + deadzone) {
+            return val - deadzone;
+        }
+        if (val < mid - deadzone) {
+            return val + deadzone;
+        }
+        return mid;
+    }
+
+    bool isLow() {
+        return ((val - min) < (max - min) / 10);
+    };
+    bool isHigh() {
+        return ((max - val) < (max - min) / 10);
+    };
+    bool isMid() {
+        return (abs(val - mid) < (max - min) / 10);
+    };
+
+    // R/C controllers are sold with a specified "mode" that is either "mode 1" or "mode 2"
+    // our default settings assume a "mode 2" controller, but the channels can be remapped if desired.
+    // "mode 2" puts "throttle/yaw" on left stick and "pitch/roll" on the right
+    // "mode 1" puts "pitch/yaw" on left stick and "throttle/roll" on the right
+    // in both modes, sticks "down" or "right" produce LOW ppm values, while sticks "up" or "left" give high values
+    //
+    // We want our sticks to map cleanly over to our flyer's coordinate system:
+    //   ( +x = right, +y = forward, +z = up) which implies (+pitch = nose up, +roll = RHS down, +yaw = CCW viewed from above)
+    //
+    // ***** To match this convention, we must invert the direction of the pitch and roll commands *****
+    //
+    // This is done using 'CONFIG.data.channelInversion'.
+    //
+};
+
 class R415X {
    public:
     R415X();
-    void attemptToBind(uint16_t milliseconds);
+    void getCommandData(State* state);
+
+    struct __attribute__((packed)) 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)
+        uint16_t midpoint[6];  // ideally 1500usec
+        uint16_t deadzone[6];  // usec units
+    } channel;
+
+    static_assert(sizeof(ChannelProperties) == 6 + 1 + 6 * 2 * 2, "Data is not packed");
 
    private:
     void initialize_isr(void);
 
+    PPMchannel throttle;
+    PPMchannel pitch;
+    PPMchannel roll;
+    PPMchannel yaw;
+    PPMchannel AUX1;
+    PPMchannel AUX2;
+
 };  // class R415X
 
 // global variables used by the interrupt callback
 #define RC_CHANNEL_COUNT 6
-extern volatile uint16_t RX[RC_CHANNEL_COUNT];  // filled by the interrupt with valid data
-extern volatile uint16_t RX_errors;  // count dropped frames
-extern volatile uint16_t startPulse;  // keeps track of the last received pulse position
+extern volatile uint16_t RX[RC_CHANNEL_COUNT];         // filled by the interrupt with valid data
+extern volatile uint16_t RX_errors;                    // count dropped frames
+extern volatile uint16_t startPulse;                   // keeps track of the last received pulse position
 extern volatile uint16_t RX_buffer[RC_CHANNEL_COUNT];  // buffer data in anticipation of a valid frame
-extern volatile uint8_t RX_channel;  // we are collecting data for this channel
+extern volatile uint8_t RX_channel;                    // we are collecting data for this channel
 
 #define RX_PPM_SYNCPULSE_MIN 7500   // 2.5ms
 #define RX_PPM_SYNCPULSE_MAX 48000  // 16 ms (seems to be about 13.4ms on the scope)

airframe.cpp

@@ -6,18 +6,17 @@
 
 #include "airframe.h"
 #include <Arduino.h>
-#include "config.h"  //CONFIG variable
 #include "state.h"
 
 Airframe::Airframe(State* state) : state(state) {
 }
 
 uint16_t Airframe::mix(int32_t mFz, int32_t mTx, int32_t mTy, int32_t mTz) {
     int32_t mmax = max(max(mFz, mTx), max(mTy, mTz));
-    return (uint16_t) constrain( (mFz * state->Fz + mTx * state->Tx + mTy * state->Ty + mTz * state->Tz) / mmax, 0, 4095);
+    return constrain((mFz * state->Fz + mTx * state->Tx + mTy * state->Ty + mTz * state->Tz) / mmax, 0, 4095);
 }
 
 void Airframe::updateMotorsMix() {
     for (size_t i = 0; i < 8; ++i)
-        state->MotorOut[i] = mix(CONFIG.data.mixTableFz[i], CONFIG.data.mixTableTx[i], CONFIG.data.mixTableTy[i], CONFIG.data.mixTableTz[i]);
+        state->MotorOut[i] = mix(mix_table.fz[i], mix_table.tx[i], mix_table.ty[i], mix_table.tz[i]);
 }

airframe.h

@@ -20,6 +20,18 @@ class Airframe {
     Airframe(State* state);
     void updateMotorsMix();
 
+    struct __attribute__((packed)) MixTable {
+        bool verify() const {
+            return true;
+        }
+        int8_t fz[8];
+        int8_t tx[8];
+        int8_t ty[8];
+        int8_t tz[8];
+    } mix_table;
+
+    static_assert(sizeof(MixTable) == 4 * 8, "Data is not packed");
+
    private:
     uint16_t mix(int32_t mFz, int32_t mTx, int32_t mTy, int32_t mTz);
     State* state;

board.h

@@ -150,6 +150,7 @@ constexpr Position POSITION[]{
 
 namespace bluetooth {
 constexpr uint8_t RESET{28};
+constexpr uint8_t MODE{30};
 }
 }
 }

cascadedPID.cpp

@@ -10,8 +10,7 @@
 
 #include "cascadedPID.h"
 
-CascadedPID::CascadedPID(float* master_terms, float* slave_terms)
-    : master_(master_terms), slave_(slave_terms) {
+CascadedPID::CascadedPID(const float* master_terms, const float* slave_terms) : master_(master_terms), slave_(slave_terms) {
 }
 
 float CascadedPID::Compute(uint32_t now, bool use_master, bool use_slave) {

cascadedPID.h

@@ -15,7 +15,7 @@
 
 class CascadedPID final {
    public:
-    CascadedPID(float* master_terms, float* slave_terms);
+    CascadedPID(const float* master_terms, const float* slave_terms);
 
     void isMasterWrapped(bool wrapped = true) {
         master_.isWrapped(wrapped);

cobs.h

@@ -140,6 +140,6 @@ class CobsPayload final {
     std::size_t l{1};
 };
 
-using CobsReaderBuffer = CobsReader<500>;
+using CobsReaderBuffer = CobsReader<1000>;
 
 #endif