void disarm(void)
{
if (ARMING_FLAG(ARMED)) {
DISABLE_ARMING_FLAG(ARMED);
lastDisarmTimeUs = micros();
#ifdef USE_BLACKBOX
if (blackboxConfig()->device && blackboxConfig()->mode != BLACKBOX_MODE_ALWAYS_ON) { // Close the log upon disarm except when logging mode is ALWAYS ON
blackboxFinish();
}
#endif
BEEP_OFF;
#ifdef USE_DSHOT
if (isMotorProtocolDshot() && flipOverAfterCrashMode && !feature(FEATURE_3D)) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
#endif
flipOverAfterCrashMode = false;
// if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead
if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) {
beeper(BEEPER_DISARMING); // emit disarm tone
}
}
}
bool mscCheckFilesystemReady(void)
{
return false
#if defined(USE_SDCARD)
|| (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD && sdcard_isFunctional())
#endif
#if defined(USE_FLASHFS)
|| (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH && flashfsGetSize() > 0)
#endif
;
}
// Print the null-terminated string 's' to the blackbox device and return the number of bytes written
int blackboxPrint(const char *s)
{
int length;
const uint8_t *pos;
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
length = strlen(s);
flashfsWrite((const uint8_t*) s, length, false); // Write asynchronously
break;
#endif
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
length = strlen(s);
afatfs_fwrite(blackboxSDCard.logFile, (const uint8_t*) s, length); // Ignore failures due to buffers filling up
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
pos = (uint8_t*) s;
while (*pos) {
serialWrite(blackboxPort, *pos);
pos++;
}
length = pos - (uint8_t*) s;
break;
}
return length;
}
static long cmsx_menuBlackboxOnEnter(void)
{
if (!featureRead) {
cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
featureRead = true;
}
blackboxConfig_rate_denom = blackboxConfig()->rate_denom;
return 0;
}
void targetValidateConfiguration(void)
{
/* make sure the SDCARD cannot be turned on */
if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) {
if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE;
}
}
}
static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue)
{
#ifndef USE_BLACKBOX
UNUSED(adjustmentFunction);
UNUSED(newFloatValue);
#else
if (blackboxConfig()->device) {
flightLogEvent_inflightAdjustment_t eventData;
eventData.adjustmentFunction = adjustmentFunction;
eventData.newFloatValue = newFloatValue;
eventData.floatFlag = true;
blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
}
#endif
}
void mwDisarm(void)
{
armingCalibrationWasInitialised = false;
if (ARMING_FLAG(ARMED)) {
DISABLE_ARMING_FLAG(ARMED);
#ifdef BLACKBOX
if (blackboxConfig()->device) {
finishBlackbox();
}
#endif
BEEP_OFF;
beeper(BEEPER_DISARMING); // emit disarm tone
}
}
void blackboxWrite(uint8_t value)
{
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
flashfsWriteByte(value); // Write byte asynchronously
break;
#endif
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
afatfs_fputc(blackboxSDCard.logFile, value);
break;
#endif
case BLACKBOX_DEVICE_SERIAL:
default:
serialWrite(blackboxPort, value);
break;
}
}
static void osdGetBlackboxStatusString(char * buff)
{
bool storageDeviceIsWorking = false;
uint32_t storageUsed = 0;
uint32_t storageTotal = 0;
switch (blackboxConfig()->device) {
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY);
if (storageDeviceIsWorking) {
storageTotal = sdcard_getMetadata()->numBlocks / 2000;
storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
}
break;
#endif
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
storageDeviceIsWorking = flashfsIsReady();
if (storageDeviceIsWorking) {
const flashGeometry_t *geometry = flashfsGetGeometry();
storageTotal = geometry->totalSize / 1024;
storageUsed = flashfsGetOffset() / 1024;
}
break;
#endif
default:
break;
}
if (storageDeviceIsWorking) {
const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
tfp_sprintf(buff, "%d%%", storageUsedPercent);
} else {
tfp_sprintf(buff, "FAULT");
}
}
static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {
startTime = micros();
}
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#ifdef USE_BLACKBOX
if (!cliMode && blackboxConfig()->device) {
blackboxUpdate(currentTimeUs);
}
#else
UNUSED(currentTimeUs);
#endif
DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY)) {
gyroReadTemperature();
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#if defined(BARO) || defined(SONAR)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold();
}
}
#endif
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
#endif
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
#endif
) {
resetYawAxis();
}
if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
}
processRcCommand();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();
}
}
#endif
#ifdef USE_SDCARD
afatfs_poll();
#endif
#ifdef BLACKBOX
if (!cliMode && blackboxConfig()->device) {
handleBlackbox(currentTimeUs);
}
#else
UNUSED(currentTimeUs);
#endif
#ifdef TRANSPONDER
transponderUpdate(currentTimeUs);
#endif
DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
}
static void osdShowStats(uint16_t endBatteryVoltage)
{
uint8_t top = 2;
char buff[OSD_ELEMENT_BUFFER_LENGTH];
displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, 2, top++, " --- STATS ---");
if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
bool success = false;
#ifdef USE_RTC_TIME
success = osdFormatRtcDateTime(&buff[0]);
#endif
if (!success) {
tfp_sprintf(buff, "NO RTC");
}
displayWrite(osdDisplayPort, 2, top++, buff);
}
if (osdStatGetState(OSD_STAT_TIMER_1)) {
osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
}
if (osdStatGetState(OSD_STAT_TIMER_2)) {
osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
}
if (osdStatGetState(OSD_STAT_MAX_SPEED) && STATE(GPS_FIX)) {
itoa(stats.max_speed, buff, 10);
osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
}
if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) {
tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
}
if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT);
osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_END_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT);
osdDisplayStatisticLabel(top++, "END BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_BATTERY)) {
tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
osdDisplayStatisticLabel(top++, "BATTERY", buff);
}
if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
itoa(stats.min_rssi, buff, 10);
strcat(buff, "%");
osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
}
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
itoa(stats.max_current, buff, 10);
strcat(buff, "A");
osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
}
if (osdStatGetState(OSD_STAT_USED_MAH)) {
tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
osdDisplayStatisticLabel(top++, "USED MAH", buff);
}
}
if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
osdFormatAltitudeString(buff, stats.max_altitude);
osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
}
#ifdef USE_BLACKBOX
if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
osdGetBlackboxStatusString(buff);
osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
}
if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
itoa(blackboxGetLogNumber(), buff, 10);
osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
}
#endif
}
