diff --git a/Makefile b/Makefile index 8ed351bfa0..99bca1d953 100644 --- a/Makefile +++ b/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 \ diff --git a/src/board.h b/src/board.h index aea5f74da0..aa513e1f42 100755 --- a/src/board.h +++ b/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 diff --git a/src/cli.c b/src/cli.c index fdd6fcb260..b34aaf6521 100644 --- a/src/cli.c +++ b/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 }, diff --git a/src/config.c b/src/config.c index e79866ee9f..12556fca3d 100755 --- a/src/config.c +++ b/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 diff --git a/src/drv_hcsr04.c b/src/drv_hcsr04.c index a04a7cc9b3..1dc7821b48 100644 --- a/src/drv_hcsr04.c +++ b/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(); diff --git a/src/drv_hcsr04.h b/src/drv_hcsr04.h index 2986f976b1..565b6c0f4b 100644 --- a/src/drv_hcsr04.h +++ b/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); diff --git a/src/drv_system.c b/src/drv_system.c index 8f21648f6c..db0fbc0896 100755 --- a/src/drv_system.c +++ b/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(); diff --git a/src/gps.c b/src/gps.c index 7e559bad65..e58be8812e 100644 --- a/src/gps.c +++ b/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 diff --git a/src/imu.c b/src/imu.c index 5a2a4167d8..8470b4f26d 100755 --- a/src/imu.c +++ b/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; -int16_t vario = 0; // variometer in cm/s -float magneticDeclination = 0.0f; // calculated at startup from config +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; } diff --git a/src/mw.c b/src/mw.c index 6a9f6600bc..dffb084ded 100755 --- a/src/mw.c +++ b/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); diff --git a/src/mw.h b/src/mw.h index 081416e387..cc94b06f9a 100755 --- a/src/mw.h +++ b/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 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]; diff --git a/src/serial.c b/src/serial.c index ddf9857550..258843cfc7 100755 --- a/src/serial.c +++ b/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;