Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'multiwii/master'

Browse source 
Conflicts:
	obj/baseflight.hex
	src/sensors.c
This commit is contained in:
Dominic Clifton 2014-04-14 16:18:44 +01:00
commit c493a1579c
12 changed files with 69 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

6
README.md Normal file
View file

@ -0,0 +1,6 @@
baseflight
==========
32 bit fork of the MultiWii RC flight controller firmware
Before making any contributions, take a note of the https://github.com/multiwii/baseflight/wiki/CodingStyle

5
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
static const uint8_t EEPROM_CONF_VERSION = 61;
static const uint8_t EEPROM_CONF_VERSION = 62;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -279,7 +279,8 @@ 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
cfg.throttle_correction_value = 0; // could 10 with althold or 40 for fpv
cfg.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec

22
src/flight_imu.c
View file

@ -21,6 +21,7 @@ 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;
float throttleAngleScale;
// **************
// gyro+acc IMU
@ -36,6 +37,7 @@ void imuInit(void)
{
accZ_25deg = acc_1G * cosf(RAD * 25.0f);
accVelScale = 9.80665f / acc_1G / 10000.0f;
throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);
#ifdef MAG
// if mag sensor is enabled, use it
@ -303,9 +305,19 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_angle_correction) {
int cosZ = ((int32_t)(EstG.V.Z * 100.0f)) / acc_1G;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
if (cfg.throttle_correction_value) {
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
throttleAngleCorrection = 0;
} 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)) ;
}
}
}
@ -360,8 +372,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;

20
src/gps_common.c
View file

@ -91,7 +91,8 @@ void gpsInit(uint8_t baudrateIndex)
mode = MODE_RX;
gpsSetPIDs();
core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrateIndex].baudrate, mode);
// Open GPS UART, no callback - buffer will be read out in gpsThread()
core.gpsport = uartOpen(USART2, NULL, gpsInitData[baudrateIndex].baudrate, mode);
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
}
@ -156,6 +157,12 @@ void gpsInitHardware(void)
void gpsThread(void)
{
// read out available GPS bytes
if (core.gpsport) {
while (serialTotalBytesWaiting(core.gpsport))
gpsNewData(serialRead(core.gpsport));
}
switch (gpsData.state) {
case GPS_UNKNOWN:
break;
@ -534,9 +541,6 @@ int8_t gpsSetPassthrough(void)
if (gpsData.state != GPS_RECEIVINGDATA)
return -1;
// get rid of callback
core.gpsport->callback = NULL;
LED0_OFF;
LED1_OFF;
@ -844,8 +848,10 @@ uint32_t GPS_coord_to_degrees(char* s)
int i;
// scan for decimal point or end of field
for (p = s; isdigit((unsigned char)*p); p++)
;
for (p = s; isdigit((unsigned char)*p); p++) {
if (p >= s + 15)
return 0; // stop potential fail
}
q = s;
// convert degrees
@ -890,6 +896,8 @@ static uint32_t grab_fields(char *src, uint8_t mult)
tmp *= 10;
if (src[i] >= '0' && src[i] <= '9')
tmp += src[i] - '0';
if (i >= 15)
return 0; // out of bounds
}
return tmp;
}

2
src/main.c
View file

@ -88,7 +88,7 @@ int main(void)
break;
default:
pwm_params.adcChannel = 0;
break;
break;
}
pwmInit(&pwm_params);

22
src/mw.c
View file

@ -53,11 +53,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;
@ -101,7 +101,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;
}
@ -141,7 +141,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]
@ -361,7 +361,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
}
@ -768,8 +768,6 @@ void loop(void)
}
} else { // not in rc loop
static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
if (taskOrder > 4)
taskOrder -= 5;
switch (taskOrder) {
case 0:
taskOrder++;
@ -799,7 +797,7 @@ void loop(void)
break;
}
case 4:
taskOrder++;
taskOrder = 0;
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
Sonar_update();
@ -884,7 +882,7 @@ void loop(void)
}
#endif
if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
if (cfg.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE] += throttleAngleCorrection;
}

3
src/mw.h
View file

@ -186,7 +186,8 @@ 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; //
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
// Servo-related stuff
servoParam_t servoConf[8]; // servo configuration

2
src/rx_sbus.c
View file

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

4
src/sensors_acceleration.c
View file

@ -28,8 +28,8 @@ 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

3
src/serial_cli.c
View file

@ -161,7 +161,8 @@ const clivalue_t valueTable[] = {
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
{ "alt_hold_fast_change", VAR_UINT8, &cfg.alt_hold_fast_change, 0, 1 },
{ "throttle_angle_correction", VAR_UINT8, &cfg.throttle_angle_correction, 0, 100 },
{ "throttle_correction_value", VAR_UINT8, &cfg.throttle_correction_value, 0, 150 },
{ "throttle_correction_angle", VAR_UINT16, &cfg.throttle_correction_angle, 1, 900 },
{ "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },