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

View File

@ -176,7 +176,6 @@ char *ftoa(float x, char *floatString)
return floatString; return floatString;
} }
// Simple and fast atof (ascii to float) function. // Simple and fast atof (ascii to float) function.
// //
// - Executes about 5x faster than standard MSCRT library atof(). // - Executes about 5x faster than standard MSCRT library atof().
@ -254,8 +253,14 @@ float fastA2F(const char *p)
// Calculate scaling factor. // Calculate scaling factor.
// while (expon >= 50) { scale *= 1E50f; expon -= 50; } // while (expon >= 50) { scale *= 1E50f; expon -= 50; }
while (expon >= 8) { scale *= 1E8f; expon -= 8; } while (expon >= 8) {
while (expon > 0) { scale *= 10.0f; expon -= 1; } scale *= 1E8f;
expon -= 8;
}
while (expon > 0) {
scale *= 10.0f;
expon -= 1;
}
} }
// Return signed and scaled floating point result. // Return signed and scaled floating point result.

View File

@ -37,8 +37,9 @@
#include "config_master.h" #include "config_master.h"
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/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 #ifndef FLASH_PAGE_COUNT
#define FLASH_PAGE_COUNT 128 #define FLASH_PAGE_COUNT 128
@ -46,13 +47,11 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
#define FLASH_PAGE_SIZE ((uint16_t)0x400) #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 #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 master_t masterConfig; // master config struct with data independent from profiles
profile_t currentProfile; // profile config struct profile_t currentProfile; // profile config struct
static const uint8_t EEPROM_CONF_VERSION = 66; static const uint8_t EEPROM_CONF_VERSION = 66;
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
{ {
accelerometerTrims->trims.pitch = 0; accelerometerTrims->trims.pitch = 0;
@ -340,7 +339,8 @@ void activateConfig(void)
#endif #endif
} }
void validateAndFixConfig(void) { void validateAndFixConfig(void)
{
if (!(feature(FEATURE_PARALLEL_PWM) || feature(FEATURE_PPM) || feature(FEATURE_SERIALRX))) { if (!(feature(FEATURE_PARALLEL_PWM) || feature(FEATURE_PPM) || feature(FEATURE_SERIALRX))) {
featureSet(FEATURE_PARALLEL_PWM); // Consider changing the default to PPM featureSet(FEATURE_PARALLEL_PWM); // Consider changing the default to PPM
} }
@ -438,7 +438,8 @@ void writeEEPROM(void)
#endif #endif
status = FLASH_ErasePage(FLASH_WRITE_ADDR); status = FLASH_ErasePage(FLASH_WRITE_ADDR);
for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) { 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) { if (status == FLASH_COMPLETE) {
break; break;

View File

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

View File

@ -3,8 +3,7 @@
typedef void (*baroOpFuncPtr)(void); // baro start operation 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 (*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 ut_delay;
uint16_t up_delay; uint16_t up_delay;
baroOpFuncPtr start_ut; baroOpFuncPtr start_ut;

View File

@ -18,6 +18,7 @@ typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial dr
typedef struct serialPort { typedef struct serialPort {
const struct serialPortVTable *vTable; const struct serialPortVTable *vTable;
uint8_t identifier; uint8_t identifier;
portMode_t mode; portMode_t mode;
serialInversion_e inversion; serialInversion_e inversion;

View File

@ -17,17 +17,17 @@ int16_t heading, magHold;
int16_t axisPID[3]; int16_t axisPID[3];
uint8_t dynP8[3], dynI8[3], dynD8[3]; uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 }; static int32_t errorGyroI[3] = { 0, 0, 0 };
static int32_t errorAngleI[2] = { 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 pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void mwDisarm(void) void mwDisarm(void)
{ {
if (f.ARMED) if (f.ARMED)
@ -53,7 +53,8 @@ void resetErrorGyro(void)
errorGyroI[YAW] = 0; 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; int axis, prop;
int32_t error, errorAngle; int32_t error, errorAngle;
@ -68,7 +69,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// 50 degrees max inclination // 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 = 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); PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
@ -112,7 +114,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#define GYRO_I_MAX 256 #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; int32_t errorAngle = 0;
int axis; int axis;
@ -127,7 +130,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----Get the desired angle rate depending on flight mode // -----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 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 // 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) if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5); AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);

View File

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

View File

@ -1128,8 +1128,7 @@ typedef struct {
uint32_t heading_accuracy; uint32_t heading_accuracy;
} ubx_nav_velned; } ubx_nav_velned;
typedef struct typedef struct {
{
uint8_t chn; // Channel number, 255 for SVx not assigned to channel uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask uint8_t flags; // Bitmask
@ -1140,8 +1139,7 @@ typedef struct
int32_t prRes; // Pseudo range residual in centimetres int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel; } ubx_nav_svinfo_channel;
typedef struct typedef struct {
{
uint32_t time; // GPS Millisecond time of week uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6 uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6

View File

@ -43,8 +43,7 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
return sBusPort != NULL; return sBusPort != NULL;
} }
struct sbus_dat struct sbus_dat {
{
unsigned int chan0 : 11; unsigned int chan0 : 11;
unsigned int chan1 : 11; unsigned int chan1 : 11;
unsigned int chan2 : 11; unsigned int chan2 : 11;
@ -59,8 +58,7 @@ struct sbus_dat
unsigned int chan11 : 11; unsigned int chan11 : 11;
} __attribute__ ((__packed__)); } __attribute__ ((__packed__));
typedef union typedef union {
{
uint8_t in[SBUS_FRAME_SIZE]; uint8_t in[SBUS_FRAME_SIZE];
struct sbus_dat msg; struct sbus_dat msg;
} sbus_msg; } sbus_msg;

View File

@ -312,7 +312,8 @@ void handleHoTTTelemetry(void)
switch (c) { switch (c) {
case 0x8A: case 0x8A:
if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse(); if (sensors(SENSOR_GPS))
hottV4FormatAndSendGPSResponse();
break; break;
case 0x8E: case 0x8E:
hottV4FormatAndSendEAMResponse(); hottV4FormatAndSendEAMResponse();