mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Rename the LOG function macros
This commit is contained in:
parent
c30025b9fc
commit
79601a3d25
17 changed files with 90 additions and 90 deletions
|
@ -96,7 +96,7 @@ void logInit(void)
|
|||
}
|
||||
}
|
||||
// Initialization done
|
||||
LOG_I(SYSTEM, "%s/%s %s %s / %s (%s)",
|
||||
LOG_INFO(SYSTEM, "%s/%s %s %s / %s (%s)",
|
||||
FC_FIRMWARE_NAME,
|
||||
targetName,
|
||||
FC_VERSION_STRING,
|
||||
|
|
|
@ -48,41 +48,41 @@ void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) __attribute__
|
|||
void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size);
|
||||
|
||||
#if defined(USE_LOG)
|
||||
#define LOG_E(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUFFER_E(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size)
|
||||
#define LOG_ERROR(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUFFER_ERROR(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size)
|
||||
#else
|
||||
#define LOG_E(...)
|
||||
#define LOG_BUFFER_E(...)
|
||||
#define LOG_ERROR(...)
|
||||
#define LOG_BUFFER_ERROR(...)
|
||||
#endif
|
||||
|
||||
#if defined(USE_LOG)
|
||||
#define LOG_W(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_W(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size)
|
||||
#define LOG_WARNING(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_WARNING(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size)
|
||||
#else
|
||||
#define LOG_W(...)
|
||||
#define LOG_BUF_W(...)
|
||||
#define LOG_WARNING(...)
|
||||
#define LOG_BUF_WARNING(...)
|
||||
#endif
|
||||
|
||||
#if defined(USE_LOG)
|
||||
#define LOG_I(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_I(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size)
|
||||
#define LOG_INFO(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_INFO(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size)
|
||||
#else
|
||||
#define LOG_I(...)
|
||||
#define LOG_BUF_I(...)
|
||||
#define LOG_INFO(...)
|
||||
#define LOG_BUF_INFO(...)
|
||||
#endif
|
||||
|
||||
#if defined(USE_LOG)
|
||||
#define LOG_V(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_V(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size)
|
||||
#define LOG_VERBOSE(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_VERBOSE(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size)
|
||||
#else
|
||||
#define LOG_V(...)
|
||||
#define LOG_BUF_V(...)
|
||||
#define LOG_VERBOSE(...)
|
||||
#define LOG_BUF_VERBOSE(...)
|
||||
#endif
|
||||
|
||||
#if defined(USE_LOG)
|
||||
#define LOG_D(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_D(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size)
|
||||
#define LOG_DEBUG(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__)
|
||||
#define LOG_BUF_DEBUG(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size)
|
||||
#else
|
||||
#define LOG_D(...)
|
||||
#define LOG_BUF_D(...)
|
||||
#define LOG_DEBUG(...)
|
||||
#define LOG_BUF_DEBUG(...)
|
||||
#endif
|
||||
|
|
|
@ -52,11 +52,11 @@ void * memAllocate(size_t wantedSize, resourceOwner_e owner)
|
|||
retPointer = &dynHeap[dynHeapFreeWord];
|
||||
dynHeapFreeWord += wantedWords;
|
||||
dynHeapUsage[owner] += wantedWords * sizeof(uint32_t);
|
||||
LOG_D(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes());
|
||||
LOG_DEBUG(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes());
|
||||
}
|
||||
else {
|
||||
// OOM
|
||||
LOG_E(SYSTEM, "Out of memory");
|
||||
LOG_ERROR(SYSTEM, "Out of memory");
|
||||
ENABLE_ARMING_FLAG(ARMING_DISABLED_OOM);
|
||||
}
|
||||
|
||||
|
|
|
@ -62,7 +62,7 @@ const gyroFilterAndRateConfig_t * chooseGyroConfig(uint8_t desiredLpf, uint16_t
|
|||
}
|
||||
}
|
||||
|
||||
LOG_V(GYRO, "GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X",
|
||||
LOG_VERBOSE(GYRO, "GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X",
|
||||
desiredLpf, desiredRateHz,
|
||||
candidate->gyroLpf, candidate->gyroRateHz,
|
||||
candidate->gyroConfigValues[0], candidate->gyroConfigValues[1]);
|
||||
|
|
|
@ -243,7 +243,7 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
|
|||
|
||||
// Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC)
|
||||
if (checkPwmTimerConflicts(timHw)) {
|
||||
LOG_W(PWM, "Timer output %d skipped", idx);
|
||||
LOG_WARNING(PWM, "Timer output %d skipped", idx);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
@ -305,7 +305,7 @@ static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
|
|||
// Check if too many motors
|
||||
if (motorCount > MAX_MOTORS) {
|
||||
pwmInitError = PWM_INIT_ERROR_TOO_MANY_MOTORS;
|
||||
LOG_E(PWM, "Too many motors. Mixer requested %d, max %d", motorCount, MAX_MOTORS);
|
||||
LOG_ERROR(PWM, "Too many motors. Mixer requested %d, max %d", motorCount, MAX_MOTORS);
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -314,14 +314,14 @@ static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
|
|||
|
||||
// Now if we need to configure individual motor outputs - do that
|
||||
if (!motorsUseHardwareTimers()) {
|
||||
LOG_I(PWM, "Skipped timer init for motors");
|
||||
LOG_INFO(PWM, "Skipped timer init for motors");
|
||||
return;
|
||||
}
|
||||
|
||||
// If mixer requests more motors than we have timer outputs - throw an error
|
||||
if (motorCount > timOutputs->maxTimMotorCount) {
|
||||
pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS;
|
||||
LOG_E(PWM, "Not enough motor outputs. Mixer requested %d, outputs %d", motorCount, timOutputs->maxTimMotorCount);
|
||||
LOG_ERROR(PWM, "Not enough motor outputs. Mixer requested %d, outputs %d", motorCount, timOutputs->maxTimMotorCount);
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -330,7 +330,7 @@ static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
|
|||
const timerHardware_t *timHw = timOutputs->timMotors[idx];
|
||||
if (!pwmMotorConfig(timHw, idx, feature(FEATURE_PWM_OUTPUT_ENABLE))) {
|
||||
pwmInitError = PWM_INIT_ERROR_TIMER_INIT_FAILED;
|
||||
LOG_E(PWM, "Timer allocation failed for motor %d", idx);
|
||||
LOG_ERROR(PWM, "Timer allocation failed for motor %d", idx);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
@ -341,14 +341,14 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
|
|||
const int servoCount = getServoCount();
|
||||
|
||||
if (!isMixerUsingServos()) {
|
||||
LOG_I(PWM, "Mixer does not use servos");
|
||||
LOG_INFO(PWM, "Mixer does not use servos");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check if too many servos
|
||||
if (servoCount > MAX_SERVOS) {
|
||||
pwmInitError = PWM_INIT_ERROR_TOO_MANY_SERVOS;
|
||||
LOG_E(PWM, "Too many servos. Mixer requested %d, max %d", servoCount, MAX_SERVOS);
|
||||
LOG_ERROR(PWM, "Too many servos. Mixer requested %d, max %d", servoCount, MAX_SERVOS);
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -358,14 +358,14 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
|
|||
// Check if we need to init timer output for servos
|
||||
if (!servosUseHardwareTimers()) {
|
||||
// External PWM servo driver
|
||||
LOG_I(PWM, "Skipped timer init for servos - using external servo driver");
|
||||
LOG_INFO(PWM, "Skipped timer init for servos - using external servo driver");
|
||||
return;
|
||||
}
|
||||
|
||||
// If mixer requests more servos than we have timer outputs - throw an error
|
||||
if (servoCount > timOutputs->maxTimServoCount) {
|
||||
pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS;
|
||||
LOG_E(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount);
|
||||
LOG_ERROR(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount);
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -375,7 +375,7 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
|
|||
|
||||
if (!pwmServoConfig(timHw, idx, servoConfig()->servoPwmRate, servoConfig()->servoCenterPulse, feature(FEATURE_PWM_OUTPUT_ENABLE))) {
|
||||
pwmInitError = PWM_INIT_ERROR_TIMER_INIT_FAILED;
|
||||
LOG_E(PWM, "Timer allocation failed for servo %d", idx);
|
||||
LOG_ERROR(PWM, "Timer allocation failed for servo %d", idx);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -130,7 +130,7 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin
|
|||
static pwmOutputPort_t *pwmOutAllocatePort(void)
|
||||
{
|
||||
if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) {
|
||||
LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
|
||||
LOG_ERROR(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
|
|
@ -80,7 +80,7 @@ TCH_t * timerGetTCH(const timerHardware_t * timHw)
|
|||
const int timerIndex = lookupTimerIndex(timHw->tim);
|
||||
|
||||
if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) {
|
||||
LOG_E(TIMER, "Can't find hardware timer definition");
|
||||
LOG_ERROR(TIMER, "Can't find hardware timer definition");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
@ -90,7 +90,7 @@ TCH_t * timerGetTCH(const timerHardware_t * timHw)
|
|||
|
||||
// Check for OOM
|
||||
if (timerCtx[timerIndex] == NULL) {
|
||||
LOG_E(TIMER, "Can't allocate TCH object");
|
||||
LOG_ERROR(TIMER, "Can't allocate TCH object");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
|
|
@ -3432,7 +3432,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
}
|
||||
#endif
|
||||
ENABLE_ARMING_FLAG(SIMULATOR_MODE);
|
||||
LOG_D(SYSTEM, "Simulator enabled");
|
||||
LOG_DEBUG(SYSTEM, "Simulator enabled");
|
||||
}
|
||||
|
||||
if (dataSize >= 14) {
|
||||
|
|
|
@ -275,13 +275,13 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
|
|||
// Previous quaternion valid. Reset to it
|
||||
orientation = *quat;
|
||||
imuErrorEvent.errorCode = 1;
|
||||
LOG_E(IMU, "AHRS orientation quaternion error. Reset to last known good value");
|
||||
LOG_ERROR(IMU, "AHRS orientation quaternion error. Reset to last known good value");
|
||||
}
|
||||
else {
|
||||
// No valid reference. Best guess from accelerometer
|
||||
imuResetOrientationQuaternion(accBF);
|
||||
imuErrorEvent.errorCode = 2;
|
||||
LOG_E(IMU, "AHRS orientation quaternion error. Best guess from ACC");
|
||||
LOG_ERROR(IMU, "AHRS orientation quaternion error. Best guess from ACC");
|
||||
}
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
|
|
|
@ -3167,7 +3167,7 @@ uint32_t afatfs_fwriteSync(afatfsFilePtr_t file, uint8_t *data, uint32_t length)
|
|||
while (true) {
|
||||
uint32_t leftToWrite = length - written;
|
||||
uint32_t justWritten = afatfs_fwrite(file, data + written, leftToWrite);
|
||||
/*if (justWritten != leftToWrite) LOG_E(SYSTEM, "%ld -> %ld", length, written);*/
|
||||
/*if (justWritten != leftToWrite) LOG_ERROR(SYSTEM, "%ld -> %ld", length, written);*/
|
||||
written += justWritten;
|
||||
|
||||
if (written < length) {
|
||||
|
|
|
@ -44,8 +44,8 @@
|
|||
#define FRSKY_OSD_INFO_READY_INTERVAL_MS 5000
|
||||
|
||||
#define FRSKY_OSD_TRACE(fmt, ...)
|
||||
#define FRSKY_OSD_DEBUG(fmt, ...) LOG_D(OSD, "FrSky OSD: " fmt, ##__VA_ARGS__)
|
||||
#define FRSKY_OSD_ERROR(fmt, ...) LOG_E(OSD, "FrSky OSD: " fmt, ##__VA_ARGS__)
|
||||
#define FRSKY_OSD_DEBUG(fmt, ...) LOG_DEBUG(OSD, "FrSky OSD: " fmt, ##__VA_ARGS__)
|
||||
#define FRSKY_OSD_ERROR(fmt, ...) LOG_ERROR(OSD, "FrSky OSD: " fmt, ##__VA_ARGS__)
|
||||
#define FRSKY_OSD_ASSERT(x)
|
||||
|
||||
typedef enum
|
||||
|
|
|
@ -3634,7 +3634,7 @@ static void osdCompleteAsyncInitialization(void)
|
|||
uint8_t y = 1;
|
||||
displayFontMetadata_t metadata;
|
||||
bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort);
|
||||
LOG_D(OSD, "Font metadata version %s: %u (%u chars)",
|
||||
LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)",
|
||||
fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount);
|
||||
|
||||
if (fontHasMetadata && metadata.charCount > 256) {
|
||||
|
|
|
@ -183,18 +183,18 @@ static uint8_t CRC8(const uint8_t *data, const int8_t len)
|
|||
|
||||
static void saPrintSettings(void)
|
||||
{
|
||||
LOG_D(VTX, "Current status: version: %d", saDevice.version);
|
||||
LOG_D(VTX, " mode(0x%x): fmode=%s", saDevice.mode, (saDevice.mode & 1) ? "freq" : "chan");
|
||||
LOG_D(VTX, " pit=%s ", (saDevice.mode & 2) ? "on " : "off");
|
||||
LOG_D(VTX, " inb=%s", (saDevice.mode & 4) ? "on " : "off");
|
||||
LOG_D(VTX, " outb=%s", (saDevice.mode & 8) ? "on " : "off");
|
||||
LOG_D(VTX, " lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked");
|
||||
LOG_D(VTX, " deferred=%s", (saDevice.mode & 32) ? "on" : "off");
|
||||
LOG_D(VTX, " channel: %d ", saDevice.channel);
|
||||
LOG_D(VTX, "freq: %d ", saDevice.freq);
|
||||
LOG_D(VTX, "power: %d ", saDevice.power);
|
||||
LOG_D(VTX, "pitfreq: %d ", saDevice.orfreq);
|
||||
LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no");
|
||||
LOG_DEBUG(VTX, "Current status: version: %d", saDevice.version);
|
||||
LOG_DEBUG(VTX, " mode(0x%x): fmode=%s", saDevice.mode, (saDevice.mode & 1) ? "freq" : "chan");
|
||||
LOG_DEBUG(VTX, " pit=%s ", (saDevice.mode & 2) ? "on " : "off");
|
||||
LOG_DEBUG(VTX, " inb=%s", (saDevice.mode & 4) ? "on " : "off");
|
||||
LOG_DEBUG(VTX, " outb=%s", (saDevice.mode & 8) ? "on " : "off");
|
||||
LOG_DEBUG(VTX, " lock=%s", (saDevice.mode & 16) ? "unlocked" : "locked");
|
||||
LOG_DEBUG(VTX, " deferred=%s", (saDevice.mode & 32) ? "on" : "off");
|
||||
LOG_DEBUG(VTX, " channel: %d ", saDevice.channel);
|
||||
LOG_DEBUG(VTX, "freq: %d ", saDevice.freq);
|
||||
LOG_DEBUG(VTX, "power: %d ", saDevice.power);
|
||||
LOG_DEBUG(VTX, "pitfreq: %d ", saDevice.orfreq);
|
||||
LOG_DEBUG(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no");
|
||||
}
|
||||
|
||||
int saDacToPowerIndex(int dac)
|
||||
|
@ -243,19 +243,19 @@ static void saAutobaud(void)
|
|||
return;
|
||||
}
|
||||
|
||||
LOG_D(VTX, "autobaud: adjusting");
|
||||
LOG_DEBUG(VTX, "autobaud: adjusting");
|
||||
|
||||
if ((sa_adjdir == 1) && (sa_smartbaud == SMARTBAUD_MAX)) {
|
||||
sa_adjdir = -1;
|
||||
LOG_D(VTX, "autobaud: now going down");
|
||||
LOG_DEBUG(VTX, "autobaud: now going down");
|
||||
} else if ((sa_adjdir == -1 && sa_smartbaud == SMARTBAUD_MIN)) {
|
||||
sa_adjdir = 1;
|
||||
LOG_D(VTX, "autobaud: now going up");
|
||||
LOG_DEBUG(VTX, "autobaud: now going up");
|
||||
}
|
||||
|
||||
sa_smartbaud += sa_baudstep * sa_adjdir;
|
||||
|
||||
LOG_D(VTX, "autobaud: %d", sa_smartbaud);
|
||||
LOG_DEBUG(VTX, "autobaud: %d", sa_smartbaud);
|
||||
|
||||
smartAudioSerialPort->vTable->serialSetBaudRate(smartAudioSerialPort, sa_smartbaud);
|
||||
|
||||
|
@ -280,7 +280,7 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
sa_outstanding = SA_CMD_NONE;
|
||||
} else {
|
||||
saStat.ooopresp++;
|
||||
LOG_D(VTX, "processResponse: outstanding %d got %d", sa_outstanding, resp);
|
||||
LOG_DEBUG(VTX, "processResponse: outstanding %d got %d", sa_outstanding, resp);
|
||||
}
|
||||
|
||||
switch (resp) {
|
||||
|
@ -307,7 +307,7 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
if (saDevice.mode & SA_MODE_GET_PITMODE) {
|
||||
bool newBootMode = (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE);
|
||||
if (newBootMode != saDevice.willBootIntoPitMode) {
|
||||
LOG_D(VTX, "saProcessResponse: willBootIntoPitMode is now %s\r\n", newBootMode ? "true" : "false");
|
||||
LOG_DEBUG(VTX, "saProcessResponse: willBootIntoPitMode is now %s\r\n", newBootMode ? "true" : "false");
|
||||
}
|
||||
saDevice.willBootIntoPitMode = newBootMode;
|
||||
}
|
||||
|
@ -315,7 +315,7 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
if(saDevice.version == SA_2_1) {
|
||||
//read dbm based power levels
|
||||
if(len < 10) { //current power level in dbm field missing or power level length field missing or zero power levels reported
|
||||
LOG_D(VTX, "processResponse: V2.1 vtx didn't report any power levels\r\n");
|
||||
LOG_DEBUG(VTX, "processResponse: V2.1 vtx didn't report any power levels\r\n");
|
||||
break;
|
||||
}
|
||||
saPowerCount = constrain((int8_t)buf[8], 0, VTX_SMARTAUDIO_MAX_POWER_COUNT);
|
||||
|
@ -336,14 +336,14 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
}
|
||||
}
|
||||
|
||||
LOG_D(VTX, "processResponse: %d power values: %d, %d, %d, %d\r\n",
|
||||
LOG_DEBUG(VTX, "processResponse: %d power values: %d, %d, %d, %d\r\n",
|
||||
saPowerCount, saPowerTable[0].dbi, saPowerTable[1].dbi,
|
||||
saPowerTable[2].dbi, saPowerTable[3].dbi);
|
||||
//LOG_D(VTX, "processResponse: V2.1 received vtx power value %d\r\n",buf[7]);
|
||||
//LOG_DEBUG(VTX, "processResponse: V2.1 received vtx power value %d\r\n",buf[7]);
|
||||
rawPowerValue = buf[7];
|
||||
|
||||
saDevice.power = 0; //set to unknown power level if the reported one doesnt match any of the known ones
|
||||
LOG_D(VTX, "processResponse: rawPowerValue is %d, legacy power is %d\r\n", rawPowerValue, buf[3]);
|
||||
LOG_DEBUG(VTX, "processResponse: rawPowerValue is %d, legacy power is %d\r\n", rawPowerValue, buf[3]);
|
||||
for (int8_t i = 0; i < saPowerCount; i++) {
|
||||
if (rawPowerValue == saPowerTable[i].dbi) {
|
||||
saDevice.power = i + 1;
|
||||
|
@ -374,18 +374,18 @@ static void saProcessResponse(uint8_t *buf, int len)
|
|||
|
||||
if (freq & SA_FREQ_GETPIT) {
|
||||
saDevice.orfreq = freq & SA_FREQ_MASK;
|
||||
LOG_D(VTX, "saProcessResponse: GETPIT freq %d", saDevice.orfreq);
|
||||
LOG_DEBUG(VTX, "saProcessResponse: GETPIT freq %d", saDevice.orfreq);
|
||||
} else if (freq & SA_FREQ_SETPIT) {
|
||||
saDevice.orfreq = freq & SA_FREQ_MASK;
|
||||
LOG_D(VTX, "saProcessResponse: SETPIT freq %d", saDevice.orfreq);
|
||||
LOG_DEBUG(VTX, "saProcessResponse: SETPIT freq %d", saDevice.orfreq);
|
||||
} else {
|
||||
saDevice.freq = freq;
|
||||
LOG_D(VTX, "saProcessResponse: SETFREQ freq %d", freq);
|
||||
LOG_DEBUG(VTX, "saProcessResponse: SETFREQ freq %d", freq);
|
||||
}
|
||||
break;
|
||||
|
||||
case SA_CMD_SET_MODE: // Set Mode
|
||||
LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x, (pir %s, por %s, pitdsbl %s, %s)\r\n",
|
||||
LOG_DEBUG(VTX, "saProcessResponse: SET_MODE 0x%x, (pir %s, por %s, pitdsbl %s, %s)\r\n",
|
||||
buf[2], (buf[2] & 1) ? "on" : "off", (buf[2] & 2) ? "on" : "off", (buf[3] & 4) ? "on" : "off",
|
||||
(buf[4] & 8) ? "unlocked" : "locked");
|
||||
break;
|
||||
|
@ -595,7 +595,7 @@ static void saGetSettings(void)
|
|||
{
|
||||
static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
|
||||
|
||||
LOG_D(VTX, "smartAudioGetSettings\r\n");
|
||||
LOG_DEBUG(VTX, "smartAudioGetSettings\r\n");
|
||||
saQueueCmd(bufGetSettings, 5);
|
||||
}
|
||||
|
||||
|
@ -605,11 +605,11 @@ void saSetFreq(uint16_t freq)
|
|||
static uint8_t switchBuf[7];
|
||||
|
||||
if (freq & SA_FREQ_GETPIT) {
|
||||
LOG_D(VTX, "smartAudioSetFreq: GETPIT");
|
||||
LOG_DEBUG(VTX, "smartAudioSetFreq: GETPIT");
|
||||
} else if (freq & SA_FREQ_SETPIT) {
|
||||
LOG_D(VTX, "smartAudioSetFreq: SETPIT %d", freq & SA_FREQ_MASK);
|
||||
LOG_DEBUG(VTX, "smartAudioSetFreq: SETPIT %d", freq & SA_FREQ_MASK);
|
||||
} else {
|
||||
LOG_D(VTX, "smartAudioSetFreq: SET %d", freq);
|
||||
LOG_DEBUG(VTX, "smartAudioSetFreq: SET %d", freq);
|
||||
}
|
||||
|
||||
buf[4] = (freq >> 8) & 0xff;
|
||||
|
@ -649,7 +649,7 @@ void saSetBandAndChannel(uint8_t band, uint8_t channel)
|
|||
|
||||
buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
|
||||
buf[5] = CRC8(buf, 5);
|
||||
LOG_D(VTX, "vtxSASetBandAndChannel set index band %d channel %d value sent 0x%x\r\n", band, channel, buf[4]);
|
||||
LOG_DEBUG(VTX, "vtxSASetBandAndChannel set index band %d channel %d value sent 0x%x\r\n", band, channel, buf[4]);
|
||||
|
||||
//this will clear saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ
|
||||
saQueueCmd(buf, 6);
|
||||
|
@ -666,7 +666,7 @@ void saSetMode(int mode)
|
|||
//the response will just say pit=off but the device will still go into pitmode on reboot.
|
||||
//therefore we have to memorize this change here.
|
||||
}
|
||||
LOG_D(VTX, "saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off",
|
||||
LOG_DEBUG(VTX, "saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off",
|
||||
(mode & 4)? "on " : "off", (mode & 8) ? "locked" : "unlocked");
|
||||
|
||||
buf[5] = CRC8(buf, 5);
|
||||
|
@ -735,7 +735,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
|
|||
if (saDevice.version >= SA_2_0 ) {
|
||||
//did the device boot up in pit mode on its own?
|
||||
saDevice.willBootIntoPitMode = (saDevice.mode & SA_MODE_GET_PITMODE) ? true : false;
|
||||
LOG_D(VTX, "sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false");
|
||||
LOG_DEBUG(VTX, "sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false");
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
@ -757,17 +757,17 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
|
|||
|
||||
if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > SMARTAUDIO_CMD_TIMEOUT)) {
|
||||
// Last command timed out
|
||||
// LOG_D(VTX, "process: resending 0x%x", sa_outstanding);
|
||||
// LOG_DEBUG(VTX, "process: resending 0x%x", sa_outstanding);
|
||||
// XXX Todo: Resend termination and possible offline transition
|
||||
saResendCmd();
|
||||
lastCommandSentMs = nowMs;
|
||||
} else if (!saQueueEmpty()) {
|
||||
// Command pending. Send it.
|
||||
// LOG_D(VTX, "process: sending queue");
|
||||
// LOG_DEBUG(VTX, "process: sending queue");
|
||||
saSendQueue();
|
||||
lastCommandSentMs = nowMs;
|
||||
} else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
|
||||
//LOG_D(VTX, "process: sending status change polling");
|
||||
//LOG_DEBUG(VTX, "process: sending status change polling");
|
||||
saGetSettings();
|
||||
saSendQueue();
|
||||
}
|
||||
|
@ -803,16 +803,16 @@ void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channe
|
|||
}
|
||||
|
||||
if (index == 0) {
|
||||
LOG_D(VTX, "SmartAudio doesn't support power off");
|
||||
LOG_DEBUG(VTX, "SmartAudio doesn't support power off");
|
||||
return;
|
||||
}
|
||||
|
||||
if (index > saPowerCount) {
|
||||
LOG_D(VTX, "Invalid power level");
|
||||
LOG_DEBUG(VTX, "Invalid power level");
|
||||
return;
|
||||
}
|
||||
|
||||
LOG_D(VTX, "saSetPowerByIndex: index %d, value %d\r\n", index, buf[4]);
|
||||
LOG_DEBUG(VTX, "saSetPowerByIndex: index %d, value %d\r\n", index, buf[4]);
|
||||
|
||||
index--;
|
||||
switch (saDevice.version) {
|
||||
|
@ -853,10 +853,10 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
|
|||
buf[4] = 0 | 128;
|
||||
buf[5] = CRC8(buf, 5);
|
||||
saQueueCmd(buf, 6);
|
||||
LOG_D(VTX, "vtxSASetPitMode: set power to 0 dbm\r\n");
|
||||
LOG_DEBUG(VTX, "vtxSASetPitMode: set power to 0 dbm\r\n");
|
||||
} else {
|
||||
saSetMode(SA_MODE_CLR_PITMODE);
|
||||
LOG_D(VTX, "vtxSASetPitMode: clear pitmode permanently");
|
||||
LOG_DEBUG(VTX, "vtxSASetPitMode: clear pitmode permanently");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
@ -871,7 +871,7 @@ static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
|
|||
// ensure when turning on pit mode that pit mode gets actually enabled
|
||||
newMode |= SA_MODE_SET_IN_RANGE_PITMODE;
|
||||
}
|
||||
LOG_D(VTX, "vtxSASetPitMode %s with stored mode 0x%x por %s, pir %s, newMode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode,
|
||||
LOG_DEBUG(VTX, "vtxSASetPitMode %s with stored mode 0x%x por %s, pir %s, newMode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode,
|
||||
(saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) ? "on" : "off",
|
||||
(saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) ? "on" : "off" , newMode);
|
||||
|
||||
|
|
|
@ -438,7 +438,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
|
|||
if (gravityCalibrationComplete()) {
|
||||
zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
|
||||
setGravityCalibrationAndWriteEEPROM(posEstimator.imu.calibratedGravityCMSS);
|
||||
LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
|
||||
LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
|
|
|
@ -317,7 +317,7 @@ int32_t baroCalculateAltitude(void)
|
|||
if (zeroCalibrationIsCompleteS(&zeroCalibration)) {
|
||||
zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure);
|
||||
baroGroundAltitude = pressureToAltitude(baroGroundPressure);
|
||||
LOG_D(BARO, "Barometer calibration complete (%d)", (int)lrintf(baroGroundAltitude));
|
||||
LOG_DEBUG(BARO, "Barometer calibration complete (%d)", (int)lrintf(baroGroundAltitude));
|
||||
}
|
||||
|
||||
baro.BaroAlt = 0;
|
||||
|
|
|
@ -365,7 +365,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe
|
|||
setGyroCalibrationAndWriteEEPROM(dev->gyroZero);
|
||||
#endif
|
||||
|
||||
LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]);
|
||||
LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]);
|
||||
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
|
||||
} else {
|
||||
dev->gyroZero[X] = 0;
|
||||
|
|
|
@ -177,7 +177,7 @@ static void performPitotCalibrationCycle(void)
|
|||
|
||||
if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
|
||||
zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
|
||||
LOG_D(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
|
||||
LOG_DEBUG(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue