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

Merge remote-tracking branch 'multiwii/upstream' 

Conflicts:
	src/drv_system.c
	src/main.c
This commit is contained in:
Dominic Clifton 2013-10-11 21:39:40 +01:00
parent 767babed1d 3bb1063001
commit d8e9282f04
11 changed files with 3348 additions and 3229 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6399
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

4
src/board.h
View File

@ -237,8 +237,8 @@ typedef struct baro_t
#ifdef BEEP_GPIO
#define BEEP_TOGGLE digitalToggle(BEEP_GPIO, BEEP_PIN);
#define BEEP_OFF digitalHi(BEEP_GPIO, BEEP_PIN);
#define BEEP_ON digitalLo(BEEP_GPIO, BEEP_PIN);
#define BEEP_OFF systemBeep(false);
#define BEEP_ON systemBeep(true);
#else
#define BEEP_TOGGLE ;
#define BEEP_OFF ;

2
src/drv_serial.c
View File

@ -3,7 +3,7 @@
void serialPrint(serialPort_t *instance, const char *str)
{
uint8_t ch;
while ((ch = *(str++))) {
while ((ch = *(str++)) != 0) {
serialWrite(instance, ch);
}
}

2
src/drv_softserial.c
View File

@ -17,7 +17,7 @@ void onSerialTimer(uint8_t portIndex, uint16_t capture);
uint8_t readRxSignal(softSerial_t *softSerial)
{
return digitalIn(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin);
return !(digitalIn(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin) == 0);
}
void setTxSignal(softSerial_t *softSerial, uint8_t state)

33
src/drv_system.c
View File

@ -6,6 +6,10 @@ static volatile uint32_t usTicks = 0;
static volatile uint32_t sysTickUptime = 0;
// from system_stm32f10x.c
void SetSysClock(void);
void systemBeep(bool onoff);
static void beepRev4(bool onoff);
static void beepRev5(bool onoff);
void (* systemBeepPtr)(bool onoff) = NULL;
static void cycleCounterInit(void)
{
@ -89,6 +93,12 @@ void systemInit(void)
AFIO->MAPR |= AFIO_MAPR_SWJ_CFG_NO_JTAG_SW;
// Configure gpio
// rev5 needs inverted beeper. oops.
if (hse_value == 12000000)
systemBeepPtr = beepRev5;
else
systemBeepPtr = beepRev4;
LED0_OFF;
LED1_OFF;
BEEP_OFF;
@ -182,6 +192,29 @@ void systemReset(bool toBootloader)
// Generate system reset
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
static void beepRev4(bool onoff)
{
if (onoff) {
digitalLo(BEEP_GPIO, BEEP_PIN);
} else {
digitalHi(BEEP_GPIO, BEEP_PIN);
}
}
static void beepRev5(bool onoff)
{
if (onoff) {
digitalHi(BEEP_GPIO, BEEP_PIN);
} else {
digitalLo(BEEP_GPIO, BEEP_PIN);
}
}
void systemBeep(bool onoff)
{
systemBeepPtr(onoff);
}
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
{
switch (rotation) {

44
src/imu.c
View File

@ -222,11 +222,31 @@ void accSum_reset(void)
accTimeSum = 0;
}
// baseflight calculation by Luggi09 originates from arducopter
static int16_t calculateHeading(t_fp_vector *vec)
{
int16_t head;
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
head = lrintf(hd);
if (head < 0)
head += 360;
return head;
}
static void getEstimatedAttitude(void)
{
uint32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1000.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
@ -252,6 +272,8 @@ static void getEstimatedAttitude(void)
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle);
else
rotateV(&EstN.V, deltaGyroAngle);
// 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.
@ -277,28 +299,16 @@ static void getEstimatedAttitude(void)
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
#ifdef MAG
if (sensors(SENSOR_MAG)) {
// baseflight calculation by Luggi09 originates from arducopter
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstM.A[X] * cosinePitch + EstM.A[Y] * sineRoll * sinePitch + EstM.A[Z] * sinePitch * cosineRoll;
float Yh = EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = lrintf(hd);
if (heading < 0)
heading += 360;
}
#endif
if (sensors(SENSOR_MAG))
heading = calculateHeading(&EstM);
else
heading = calculateHeading(&EstN);
acc_calc(deltaT); // rotate acc vector into earth frame
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;
}
}
@ -378,7 +388,7 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
vel = constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D
vel_tmp = vel;

6
src/main.c
View File

@ -138,9 +138,9 @@ int main(void)
previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
// loopy

45
src/mw.c
View File

@ -87,14 +87,17 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp, tmp2;
static uint8_t buzzerFreq; //delay between buzzer ring
int32_t tmp, tmp2;
int32_t axis, prop1, prop2;
static uint8_t buzzerFreq; // delay between buzzer ring
// vbat shit
static uint8_t vbatTimer = 0;
uint8_t axis, prop1, prop2;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
uint8_t i;
int i;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < BREAKPOINT) {
@ -123,7 +126,6 @@ void annexCode(void)
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
tmp *= -mcfg.yaw_control_direction; //change control direction for yaw needed with new gyro orientation
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
@ -131,12 +133,12 @@ void annexCode(void)
tmp = 0;
}
}
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
rcCommand[axis] = tmp * -mcfg.yaw_control_direction;
prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500;
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t) cfg.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < mcfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
@ -435,28 +437,25 @@ void loop(void)
uint16_t auxState = 0;
static uint8_t GPSNavReset = 1;
bool isThrottleLow = false;
bool rcReady = false;
// calculate rc stuff from serial-based receivers (spek/sbus)
if (feature(FEATURE_SERIALRX)) {
bool ready = false;
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
ready = spektrumFrameComplete();
rcReady = spektrumFrameComplete();
break;
case SERIALRX_SBUS:
ready = sbusFrameComplete();
rcReady = sbusFrameComplete();
break;
}
if (ready)
computeRC();
}
if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
rcReady = false;
rcTime = currentTime + 20000;
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SERIALRX))
computeRC();
computeRC();
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
@ -539,11 +538,13 @@ void loop(void)
i = 0;
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
calibratingG = 1000;
calibratingG = CALIBRATING_GYRO_CYCLES;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
if (sensors(SENSOR_BARO))
calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
if (!sensors(SENSOR_MAG))
heading = 0; // reset heading to zero after gyro calibration
// 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
@ -581,7 +582,7 @@ void loop(void)
mwArm();
// Calibrating Acc
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = 400;
calibratingA = CALIBRATING_ACC_CYCLES;
// Calibrating Mag
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
f.CALIBRATE_MAG = 1;
@ -686,7 +687,7 @@ void loop(void)
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;

