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:
parent
4b3904ad0d
commit
eb490d65ee
24 changed files with 121 additions and 121 deletions
|
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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) \
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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();
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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)
|
||||||
{
|
{
|
||||||
|
|
|
@ -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),
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue