Permalink
Browse files

Applied patch from trunet for running at 64MHz off HSI for broken Chi…

…nese 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
  • Loading branch information...
1 parent 4a4c0d5 commit 005308b430a11089810eb3b7efc166a97d6ceab0 timecop@gmail.com committed Oct 6, 2012
Showing with 3,106 additions and 3,839 deletions.
  1. +51 −889 lib/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
  2. +2,988 −2,932 obj/baseflight.hex
  3. +2 −2 src/buzzer.c
  4. +2 −2 src/cli.c
  5. +5 −0 src/drv_mpu6050.c
  6. +1 −1 src/drv_pwm.c
  7. +3 −0 src/drv_system.c
  8. +1 −1 src/drv_uart.c
  9. +11 −11 src/gps.c
  10. +42 −1 src/imu.c

Large diffs are not rendered by default.

Oops, something went wrong.
View

Large diffs are not rendered by default.

Oops, something went wrong.
View
@@ -73,7 +73,7 @@ void beep_code(char first, char second, char third, char pause)
char patternChar[4];
uint16_t Duration;
static uint8_t icnt = 0;
-
+
patternChar[0] = first;
patternChar[1] = second;
patternChar[2] = third;
@@ -95,7 +95,7 @@ void beep_code(char first, char second, char third, char pause)
Duration = 50;
break;
}
-
+
if (icnt < 3 && Duration != 0)
beep(Duration);
if (icnt >= 3 && (buzzerLastToggleTime < millis() - Duration)) {
View
@@ -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)
View
@@ -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;
View
@@ -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);
View
@@ -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);
View
@@ -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;
View
@@ -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];
View
@@ -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,9 +274,15 @@ 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)) {
// Attitude of the cross product vector GxM

0 comments on commit 005308b

Please sign in to comment.