5
src/mw.h
View File

@ -141,6 +141,10 @@ enum {
ALIGN_MAG = 2
};
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_CYCLES 200
typedef struct config_t {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PIDITEMS];
@ -457,6 +461,7 @@ bool sbusFrameComplete(void);
// buzzer
void buzzer(uint8_t warn_vbat);
void systemBeep(bool onoff);
// cli
void cliProcess(void);

21
src/sensors.c
View File

@ -124,7 +124,10 @@ retry:
// calculate magnetic declination
deg = cfg.mag_declination / 100;
min = cfg.mag_declination % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
if (sensors(SENSOR_MAG))
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
else
magneticDeclination = 0.0f;
}
#endif
@ -165,9 +168,9 @@ static void ACC_Common(void)
if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400)
if (calibratingA == CALIBRATING_ACC_CYCLES)
a[axis] = 0;
// Sum up 400 readings
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
@ -175,9 +178,9 @@ static void ACC_Common(void)
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
mcfg.accZero[ROLL] = a[ROLL] / 400;
mcfg.accZero[PITCH] = a[PITCH] / 400;
mcfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeEEPROM(1, true); // write accZero in EEPROM
@ -334,7 +337,7 @@ static void GYRO_Common(void)
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == 1000) {
if (calibratingG == CALIBRATING_GYRO_CYCLES) {
g[axis] = 0;
devClear(&var[axis]);
}
@ -348,14 +351,14 @@ static void GYRO_Common(void)
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
calibratingG = 1000;
calibratingG = CALIBRATING_GYRO_CYCLES;
devClear(&var[0]);
devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0;
continue;
}
gyroZero[axis] = g[axis] / 1000;
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLED(10, 15, 1);
}
}

16
src/serial.c
View File

@ -267,6 +267,7 @@ void serializeBoxNamesReply(void)
void serialInit(uint32_t baudrate)
{
int idx;
bool hfadded = false;
core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
// TODO fix/hax
@ -279,6 +280,10 @@ void serialInit(uint32_t baudrate)
if (sensors(SENSOR_ACC)) {
availableBoxes[idx++] = BOXANGLE;
availableBoxes[idx++] = BOXHORIZON;
availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ;
hfadded = true;
}
if (sensors(SENSOR_BARO)) {
availableBoxes[idx++] = BOXBARO;
@ -286,9 +291,12 @@ void serialInit(uint32_t baudrate)
availableBoxes[idx++] = BOXVARIO;
}
if (sensors(SENSOR_MAG)) {
availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ;
// this really shouldn't even needed to be tested as it wouldn't be possible without acc anyway
if (!hfadded) {
availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ;
}
}
if (feature(FEATURE_SERVO_TILT))
availableBoxes[idx++] = BOXCAMSTAB;
@ -606,7 +614,7 @@ static void evaluateCommand(void)
break;
case MSP_ACC_CALIBRATION:
if (!f.ARMED)
calibratingA = 400;
calibratingA = CALIBRATING_ACC_CYCLES;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION: