1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00

Removed whitespace from end of lines

This commit is contained in:
Martin Budden 2016-07-31 23:56:31 +01:00
parent 4b3904ad0d
commit eb490d65ee
24 changed files with 121 additions and 121 deletions

View file

@ -448,7 +448,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
return masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI; return masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI;
@ -957,7 +957,7 @@ void startBlackbox(void)
* cache those now. * cache those now.
*/ */
blackboxBuildConditionCache(); blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX); blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX);
blackboxIteration = 0; blackboxIteration = 0;
@ -1577,7 +1577,7 @@ void handleBlackbox(void)
blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume); blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
blackboxSetState(BLACKBOX_STATE_RUNNING); blackboxSetState(BLACKBOX_STATE_RUNNING);
blackboxLogIteration(); blackboxLogIteration();
} }

View file

@ -56,7 +56,7 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
(32*((v)/2L>>31 > 0) \ (32*((v)/2L>>31 > 0) \
+ LOG2_32BIT((v)*1L >>16*((v)/2L>>31 > 0) \ + LOG2_32BIT((v)*1L >>16*((v)/2L>>31 > 0) \
>>16*((v)/2L>>31 > 0))) >>16*((v)/2L>>31 > 0)))
#if 0 #if 0
// ISO C version, but no type checking // ISO C version, but no type checking
#define container_of(ptr, type, member) \ #define container_of(ptr, type, member) \

View file

@ -32,7 +32,7 @@
#define GPS_I2C_INSTANCE I2CDEV_1 #define GPS_I2C_INSTANCE I2CDEV_1
#endif #endif
#define I2C_GPS_ADDRESS 0x20 //7 bits #define I2C_GPS_ADDRESS 0x20 //7 bits
#define I2C_GPS_STATUS_00 00 //(Read only) #define I2C_GPS_STATUS_00 00 //(Read only)
#define I2C_GPS_STATUS_NEW_DATA 0x01 // New data is available (after every GGA frame) #define I2C_GPS_STATUS_NEW_DATA 0x01 // New data is available (after every GGA frame)
@ -50,12 +50,12 @@ bool i2cnavGPSModuleDetect(void)
{ {
bool ack; bool ack;
uint8_t i2cGpsStatus; uint8_t i2cGpsStatus;
ack = i2cRead(GPS_I2C_INSTANCE, I2C_GPS_ADDRESS, I2C_GPS_STATUS_00, 1, &i2cGpsStatus); /* status register */ ack = i2cRead(GPS_I2C_INSTANCE, I2C_GPS_ADDRESS, I2C_GPS_STATUS_00, 1, &i2cGpsStatus); /* status register */
if (ack) if (ack)
return true; return true;
return false; return false;
} }

View file

@ -1028,7 +1028,7 @@ bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationComp
sdcard.pendingOperation.blockIndex = blockIndex; sdcard.pendingOperation.blockIndex = blockIndex;
sdcard.pendingOperation.callback = callback; sdcard.pendingOperation.callback = callback;
sdcard.pendingOperation.callbackData = callbackData; sdcard.pendingOperation.callbackData = callbackData;
sdcard.state = SDCARD_STATE_READING; sdcard.state = SDCARD_STATE_READING;
sdcard.operationStartTime = millis(); sdcard.operationStartTime = millis();

View file

@ -94,14 +94,14 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t optio
s = &uartPort1; s = &uartPort1;
s->port.vTable = uartVTable; s->port.vTable = uartVTable;
s->port.baudRate = baudRate; s->port.baudRate = baudRate;
s->port.rxBuffer = rx1Buffer; s->port.rxBuffer = rx1Buffer;
s->port.txBuffer = tx1Buffer; s->port.txBuffer = tx1Buffer;
s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.rxBufferSize = UART1_RX_BUFFER_SIZE;
s->port.txBufferSize = UART1_TX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE;
s->USARTx = USART1; s->USARTx = USART1;
@ -190,14 +190,14 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t optio
s = &uartPort2; s = &uartPort2;
s->port.vTable = uartVTable; s->port.vTable = uartVTable;
s->port.baudRate = baudRate; s->port.baudRate = baudRate;
s->port.rxBufferSize = UART2_RX_BUFFER_SIZE; s->port.rxBufferSize = UART2_RX_BUFFER_SIZE;
s->port.txBufferSize = UART2_TX_BUFFER_SIZE; s->port.txBufferSize = UART2_TX_BUFFER_SIZE;
s->port.rxBuffer = rx2Buffer; s->port.rxBuffer = rx2Buffer;
s->port.txBuffer = tx2Buffer; s->port.txBuffer = tx2Buffer;
s->USARTx = USART2; s->USARTx = USART2;
s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR; s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;

View file

@ -92,14 +92,14 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t optio
s = &uartPort1; s = &uartPort1;
s->port.vTable = uartVTable; s->port.vTable = uartVTable;
s->port.baudRate = baudRate; s->port.baudRate = baudRate;
s->port.rxBuffer = rx1Buffer; s->port.rxBuffer = rx1Buffer;
s->port.txBuffer = tx1Buffer; s->port.txBuffer = tx1Buffer;
s->port.rxBufferSize = UART1_RX_BUFFER_SIZE; s->port.rxBufferSize = UART1_RX_BUFFER_SIZE;
s->port.txBufferSize = UART1_TX_BUFFER_SIZE; s->port.txBufferSize = UART1_TX_BUFFER_SIZE;
#ifdef USE_USART1_RX_DMA #ifdef USE_USART1_RX_DMA
s->rxDMAChannel = DMA1_Channel5; s->rxDMAChannel = DMA1_Channel5;
#endif #endif
@ -169,16 +169,16 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t optio
s = &uartPort2; s = &uartPort2;
s->port.vTable = uartVTable; s->port.vTable = uartVTable;
s->port.baudRate = baudRate; s->port.baudRate = baudRate;
s->port.rxBufferSize = UART2_RX_BUFFER_SIZE; s->port.rxBufferSize = UART2_RX_BUFFER_SIZE;
s->port.txBufferSize = UART2_TX_BUFFER_SIZE; s->port.txBufferSize = UART2_TX_BUFFER_SIZE;
s->port.rxBuffer = rx2Buffer; s->port.rxBuffer = rx2Buffer;
s->port.txBuffer = tx2Buffer; s->port.txBuffer = tx2Buffer;
s->USARTx = USART2; s->USARTx = USART2;
#ifdef USE_USART2_RX_DMA #ifdef USE_USART2_RX_DMA
s->rxDMAChannel = DMA1_Channel6; s->rxDMAChannel = DMA1_Channel6;
s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR; s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR;

View file

@ -112,7 +112,7 @@ static bool usbVcpFlush(vcpPort_t *port)
if (count == 0) { if (count == 0) {
return true; return true;
} }
if (!usbIsConnected() || !usbIsConfigured()) { if (!usbIsConnected() || !usbIsConfigured()) {
return false; return false;
} }

View file

@ -94,7 +94,7 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
float q1q1 = q1 * q1; float q1q1 = q1 * q1;
float q2q2 = q2 * q2; float q2q2 = q2 * q2;
float q3q3 = q3 * q3; float q3q3 = q3 * q3;
float q0q1 = q0 * q1; float q0q1 = q0 * q1;
float q0q2 = q0 * q2; float q0q2 = q0 * q2;
float q0q3 = q0 * q3; float q0q3 = q0 * q3;
@ -154,7 +154,7 @@ void imuTransformVectorEarthToBody(t_fp_vector * v)
float x,y,z; float x,y,z;
v->V.Y = -v->V.Y; v->V.Y = -v->V.Y;
/* From earth frame to body frame */ /* From earth frame to body frame */
x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y + rMat[2][0] * v->V.Z; x = rMat[0][0] * v->V.X + rMat[1][0] * v->V.Y + rMat[2][0] * v->V.Z;
y = rMat[0][1] * v->V.X + rMat[1][1] * v->V.Y + rMat[2][1] * v->V.Z; y = rMat[0][1] * v->V.X + rMat[1][1] * v->V.Y + rMat[2][1] * v->V.Z;

