bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
{
uint16_t period_us = SPEKTRUM_RPM_UNUSED;
#ifdef USE_ESC_SENSOR_TELEMETRY
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData != NULL) {
period_us = 60000000 / escData->rpm; // revs/minute -> microSeconds
}
#endif
int16_t coreTemp = SPEKTRUM_TEMP_UNUSED;
#if defined(USE_ADC_INTERNAL)
coreTemp = getCoreTemperatureCelsius();
coreTemp = coreTemp * 9 / 5 + 32; // C -> F
#endif
UNUSED(currentTimeUs);
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16BigEndian(dst, period_us); // pulse leading edges
if (telemetryConfig()->report_cell_voltage) {
sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V
} else {
sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V
}
sbufWriteU16BigEndian(dst, coreTemp); // temperature
sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);
// Mandatory frame, send it unconditionally.
return true;
}
static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
{
static uint32_t lastHottAlarmSoundTime = 0;
if (((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND))){
lastHottAlarmSoundTime = millis();
const batteryState_e batteryState = getBatteryState();
if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL){
hottEAMMessage->warning_beeps = 0x10;
hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1;
} else {
hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE;
hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE;
}
}
}
static uint8_t dispatchMeasurementReply(ibusAddress_t address)
{
int value;
switch (sensorAddressTypeLookup[address - ibusBaseAddress]) {
case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
value = getBatteryVoltage() * 10;
if (telemetryConfig()->report_cell_voltage) {
value /= getBatteryCellCount();
}
return sendIbusMeasurement(address, value);
case IBUS_SENSOR_TYPE_TEMPERATURE:
value = gyroGetTemperature() * 10;
return sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
case IBUS_SENSOR_TYPE_RPM:
return sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]);
}
return 0;
}
void targetPreInit(void)
{
switch (hardwareRevision) {
case BJF4_REV3:
case BJF4_MINI_REV3A:
case BJF4_REV4:
break;
default:
return;
}
IO_t inverter = IOGetByTag(IO_TAG(UART1_INVERTER));
IOInit(inverter, OWNER_INVERTER, 1);
IOConfigGPIO(inverter, IOCFG_OUT_PP);
bool high = false;
serialPortConfig_t *portConfig = serialFindPortConfiguration(SERIAL_PORT_USART1);
if (portConfig) {
bool smartportEnabled = (portConfig->functionMask & FUNCTION_TELEMETRY_SMARTPORT);
if (smartportEnabled && (telemetryConfig()->telemetry_inversion) && (feature(FEATURE_TELEMETRY))) {
high = true;
}
}
/* reverse this for rev4, as it does not use the XOR gate */
if (hardwareRevision == BJF4_REV4) {
high = !high;
}
IOWrite(inverter, high);
/* ensure the CS pin for the flash is pulled hi so any SD card initialisation does not impact the chip */
if (hardwareRevision == BJF4_REV3) {
IO_t flashIo = IOGetByTag(IO_TAG(M25P16_CS_PIN));
IOConfigGPIO(flashIo, IOCFG_OUT_PP);
IOHi(flashIo);
IO_t sdcardIo = IOGetByTag(IO_TAG(SDCARD_SPI_CS_PIN));
IOConfigGPIO(sdcardIo, IOCFG_OUT_PP);
IOHi(sdcardIo);
}
}
/*
* Send voltage via ID_VOLT
*
* NOTE: This sends voltage divided by batteryCellCount. To get the real
* battery voltage, you need to multiply the value by batteryCellCount.
*/
static void sendVoltage(void)
{
if (!telemetryConfig()->telemetry_send_cells) {
return;
}
static uint16_t currentCell = 0;
uint32_t cellVoltage;
uint16_t payload;
/*
* Format for Voltage Data for single cells is like this:
*
* llll llll cccc hhhh
* l: Low voltage bits
* h: High voltage bits
* c: Cell number (starting at 0)
*
* The actual value sent for cell voltage has resolution of 0.002 volts
* Since vbat has resolution of 0.1 volts it has to be multiplied by 50
*/
cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
// Cell number is at bit 9-12
payload = (currentCell << 4);
// Lower voltage bits are at bit 0-8
payload |= ((cellVoltage & 0x0ff) << 8);
// Higher voltage bits are at bits 13-15
payload |= ((cellVoltage & 0xf00) >> 8);
sendDataHead(ID_VOLT);
serialize16(payload);
currentCell++;
currentCell %= batteryCellCount;
}
void configureSmartPortTelemetryPort(void)
{
portOptions_t portOptions;
if (!portConfig) {
return;
}
portOptions = SERIAL_BIDIR;
if (telemetryConfig()->telemetry_inversion) {
portOptions |= SERIAL_INVERTED;
}
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
if (!smartPortSerialPort) {
return;
}
smartPortState = SPSTATE_INITIALIZED;
smartPortTelemetryEnabled = true;
smartPortLastRequestTime = millis();
}
void processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
static bool airmodeIsActivated;
calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
mwDisarm();
}
updateRSSI(currentTimeUs);
if (feature(FEATURE_FAILSAFE)) {
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
}
const throttleStatus_e throttleStatus = calculateThrottleStatus();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
} else {
airmodeIsActivated = false;
}
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetErrorGyroState();
if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON);
else
pidStabilisationState(PID_STABILISATION_OFF);
} else {
pidStabilisationState(PID_STABILISATION_ON);
}
// When armed and motors aren't spinning, do beeps and then disarm
// board after delay so users without buzzer won't lose fingers.
// mixTable constrains motor commands, so checking throttleStatus is enough
if (ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP)
&& !STATE(FIXED_WING)
&& !feature(FEATURE_3D)
&& !isAirmodeActive()
) {
if (isUsingSticksForArming()) {
if (throttleStatus == THROTTLE_LOW) {
if (armingConfig()->auto_disarm_delay != 0
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
mwDisarm();
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed
beeper(BEEPER_ARMED);
armedBeeperOn = true;
}
} else {
// throttle is not low
if (armingConfig()->auto_disarm_delay != 0) {
// extend disarm time
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
}
if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
} else {
// arming is via AUX switch; beep while throttle low
if (throttleStatus == THROTTLE_LOW) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
}
processRcStickPositions(throttleStatus);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
}
updateActivatedModes();
if (!cliMode) {
updateAdjustmentStates();
processRcAdjustments(currentControlRateProfile);
}
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HORIZON_MODE);
}
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
} else {
LED1_OFF;
}
#if defined(ACC) || defined(MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
#if defined(GPS) || defined(MAG)
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
}
#endif
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
}
}
#endif
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsWaypointsAndMode();
}
#endif
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
(telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
releaseSharedTelemetryPorts();
} else {
// the telemetry state must be checked immediately so that shared serial ports are released.
telemetryCheckState();
mspSerialAllocatePorts();
}
}
#endif
#ifdef VTX
vtxUpdateActivatedChannel();
#endif
}
void processRx(void)
{
static bool armedBeeperOn = false;
calculateRxChannelsAndUpdateFailsafe(currentTime);
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!rcModeIsActive(BOXARM))
mwDisarm();
}
updateRSSI(currentTime);
if (feature(FEATURE_FAILSAFE)) {
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
}
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle);
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(rxConfig());
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
if (throttleStatus == THROTTLE_LOW) {
if (rcModeIsActive(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
if (rollPitchStatus == CENTERED) {
ENABLE_STATE(ANTI_WINDUP);
} else {
DISABLE_STATE(ANTI_WINDUP);
}
} else {
pidResetITerm();
}
} else {
DISABLE_STATE(ANTI_WINDUP);
}
// When armed and motors aren't spinning, do beeps and then disarm
// board after delay so users without buzzer won't lose fingers.
// mixTable constrains motor commands, so checking throttleStatus is enough
if (ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP)
&& !STATE(FIXED_WING)
) {
if (isUsingSticksForArming()) {
if (throttleStatus == THROTTLE_LOW) {
if (armingConfig()->auto_disarm_delay != 0
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
mwDisarm();
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed
beeper(BEEPER_ARMED);
armedBeeperOn = true;
}
} else {
// throttle is not low
if (armingConfig()->auto_disarm_delay != 0) {
// extend disarm time
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
}
if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
} else {
// arming is via AUX switch; beep while throttle low
if (throttleStatus == THROTTLE_LOW) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
}
processRcStickPositions(rxConfig(), throttleStatus, armingConfig()->retarded_arm, armingConfig()->disarm_kill_switch);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
}
rcModeUpdateActivated(modeActivationProfile()->modeActivationConditions);
if (!cliMode) {
updateAdjustmentStates(adjustmentProfile()->adjustmentRanges);
processRcAdjustments(currentControlRateProfile, rxConfig());
}
bool canUseHorizonMode = true;
if ((rcModeIsActive(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (rcModeIsActive(BOXHORIZON) && canUseHorizonMode) {
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HORIZON_MODE);
}
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
} else {
LED1_OFF;
}
#ifdef MAG
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (rcModeIsActive(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
}
if (rcModeIsActive(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (rcModeIsActive(BOXHEADADJ)) {
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
}
}
#endif
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsWaypointsAndMode();
}
#endif
if (rcModeIsActive(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED))
|| (telemetryConfig()->telemetry_switch && rcModeIsActive(BOXTELEMETRY))) {
releaseSharedTelemetryPorts();
} else {
// the telemetry state must be checked immediately so that shared serial ports are released.
bool telemetryStateChanged = telemetryCheckState();
if (telemetryStateChanged) {
mspSerialAllocatePorts();
}
}
}
#endif
#ifdef VTX
if (canUpdateVTX()) {
updateVTXState();
}
#endif
}
void handleSmartPortTelemetry(void)
{
uint32_t smartPortLastServiceTime = millis();
if (!smartPortTelemetryEnabled) {
return;
}
if (!canSendSmartPortTelemetry()) {
return;
}
while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
uint8_t c = serialRead(smartPortSerialPort);
smartPortDataReceive(c);
}
uint32_t now = millis();
// if timed out, reconfigure the UART back to normal so the GUI or CLI works
if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
smartPortState = SPSTATE_TIMEDOUT;
return;
}
while (smartPortHasRequest) {
// Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
smartPortHasRequest = 0;
return;
}
// 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];
if (id == 0) { // end of table reached, loop back
smartPortIdCnt = 0;
id = frSkyDataIdTable[smartPortIdCnt];
}
smartPortIdCnt++;
int32_t tmpi;
switch(id) {
#ifdef GPS
case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
smartPortHasRequest = 0;
}
break;
#endif
case FSSP_DATAID_VFAS :
if (feature(FEATURE_VBAT)) {
smartPortSendPackage(id, vbat * 10); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_CELLS :
if (feature(FEATURE_VBAT) && telemetryConfig()->telemetry_send_cells) {
/*
* A cell packet is formated this way: https://github.com/jcheger/frsky-arduino/blob/master/FrskySP/FrskySP.cpp
* content | length
* ---------- | ------
* volt[id] | 12-bits
* celltotal | 4 bits
* cellid | 4 bits
*/
static uint8_t currentCell = 0; // Track current cell index number
// Cells Data Payload
uint32_t payload = 0;
payload |= ((uint16_t)(vbat * 100 + batteryCellCount) / (batteryCellCount * 2)) & 0x0FFF; // Cell Voltage formatted for payload, modified code from cleanflight Frsky.c, TESTING NOTE: (uint16_t)(4.2 * 500.0) & 0x0FFF;
payload <<= 4;
payload |= (uint8_t)batteryCellCount & 0x0F; // Cell Total Count formatted for payload
payload <<= 4;
payload |= (uint8_t)currentCell & 0x0F; // Current Cell Index Number formatted for payload
// Send Payload
smartPortSendPackage(id, payload);
smartPortHasRequest = 0;
// Incremental Counter
currentCell++;
currentCell %= batteryCellCount; // Reset counter @ max index
}
break;
case FSSP_DATAID_CURRENT :
if (feature(FEATURE_AMPERAGE_METER)) {
amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
smartPortSendPackage(id, state->amperage / 10); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
break;
//case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_FUEL :
if (feature(FEATURE_AMPERAGE_METER)) {
amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
smartPortSendPackage(id, state->mAhDrawn); // given in mAh, unknown requested unit
smartPortHasRequest = 0;
}
break;
//case FSSP_DATAID_ADC1 :
//case FSSP_DATAID_ADC2 :
#ifdef GPS
case FSSP_DATAID_LATLONG :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
uint32_t tmpui = 0;
// the same ID is sent twice, one for longitude, one for latitude
// the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) {
tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
}
else {
tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
}
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
}
break;
#endif
//case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_HEADING :
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCX :
smartPortSendPackage(id, accSmooth[X] / 44);
// unknown input and unknown output unit
// we can only show 00.00 format, another digit won't display right on Taranis
// dividing by roughly 44 will give acceleration in G units
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCY :
smartPortSendPackage(id, accSmooth[Y] / 44);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, accSmooth[Z] / 44);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T1 :
// we send all the flags as decimal digits for easy reading
tmpi = 10000; // start off with at least one digit so the most significant 0 won't be cut off
// the Taranis seems to be able to fit 5 digits on the screen
// the Taranis seems to consider this number a signed 16 bit integer
if (ARMING_FLAG(OK_TO_ARM))
tmpi += 1;
if (ARMING_FLAG(PREVENT_ARMING))
tmpi += 2;
if (ARMING_FLAG(ARMED))
tmpi += 4;
if (FLIGHT_MODE(ANGLE_MODE))
tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE))
tmpi += 20;
if (FLIGHT_MODE(GTUNE_MODE) || FLIGHT_MODE(PASSTHRU_MODE))
tmpi += 40;
if (FLIGHT_MODE(MAG_MODE))
tmpi += 100;
if (FLIGHT_MODE(BARO_MODE))
tmpi += 200;
if (FLIGHT_MODE(SONAR_MODE))
tmpi += 400;
if (FLIGHT_MODE(GPS_HOLD_MODE))
tmpi += 1000;
if (FLIGHT_MODE(GPS_HOME_MODE))
tmpi += 2000;
if (FLIGHT_MODE(HEADFREE_MODE))
tmpi += 4000;
smartPortSendPackage(id, (uint32_t)tmpi);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T2 :
if (sensors(SENSOR_GPS)) {
#ifdef GPS
// provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
smartPortHasRequest = 0;
#endif
}
else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0);
smartPortHasRequest = 0;
}
break;
#ifdef GPS
case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortHasRequest = 0;
}
break;
#endif
case FSSP_DATAID_A4 :
if (feature(FEATURE_VBAT)) {
smartPortSendPackage(id, vbat * 10 / batteryCellCount ); //sending calculated average cell value with 0.01 precision
smartPortHasRequest = 0;
}
break;
default:
break;
// if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
}
}
}
void configureMAVLinkTelemetryPort(void)
{
if (!portConfig) {
return;
}
baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
if (baudRateIndex == BAUD_AUTO) {
// default rate for minimOSD
baudRateIndex = BAUD_57600;
}
mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
if (!mavlinkPort) {
return;
}
mavlinkTelemetryEnabled = true;
}
void configureIbusTelemetryPort(void)
{
if (!ibusSerialPortConfig) {
return;
}
if (isSerialPortShared(ibusSerialPortConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS)) {
// serialRx will open port and handle telemetry
return;
}
ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, SERIAL_BIDIR | (telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED));
if (!ibusSerialPort) {
return;
}
initSharedIbusTelemetry(ibusSerialPort);
ibusTelemetryEnabled = true;
outboundBytesToIgnoreOnRxCount = 0;
}
void configureFrSkyTelemetryPort(void)
{
if (!portConfig) {
return;
}
frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
if (!frskyPort) {
return;
}
frskyTelemetryEnabled = true;
}
void validateAndFixConfig(void)
{
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
featureSet(DEFAULT_RX_FEATURE);
}
if (featureConfigured(FEATURE_RX_PPM)) {
featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_SERIAL | FEATURE_RX_MSP);
}
if (featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM);
}
if (featureConfigured(FEATURE_RX_SERIAL)) {
featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM);
}
if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM);
}
#ifdef STM32F10X
// avoid overloading the CPU on F1 targets when using gyro sync and GPS.
if (imuConfig()->gyroSync && imuConfig()->gyroSyncDenominator < 2 && featureConfigured(FEATURE_GPS)) {
imuConfig()->gyroSyncDenominator = 2;
}
#endif
#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
if (featureConfigured(FEATURE_SOFTSERIAL) && (
0
#ifdef USE_SOFTSERIAL1
|| (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
#endif
#ifdef USE_SOFTSERIAL2
|| (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
#endif
)) {
// led strip needs the same timer as softserial
featureClear(FEATURE_LED_STRIP);
}
#endif
#if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3)
if (doesConfigurationUsePort(SERIAL_PORT_UART3) && featureConfigured(FEATURE_DISPLAY)) {
featureClear(FEATURE_DISPLAY);
}
#endif
#ifdef STM32F303xC
// hardware supports serial port inversion, make users life easier for those that want to connect SBus RX's
#ifdef TELEMETRY
telemetryConfig()->telemetry_inversion = 1;
#endif
#endif
/*
* The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
* The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
*/
if (armingConfig()->retarded_arm && mixerConfig()->pid_at_min_throttle) {
mixerConfig()->pid_at_min_throttle = 0;
}
#if defined(LED_STRIP) && defined(TRANSPONDER) && !defined(UNIT_TEST)
if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) {
featureClear(FEATURE_LED_STRIP);
}
#endif
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
featureClear(FEATURE_SONAR);
featureClear(FEATURE_SOFTSERIAL);
featureClear(FEATURE_RSSI_ADC);
}
#endif
#if defined(COLIBRI_RACE)
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
if (featureConfigured(FEATURE_RX_SERIAL)) {
serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
}
#endif
if (!isSerialConfigValid(serialConfig())) {
PG_RESET_CURRENT(serialConfig);
}
#if defined(USE_VCP)
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
#endif
}
void configureLtmTelemetryPort(void)
{
if (!portConfig) {
return;
}
baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
if (baudRateIndex == BAUD_AUTO) {
baudRateIndex = BAUD_19200;
}
ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
if (!ltmPort)
return;
ltmEnabled = true;
}
