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

format code properly 

match the comment from pullrequest about spacing
remains : some hand alignment for comment and wrong /** */ usage.
Conflicts:

	src/board.h
	src/buzzer.c
	src/config.c
	src/drivers/serial_common.h
	src/drivers/system_common.c
	src/drv_gpio.h
	src/drv_pwm.c
	src/drv_timer.c
	src/drv_uart.c
	src/flight_imu.c
	src/mw.c
	src/serial_cli.c
This commit is contained in:
treymarc 2014-05-08 00:36:19 +00:00 committed by Dominic Clifton
parent ab2273f93e
commit cabc57774c
27 changed files with 141 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/buzzer.c
View File

@ -113,7 +113,7 @@ void beep_code(char first, char second, char third, char pause)
patternChar[1] = second;
patternChar[2] = third;
patternChar[3] = pause;
switch(patternChar[icnt]) {
switch (patternChar[icnt]) {
case 'M':
Duration = 100;
break;

41
src/common/typeconversion.c
View File

@ -93,18 +93,18 @@ char a2i(char ch, char **src, int base, int *nump)
#ifndef HAVE_ITOA_FUNCTION
/*
** The following two functions together make up an itoa()
** implementation. Function i2a() is a 'private' function
** called by the public itoa() function.
**
** itoa() takes three arguments:
** 1) the integer to be converted,
** 2) a pointer to a character conversion buffer,
** 3) the radix for the conversion
** which can range between 2 and 36 inclusive
** range errors on the radix default it to base10
** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1
*/
** The following two functions together make up an itoa()
** implementation. Function i2a() is a 'private' function
** called by the public itoa() function.
**
** itoa() takes three arguments:
** 1) the integer to be converted,
** 2) a pointer to a character conversion buffer,
** 3) the radix for the conversion
** which can range between 2 and 36 inclusive
** range errors on the radix default it to base10
** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1
*/
static char *_i2a(unsigned i, char *a, unsigned r)
{
@ -120,7 +120,7 @@ char *itoa(int i, char *a, int r)
r = 10;
if (i < 0) {
*a = '-';
*_i2a(-(unsigned)i, a + 1, r) = 0;
*_i2a(-(unsigned) i, a + 1, r) = 0;
} else
*_i2a(i, a, r) = 0;
return a;
@ -141,7 +141,7 @@ char *ftoa(float x, char *floatString)
else
x -= 0.0005f;
value = (int32_t) (x * 1000.0f); // Convert float * 1000 to an integer
value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer
itoa(abs(value), intString1, 10); // Create string from abs of integer value
@ -176,7 +176,6 @@ char *ftoa(float x, char *floatString)
return floatString;
}
// Simple and fast atof (ascii to float) function.
//
// - Executes about 5x faster than standard MSCRT library atof().
@ -194,7 +193,7 @@ float fastA2F(const char *p)
float sign, value, scale;
// Skip leading white space, if any.
while (white_space(*p) ) {
while (white_space(*p)) {
p += 1;
}
@ -254,8 +253,14 @@ float fastA2F(const char *p)
// Calculate scaling factor.
// while (expon >= 50) { scale *= 1E50f; expon -= 50; }
while (expon >= 8) { scale *= 1E8f; expon -= 8; }
while (expon > 0) { scale *= 10.0f; expon -= 1; }
while (expon >= 8) {
scale *= 1E8f;
expon -= 8;
}
while (expon > 0) {
scale *= 10.0f;
expon -= 1;
}
}
// Return signed and scaled floating point result.

59
src/config.c
View File

@ -37,22 +37,21 @@
#include "config_master.h"
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
#ifndef FLASH_PAGE_COUNT
#define FLASH_PAGE_COUNT 128
#endif
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
master_t masterConfig; // master config struct with data independent from profiles
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
master_t masterConfig; // master config struct with data independent from profiles
profile_t currentProfile; // profile config struct
static const uint8_t EEPROM_CONF_VERSION = 66;
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
{
accelerometerTrims->trims.pitch = 0;
@ -168,10 +167,10 @@ static void resetConf(void)
featureSet(FEATURE_VBAT);
// global settings
masterConfig.current_profile_index = 0; // default profile
masterConfig.gyro_cmpf_factor = 600; // default MWC
masterConfig.gyro_cmpfm_factor = 250; // default MWC
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
masterConfig.current_profile_index = 0; // default profile
masterConfig.gyro_cmpf_factor = 600; // default MWC
masterConfig.gyro_cmpfm_factor = 250; // default MWC
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
resetAccelerometerTrims(&masterConfig.accZero);
@ -195,7 +194,7 @@ static void resetConf(void)
masterConfig.rxConfig.midrc = 1500;
masterConfig.rxConfig.mincheck = 1100;
masterConfig.rxConfig.maxcheck = 1900;
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
masterConfig.airplaneConfig.flaps_speed = 0;
masterConfig.fixedwing_althold_dir = 1;
@ -250,10 +249,10 @@ static void resetConf(void)
currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
// servos
for (i = 0; i < 8; i++) {
@ -293,7 +292,7 @@ static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
static bool isEEPROMContentValid(void)
{
const master_t *temp = (const master_t *)FLASH_WRITE_ADDR;
const master_t *temp = (const master_t *) FLASH_WRITE_ADDR;
uint8_t checksum = 0;
// check version number
@ -305,7 +304,7 @@ static bool isEEPROMContentValid(void)
return false;
// verify integrity of temporary copy
checksum = calculateChecksum((const uint8_t *)temp, sizeof(master_t));
checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
if (checksum != 0)
return false;
@ -326,21 +325,22 @@ void activateConfig(void)
useFailsafeConfig(&currentProfile.failsafeConfig);
setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs(
currentProfile.servoConf,
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig,
&currentProfile.mixerConfig,
&masterConfig.airplaneConfig,
&masterConfig.rxConfig,
&currentProfile.gimbalConfig
);
currentProfile.servoConf,
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig,
&currentProfile.mixerConfig,
&masterConfig.airplaneConfig,
&masterConfig.rxConfig,
&currentProfile.gimbalConfig
);
#ifdef BARO
useBarometerConfig(&currentProfile.barometerConfig);
#endif
}
void validateAndFixConfig(void) {
void validateAndFixConfig(void)
{
if (!(feature(FEATURE_PARALLEL_PWM) || feature(FEATURE_PPM) || feature(FEATURE_SERIALRX))) {
featureSet(FEATURE_PARALLEL_PWM); // Consider changing the default to PPM
}
@ -390,7 +390,7 @@ void readEEPROM(void)
failureMode(10);
// Read flash
memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
memcpy(&masterConfig, (char *) FLASH_WRITE_ADDR, sizeof(master_t));
// Copy current profile
if (masterConfig.current_profile_index > 2) // sanity check
masterConfig.current_profile_index = 0;
@ -425,7 +425,7 @@ void writeEEPROM(void)
masterConfig.magic_be = 0xBE;
masterConfig.magic_ef = 0xEF;
masterConfig.chk = 0; // erase checksum before recalculating
masterConfig.chk = calculateChecksum((const uint8_t *)&masterConfig, sizeof(master_t));
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
// write it
FLASH_Unlock();
@ -438,7 +438,8 @@ void writeEEPROM(void)
#endif
status = FLASH_ErasePage(FLASH_WRITE_ADDR);
for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset));
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset,
*(uint32_t *) ((char *) &masterConfig + wordOffset));
}
if (status == FLASH_COMPLETE) {
break;

2
src/drivers/accgyro_adxl345.c
View File

@ -66,7 +66,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
static void adxl345Init(void)
{
if (useFifo) {
if (useFifo) {
uint8_t fifoDepth = 16;
i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);

10
src/drivers/accgyro_common.h
View File

@ -2,19 +2,17 @@
extern uint16_t acc_1G;
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef struct gyro_s
{
typedef struct gyro_s {
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor
} gyro_t;
typedef struct acc_s
{
typedef struct acc_s {
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
char revisionCode; // a revision code for the sensor, if known

8
src/drivers/barometer_bmp085.c
View File

@ -39,7 +39,7 @@ typedef struct {
int16_t md;
} bmp085_smd500_calibration_param_t;
typedef struct {
typedef struct {
bmp085_smd500_calibration_param_t cal_param;
uint8_t mode;
uint8_t chip_id, ml_version, al_version;
@ -198,9 +198,9 @@ static int32_t bmp085_get_pressure(uint32_t up)
// *****calculate B4************
x1 = (bmp085.cal_param.ac3 * b6) >> 13;
x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12) ) >> 16;
x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
b4 = (bmp085.cal_param.ac4 * (uint32_t) (x3 + 32768)) >> 15;
b4 = (bmp085.cal_param.ac4 * (uint32_t)(x3 + 32768)) >> 15;
b7 = ((uint32_t)(up - b3) * (50000 >> bmp085.oversampling_setting));
if (b7 < 0x80000000) {
@ -258,7 +258,7 @@ static void bmp085_get_up(void)
convOverrun++;
i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting);
bmp085_up = (((uint32_t)data[0] << 16) | ((uint32_t)data[1] << 8) | (uint32_t)data[2]) >> (8 - bmp085.oversampling_setting);
}
static void bmp085_calculate(int32_t *pressure, int32_t *temperature)

7
src/drivers/barometer_common.h
View File

@ -1,10 +1,9 @@
#pragma once
typedef void (* baroOpFuncPtr)(void); // baro start operation
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef void (*baroOpFuncPtr)(void); // baro start operation
typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef struct baro_t
{
typedef struct baro_t {
uint16_t ut_delay;
uint16_t up_delay;
baroOpFuncPtr start_ut;

8
src/drivers/barometer_ms5611.c
View File

@ -165,11 +165,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
{
uint32_t press;
int64_t temp;
int64_t delt;
int64_t delt;
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
if (temp < 2000) { // temperature lower than 20degC
delt = temp - 2000;
@ -179,11 +179,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
if (temp < -1500) { // temperature lower than -15degC
delt = temp + 1500;
delt = delt * delt;
off -= 7 * delt;
off -= 7 * delt;
sens -= (11 * delt) >> 1;
}
}
press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15;
if (pressure)
*pressure = press;

2
src/drivers/gpio_stm32f10x.c
View File

@ -77,7 +77,7 @@ void gpioPinRemapConfig(uint32_t remap, bool enable)
}
if (enable)
tmpreg |= (tmp << ((remap >> 0x15)*0x10));
tmpreg |= (tmp << ((remap >> 0x15) * 0x10));
if ((remap & 0x80000000) == 0x80000000)
AFIO->MAPR2 = tmpreg;

8
src/drivers/pwm_fy90q.c
View File

@ -56,7 +56,7 @@ static struct PWM_State {
uint16_t capture;
} Inputs[8] = { { 0, } };
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
static bool usePPMFlag = false;
static uint8_t numOutputChannels = 0;
@ -158,11 +158,11 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
static void pwmInitializeInput(bool usePPM)
{
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
uint8_t i;
// Input pins
// Input pins
if (usePPM) {
// Configure TIM2_CH1 for PPM input
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
@ -246,7 +246,7 @@ static void pwmInitializeInput(bool usePPM)
void pwmInit(drv_pwm_config_t *init)
{
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
TIM_OCInitTypeDef TIM_OCInitStructure = { 0, };
uint8_t i;

2
src/drivers/pwm_output.c
View File

@ -14,7 +14,7 @@
#include "pwm_output.h"
typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
typedef struct {
#ifdef STM32F303xC

15
src/drivers/serial_common.h
View File

@ -7,17 +7,18 @@ typedef enum {
} serialInversion_e;
typedef enum portMode_t {
MODE_RX = 1 << 0,
MODE_RX = 1 << 0,
MODE_TX = 1 << 1,
MODE_RXTX = MODE_RX | MODE_TX,
MODE_SBUS = 1 << 2,
} portMode_t;
typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
typedef struct serialPort {
const struct serialPortVTable *vTable;
uint8_t identifier;
portMode_t mode;
serialInversion_e inversion;
@ -38,14 +39,14 @@ typedef struct serialPort {
struct serialPortVTable {
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance);
uint8_t (*serialRead)(serialPort_t *instance);
// Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use.
void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
void (*setMode)(serialPort_t *instance, portMode_t mode);

4
src/drivers/serial_softserial.c
View File

@ -91,7 +91,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t
static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
{
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = channel;
@ -231,7 +231,7 @@ void applyChangedBits(softSerial_t *softSerial)
{
if (softSerial->rxEdge == TRAILING) {
uint8_t bitToSet;
for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex ; bitToSet++) {
for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex; bitToSet++) {
softSerial->internalRxBuffer |= 1 << bitToSet;
}
}

2
src/drivers/serial_uart_common.h
View File

@ -8,7 +8,7 @@
// FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it
typedef struct {
serialPort_t port;
// FIXME these are uart specific and do not belong in here
DMA_Channel_TypeDef *rxDMAChannel;
DMA_Channel_TypeDef *txDMAChannel;

2
src/drivers/sonar_hcsr04.c
View File

@ -69,7 +69,7 @@ void hcsr04_init(sonar_config_t config)
// enable AFIO for EXTI support - already done is drv_system.c
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
switch(config) {
switch (config) {
case sonar_pwm56:
trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant
echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant

2
src/drivers/sound_beeper.c
View File

@ -13,7 +13,7 @@
#ifdef BUZZER
void (* systemBeepPtr)(bool onoff) = NULL;
void (*systemBeepPtr)(bool onoff) = NULL;
static void beepNormal(bool onoff)
{

34
src/flight_common.c
View File

@ -17,17 +17,17 @@ int16_t heading, magHold;
int16_t axisPID[3];
uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void mwDisarm(void)
{
if (f.ARMED)
@ -53,7 +53,8 @@ void resetErrorGyro(void)
errorGyroI[YAW] = 0;
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
{
int axis, prop;
int32_t error, errorAngle;
@ -68,7 +69,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
@ -76,7 +78,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
@ -99,7 +101,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
@ -112,7 +114,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#define GYRO_I_MAX 256
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim)
{
int32_t errorAngle = 0;
int axis;
@ -127,13 +130,14 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
// calculate error and limit the angle to max configured inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
}
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
} else {
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
@ -160,16 +164,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];

1
src/flight_imu.c
View File

@ -458,6 +458,7 @@ int getEstimatedAltitude(void)
vario = applyDeadband(vel_tmp, 5);
BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
return 1;

2
src/flight_mixer.c
View File

@ -478,7 +478,7 @@ void mixTable(void)
break;
case MULTITYPE_DUALCOPTER:
for (i = 4; i < 6; i++ ) {
for (i = 4; i < 6; i++) {
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}

28
src/gps_common.c
View File

@ -118,10 +118,10 @@ typedef struct gpsData_t {
int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit.
uint32_t lastMessage; // last time valid GPS data was received (millis)
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
uint32_t state_position; // incremental variable for loops
uint32_t state_ts; // timestamp for last state_position increment
} gpsData_t;
static gpsData_t gpsData;
@ -495,7 +495,7 @@ static void gpsNewData(uint16_t c)
#endif
// dTnav calculation
// Time for calculating x,y speed and navigation pids
dTnav = (float) (millis() - nav_loopTimer) / 1000.0f;
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0f);
@ -684,7 +684,7 @@ static bool check_missed_wp(void)
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing)
{
float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float) (*lon2 - *lon1) * GPS_scaleLonDown;
float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * 1.113195f;
*bearing = 9000.0f + atan2f(-dLat, dLon) * 5729.57795f; // Convert the output radians to 100xdeg
@ -716,8 +716,8 @@ static void GPS_calc_velocity(void)
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp;
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
@ -742,7 +742,7 @@ static void GPS_calc_velocity(void)
//
static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng)
{
error[LON] = (float) (*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
error[LON] = (float)(*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
error[LAT] = *target_lat - *gps_lat; // Y Error
}
@ -770,7 +770,7 @@ static void GPS_calc_poshold(void)
d = 0;
#endif
nav[axis] +=d;
nav[axis] += d;
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
navPID[axis].integrator = poshold_ratePID[axis].integrator;
}
@ -846,7 +846,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
// limit the ramp up of the speed
// waypoint_speed_gov is reset to 0 at each new WP command
if (max_speed > waypoint_speed_gov) {
waypoint_speed_gov += (int) (100.0f * dTnav); // increase at .5/ms
waypoint_speed_gov += (int)(100.0f * dTnav); // increase at .5/ms
max_speed = waypoint_speed_gov;
}
return max_speed;
@ -970,7 +970,7 @@ 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)
if (i >= 15)
return 0; // out of bounds
}
return tmp;
@ -1128,8 +1128,7 @@ typedef struct {
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct
{
typedef struct {
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
@ -1140,8 +1139,7 @@ typedef struct
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct
{
typedef struct {
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
@ -1252,7 +1250,7 @@ static bool gpsNewFrameUBLOX(uint8_t data)
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t) (data << 8);
_payload_length += (uint16_t)(data << 8);
if (_payload_length > 512) {
_payload_length = 0;
_step = 0;

6
src/mw.c
View File

@ -68,7 +68,7 @@ uint16_t rssi; // range: [0;1023]
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
extern failsafe_t *failsafe;
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
extern pidControllerFuncPtr pid_controller;
@ -224,7 +224,7 @@ static void mwVario(void)
{
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@ -395,7 +395,7 @@ void loop(void)
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;
AccInflightCalibrationActive = true;
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationMeasurementDone = false;
AccInflightCalibrationSavetoEEProm = true;

2
src/rx_common.h
View File

@ -38,7 +38,7 @@ typedef struct rxRuntimeConfig_s {
extern rxRuntimeConfig_t rxRuntimeConfig;
typedef uint16_t (* rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

8
src/rx_sbus.c
View File

@ -43,8 +43,7 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
return sBusPort != NULL;
}
struct sbus_dat
{
struct sbus_dat {
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
@ -59,8 +58,7 @@ struct sbus_dat
unsigned int chan11 : 11;
} __attribute__ ((__packed__));
typedef union
{
typedef union {
uint8_t in[SBUS_FRAME_SIZE];
struct sbus_dat msg;
} sbus_msg;
@ -72,7 +70,7 @@ static void sbusDataReceive(uint16_t c)
{
uint32_t sbusTime;
static uint32_t sbusTimeLast;
static uint8_t sbusFramePosition;
static uint8_t sbusFramePosition;
sbusTime = micros();
if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing

6
src/rx_sumd.c
View File

@ -49,17 +49,17 @@ static void sumdDataReceive(uint16_t c)
static uint8_t sumdIndex;
sumdTime = micros();
if ((sumdTime - sumdTimeLast) > 4000)
if ((sumdTime - sumdTimeLast) > 4000)
sumdIndex = 0;
sumdTimeLast = sumdTime;
if (sumdIndex == 0) {
if (c != SUMD_SYNCBYTE)
if (c != SUMD_SYNCBYTE)
return;
else
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
}
if (sumdIndex == 2)
if (sumdIndex == 2)
sumdSize = (uint8_t)c;
if (sumdIndex < SUMD_BUFFSIZE)
sumd[sumdIndex] = (uint8_t)c;

2
src/serial_cli.c
View File

@ -303,7 +303,7 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value);
static void cliPrintVar(const clivalue_t *var, uint32_t full);
static void cliPrint(const char *str);
static void cliWrite(uint8_t ch);
static void cliPrompt(void)
{
cliPrint("\r\n# ");

4
src/serial_msp.c
View File

@ -202,14 +202,14 @@ uint8_t read8(void)
uint16_t read16(void)
{
uint16_t t = read8();
t += (uint16_t) read8() << 8;
t += (uint16_t)read8() << 8;
return t;
}
uint32_t read32(void)
{
uint32_t t = read16();
t += (uint32_t) read16() << 16;
t += (uint32_t)read16() << 16;
return t;
}

9
src/telemetry_hott.c
View File

@ -112,13 +112,13 @@ void hottV4FormatAndSendGPSResponse(void)
void hottV4GPSUpdate(void)
{
// Number of Satelites
HoTTV4GPSModule.GPSNumSat=GPS_numSat;
HoTTV4GPSModule.GPSNumSat = GPS_numSat;
if (f.GPS_FIX > 0) {
// GPS fix
HoTTV4GPSModule.GPS_fix = 0x66; // Displays a 'f' for fix
// latitude
HoTTV4GPSModule.LatitudeNS=(GPS_coord[LAT]<0);
HoTTV4GPSModule.LatitudeNS = (GPS_coord[LAT] < 0);
uint8_t deg = GPS_coord[LAT] / 100000;
uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6;
uint8_t min = sec / 10000;
@ -130,7 +130,7 @@ void hottV4GPSUpdate(void)
HoTTV4GPSModule.LatitudeSecHigh = sec >> 8;
// longitude
HoTTV4GPSModule.longitudeEW=(GPS_coord[LON]<0);
HoTTV4GPSModule.longitudeEW = (GPS_coord[LON] < 0);
deg = GPS_coord[LON] / 100000;
sec = (GPS_coord[LON] - (deg * 100000)) * 6;
min = sec / 10000;
@ -312,7 +312,8 @@ void handleHoTTTelemetry(void)
switch (c) {
case 0x8A:
if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse();
if (sensors(SENSOR_GPS))
hottV4FormatAndSendGPSResponse();
break;
case 0x8E:
hottV4FormatAndSendEAMResponse();