fixed Makefile to build w/new drivers (thanks Hydra)

int32 updates in sonar driver
added 12mhz buzzer check
Removed debug output from GPS module
int32'ified althold
output HSE MHz+SysClkMHz on debug[3]

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@379 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

Makefile

@@ -54,6 +54,7 @@ COMMON_SRC	 = startup_stm32f10x_md_gcc.S \
 		   serial.c \
 		   spektrum.c \
 		   telemetry.c \
+		   drv_gpio.c \
 		   drv_i2c.c \
 		   drv_i2c_soft.c \
 		   drv_system.c \
@@ -63,7 +64,8 @@ COMMON_SRC	 = startup_stm32f10x_md_gcc.S \
 		   $(STDPERIPH_SRC)
 
 # Source files for the NAZE target
-NAZE_SRC	 = drv_adc.c \
+NAZE_SRC	 = drv_spi.c \
+		   drv_adc.c \
 		   drv_adxl345.c \
 		   drv_bmp085.c \
 		   drv_ms5611.c \
@@ -83,7 +85,8 @@ FY90Q_SRC	 = drv_adc_fy90q.c \
 		   $(COMMON_SRC)
 
 # Source files for the OLIMEXINO target
-OLIMEXINO_SRC	 = drv_adc.c \
+OLIMEXINO_SRC	 = drv_spi.c \
+		   drv_adc.c \
 		   drv_adxl345.c \
 		   drv_mpu3050.c \
 		   drv_mpu6050.c \

src/board.h

@@ -119,7 +119,7 @@ typedef struct baro_t
 
 #define SENSORS_SET (SENSOR_ACC)
 
-#endif
+#else
 
 #ifdef OLIMEXINO
 // OLIMEXINO
@@ -156,6 +156,7 @@ typedef struct baro_t
 
 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
 
+#endif
 #endif
 
 // Helpful macros
@@ -185,7 +186,7 @@ typedef struct baro_t
 #include "drv_i2c.h"
 #include "drv_pwm.h"
 #include "drv_uart.h"
-#endif
+#else
 
 #ifdef OLIMEXINO
 // OLIMEXINO
@@ -217,3 +218,4 @@ typedef struct baro_t
 #include "drv_hcsr04.h"
 
 #endif
+#endif

src/cli.c

