rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
timecop@gmail.com 2013-08-19 23:29:20 +00:00
parent 3b8c1841f8
commit 0664b3005d
12 changed files with 59 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Makefile
View File

@ -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 \

6
src/board.h
View File

@ -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

1
src/cli.c
View File

@ -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 },

1
src/config.c
View File

@ -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

4
src/drv_hcsr04.c
View File

@ -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();

2
src/drv_hcsr04.h
View File

@ -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);

5
src/drv_system.c
View File

@ -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();

1
src/gps.c
View File

@ -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

58
src/imu.c
View File

@ -7,12 +7,13 @@ 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;
int16_t vario = 0; // variometer in cm/s
int32_t errorAltitudeI = 0;
int32_t vario = 0; // variometer in cm/s
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;
int16_t baroVel;
int16_t accZ;
static int16_t accZoffset = 0;
int32_t error;
int32_t baroVel;
int32_t accZ;
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);
error16 = applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error16 >> 7), -150, +150);
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
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;
}

5
src/mw.c
View File

@ -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);

11
src/mw.h
View File

@ -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 int16_t BaroPID;
extern int16_t vario;
extern int32_t errorAltitudeI;
extern int32_t BaroPID;
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];

2
src/serial.c
View File

@ -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;