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
This commit is contained in:
parent
4a4c0d585d
commit
005308b430
File diff suppressed because it is too large
Load Diff
5920
obj/baseflight.hex
5920
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
20
src/gps.c
20
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
|
||||
|
||||
|
@ -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];
|
||||
|
|
41
src/imu.c
41
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)) {
|
||||
|
|
Loading…
Reference in New Issue