@@ -140,6 +140,7 @@ const clivalue_t valueTable[] = {
     { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
     { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
     { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
+    { "throttle_angle_correction", VAR_UINT8, &cfg.throttle_angle_correction, 0, 100 },
     { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
     { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
     { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },

src/config.c

@@ -260,6 +260,7 @@ static void resetConf(void)
     cfg.yawdeadband = 0;
     cfg.alt_hold_throttle_neutral = 40;
     cfg.alt_hold_fast_change = 1;
+    cfg.throttle_angle_correction = 0;      // could be 40
 
     // Failsafe Variables
     cfg.failsafe_delay = 10;                // 1sec

src/drv_hcsr04.c

@@ -19,7 +19,7 @@ static uint8_t exti_pin_source;
 static IRQn_Type exti_irqn;
 
 static uint32_t last_measurement;
-static volatile int16_t *distance_ptr;
+static volatile int32_t *distance_ptr;
 
 void ECHO_EXTI_IRQHandler(void)
 {
@@ -106,7 +106,7 @@ void hcsr04_init(sonar_config_t config)
 }
 
 // distance calculation is done asynchronously, using interrupt
-void hcsr04_get_distance(volatile int16_t *distance)
+void hcsr04_get_distance(volatile int32_t *distance)
 {
     uint32_t current_time = millis();
 

src/drv_hcsr04.h

@@ -6,4 +6,4 @@ typedef enum {
 } sonar_config_t;
 
 void hcsr04_init(sonar_config_t config);
-void hcsr04_get_distance(volatile int16_t* distance);
+void hcsr04_get_distance(volatile int32_t *distance);

src/drv_system.c

@@ -88,8 +88,11 @@ void systemInit(void)
     LED1_OFF;
     BEEP_OFF;
 
-    for (i = 0; i < gpio_count; i++)
+    for (i = 0; i < gpio_count; i++) {
+        if (hse_value == 12000000 && gpio_setup[i].cfg.mode == Mode_Out_OD)
+            gpio_setup[i].cfg.mode = Mode_Out_PP;
         gpioInit(gpio_setup[i].gpio, &gpio_setup[i].cfg);
+    }
 
     // Init cycle counter
     cycleCounterInit();

src/gps.c

@@ -1112,7 +1112,6 @@ static bool UBLOX_parse_gps(void)
             f.GPS_FIX = false;
         GPS_numSat = _buffer.solution.satellites;
         // GPS_hdop                        = _buffer.solution.position_DOP;
-        // debug[3] = GPS_hdop;
         break;
     case MSG_VELNED:
         // speed_3d                        = _buffer.velned.speed_3d;  // cm/s

src/imu.c

@@ -7,13 +7,14 @@ int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
 int32_t baroPressureSum = 0;
 int32_t BaroAlt = 0;
-int16_t sonarAlt;              // to think about the unit
+int32_t sonarAlt;              // to think about the unit
 int32_t EstAlt;                // in cm
-int16_t BaroPID = 0;
+int32_t BaroPID = 0;
 int32_t AltHold;
-int16_t errorAltitudeI = 0;
+int32_t errorAltitudeI = 0;
-int16_t vario = 0;              // variometer in cm/s
+int32_t vario = 0;                      // variometer in cm/s
-float magneticDeclination = 0.0f; // calculated at startup from config
+int16_t throttleAngleCorrection = 0;    // correction of throttle in lateral wind,
+float magneticDeclination = 0.0f;       // calculated at startup from config
 float accVelScale;
 
 // **************
@@ -85,7 +86,7 @@ void computeIMU(void)
             Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
         }
         for (axis = 0; axis < 3; axis++) {
-            gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
+            gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1) / Smoothing[axis]);
             gyroSmooth[axis] = gyroData[axis];
         }
     } else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
@@ -115,15 +116,6 @@ void computeIMU(void)
 // code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
 // **************************************************
 
-//******  advanced users settings *******************
-
-/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
-/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
-/* Default WMC value: n/a*/
-#define GYR_CMPFM_FACTOR 200.0f
-
-//****** end of advanced users settings *************
-
 #define INV_GYR_CMPF_FACTOR   (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
 #define INV_GYR_CMPFM_FACTOR  (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
 
@@ -235,7 +227,7 @@ static void getEstimatedAttitude(void)
     // 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.
     // To do that, we just skip filter, as EstV already rotated by Gyro
-    if (72 < accMag && accMag < 133) {
+    if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
         for (axis = 0; axis < 3; axis++)
             EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
     }
@@ -291,13 +283,19 @@ static void getEstimatedAttitude(void)
             heading = heading + 360;
     }
 #endif
+
+    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;
+        
+    }
 }
 
 #ifdef BARO
 #define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
 #define INIT_DELAY      4000000 // 4 sec initialization delay
 
