Merge remote-tracking branch 'multiwii/master' into
project-structure-alternative Conflicts: src/sensors.c
This commit is contained in:
commit
3c1ba729b9
|
@ -269,10 +269,12 @@ static void getEstimatedAttitude(void)
|
|||
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
|
||||
|
||||
rotateV(&EstG.V, deltaGyroAngle);
|
||||
if (sensors(SENSOR_MAG))
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
rotateV(&EstM.V, deltaGyroAngle);
|
||||
else
|
||||
} else {
|
||||
rotateV(&EstN.V, deltaGyroAngle);
|
||||
normalizeV(&EstN.V, &EstN.V);
|
||||
}
|
||||
|
||||
// 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.
|
||||
|
@ -311,11 +313,11 @@ static void getEstimatedAttitude(void)
|
|||
|
||||
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
|
||||
throttleAngleCorrection = 0;
|
||||
} else {
|
||||
} else {
|
||||
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
|
||||
if (angle > 900)
|
||||
angle = 900;
|
||||
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / 900.0f * M_PI / 2.0f)) ;
|
||||
throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ;
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -352,7 +354,7 @@ int getEstimatedAltitude(void)
|
|||
if (calibratingB > 0) {
|
||||
baroGroundPressure -= baroGroundPressure / 8;
|
||||
baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
|
||||
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
|
||||
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
|
||||
|
||||
vel = 0;
|
||||
accAlt = 0;
|
||||
|
|
23
src/mw.c
23
src/mw.c
|
@ -60,11 +60,11 @@ uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
|
|||
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
|
||||
|
||||
// Automatic ACC Offset Calibration
|
||||
bool AccInflightCalibrationArmed = false;
|
||||
bool AccInflightCalibrationMeasurementDone = false;
|
||||
bool AccInflightCalibrationSavetoEEProm = false;
|
||||
bool AccInflightCalibrationActive = false;
|
||||
uint16_t InflightcalibratingA = 0;
|
||||
int16_t AccInflightCalibrationArmed;
|
||||
uint16_t AccInflightCalibrationMeasurementDone = 0;
|
||||
uint16_t AccInflightCalibrationSavetoEEProm = 0;
|
||||
uint16_t AccInflightCalibrationActive = 0;
|
||||
|
||||
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
|
||||
{
|
||||
|
@ -120,8 +120,8 @@ void annexCode(void)
|
|||
|
||||
tmp2 = tmp / 100;
|
||||
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
|
||||
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
|
||||
prop1 = (uint16_t) prop1 *prop2 / 100;
|
||||
prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500;
|
||||
prop1 = (uint16_t)prop1 * prop2 / 100;
|
||||
} else { // YAW
|
||||
if (cfg.yawdeadband) {
|
||||
if (tmp > cfg.yawdeadband) {
|
||||
|
@ -558,8 +558,8 @@ void loop(void)
|
|||
// Inflight ACC Calibration
|
||||
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
|
||||
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
|
||||
AccInflightCalibrationMeasurementDone = 0;
|
||||
AccInflightCalibrationSavetoEEProm = 1;
|
||||
AccInflightCalibrationMeasurementDone = false;
|
||||
AccInflightCalibrationSavetoEEProm = true;
|
||||
} else {
|
||||
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
|
||||
if (AccInflightCalibrationArmed) {
|
||||
|
@ -621,14 +621,15 @@ void loop(void)
|
|||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationArmed = 0;
|
||||
AccInflightCalibrationArmed = false;
|
||||
}
|
||||
if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
|
||||
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationActive = true;
|
||||
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
|
||||
AccInflightCalibrationMeasurementDone = 0;
|
||||
AccInflightCalibrationSavetoEEProm = 1;
|
||||
AccInflightCalibrationMeasurementDone = false;
|
||||
AccInflightCalibrationSavetoEEProm = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -45,7 +45,7 @@ static void spektrumDataReceive(uint16_t c)
|
|||
{
|
||||
uint32_t spekTime;
|
||||
static uint32_t spekTimeLast, spekTimeInterval;
|
||||
static uint8_t spekFramePosition;
|
||||
static uint8_t spekFramePosition;
|
||||
|
||||
spekDataIncoming = true;
|
||||
spekTime = micros();
|
||||
|
|
|
@ -4,10 +4,10 @@
|
|||
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
|
||||
|
||||
extern uint16_t InflightcalibratingA;
|
||||
extern int16_t AccInflightCalibrationArmed;
|
||||
extern uint16_t AccInflightCalibrationMeasurementDone;
|
||||
extern uint16_t AccInflightCalibrationSavetoEEProm;
|
||||
extern uint16_t AccInflightCalibrationActive;
|
||||
extern bool AccInflightCalibrationArmed;
|
||||
extern bool AccInflightCalibrationMeasurementDone;
|
||||
extern bool AccInflightCalibrationSavetoEEProm;
|
||||
extern bool AccInflightCalibrationActive;
|
||||
|
||||
void ACC_Common(void)
|
||||
{
|
||||
|
@ -62,8 +62,8 @@ void ACC_Common(void)
|
|||
}
|
||||
// all values are measured
|
||||
if (InflightcalibratingA == 1) {
|
||||
AccInflightCalibrationActive = 0;
|
||||
AccInflightCalibrationMeasurementDone = 1;
|
||||
AccInflightCalibrationActive = false;
|
||||
AccInflightCalibrationMeasurementDone = true;
|
||||
toggleBeep = 2; // buzzer for indicatiing the end of calibration
|
||||
// recover saved values to maintain current flight behavior until new values are transferred
|
||||
mcfg.accZero[ROLL] = accZero_saved[ROLL];
|
||||
|
@ -75,8 +75,8 @@ void ACC_Common(void)
|
|||
InflightcalibratingA--;
|
||||
}
|
||||
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
||||
if (AccInflightCalibrationSavetoEEProm == 1) { // the copter is landed, disarmed and the combo has been done again
|
||||
AccInflightCalibrationSavetoEEProm = 0;
|
||||
if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again
|
||||
AccInflightCalibrationSavetoEEProm = false;
|
||||
mcfg.accZero[ROLL] = b[ROLL] / 50;
|
||||
mcfg.accZero[PITCH] = b[PITCH] / 50;
|
||||
mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
** File : stm32_flash.ld
|
||||
**
|
||||
** Abstract : Linker script for STM32F103C8 Device with
|
||||
** 64KByte FLASH, 20KByte RAM
|
||||
** 128KByte FLASH, 20KByte RAM
|
||||
**
|
||||
*****************************************************************************
|
||||
*/
|
||||
|
@ -13,7 +13,7 @@
|
|||
ENTRY(Reset_Handler)
|
||||
|
||||
/* Highest address of the user mode stack */
|
||||
_estack = 0x20005000; /* end of 10K RAM */
|
||||
_estack = 0x20005000; /* end of 20K RAM */
|
||||
|
||||
/* Generate a link error if heap and stack don't fit into RAM */
|
||||
_Min_Heap_Size = 0; /* required amount of heap */
|
||||
|
|
Loading…
Reference in New Issue