View file

@ -1005,7 +1005,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
/ (posControl.navConfig->land_slowdown_maxalt - posControl.navConfig->land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt / (posControl.navConfig->land_slowdown_maxalt - posControl.navConfig->land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
// Do not allow descent velocity slower than -50cm/s so the landing detector works. // Do not allow descent velocity slower than -50cm/s so the landing detector works.
float descentVelLimited = MIN(-descentVelScaling * posControl.navConfig->land_descent_rate, -50.0f); float descentVelLimited = MIN(-descentVelScaling * posControl.navConfig->land_descent_rate, -50.0f);
updateAltitudeTargetFromClimbRate(descentVelLimited, CLIMB_RATE_RESET_SURFACE_TARGET); updateAltitudeTargetFromClimbRate(descentVelLimited, CLIMB_RATE_RESET_SURFACE_TARGET);
@ -1211,43 +1211,43 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
/* If timeout event defined and timeout reached - switch state */ /* If timeout event defined and timeout reached - switch state */
if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) && if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) { ((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
/* Update state */ /* Update state */
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]); previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT]);
/* Call new state's entry function */ /* Call new state's entry function */
while (navFSM[posControl.navState].onEntry) { while (navFSM[posControl.navState].onEntry) {
navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState); navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) { if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]); previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
} }
else { else {
break; break;
} }
} }
lastStateProcessTime = currentMillis; lastStateProcessTime = currentMillis;
} }
/* Inject new event */ /* Inject new event */
if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) { if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
/* Update state */ /* Update state */
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]); previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]);
/* Call new state's entry function */ /* Call new state's entry function */
while (navFSM[posControl.navState].onEntry) { while (navFSM[posControl.navState].onEntry) {
navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState); navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) { if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]); previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent]);
} }
else { else {
break; break;
} }
} }
lastStateProcessTime = currentMillis; lastStateProcessTime = currentMillis;
} }
/* Update public system state information */ /* Update public system state information */
NAV_Status.mode = MW_GPS_MODE_NONE; NAV_Status.mode = MW_GPS_MODE_NONE;

View file

@ -251,7 +251,7 @@ bool adjustFixedWingPositionFromRCInput(void)
static void updatePositionHeadingController_FW(uint32_t deltaMicros) static void updatePositionHeadingController_FW(uint32_t deltaMicros)
{ {
static bool forceTurnDirection = false; static bool forceTurnDirection = false;
// We have virtual position target, calculate heading error // We have virtual position target, calculate heading error
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);

View file

@ -517,7 +517,7 @@ bool isMulticopterLandingDetected(void)
} }
bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement; bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
navDebug[0] = isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1; navDebug[0] = isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1;
navDebug[1] = (landingThrSamples == 0) ? (navDebug[1] = 0) : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples)); navDebug[1] = (landingThrSamples == 0) ? (navDebug[1] = 0) : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples));
navDebug[2] = (currentTime - landingTimer) / 1000; navDebug[2] = (currentTime - landingTimer) / 1000;

View file

@ -235,7 +235,7 @@ static bool detectGPSGlitch(uint32_t currentTime)
isGlitching = true; isGlitching = true;
} }
} }
if (!isGlitching) { if (!isGlitching) {
previousTime = currentTime; previousTime = currentTime;
lastKnownGoodPosition = posEstimator.gps.pos; lastKnownGoodPosition = posEstimator.gps.pos;

View file

@ -474,7 +474,7 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
if (FLIGHT_MODE(HEADING_LOCK) && magHoldState != MAG_HOLD_ENABLED) { if (FLIGHT_MODE(HEADING_LOCK) && magHoldState != MAG_HOLD_ENABLED) {
pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]); pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]);
} }
// Step 4: Run gyro-driven control // Step 4: Run gyro-driven control
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Apply PID setpoint controller // Apply PID setpoint controller

View file

@ -2912,7 +2912,7 @@ bool afatfs_chdir(afatfsFilePtr_t directory)
afatfs_initFileHandle(&afatfs.currentDirectory); afatfs_initFileHandle(&afatfs.currentDirectory);
afatfs.currentDirectory.mode = AFATFS_FILE_MODE_READ | AFATFS_FILE_MODE_WRITE; afatfs.currentDirectory.mode = AFATFS_FILE_MODE_READ | AFATFS_FILE_MODE_WRITE;
if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16) if (afatfs.filesystemType == FAT_FILESYSTEM_TYPE_FAT16)
afatfs.currentDirectory.type = AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY; afatfs.currentDirectory.type = AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY;
else else

View file

@ -304,11 +304,11 @@ void showStatusPage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (feature(FEATURE_VBAT)) { if (feature(FEATURE_VBAT)) {
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
tfp_sprintf(lineBuffer, "V: %d.%1d ", vbat / 10, vbat % 10); tfp_sprintf(lineBuffer, "V: %d.%1d ", vbat / 10, vbat % 10);
padLineBufferToChar(12); padLineBufferToChar(12);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentage(); uint8_t batteryPercentage = calculateBatteryPercentage();
drawHorizonalPercentageBar(10, batteryPercentage); drawHorizonalPercentageBar(10, batteryPercentage);
} }
@ -318,25 +318,25 @@ void showStatusPage(void)
tfp_sprintf(lineBuffer, "mAh: %d", mAhDrawn); tfp_sprintf(lineBuffer, "mAh: %d", mAhDrawn);
padLineBufferToChar(12); padLineBufferToChar(12);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage(); uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage();
drawHorizonalPercentageBar(10, capacityPercentage); drawHorizonalPercentageBar(10, capacityPercentage);
} }
rowIndex++; rowIndex++;
#ifdef GPS #ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
tfp_sprintf(lineBuffer, "Sats: %d", gpsSol.numSat); tfp_sprintf(lineBuffer, "Sats: %d", gpsSol.numSat);
padHalfLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_line(rowIndex); i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Fix: %s", gpsFixTypeText[gpsSol.fixType]); tfp_sprintf(lineBuffer, "Fix: %s", gpsFixTypeText[gpsSol.fixType]);
padHalfLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "HDOP: %d.%1d", gpsSol.hdop / 100, gpsSol.hdop % 100); tfp_sprintf(lineBuffer, "HDOP: %d.%1d", gpsSol.hdop / 100, gpsSol.hdop % 100);
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
@ -346,10 +346,10 @@ void showStatusPage(void)
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
} }
#endif #endif
#ifdef MAG #ifdef MAG
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, "HDG: %d", DECIDEGREES_TO_DEGREES(attitude.values.yaw)); tfp_sprintf(lineBuffer, "HDG: %d", DECIDEGREES_TO_DEGREES(attitude.values.yaw));
@ -367,8 +367,8 @@ void showStatusPage(void)
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex); i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
} }
#endif #endif
} }
void updateDisplay(void) void updateDisplay(void)
@ -423,7 +423,7 @@ void updateDisplay(void)
if (!displayPresent) { if (!displayPresent) {
return; return;
} }
i2c_OLED_clear_display_quick(); i2c_OLED_clear_display_quick();
showTitle(); showTitle();
} }

View file

@ -1267,7 +1267,7 @@ void ledStripEnable(void)
static void ledStripDisable(void) static void ledStripDisable(void)
{ {
setStripColor(&HSV(BLACK)); setStripColor(&HSV(BLACK));
ws2811UpdateStrip(); ws2811UpdateStrip();
} }
#endif #endif

View file

@ -112,7 +112,7 @@ uint8_t ibusFrameStatus(void)
chksum = 0xFFFF; chksum = 0xFFFF;
for (i = 0; i < 30; i++) for (i = 0; i < 30; i++)
chksum -= ibus[i]; chksum -= ibus[i];
rxsum = ibus[30] + (ibus[31] << 8); rxsum = ibus[30] + (ibus[31] << 8);
if (chksum == rxsum) { if (chksum == rxsum) {
@ -126,7 +126,7 @@ uint8_t ibusFrameStatus(void)
ibusChannelData[7] = (ibus[17] << 8) + ibus[16]; ibusChannelData[7] = (ibus[17] << 8) + ibus[16];
ibusChannelData[8] = (ibus[19] << 8) + ibus[18]; ibusChannelData[8] = (ibus[19] << 8) + ibus[18];
ibusChannelData[9] = (ibus[21] << 8) + ibus[20]; ibusChannelData[9] = (ibus[21] << 8) + ibus[20];
frameStatus = SERIAL_RX_FRAME_COMPLETE; frameStatus = SERIAL_RX_FRAME_COMPLETE;
} }

View file

@ -132,7 +132,7 @@ bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
static uint16_t xBusCRC16(uint16_t crc, uint8_t value) static uint16_t xBusCRC16(uint16_t crc, uint8_t value)
{ {
uint8_t i; uint8_t i;
crc = crc ^ (int16_t)value << 8; crc = crc ^ (int16_t)value << 8;
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++) {
@ -165,7 +165,7 @@ uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed)
inData >>= 1; inData >>= 1;
} }
return seed; return seed;
} }
@ -224,7 +224,7 @@ static void xBusUnpackRJ01Frame(void)
// method. // method.
// So, we check both these values as well as the provided length // So, we check both these values as well as the provided length
// of the outer/full message (LEN) // of the outer/full message (LEN)
// //
// Check we have correct length of message // Check we have correct length of message
// //
@ -233,14 +233,14 @@ static void xBusUnpackRJ01Frame(void)
// Unknown package as length is not ok // Unknown package as length is not ok
return; return;
} }
// //
// CRC calculation & check for full message // CRC calculation & check for full message
// //
for (i = 0; i < xBusFrameLength - 1; i++) { for (i = 0; i < xBusFrameLength - 1; i++) {
outerCrc = xBusRj01CRC8(outerCrc, xBusFrame[i]); outerCrc = xBusRj01CRC8(outerCrc, xBusFrame[i]);
} }
if (outerCrc != xBusFrame[xBusFrameLength - 1]) if (outerCrc != xBusFrame[xBusFrameLength - 1])
{ {
// CRC does not match, skip this frame // CRC does not match, skip this frame
@ -265,7 +265,7 @@ static void xBusDataReceive(uint16_t c)
xBusFramePosition = 0; xBusFramePosition = 0;
xBusDataIncoming = false; xBusDataIncoming = false;
} }
// Check if we shall start a frame? // Check if we shall start a frame?
if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) { if ((xBusFramePosition == 0) && (c == XBUS_START_OF_FRAME_BYTE)) {
xBusDataIncoming = true; xBusDataIncoming = true;
@ -277,7 +277,7 @@ static void xBusDataReceive(uint16_t c)
xBusFrame[xBusFramePosition] = (uint8_t)c; xBusFrame[xBusFramePosition] = (uint8_t)c;
xBusFramePosition++; xBusFramePosition++;
} }
// Done? // Done?
if (xBusFramePosition == xBusFrameLength) { if (xBusFramePosition == xBusFrameLength) {
switch (xBusProvider) { switch (xBusProvider) {

View file

@ -80,7 +80,7 @@ static void updateBatteryVoltage(uint32_t vbatTimeDelta)
void updateBattery(uint32_t vbatTimeDelta) void updateBattery(uint32_t vbatTimeDelta)
{ {
updateBatteryVoltage(vbatTimeDelta); updateBatteryVoltage(vbatTimeDelta);
/* battery has just been connected*/ /* battery has just been connected*/
if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV) if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV)
{ {
@ -109,7 +109,7 @@ void updateBattery(uint32_t vbatTimeDelta)
batteryCellCount = 0; batteryCellCount = 0;
batteryWarningVoltage = 0; batteryWarningVoltage = 0;
batteryCriticalVoltage = 0; batteryCriticalVoltage = 0;
} }
switch(batteryState) switch(batteryState)
{ {

View file

@ -8,7 +8,7 @@
const uint16_t multiPPM[] = { const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
@ -31,7 +31,7 @@ const uint16_t multiPWM[] = {
const uint16_t airPPM[] = { const uint16_t airPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM6 | (MAP_TO_SERVO_OUTPUT << 8), PWM6 | (MAP_TO_SERVO_OUTPUT << 8),

View file

@ -8,7 +8,7 @@
* This file contains the system clock configuration for STM32F30x devices, * This file contains the system clock configuration for STM32F30x devices,
* and is generated by the clock configuration tool * and is generated by the clock configuration tool
* stm32f30x_Clock_Configuration_V1.0.0.xls * stm32f30x_Clock_Configuration_V1.0.0.xls
* *
* 1. This file provides two functions and one global variable to be called from * 1. This file provides two functions and one global variable to be called from
* user application: * user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
@ -21,7 +21,7 @@
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick * by the user application to setup the SysTick
* timer or configure other parameters. * timer or configure other parameters.
* *
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed * be called whenever the core clock is changed
* during program execution. * during program execution.
@ -41,7 +41,7 @@
* *
* 5. This file configures the system clock as follows: * 5. This file configures the system clock as follows:
*============================================================================= *=============================================================================
* Supported STM32F30x device * Supported STM32F30x device
*----------------------------------------------------------------------------- *-----------------------------------------------------------------------------
* System Clock source | PLL (HSE) * System Clock source | PLL (HSE)
*----------------------------------------------------------------------------- *-----------------------------------------------------------------------------
@ -204,34 +204,34 @@ void SystemInit(void)
* The SystemCoreClock variable contains the core clock (HCLK), it can * The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure * be used by the user application to setup the SysTick timer or configure
* other parameters. * other parameters.
* *
* @note Each time the core clock (HCLK) changes, this function must be called * @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration * to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect. * based on this variable will be incorrect.
* *
* @note - The system frequency computed by this function is not the real * @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined * frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source: * constant and the selected clock source:
* *
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
* *
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
* *
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors. * or HSI_VALUE(*) multiplied/divided by the PLL factors.
* *
* (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz) but the real value may vary depending on the variations * 8 MHz) but the real value may vary depending on the variations
* in voltage and temperature. * in voltage and temperature.
* *
* (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz), user has to ensure that HSE_VALUE is same as the real * 8 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may * frequency of the crystal used. Otherwise, this function may
* have wrong result. * have wrong result.
* *
* - The result of this function could be not correct when using fractional * - The result of this function could be not correct when using fractional
* value for HSE crystal. * value for HSE crystal.
* *
* @param None * @param None
* @retval None * @retval None
*/ */
@ -255,7 +255,7 @@ void SystemCoreClockUpdate (void)
pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
pllmull = ( pllmull >> 18) + 2; pllmull = ( pllmull >> 18) + 2;
if (pllsource == 0x00) if (pllsource == 0x00)
{ {
/* HSI oscillator clock divided by 2 selected as PLL clock entry */ /* HSI oscillator clock divided by 2 selected as PLL clock entry */
@ -266,7 +266,7 @@ void SystemCoreClockUpdate (void)
prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
/* HSE oscillator clock selected as PREDIV1 clock entry */ /* HSE oscillator clock selected as PREDIV1 clock entry */
SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;
} }
break; break;
default: /* HSI used as system clock */ default: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE; SystemCoreClock = HSI_VALUE;
@ -283,7 +283,7 @@ void SystemCoreClockUpdate (void)
* @brief Configures the System clock source, PLL Multiplier and Divider factors, * @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings * AHB/APBx prescalers and Flash settings
* @note This function should be called only once the RCC clock configuration * @note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function). * is reset to the default reset state (done in SystemInit() function).
* @param None * @param None
* @retval None * @retval None
*/ */
@ -322,10 +322,10 @@ void SetSysClock(void)
/* HCLK = SYSCLK / 1 */ /* HCLK = SYSCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 1 */ /* PCLK2 = HCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1;
/* PCLK1 = HCLK / 2 */ /* PCLK1 = HCLK / 2 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2;
@ -340,7 +340,7 @@ void SetSysClock(void)
while((RCC->CR & RCC_CR_PLLRDY) == 0) while((RCC->CR & RCC_CR_PLLRDY) == 0)
{ {
} }
/* Select PLL as system clock source */ /* Select PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;

View file

@ -307,7 +307,7 @@ static void sendFakeLatLong(void)
{ {
// Heading is only displayed on OpenTX if non-zero lat/long is also sent // Heading is only displayed on OpenTX if non-zero lat/long is also sent
int32_t coord[2] = {0,0}; int32_t coord[2] = {0,0};
coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);

View file

@ -163,7 +163,7 @@ void configureMAVLinkTelemetryPort(void)
// default rate for minimOSD // default rate for minimOSD
baudRateIndex = BAUD_57600; baudRateIndex = BAUD_57600;
} }
mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
if (!mavlinkPort) { if (!mavlinkPort) {
@ -190,9 +190,9 @@ void checkMAVLinkTelemetryState(void)
void mavlinkSendSystemStatus(void) void mavlinkSendSystemStatus(void)
{ {
uint16_t msgLength; uint16_t msgLength;
uint32_t onboardControlAndSensors = 35843; uint32_t onboardControlAndSensors = 35843;
/* /*
onboard_control_sensors_present Bitmask onboard_control_sensors_present Bitmask
fedcba9876543210 fedcba9876543210
@ -202,11 +202,11 @@ void mavlinkSendSystemStatus(void)
0100000000100000 With GPS = 16416 0100000000100000 With GPS = 16416
0000001111111111 0000001111111111
*/ */
if (sensors(SENSOR_MAG)) onboardControlAndSensors |= 4100; if (sensors(SENSOR_MAG)) onboardControlAndSensors |= 4100;
if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200; if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200;
if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416; if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416;
mavlink_msg_sys_status_pack(0, 200, &mavMsg, mavlink_msg_sys_status_pack(0, 200, &mavMsg,
// onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present.
//Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, //Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure,
@ -277,7 +277,7 @@ void mavlinkSendPosition(void)
{ {
uint16_t msgLength; uint16_t msgLength;
uint8_t gpsFixType = 0; uint8_t gpsFixType = 0;
if (!sensors(SENSOR_GPS)) if (!sensors(SENSOR_GPS))
return; return;
@ -388,7 +388,7 @@ void mavlinkSendHUDAndHeartbeat(void)
mavGroundSpeed = gpsSol.groundSpeed / 100.0f; mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
} }
#endif #endif
// select best source for altitude // select best source for altitude
#if defined(NAV) #if defined(NAV)
mavAltitude = getEstimatedActualPosition(Z) / 100.0f; mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
@ -399,7 +399,7 @@ void mavlinkSendHUDAndHeartbeat(void)
mavAltitude = gpsSol.llh.alt; mavAltitude = gpsSol.llh.alt;
} }
#endif #endif
mavlink_msg_vfr_hud_pack(0, 200, &mavMsg, mavlink_msg_vfr_hud_pack(0, 200, &mavMsg,
// airspeed Current airspeed in m/s // airspeed Current airspeed in m/s
mavAirSpeed, mavAirSpeed,
@ -416,11 +416,11 @@ void mavlinkSendHUDAndHeartbeat(void)
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength); mavlinkSerialWrite(mavBuffer, msgLength);
uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
if (ARMING_FLAG(ARMED)) if (ARMING_FLAG(ARMED))
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t mavSystemType; uint8_t mavSystemType;
switch(masterConfig.mixerMode) switch(masterConfig.mixerMode)
{ {
@ -454,11 +454,11 @@ void mavlinkSendHUDAndHeartbeat(void)
default: default:
mavSystemType = MAV_TYPE_GENERIC; mavSystemType = MAV_TYPE_GENERIC;
break; break;
} }
// Custom mode for compatibility with APM OSDs // Custom mode for compatibility with APM OSDs
uint8_t mavCustomMode = 1; // Acro by default uint8_t mavCustomMode = 1; // Acro by default
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
mavCustomMode = 0; //Stabilize mavCustomMode = 0; //Stabilize
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
@ -469,7 +469,7 @@ void mavlinkSendHUDAndHeartbeat(void)
mavCustomMode = 6; //Return to Launch mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(NAV_POSHOLD_MODE)) if (FLIGHT_MODE(NAV_POSHOLD_MODE))
mavCustomMode = 16; //Position Hold (Earlier called Hybrid) mavCustomMode = 16; //Position Hold (Earlier called Hybrid)
uint8_t mavSystemState = 0; uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (failsafeIsActive()) { if (failsafeIsActive()) {
@ -485,7 +485,7 @@ void mavlinkSendHUDAndHeartbeat(void)
else { else {
mavSystemState = MAV_STATE_STANDBY; mavSystemState = MAV_STATE_STANDBY;
} }
mavlink_msg_heartbeat_pack(0, 200, &mavMsg, mavlink_msg_heartbeat_pack(0, 200, &mavMsg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
mavSystemType, mavSystemType,
@ -507,7 +507,7 @@ void processMAVLinkTelemetry(void)
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) { if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) {
mavlinkSendSystemStatus(); mavlinkSendSystemStatus();
} }
if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) { if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) {
mavlinkSendRCChannelsAndRSSI(); mavlinkSendRCChannelsAndRSSI();
} }
@ -521,7 +521,7 @@ void processMAVLinkTelemetry(void)
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) { if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) {
mavlinkSendAttitude(); mavlinkSendAttitude();
} }
if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) { if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) {
mavlinkSendHUDAndHeartbeat(); mavlinkSendHUDAndHeartbeat();
} }
@ -536,7 +536,7 @@ void handleMAVLinkTelemetry(void)
if (!mavlinkPort) { if (!mavlinkPort) {
return; return;
} }
uint32_t now = micros(); uint32_t now = micros();
if ((now - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) { if ((now - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
processMAVLinkTelemetry(); processMAVLinkTelemetry();

View file

@ -272,7 +272,7 @@ void checkSmartPortTelemetryState(void)
void handleSmartPortTelemetry(void) void handleSmartPortTelemetry(void)
{ {
uint32_t smartPortLastServiceTime = millis(); uint32_t smartPortLastServiceTime = millis();
if (!smartPortTelemetryEnabled) { if (!smartPortTelemetryEnabled) {
return; return;
} }
@ -300,7 +300,7 @@ void handleSmartPortTelemetry(void)
smartPortHasRequest = 0; smartPortHasRequest = 0;
return; return;
} }
// we can send back any data we want, our table keeps track of the order and frequency of each data type we send // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
uint16_t id = frSkyDataIdTable[smartPortIdCnt]; uint16_t id = frSkyDataIdTable[smartPortIdCnt];
if (id == 0) { // end of table reached, loop back if (id == 0) { // end of table reached, loop back