-int16_t applyDeadband(int16_t value, int16_t deadband)
+int16_t applyDeadband(int32_t value, int32_t deadband)
 {
     if (abs(value) < deadband) {
         value = 0;
@@ -315,13 +313,13 @@ int getEstimatedAltitude(void)
     static uint32_t previousT;
     uint32_t currentT = micros();
     uint32_t dTime;
-    int16_t error16;
+    int32_t error;
-    int16_t baroVel;
+    int32_t baroVel;
-    int16_t accZ;
+    int32_t accZ;
-    static int16_t accZoffset = 0;
+    int32_t vel_tmp;
+    static int32_t accZoffset = 0;
     static float vel = 0.0f;
     static int32_t lastBaroAlt;
-    int16_t vel_tmp;
 
     dTime = currentT - previousT;
     if (dTime < UPDATE_INTERVAL)
@@ -337,27 +335,27 @@ int getEstimatedAltitude(void)
     // baroGroundPressure is not supposed to be 0 here
     // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
     BaroAlt = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter 
-    EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3;   // additional LPF to reduce baro noise
+    EstAlt = (EstAlt * 6 + BaroAlt * 2) / 8;   // additional LPF to reduce baro noise
 
     //P
-    error16 = constrain(AltHold - EstAlt, -300, 300);
+    error = constrain(AltHold - EstAlt, -300, 300);
-    error16 = applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
+    error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
-    BaroPID = constrain((cfg.P8[PIDALT] * error16 >> 7), -150, +150);
+    BaroPID = constrain(((cfg.P8[PIDALT] * error) / 128), -150, +150);
 
     //I
-    errorAltitudeI += cfg.I8[PIDALT] * error16 >> 6;
+    errorAltitudeI += (cfg.I8[PIDALT] * error) / 64;
     errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
-    BaroPID += errorAltitudeI >> 9;     // I in range +/-60
+    BaroPID += errorAltitudeI / 512;     // I in range +/-60
 
     // projection of ACC vector to global Z, with 1G subtructed
     // Math: accZ = A * G / |G| - 1G (invG is calculated in getEstimatedAttitude)
     accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG;
 
     if (!f.ARMED) {
-        accZoffset -= accZoffset >> 3;
+        accZoffset -= accZoffset / 8;
         accZoffset += accZ;
     }
-    accZ -= accZoffset >> 3;
+    accZ -= accZoffset / 8;
     accZ = applyDeadband(accZ, cfg.accz_deadband);
 
     // Integrator - velocity, cm/sec
@@ -377,7 +375,7 @@ int getEstimatedAltitude(void)
     vel_tmp = vel;
     vel_tmp = applyDeadband(vel_tmp, 5);
     vario = vel_tmp;
-    BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp >> 4, -150, 150);
+    BaroPID -= constrain((cfg.D8[PIDALT] * vel_tmp) / 16, -150, 150);
 
     return 1;
 }

src/mw.c

@@ -762,7 +762,6 @@ void loop(void)
 #ifdef SONAR
             if (sensors(SENSOR_SONAR)) {
                 Sonar_update();
-                debug[2] = sonarAlt;
             }
 #endif
             if (feature(FEATURE_VARIO) && f.VARIO_MODE)
@@ -838,6 +837,10 @@ void loop(void)
         }
 #endif
 
+        if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
+            rcCommand[THROTTLE]+= throttleAngleCorrection;
+        }
+
         if (sensors(SENSOR_GPS)) {
             if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
                 float sin_yaw_y = sinf(heading * 0.0174532925f);

src/mw.h

@@ -176,6 +176,7 @@ typedef struct config_t {
     uint8_t yawdeadband;                    // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
     uint8_t alt_hold_throttle_neutral;      // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
+    uint8_t throttle_angle_correction;      // 
 
     // Failsafe related configuration
     uint8_t failsafe_delay;                 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
@@ -327,14 +328,14 @@ extern int32_t baroPressure;
 extern int32_t baroTemperature;
 extern int32_t baroPressureSum;
 extern int32_t BaroAlt;
-extern int16_t sonarAlt;
+extern int32_t sonarAlt;
 extern int32_t EstAlt;
 extern int32_t AltHold;
-extern int16_t errorAltitudeI;
+extern int32_t errorAltitudeI;
-extern int16_t BaroPID;
+extern int32_t BaroPID;
-extern int16_t vario;
+extern int32_t vario;
+extern int16_t throttleAngleCorrection;
 extern int16_t headFreeModeHold;
-extern int16_t zVelocity;
 extern int16_t heading, magHold;
 extern int16_t motor[MAX_MOTORS];
 extern int16_t servo[8];

src/serial.c

@@ -602,6 +602,8 @@ static void evaluateCommand(void)
         break;
     case MSP_DEBUG:
         headSerialReply(8);
+        // make use of this crap, output some useful QA statistics
+        debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
         for (i = 0; i < 4; i++)
             serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
         break;