Merge remote-tracking branch 'multiwii/upstream'

Conflicts:
	src/drv_system.c
	src/main.c

src/board.h

@@ -237,8 +237,8 @@ typedef struct baro_t
 
 #ifdef BEEP_GPIO
 #define BEEP_TOGGLE              digitalToggle(BEEP_GPIO, BEEP_PIN);
-#define BEEP_OFF                 digitalHi(BEEP_GPIO, BEEP_PIN);
+#define BEEP_OFF                 systemBeep(false);
-#define BEEP_ON                  digitalLo(BEEP_GPIO, BEEP_PIN);
+#define BEEP_ON                  systemBeep(true);
 #else
 #define BEEP_TOGGLE              ;
 #define BEEP_OFF                 ;

src/drv_serial.c

@@ -3,7 +3,7 @@
 void serialPrint(serialPort_t *instance, const char *str)
 {
     uint8_t ch;
-    while ((ch = *(str++))) {
+    while ((ch = *(str++)) != 0) {
         serialWrite(instance, ch);
     }
 }

src/drv_softserial.c

@@ -17,7 +17,7 @@ void onSerialTimer(uint8_t portIndex, uint16_t capture);
 
 uint8_t readRxSignal(softSerial_t *softSerial)
 {
-    return digitalIn(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin);
+    return !(digitalIn(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin) == 0);
 }
 
 void setTxSignal(softSerial_t *softSerial, uint8_t state)

src/drv_system.c

@@ -6,6 +6,10 @@ static volatile uint32_t usTicks = 0;
 static volatile uint32_t sysTickUptime = 0;
 // from system_stm32f10x.c
 void SetSysClock(void);
+void systemBeep(bool onoff);
+static void beepRev4(bool onoff);
+static void beepRev5(bool onoff);
+void (* systemBeepPtr)(bool onoff) = NULL;
 
 static void cycleCounterInit(void)
 {
@@ -89,6 +93,12 @@ void systemInit(void)
     AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW;
 
     // Configure gpio
+    // rev5 needs inverted beeper. oops.
+    if (hse_value == 12000000)
+        systemBeepPtr = beepRev5;
+    else
+        systemBeepPtr = beepRev4;
+
     LED0_OFF;
     LED1_OFF;
     BEEP_OFF;
@@ -182,6 +192,29 @@ void systemReset(bool toBootloader)
     // Generate system reset
     SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
 }
+static void beepRev4(bool onoff)
+{
+    if (onoff) {
+        digitalLo(BEEP_GPIO, BEEP_PIN);
+    } else {
+        digitalHi(BEEP_GPIO, BEEP_PIN);
+    }
+}
+
+static void beepRev5(bool onoff)
+{
+    if (onoff) {
+        digitalHi(BEEP_GPIO, BEEP_PIN);
+    } else {
+        digitalLo(BEEP_GPIO, BEEP_PIN);
+    }
+}
+
+void systemBeep(bool onoff)
+{
+    systemBeepPtr(onoff);
+}
+
 void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
 {
     switch (rotation) {

src/imu.c

@@ -222,11 +222,31 @@ void accSum_reset(void)
     accTimeSum = 0;
 }
 
+// baseflight calculation by Luggi09 originates from arducopter
+static int16_t calculateHeading(t_fp_vector *vec)
+{
+    int16_t head;
+
+    float cosineRoll = cosf(anglerad[ROLL]);
+    float sineRoll = sinf(anglerad[ROLL]);
+    float cosinePitch = cosf(anglerad[PITCH]);
+    float sinePitch = sinf(anglerad[PITCH]);
+    float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
+    float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
+    float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
+    head = lrintf(hd);
+    if (head < 0)
+        head += 360;
+
+    return head;
+}
+
 static void getEstimatedAttitude(void)
 {
     uint32_t axis;
     int32_t accMag = 0;
     static t_fp_vector EstM;
+    static t_fp_vector EstN = { .A = { 1000.0f, 0.0f, 0.0f } };
     static float accLPF[3];
     static uint32_t previousT;
     uint32_t currentT = micros();
@@ -252,6 +272,8 @@ static void getEstimatedAttitude(void)
     rotateV(&EstG.V, deltaGyroAngle);
     if (sensors(SENSOR_MAG))
         rotateV(&EstM.V, deltaGyroAngle);
+    else
+        rotateV(&EstN.V, deltaGyroAngle);
 
     // Apply complimentary filter (Gyro drift correction)
     // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
@@ -277,28 +299,16 @@ static void getEstimatedAttitude(void)
     angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
     angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
 
-#ifdef MAG
+    if (sensors(SENSOR_MAG))
-    if (sensors(SENSOR_MAG)) {
+        heading = calculateHeading(&EstM);
-        // baseflight calculation by Luggi09 originates from arducopter
+    else
-        float cosineRoll = cosf(anglerad[ROLL]);
+        heading = calculateHeading(&EstN);
-        float sineRoll = sinf(anglerad[ROLL]);
-        float cosinePitch = cosf(anglerad[PITCH]);
-        float sinePitch = sinf(anglerad[PITCH]);
-        float Xh = EstM.A[X] * cosinePitch + EstM.A[Y] * sineRoll * sinePitch + EstM.A[Z] * sinePitch * cosineRoll;
-        float Yh = EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll;
-        float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
-        heading = lrintf(hd);
-        if (heading < 0)
-            heading += 360;
-    }
-#endif
 
     acc_calc(deltaT); // rotate acc vector into earth frame
 
     if (cfg.throttle_angle_correction) {
         int cosZ = EstG.V.Z / acc_1G * 100.0f;
         throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
-
     }
 }
 
@@ -378,7 +388,7 @@ int getEstimatedAltitude(void)
     // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
     // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
     vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
-    constrain(vel, -1000, 1000);                // limit max velocity to +/- 10m/s (36km/h)
+    vel = constrain(vel, -1000, 1000);                // limit max velocity to +/- 10m/s (36km/h)
 
     // D
     vel_tmp = vel;

src/main.c

@@ -138,9 +138,9 @@ int main(void)
 
     previousTime = micros();
     if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
-        calibratingA = 400;
+        calibratingA = CALIBRATING_ACC_CYCLES;
-    calibratingG = 1000;
+    calibratingG = CALIBRATING_GYRO_CYCLES;
-    calibratingB = 200;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
+    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
     f.SMALL_ANGLES_25 = 1;
 
     // loopy

src/mw.c

@@ -87,14 +87,17 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
 void annexCode(void)
 {
     static uint32_t calibratedAccTime;
-    uint16_t tmp, tmp2;
+    int32_t tmp, tmp2;
-    static uint8_t buzzerFreq;  //delay between buzzer ring
+    int32_t axis, prop1, prop2;
+    static uint8_t buzzerFreq;  // delay between buzzer ring
+
+    // vbat shit
     static uint8_t vbatTimer = 0;
-    uint8_t axis, prop1, prop2;
     static uint8_t ind = 0;
     uint16_t vbatRaw = 0;
     static uint16_t vbatRawArray[8];
-    uint8_t i;
+
+    int i;
 
     // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
     if (rcData[THROTTLE] < BREAKPOINT) {
@@ -123,7 +126,6 @@ void annexCode(void)
             prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
             prop1 = (uint16_t) prop1 *prop2 / 100;
         } else {                // YAW
-            tmp *= -mcfg.yaw_control_direction; //change control direction for yaw needed with new gyro orientation
             if (cfg.yawdeadband) {
                 if (tmp > cfg.yawdeadband) {
                     tmp -= cfg.yawdeadband;
@@ -131,12 +133,12 @@ void annexCode(void)
                     tmp = 0;
                 }
             }
-            rcCommand[axis] = tmp;
+            rcCommand[axis] = tmp * -mcfg.yaw_control_direction;
-            prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
+            prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500;
         }
-        dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
+        dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
-        dynI8[axis] = (uint16_t) cfg.I8[axis] * prop1 / 100;
+        dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100;
-        dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
+        dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
         if (rcData[axis] < mcfg.midrc)
             rcCommand[axis] = -rcCommand[axis];
     }
@@ -435,27 +437,24 @@ void loop(void)
     uint16_t auxState = 0;
     static uint8_t GPSNavReset = 1;
     bool isThrottleLow = false;
+    bool rcReady = false;
 
     // calculate rc stuff from serial-based receivers (spek/sbus)
     if (feature(FEATURE_SERIALRX)) {
-        bool ready = false;
         switch (mcfg.serialrx_type) {
             case SERIALRX_SPEKTRUM1024:
             case SERIALRX_SPEKTRUM2048:
-                ready = spektrumFrameComplete();
+                rcReady = spektrumFrameComplete();
                 break;
             case SERIALRX_SBUS:
-                ready = sbusFrameComplete();
+                rcReady = sbusFrameComplete();
                 break;
         }
-        if (ready)
-            computeRC();
     }
 
-    if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
+    if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
+        rcReady = false;
         rcTime = currentTime + 20000;
-        // TODO clean this up. computeRC should handle this check
-        if (!feature(FEATURE_SERIALRX))
         computeRC();
 
         // in 3D mode, we need to be able to disarm by switch at any time
@@ -539,11 +538,13 @@ void loop(void)
                 i = 0;
                 // GYRO calibration
                 if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
-                    calibratingG = 1000;
+                    calibratingG = CALIBRATING_GYRO_CYCLES;
                     if (feature(FEATURE_GPS))
                         GPS_reset_home_position();
                     if (sensors(SENSOR_BARO))
                         calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
+                    if (!sensors(SENSOR_MAG))
+                        heading = 0; // reset heading to zero after gyro calibration
                 // Inflight ACC Calibration
                 } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
                     if (AccInflightCalibrationMeasurementDone) {        // trigger saving into eeprom after landing
@@ -581,7 +582,7 @@ void loop(void)
                     mwArm();
                 // Calibrating Acc
                 else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
-                    calibratingA = 400;
+                    calibratingA = CALIBRATING_ACC_CYCLES;
                 // Calibrating Mag
                 else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
                     f.CALIBRATE_MAG = 1;
@@ -686,7 +687,7 @@ void loop(void)
 #endif
 
 #ifdef  MAG
-        if (sensors(SENSOR_MAG)) {
+        if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
             if (rcOptions[BOXMAG]) {
                 if (!f.MAG_MODE) {
                     f.MAG_MODE = 1;

src/mw.h

@@ -141,6 +141,10 @@ enum {
     ALIGN_MAG = 2
 };
 
+#define CALIBRATING_GYRO_CYCLES             1000
+#define CALIBRATING_ACC_CYCLES              400
+#define CALIBRATING_BARO_CYCLES             200
+
 typedef struct config_t {
     uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
     uint8_t P8[PIDITEMS];
@@ -457,6 +461,7 @@ bool sbusFrameComplete(void);
 
 // buzzer
 void buzzer(uint8_t warn_vbat);
+void systemBeep(bool onoff);
 
 // cli
 void cliProcess(void);

src/sensors.c

@@ -124,7 +124,10 @@ retry:
     // calculate magnetic declination
     deg = cfg.mag_declination / 100;
     min = cfg.mag_declination % 100;
+    if (sensors(SENSOR_MAG))
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
+    else
+        magneticDeclination = 0.0f;
 }
 #endif
 
@@ -165,9 +168,9 @@ static void ACC_Common(void)
     if (calibratingA > 0) {
         for (axis = 0; axis < 3; axis++) {
             // Reset a[axis] at start of calibration
-            if (calibratingA == 400)
+            if (calibratingA == CALIBRATING_ACC_CYCLES)
                 a[axis] = 0;
-            // Sum up 400 readings
+            // Sum up CALIBRATING_ACC_CYCLES readings
             a[axis] += accADC[axis];
             // Clear global variables for next reading
             accADC[axis] = 0;
@@ -175,9 +178,9 @@ static void ACC_Common(void)
         }
         // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
         if (calibratingA == 1) {
-            mcfg.accZero[ROLL] = a[ROLL] / 400;
+            mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-            mcfg.accZero[PITCH] = a[PITCH] / 400;
+            mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES;
-            mcfg.accZero[YAW] = a[YAW] / 400 - acc_1G;       // for nunchuk 200=1G
+            mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2))  / CALIBRATING_ACC_CYCLES - acc_1G;
             cfg.angleTrim[ROLL] = 0;
             cfg.angleTrim[PITCH] = 0;
             writeEEPROM(1, true);      // write accZero in EEPROM
@@ -334,7 +337,7 @@ static void GYRO_Common(void)
     if (calibratingG > 0) {
         for (axis = 0; axis < 3; axis++) {
             // Reset g[axis] at start of calibration
-            if (calibratingG == 1000) {
+            if (calibratingG == CALIBRATING_GYRO_CYCLES) {
                 g[axis] = 0;
                 devClear(&var[axis]);
             }
@@ -348,14 +351,14 @@ static void GYRO_Common(void)
                 float dev = devStandardDeviation(&var[axis]);
                 // check deviation and startover if idiot was moving the model
                 if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
-                    calibratingG = 1000;
+                    calibratingG = CALIBRATING_GYRO_CYCLES;
                     devClear(&var[0]);
                     devClear(&var[1]);
                     devClear(&var[2]);
                     g[0] = g[1] = g[2] = 0;
                     continue;
                 }
-                gyroZero[axis] = g[axis] / 1000;
+                gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
                 blinkLED(10, 15, 1);
             }
         }

src/serial.c

@@ -267,6 +267,7 @@ void serializeBoxNamesReply(void)
 void serialInit(uint32_t baudrate)
 {
     int idx;
+    bool hfadded = false;
 
     core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
     // TODO fix/hax
@@ -279,6 +280,10 @@ void serialInit(uint32_t baudrate)
     if (sensors(SENSOR_ACC)) {
         availableBoxes[idx++] = BOXANGLE;
         availableBoxes[idx++] = BOXHORIZON;
+        availableBoxes[idx++] = BOXMAG;
+        availableBoxes[idx++] = BOXHEADFREE;
+        availableBoxes[idx++] = BOXHEADADJ;
+        hfadded = true;
     }
     if (sensors(SENSOR_BARO)) {
         availableBoxes[idx++] = BOXBARO;
@@ -286,10 +291,13 @@ void serialInit(uint32_t baudrate)
             availableBoxes[idx++] = BOXVARIO;
     }
     if (sensors(SENSOR_MAG)) {
+        // this really shouldn't even needed to be tested as it wouldn't be possible without acc anyway
+        if (!hfadded) {
             availableBoxes[idx++] = BOXMAG;
             availableBoxes[idx++] = BOXHEADFREE;
             availableBoxes[idx++] = BOXHEADADJ;
         }
+    }
     if (feature(FEATURE_SERVO_TILT))
         availableBoxes[idx++] = BOXCAMSTAB;
     if (feature(FEATURE_GPS) && sensors(SENSOR_GPS)) {
@@ -606,7 +614,7 @@ static void evaluateCommand(void)
         break;
     case MSP_ACC_CALIBRATION:
         if (!f.ARMED)
-            calibratingA = 400;
+            calibratingA = CALIBRATING_ACC_CYCLES;
         headSerialReply(0);
         break;
     case MSP_MAG_CALIBRATION: