commit
11cd1ed39b
|
@ -191,7 +191,7 @@ void acc_calc(uint32_t deltaT)
|
|||
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
|
||||
} else
|
||||
accel_ned.V.Z -= acc_1G;
|
||||
|
||||
|
||||
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
|
||||
|
||||
// apply Deadband to reduce integration drift and vibration influence
|
||||
|
@ -339,7 +339,7 @@ int getEstimatedAltitude(void)
|
|||
baroGroundPressure -= baroGroundPressure / 8;
|
||||
baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
|
||||
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
|
||||
|
||||
|
||||
vel = 0;
|
||||
accAlt = 0;
|
||||
calibratingB--;
|
||||
|
@ -358,8 +358,8 @@ int getEstimatedAltitude(void)
|
|||
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
|
||||
|
||||
// Integrator - Altitude in cm
|
||||
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
|
||||
accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
|
||||
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
|
||||
accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
|
||||
EstAlt = accAlt;
|
||||
vel += vel_acc;
|
||||
|
||||
|
|
|
@ -88,7 +88,7 @@ int main(void)
|
|||
break;
|
||||
default:
|
||||
pwm_params.adcChannel = 0;
|
||||
break;
|
||||
break;
|
||||
}
|
||||
|
||||
pwmInit(&pwm_params);
|
||||
|
|
16
src/mw.c
16
src/mw.c
|
@ -51,11 +51,11 @@ uint16_t GPS_ground_course = 0; // degrees * 10
|
|||
int16_t nav[2];
|
||||
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
|
||||
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
|
||||
uint8_t GPS_numCh; // Number of channels
|
||||
uint8_t GPS_svinfo_chn[16]; // Channel number
|
||||
uint8_t GPS_svinfo_svid[16]; // Satellite ID
|
||||
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
|
||||
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
|
||||
uint8_t GPS_numCh; // Number of channels
|
||||
uint8_t GPS_svinfo_chn[16]; // Channel number
|
||||
uint8_t GPS_svinfo_svid[16]; // Satellite ID
|
||||
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
|
||||
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
|
||||
|
||||
// Automatic ACC Offset Calibration
|
||||
uint16_t InflightcalibratingA = 0;
|
||||
|
@ -103,7 +103,7 @@ void annexCode(void)
|
|||
prop2 = 100;
|
||||
} else {
|
||||
if (rcData[THROTTLE] < 2000) {
|
||||
prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpaBreakPoint) / (2000 - cfg.tpaBreakPoint);
|
||||
prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpaBreakPoint) / (2000 - cfg.tpaBreakPoint);
|
||||
} else {
|
||||
prop2 = 100 - cfg.dynThrPID;
|
||||
}
|
||||
|
@ -143,7 +143,7 @@ void annexCode(void)
|
|||
}
|
||||
|
||||
tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000);
|
||||
tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000]
|
||||
tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000]
|
||||
tmp2 = tmp / 100;
|
||||
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
|
||||
|
||||
|
@ -363,7 +363,7 @@ static void pidRewrite(void)
|
|||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// -----Get the desired angle rate depending on flight mode
|
||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
|
||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
|
||||
// calculate error and limit the angle to max configured inclination
|
||||
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
|
||||
}
|
||||
|
|
10
src/sbus.c
10
src/sbus.c
|
@ -20,7 +20,7 @@ static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
|
|||
void sbusInit(rcReadRawDataPtr *callback)
|
||||
{
|
||||
int b;
|
||||
for (b = 0; b < SBUS_MAX_CHANNEL; b ++)
|
||||
for (b = 0; b < SBUS_MAX_CHANNEL; b++)
|
||||
sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET);
|
||||
core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS));
|
||||
if (callback)
|
||||
|
@ -49,9 +49,9 @@ typedef union
|
|||
uint8_t in[SBUS_FRAME_SIZE];
|
||||
struct sbus_dat msg;
|
||||
} sbus_msg;
|
||||
|
||||
|
||||
static sbus_msg sbus;
|
||||
|
||||
|
||||
// Receive ISR callback
|
||||
static void sbusDataReceive(uint16_t c)
|
||||
{
|
||||
|
@ -63,12 +63,12 @@ static void sbusDataReceive(uint16_t c)
|
|||
if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing
|
||||
sbusFramePosition = 0;
|
||||
sbusTimeLast = sbusTime;
|
||||
|
||||
|
||||
if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE)
|
||||
return;
|
||||
|
||||
sbusFrameDone = false; // lazy main loop didnt fetch the stuff
|
||||
if (sbusFramePosition != 0)
|
||||
if (sbusFramePosition != 0)
|
||||
sbus.in[sbusFramePosition - 1] = (uint8_t)c;
|
||||
|
||||
if (sbusFramePosition == SBUS_FRAME_SIZE - 1) {
|
||||
|
|
|
@ -179,8 +179,8 @@ 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] + (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;
|
||||
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
|
||||
|
@ -264,7 +264,6 @@ void Baro_Common(void)
|
|||
baroHistIdx = indexplus1;
|
||||
}
|
||||
|
||||
|
||||
int Baro_update(void)
|
||||
{
|
||||
static uint32_t baroDeadline = 0;
|
||||
|
|
|
@ -16,7 +16,7 @@ static uint32_t sumdChannelData[SUMD_MAX_CHANNEL];
|
|||
void sumdInit(rcReadRawDataPtr *callback)
|
||||
{
|
||||
core.rcvrport = uartOpen(USART2, sumdDataReceive, 115200, MODE_RX);
|
||||
if (callback)
|
||||
if (callback)
|
||||
*callback = sumdReadRawRC;
|
||||
}
|
||||
|
||||
|
@ -60,7 +60,7 @@ bool sumdFrameComplete(void)
|
|||
sumdFrameDone = false;
|
||||
if (sumd[1] == 0x01) {
|
||||
failsafeCnt = 0;
|
||||
if (sumdSize > SUMD_MAX_CHANNEL)
|
||||
if (sumdSize > SUMD_MAX_CHANNEL)
|
||||
sumdSize = SUMD_MAX_CHANNEL;
|
||||
for (b = 0; b < sumdSize; b++)
|
||||
sumdChannelData[b] = ((sumd[2 * b + 3] << 8) | sumd[2 * b + 4]);
|
||||
|
|
|
@ -30,7 +30,7 @@ void initBoardAlignment(void)
|
|||
roll = mcfg.board_align_roll * M_PI / 180.0f;
|
||||
pitch = mcfg.board_align_pitch * M_PI / 180.0f;
|
||||
yaw = mcfg.board_align_yaw * M_PI / 180.0f;
|
||||
|
||||
|
||||
cosx = cosf(roll);
|
||||
sinx = sinf(roll);
|
||||
cosy = cosf(pitch);
|
||||
|
@ -48,11 +48,11 @@ void initBoardAlignment(void)
|
|||
boardRotation[0][0] = coszcosy;
|
||||
boardRotation[0][1] = -cosy * sinz;
|
||||
boardRotation[0][2] = siny;
|
||||
|
||||
|
||||
boardRotation[1][0] = sinzcosx + (coszsinx * siny);
|
||||
boardRotation[1][1] = coszcosx - (sinzsinx * siny);
|
||||
boardRotation[1][2] = -sinx * cosy;
|
||||
|
||||
|
||||
boardRotation[2][0] = (sinzsinx) - (coszcosx * siny);
|
||||
boardRotation[2][1] = (coszsinx) + (sinzcosx * siny);
|
||||
boardRotation[2][2] = cosy * cosx;
|
||||
|
|
Loading…
Reference in New Issue