Applied patch from trunet for running at 64MHz off HSI for broken Chinese kcopter clones

status in cli now prints cpu MHz, 72 when running off crystal, 64 when running off intrc.
corrected WHO_AM_I check for mpu6050 when it's used on alternate address (AD0 high).
corrected PWM driver to consider CPU clock for timer prescalers - now things properly work below 72MHz
added a neat hack for pitch angle calculation so that it's not affected when roll reaches 90deg
added proper math for vector rotation instead of small-angle approximation

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

src/cli.c

@@ -728,7 +728,7 @@ static void cliStatus(char *cmdline)
         millis() / 1000, vbat, batteryCellCount);
     mask = sensorsMask();
 
-    uartPrint("Detected sensors: ");
+    printf("CPU %dMHz, detected sensors: ", (SystemCoreClock / 1000000));
     for (i = 0; ; i++) {
         if (sensorNames[i] == NULL)
             break;
@@ -744,7 +744,7 @@ static void cliStatus(char *cmdline)
 
 static void cliVersion(char *cmdline)
 {
-    uartPrint("Afro32 CLI version 2.0 " __DATE__ " / " __TIME__);
+    uartPrint("Afro32 CLI version 2.1 " __DATE__ " / " __TIME__);
 }
 
 void cliProcess(void)

src/drv_mpu6050.c

@@ -156,6 +156,11 @@ bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t scale)
     if (!ack)
         return false;
 
+    // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
+    // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0’s 7-bit I2C address.
+    // The least significant bit of the MPU-60X0’s I2C address is determined by the value of the AD0 pin. (we know that already).
+    // But here's the best part: The value of the AD0 pin is not reflected in this register.
+    sig &= 0x7e;
     if (sig != MPU6050_ADDRESS)
         return false;
 

src/drv_pwm.c

@@ -228,7 +228,7 @@ static void pwmTimeBase(TIM_TypeDef *tim, uint32_t period)
 
     TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
     TIM_TimeBaseStructure.TIM_Period = period - 1;
-    TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1; // all TIM on F1 runs at 72MHz
+    TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / 1000000) - 1; // all timers run at 1MHz
     TIM_TimeBaseStructure.TIM_ClockDivision = 0;
     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
     TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);

src/drv_system.c

@@ -56,6 +56,9 @@ void systemInit(void)
     };
     uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]);
 
+    // This is needed because some shit inside Keil startup fucks with SystemCoreClock, setting it back to 72MHz even on HSI.
+    SystemCoreClockUpdate();
+
     // Turn on clocks for stuff we use
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE);
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1, ENABLE);

src/drv_uart.c

@@ -150,7 +150,7 @@ void uartPrint(char *str)
 
 /* -------------------------- UART2 (Spektrum, GPS) ----------------------------- */
 uartReceiveCallbackPtr uart2Callback = NULL;
-#define UART2_BUFFER_SIZE    64
+#define UART2_BUFFER_SIZE    128
 
 volatile uint8_t tx2Buffer[UART2_BUFFER_SIZE];
 uint32_t tx2BufferTail = 0;

src/gps.c

@@ -193,8 +193,8 @@ static void reset_PID(PID *pid)
     pid->last_derivative = 0;
 }
 
-#define _X 1
-#define _Y 0
+#define GPS_X 1
+#define GPS_Y 0
 
 /****************** PI and PID controllers for GPS ********************///32938 -> 33160
 
@@ -212,7 +212,7 @@ static float GPS_scaleLonDown;  // this is used to offset the shrinking longitud
 static int16_t rate_error[2];
 static int32_t error[2];
 
-//Currently used WP
+// Currently used WP
 static int32_t GPS_WP[2];
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -485,14 +485,14 @@ static void GPS_calc_velocity(void)
 
     if (init) {
         float tmp = 1.0f / dTnav;
-        actual_speed[_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
-        actual_speed[_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp;
+        actual_speed[GPS_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
+        actual_speed[GPS_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp;
 
-        actual_speed[_X] = (actual_speed[_X] + speed_old[_X]) / 2;
-        actual_speed[_Y] = (actual_speed[_Y] + speed_old[_Y]) / 2;
+        actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
+        actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
 
-        speed_old[_X] = actual_speed[_X];
-        speed_old[_Y] = actual_speed[_Y];
+        speed_old[GPS_X] = actual_speed[GPS_X];
+        speed_old[GPS_Y] = actual_speed[GPS_Y];
     }
     init = 1;
 
@@ -559,8 +559,8 @@ static void GPS_calc_nav_rate(int max_speed)
 
     // nav_bearing includes crosstrack
     temp = (9000l - nav_bearing) * RADX100;
-    trig[_X] = cosf(temp);
-    trig[_Y] = sinf(temp);
+    trig[GPS_X] = cosf(temp);
+    trig[GPS_Y] = sinf(temp);
 
     for (axis = 0; axis < 2; axis++) {
         rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];

src/imu.c

@@ -166,9 +166,44 @@ t_fp_vector EstG;
 void rotateV(struct fp_vector *v, float *delta)
 {
     struct fp_vector v_tmp = *v;
+
+#if INACCURATE
     v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
     v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
     v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
+#else
+    // This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
+    float mat[3][3];
+    float cosx, sinx, cosy, siny, cosz, sinz;
+    float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
+
+    cosx = cosf(-delta[PITCH]);
+    sinx = sinf(-delta[PITCH]);
+    cosy = cosf(delta[ROLL]);
+    siny = sinf(delta[ROLL]);
+    cosz = cosf(delta[YAW]);
+    sinz = sinf(delta[YAW]);
+
+    coszcosx = cosz * cosx;
+    coszcosy = cosz * cosy;
+    sinzcosx = sinz * cosx;
+    coszsinx = sinx * cosz;
+    sinzsinx = sinx * sinz;
+
+    mat[0][0] = coszcosy;
+    mat[0][1] = sinz * cosy;
+    mat[0][2] = -siny;
+    mat[1][0] = (coszsinx * siny) - sinzcosx;
+    mat[1][1] = (sinzsinx * siny) + (coszcosx);
+    mat[1][2] = cosy * sinx;
+    mat[2][0] = (coszcosx * siny) + (sinzsinx);
+    mat[2][1] = (sinzcosx * siny) - (coszsinx);
+    mat[2][2] = cosy * cosx;
+
+    v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
+    v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
+    v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
+#endif
 }
 
 static int16_t _atan2f(float y, float x)
@@ -239,8 +274,14 @@ static void getEstimatedAttitude(void)
     }
 
     // Attitude of the estimated vector
+#if INACCURATE
     angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
     angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
+#else
+    // This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
+    angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
+    angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
+#endif
     
 #ifdef MAG
     if (sensors(SENSOR_MAG)) {