diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b3bdf0d5c0..22893ab8d1 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -489,11 +489,11 @@ const char * const lookupTableCrsfFmReuse[] = { }; const char * const lookupTableCrsfAttReuse[] = { - "NONE", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", + "NONE", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", }; const char * const lookupTableCrsfGpsReuse[] = { - "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", + "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", }; const char * const lookupTableCrsfGpsSatsReuse[] = { diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 7f56979979..670d56811f 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -804,6 +804,16 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, escSensorConfig()->hw4_current_gain); sbufWriteU8(dst, escSensorConfig()->hw4_voltage_gain); break; + + case MSP_ESC_PARAMETERS: + { + const uint8_t len = escGetParamBufferLength(); + if (len == 0) + return false; + + sbufWriteData(dst, escGetParamBuffer(), len); + } + break; #endif case MSP_BATTERY_STATE: @@ -2641,6 +2651,19 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, escSensorConfigMutable()->hw4_current_gain = sbufReadU8(src); escSensorConfigMutable()->hw4_voltage_gain = sbufReadU8(src); break; + + case MSP_SET_ESC_PARAMETERS: + { + const uint8_t len = escGetParamBufferLength(); + if (len == 0) + return MSP_RESULT_ERROR; + + sbufReadData(src, escGetParamUpdBuffer(), len); + + if (!escCommitParameters()) + return MSP_RESULT_ERROR; + } + break; #endif case MSP_EEPROM_WRITE: diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index b198ddb98a..11b1aa9919 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -234,6 +234,9 @@ #define MSP_SET_NAV_CONFIG 215 #define MSP_SET_ESC_SENSOR_CONFIG 216 +#define MSP_ESC_PARAMETERS 217 +#define MSP_SET_ESC_PARAMETERS 218 + #define MSP_SET_RESET_CURR_PID 219 #define MSP_SET_SENSOR_ALIGNMENT 220 #define MSP_SET_LED_STRIP_MODECOLOR 221 diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 06235816fa..3dfcbaa6fd 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -100,6 +100,15 @@ enum { }; #define TELEMETRY_BUFFER_SIZE 40 +#define REQUEST_BUFFER_SIZE 40 +#define PARAM_BUFFER_SIZE 96 +#define PARAM_HEADER_SIZE 2 +#define PARAM_HEADER_SIG 0 +#define PARAM_HEADER_VER 1 +#define PARAM_HEADER_VER_MASK 0x3F +#define PARAM_HEADER_CMD_MASK 0xC0 +#define PARAM_HEADER_RDONLY 0x40 +#define PARAM_HEADER_USER 0x80 static serialPort_t *escSensorPort = NULL; @@ -124,9 +133,26 @@ static uint8_t buffer[TELEMETRY_BUFFER_SIZE] = { 0, }; static volatile uint8_t bufferSize = 0; static volatile uint8_t bufferPos = 0; -static uint8_t readBytes = 0; +static volatile uint8_t readBytes = 0; +static volatile uint8_t readIngoreBytes = 0; static uint32_t syncCount = 0; +static uint8_t reqLength = 0; +static uint8_t reqbuffer[REQUEST_BUFFER_SIZE] = { 0, }; + +static uint8_t paramPayloadLength = 0; +static uint8_t paramBuffer[PARAM_BUFFER_SIZE] = { 0, }; +static uint8_t paramUpdBuffer[PARAM_BUFFER_SIZE] = { 0, }; +static uint8_t *paramPayload = paramBuffer + PARAM_HEADER_SIZE; +static uint8_t *paramUpdPayload = paramUpdBuffer + PARAM_HEADER_SIZE; +static uint8_t paramSig = 0; +static uint8_t paramVer = 0; +static bool paramMspActive = false; + +// called on MSP_SET_ESC_PARAMETERS when paramUpdPayload / paramUpdBuffer ready +typedef bool (*paramCommitCallbackPtr)(uint8_t cmd); +static paramCommitCallbackPtr paramCommit = NULL; + bool isEscSensorActive(void) { @@ -603,6 +629,7 @@ static void hw4SensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = thr; escSensorData[0].pwm = pwm; escSensorData[0].voltage = lrintf(voltage * 1000); escSensorData[0].current = lrintf(current * 1000); @@ -772,10 +799,13 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) if (calculateCRC16_MODBUS(buffer, 30) == crc) { uint32_t rpm = buffer[14] << 8 | buffer[13]; uint16_t power = buffer[9]; + uint16_t fault = buffer[12]; uint16_t voltage = buffer[16] << 8 | buffer[15]; uint16_t current = buffer[18] << 8 | buffer[17]; uint16_t tempFET = buffer[19]; uint16_t tempBEC = buffer[20]; + uint16_t voltBEC = buffer[22]; + uint16_t currBEC = buffer[23]; // When throttle changes to zero, the last current reading is // repeated until the motor has totally stopped. @@ -787,11 +817,15 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = power * 10; escSensorData[0].pwm = power * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; escSensorData[0].temperature = tempFET * 10; escSensorData[0].temperature2 = tempBEC * 10; + escSensorData[0].bec_voltage = voltBEC * 100; + escSensorData[0].bec_current = currBEC * 100; + escSensorData[0].status = fault; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tempFET * 10); @@ -824,138 +858,6 @@ static void hw5SensorProcess(timeUs_t currentTimeUs) } -/* - * Scorpion Unsolicited Telemetry - * - * - ESC must be set to "Unsolicited mode" - * - Serial protocol is 38400,8N1 - * - Frame rate running:10Hz idle:1Hz - * - Little-Endian fields - * - CRC16-CCITT - * - Error Code bits: - * 0: N/A - * 1: BEC voltage error - * 2: Temperature error - * 3: Consumption error - * 4: Input voltage error - * 5: Current error - * 6: N/A - * 7: Throttle error - * - * Frame Format - * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― - * 0: Header Sync (0x55) - * 1: Message format version (0x00) - * 2: Message Length incl. header and CRC (22) - * 3: Device ID - * 4-6: Timestamp ms - * 7: Throttle in 0.5% - * 8-9: Current in 0.1A - * 10-11: Voltage in 0.1V - * 12-13: Consumption in mAh - * 14: Temperature in °C - * 15: PWM duty cycle in 0.5% - * 16: BEC voltage in 0.1V - * 17-18: RPM in 5rpm steps - * 19: Error code - * 20-21: CRC16 CCITT - * - */ - -static uint16_t calculateCRC16_CCITT(const uint8_t *ptr, size_t len) -{ - uint16_t crc = 0; - - while (len--) { - crc ^= *ptr++; - for (int i = 0; i < 8; i++) - crc = (crc & 1) ? (crc >> 1) ^ 0x8408 : (crc >> 1); - } - - return crc; -} - -static bool processUNCTelemetryStream(uint8_t dataByte) -{ - totalByteCount++; - - buffer[readBytes++] = dataByte; - - if (readBytes == 1) { - if (dataByte != 0x55) - frameSyncError(); - } - else if (readBytes == 2) { - if (dataByte != 0x00) // Proto v0 - frameSyncError(); - } - else if (readBytes == 3) { - if (dataByte != 22) // Message v0 is 22 bytes - frameSyncError(); - else - syncCount++; - } - else if (readBytes == 22) { - readBytes = 0; - return true; - } - - return false; -} - -static void uncSensorProcess(timeUs_t currentTimeUs) -{ - // check for any available bytes in the rx buffer - while (serialRxBytesWaiting(escSensorPort)) { - if (processUNCTelemetryStream(serialRead(escSensorPort))) { - uint16_t crc = buffer[21] << 8 | buffer[20]; - - if (calculateCRC16_CCITT(buffer, 20) == crc) { - uint16_t rpm = buffer[18] << 8 | buffer[17]; - uint16_t temp = buffer[14]; - uint16_t power = buffer[15]; - uint16_t voltage = buffer[11] << 8 | buffer[10]; - uint16_t current = buffer[9] << 8 | buffer[8]; - uint16_t capacity = buffer[13] << 8 | buffer[12]; - uint16_t status = buffer[19]; - - escSensorData[0].age = 0; - escSensorData[0].erpm = rpm * 5; - escSensorData[0].pwm = power * 5; - escSensorData[0].voltage = voltage * 100; - escSensorData[0].current = current * 100; - escSensorData[0].consumption = capacity; - escSensorData[0].temperature = temp * 10; - - DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 5); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, voltage * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, current * 10); - - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, rpm); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, power); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, temp); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, voltage); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, current); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, capacity); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, status); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); - - dataUpdateUs = currentTimeUs; - - totalFrameCount++; - } - else { - totalCrcErrorCount++; - } - } - } - - // Maximum frame spacing 1000ms - checkFrameTimeout(currentTimeUs, 1200000); -} - - /* * Kontronik Telemetry V4 (23.11.2018) * @@ -1106,21 +1008,29 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs) uint32_t crc = kontronikDecodeCRC(kontronikPacketLength - KON_CRC_LENGTH); if (calculateCRC32(buffer, kontronikPacketLength - kontronikCrcExclude - KON_CRC_LENGTH) == crc) { uint32_t rpm = buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]; + int16_t throttle = (int8_t)buffer[24]; uint16_t pwm = buffer[23] << 8 | buffer[22]; uint16_t voltage = buffer[9] << 8 | buffer[8]; uint16_t current = buffer[11] << 8 | buffer[10]; uint16_t capacity = buffer[17] << 8 | buffer[16]; int16_t tempFET = (int8_t)buffer[26]; int16_t tempBEC = (int8_t)buffer[27]; + uint16_t currBEC = buffer[19] << 8 | buffer[18]; + uint16_t voltBEC = buffer[21] << 8 | buffer[20]; + uint32_t status = buffer[31] << 24 | buffer[30] << 16 | buffer[29] << 8 | buffer[28]; escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = (throttle + 100) * 5; escSensorData[0].pwm = pwm * 10; escSensorData[0].voltage = voltage * 10; escSensorData[0].current = current * 100; escSensorData[0].consumption = capacity; escSensorData[0].temperature = tempFET * 10; escSensorData[0].temperature2 = tempBEC * 10; + escSensorData[0].bec_voltage = voltBEC; + escSensorData[0].bec_current = currBEC; + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tempFET * 10); @@ -1216,6 +1126,7 @@ static void ompSensorProcess(timeUs_t currentTimeUs) // Make sure this is OMP M4 ESC if (buffer[1] == 0x01 && buffer[2] == 0x20 && buffer[11] == 0 && buffer[18] == 0 && buffer[20] == 0) { uint16_t rpm = buffer[8] << 8 | buffer[9]; + uint16_t throttle = buffer[7]; uint16_t pwm = buffer[12]; uint16_t temp = buffer[10]; uint16_t voltage = buffer[3] << 8 | buffer[4]; @@ -1225,11 +1136,13 @@ static void ompSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].pwm = pwm * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; escSensorData[0].consumption = capacity; escSensorData[0].temperature = temp * 10; + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); @@ -1334,19 +1247,24 @@ static void ztwSensorProcess(timeUs_t currentTimeUs) if (buffer[1] == 0x01 && buffer[2] == 0x20) { uint16_t rpm = buffer[8] << 8 | buffer[9]; uint16_t temp = buffer[10]; + uint16_t throttle = buffer[7]; uint16_t power = buffer[12]; uint16_t voltage = buffer[3] << 8 | buffer[4]; uint16_t current = buffer[5] << 8 | buffer[6]; uint16_t capacity = buffer[15] << 8 | buffer[16]; uint16_t status = buffer[13] << 8 | buffer[14]; + uint16_t voltBEC = buffer[19]; escSensorData[0].age = 0; escSensorData[0].erpm = rpm * 10; + escSensorData[0].throttle = throttle * 10; escSensorData[0].pwm = power * 10; escSensorData[0].voltage = voltage * 100; escSensorData[0].current = current * 100; escSensorData[0].consumption = capacity; escSensorData[0].temperature = temp * 10; + escSensorData[0].bec_voltage = voltBEC * 1000; + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); @@ -1472,6 +1390,7 @@ static void apdSensorProcess(timeUs_t currentTimeUs) if (calculateFletcher16(buffer + 2, 18) == crc) { uint16_t rpm = buffer[13] << 24 | buffer[12] << 16 | buffer[11] << 8 | buffer[10]; uint16_t tadc = buffer[3] << 8 | buffer[2]; + uint16_t throttle = buffer[15] << 8 | buffer[14]; uint16_t power = buffer[17] << 8 | buffer[16]; uint16_t voltage = buffer[1] << 8 | buffer[0]; uint16_t current = buffer[5] << 8 | buffer[4]; @@ -1483,10 +1402,12 @@ static void apdSensorProcess(timeUs_t currentTimeUs) escSensorData[0].age = 0; escSensorData[0].erpm = rpm; + escSensorData[0].throttle = throttle; escSensorData[0].pwm = power; escSensorData[0].voltage = voltage * 10; escSensorData[0].current = current * 80; escSensorData[0].temperature = lrintf(temp * 10); + escSensorData[0].status = status; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, lrintf(temp * 10)); @@ -1518,6 +1439,658 @@ static void apdSensorProcess(timeUs_t currentTimeUs) } +/* + * RRFSM + * + */ +// > 0: frame accepted, count as synced (may not be complete), continue accepting +// < 0: frame rejected, will be logged as sync error +// 0: continue accepting +typedef int8_t (*rrfsmAcceptCallbackPtr)(uint16_t c); + +typedef bool (*rrfsmStartCallbackPtr)(timeMs_t currentTimeMs); // return true to continue w/ default initialization (if in doubt return true) +typedef bool (*rrfsmDecodeCallbackPtr)(timeMs_t currentTimeMs); // return true if frame was decoded successfully +typedef bool (*rrfsmCrankCallbackPtr)(timeMs_t currentTimeMs); // return true to continue w/ default loop (advanced, if in doubt return true) + +static rrfsmAcceptCallbackPtr rrfsmAccept = NULL; +static rrfsmStartCallbackPtr rrfsmStart = NULL; +static rrfsmDecodeCallbackPtr rrfsmDecode = NULL; +static rrfsmCrankCallbackPtr rrfsmCrank = NULL; + +static uint16_t rrfsmBootDelayMs = 0; +static uint8_t rrfsmMinFrameLength = 0; + +static uint32_t rrfsmFrameTimestamp = 0; +static volatile uint8_t rrfsmFrameLength = 0; +static uint16_t rrfsmFramePeriod = 0; +static uint16_t rrfsmFrameTimeout = 0; + +static void rrfsmFrameSyncError(void) +{ + readBytes = 0; + syncCount = 0; + + totalSyncErrorCount++; +} + +static void rrfsmStartFrame(timeMs_t currentTimeMs) +{ + readBytes = 0; + rrfsmFrameLength = rrfsmMinFrameLength; + rrfsmFrameTimestamp = currentTimeMs; +} + +static void rrfsmStartFrameAndSendPendingReq(timeMs_t currentTimeMs) +{ + rrfsmStartFrame(currentTimeMs); + + rrfsmFramePeriod = 0; + readIngoreBytes = reqLength; + if (reqLength != 0) + serialWriteBuf(escSensorPort, reqbuffer, reqLength); +} + +static void rrfsmInvalidateReq(void) +{ + reqbuffer[0] = 0x00; // no req + reqLength = 0; + rrfsmFrameTimeout = 0; +} + +static FAST_CODE void rrfsmDataReceive(uint16_t c, void *data) +{ + UNUSED(data); + + // don't listen to self + if (readIngoreBytes > 0) { + readIngoreBytes--; + return; + } + + totalByteCount++; + + if (readBytes >= TELEMETRY_BUFFER_SIZE) { + // avoid buffer overrun + rrfsmFrameSyncError(); + } + else if (readBytes < rrfsmFrameLength) { + buffer[readBytes++] = c; + + int8_t accepted = (rrfsmAccept != NULL ) ? rrfsmAccept(c) : -1; + if (accepted > 0) { + // frame accepted (may not be complete) + syncCount++; + } + else if (accepted < 0) { + // frame rejected + rrfsmFrameSyncError(); + } + } +} + +static void rrfsmSensorProcess(timeUs_t currentTimeUs) +{ + const timeMs_t currentTimeMs = currentTimeUs / 1000; + + // wait before initializing + if (currentTimeMs < rrfsmBootDelayMs) + return; + + // request first log record or just listen if in e.g. UNC mode + if (rrfsmFrameTimestamp == 0) { + if (rrfsmStart == NULL || rrfsmStart(currentTimeMs)) + rrfsmStartFrame(currentTimeMs); + return; + } + + // frame period in effect? request next frame if elapsed + if (rrfsmFramePeriod != 0) { + if (currentTimeMs > rrfsmFrameTimestamp + rrfsmFramePeriod) { + rrfsmStartFrameAndSendPendingReq(currentTimeMs); + } + return; + } + + // timeout waiting for frame? log error, request again + if (rrfsmFrameTimeout != 0 && currentTimeMs > rrfsmFrameTimestamp + rrfsmFrameTimeout) { + increaseDataAge(0); + totalTimeoutCount++; + rrfsmStartFrameAndSendPendingReq(currentTimeMs); + return; + } + + // custom execution (advanced) + if (rrfsmCrank != NULL && !rrfsmCrank(currentTimeMs)) { + return; + } + + // frame incomplete? check later + if (readBytes < rrfsmFrameLength) { + return; + } + + // frame complate, process, prepare for next frame + if (rrfsmDecode == NULL || rrfsmDecode(currentTimeMs)) { + // good frame, log, response handler will have prep'ed next request + totalFrameCount++; + } + else { + // bad frame, log error, retry + increaseDataAge(0); + totalCrcErrorCount++; + } + rrfsmStartFrame(currentTimeMs); +} + + +/* + * Scorpion Telemetry + * - Serial protocol is 38400,8N1 + * - Frame rate running:10Hz idle:1Hz + * - Little-Endian fields + * - CRC16-CCITT + * - Error Code bits: + * 0: N/A + * 1: BEC voltage error + * 2: Temperature error + * 3: Consumption error + * 4: Input voltage error + * 5: Current error + * 6: N/A + * 7: Throttle error + * + * Request Format + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0: Req: MSN - read = 0x5, write = 0xD, LSN - region + * 1,2,3: Address: Starting address in region to access + * 4: Length: Data length to read/write, does not include Data crc + * Requested data length in bytes should not be more than 32 bytes and should be a multiple of 2 bytes + * 5: CRC: Header CRC. 0 value means that crc is not used. And no additional bytes for data crc + * Regions + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0 – System region, read only + * 1 – Status region, read only + * 2 – Control region, stay away + * 3 – Settings region, read/write access + * 4 – FW region, write only, encrypted. Stay away + * 5 - Log region, read only + * 6 – User data + + * + * Telemetry Frame Format + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0-5: Header + * +0-+2: Timestamp ms + * +3: Throttle in 0.5% + * +4-+5: Current in 0.1A + * +6-+7: Voltage in 0.1V + * +8-+9: Consumption in mAh + * +10: Temperature in °C + * +11: PWM duty cycle in 0.5% + * +12: BEC voltage in 0.1V + *+13-+14: RPM in 5rpm steps + * +15: Error code + *+16-+17: CRC16 CCITT + * + */ +#define TRIB_REQ_WRITE 0x80 // request write bit +#define TRIB_REQ_SIG 0x50 // request signature bits +#define TRIB_BOOT_DELAY 10000 // 10 seconds quiet time before expecting first frame +#define TRIB_HEADER_LENGTH 6 // assume header only until actual length of current frame known +#define TRIB_UNC_HEADER_LENGTH 4 // UNC packet header length +#define TRIB_FRAME_PERIOD 100 // aim for approx. 20Hz (note UNC is 10Hz, may have to reduce this) +#define TRIB_REQ_BOOT_DELAY 4000 // 4 seconds quiet time before ESC requests +#define TRIB_REQ_READ_TIMEOUT 200 // Response timeout for read requests +#define TRIB_REQ_WRITE_TIMEOUT 1200 // Response timeout for write requests +#define TRIB_RESP_FRAME_TIMEOUT 200 // Response timeout depends on ESC business but no more than 200ms + // Host must wait ESC response or timeout before sending new packet +#define TRIB_UNC_FRAME_TIMEOUT 1200 // Timeout for UNC packets +#define TRIB_PARAM_FRAME_PERIOD 2 // asap +#define TRIB_PARAM_READ_TIMEOUT 200 // usually less than 200Us +#define TRIB_PARAM_WRITE_TIMEOUT 1200 // usually less than 1200Us +#define TRIB_PARAM_SIG 0x53 // parameter payload signature for this ESC +#define TRIB_PARAM_IQ22_ADDR 0x18 // address of param range w/ IQ22 params +#define TRIB_PARAM_CAP_RESET PARAM_HEADER_USER // reset ESC capable +#define TRIB_PARAM_CMD_RESET PARAM_HEADER_USER // reset ESC command + +#define TRIB_PAL_SYS_MASK 0x8000 // param address/length range system bit mask +#define TRIB_PAL_ADDR_MASK 0x7F00 // param address/length range address mask +#define TRIB_PAL_LEN_MASK 0x00FF // param address/length range length mask + +#define TRIB_PKT_READ_SYSTEM 0x50 // read system region packet +#define TRIB_PKT_READ_STATUS 0x51 // read status region packet +#define TRIB_PKT_READ_SETTINGS 0x53 // read settings region packet +#define TRIB_PKT_WRITE_SETTINGS 0xD3 // write settings region packet +#define TRIB_PKT_UNSOLICITED 0x55 // unsolicited packet + +typedef enum { + TRIB_UNCSETUP_INACTIVE = 0, + TRIB_UNCSETUP_PARAMSREADY, + TRIB_UNCSETUP_ABORTUNC, + TRIB_UNCSETUP_ACTIVE, + TRIB_UNCSETUP_WAIT, +} tribUncSetup_e; + +static tribUncSetup_e tribUncSetup = TRIB_UNCSETUP_INACTIVE; + +// param ranges - hibyte=addr (system region if 0x80 set or setting region otherwise), lobyte=length +static uint16_t tribParamAddrLen[] = { 0x0020, 0x1008, 0x230E, 0x8204, 0x8502, 0x1406, 0x1808, 0x3408 }; + +static uint16_t tribInvalidParams = 0; // bit per param address range not yet received from ESC +static uint16_t tribDirtyParams = 0; // bit per param address that needs to be written to ESC +static bool tribResetEsc = false; + +static uint16_t calculateCRC16_CCITT(const uint8_t *ptr, size_t len) +{ + uint16_t crc = 0; + + while (len--) { + crc ^= *ptr++; + for (int i = 0; i < 8; i++) + crc = (crc & 1) ? (crc >> 1) ^ 0x8408 : (crc >> 1); + } + + return crc; +} + +static bool tribParamCommit(uint8_t cmd) +{ + if (cmd == 0) { + // save page + // find dirty params, settings only + uint8_t offset = 0; + for (uint8_t i = 0; i < ARRAYLEN(tribParamAddrLen); i++) { + const uint16_t *pal = tribParamAddrLen + i; + const uint8_t len = *pal & TRIB_PAL_LEN_MASK; + if ((*pal & TRIB_PAL_SYS_MASK) == 0) { + // schedule writes for dirty address ranges + if (memcmp(paramPayload + offset, paramUpdPayload + offset, len) != 0) { + // set dirty bit + tribDirtyParams |= (1U << i); + // invalidate param + tribInvalidParams |= (1 << i); + // invalidate param payload - will be available again when all params again cached + paramPayloadLength = 0; + } + } + offset += len; + } + return true; + } + else if (cmd == TRIB_PARAM_CMD_RESET) { + // reset ESC + tribResetEsc = true; + return true; + } + else { + // invalid command + return false; + } +} + +static void tribBuildReq(uint8_t req, uint8_t addr, void *src, uint8_t len, uint16_t framePeriod, uint16_t frameTimeout) +{ + reqbuffer[0] = req; // req + reqbuffer[1] = 0; // addr + reqbuffer[2] = 0; + reqbuffer[3] = addr; + reqbuffer[4] = len; // length + reqbuffer[5] = 0; + reqLength = TRIB_HEADER_LENGTH; + if (src != NULL) { + memcpy(reqbuffer + TRIB_HEADER_LENGTH, src, len); + reqLength += len; + } + rrfsmFramePeriod = framePeriod; + rrfsmFrameTimeout = frameTimeout; +} + +static void tribInvalidateParams(void) +{ + tribInvalidParams = ~(~1U << (ARRAYLEN(tribParamAddrLen) - 1)); +} + +static uint8_t tribCalcParamBufferLength() +{ + uint8_t len = 0; + for (uint8_t j = 0; j < ARRAYLEN(tribParamAddrLen); j++) + len += (tribParamAddrLen[j] & TRIB_PAL_LEN_MASK); + return len; +} + +static bool tribBuildNextParamReq(void) +{ + // pending reset request... + if (tribResetEsc) { + tribResetEsc = false; + + tribUncSetup = TRIB_UNCSETUP_INACTIVE; + tribInvalidParams = 0; + tribDirtyParams = 0; + paramMspActive = false; + + uint16_t reset = 0; + tribBuildReq(0xD0, 0x00, &reset, 2, TRIB_PARAM_FRAME_PERIOD, TRIB_PARAM_WRITE_TIMEOUT); + return true; + } + + // ...or pending write request... + uint8_t offset = 0; + for (uint16_t *pal = tribParamAddrLen, dirtybits = tribDirtyParams; dirtybits != 0; pal++, dirtybits >>= 1) { + const uint8_t addr = (*pal & TRIB_PAL_ADDR_MASK) >> 8; + const uint8_t len = *pal & TRIB_PAL_LEN_MASK; + if ((dirtybits & 0x01) != 0) { + void *payload; + uint32_t iq22Payload[2]; + if (addr == TRIB_PARAM_IQ22_ADDR) { + // convert scaled uint32 -> IQ22 + const uint32_t q22 = 1 << 22; + uint32_t *pw = iq22Payload; + uint32_t *pp = (uint32_t*)(paramUpdPayload + offset); + *pw++ = (((float)*pp++) / 100 * q22); + *pw = (((float)*pp) / 100000 * q22); + payload = iq22Payload; + } + else { + payload = paramUpdPayload + offset; + } + tribBuildReq(TRIB_PKT_WRITE_SETTINGS, addr, payload, len, TRIB_PARAM_FRAME_PERIOD, TRIB_PARAM_WRITE_TIMEOUT); + return true; + } + offset += len; + } + + // ...or schedule pending read request if no pending writes... + for (uint16_t *pal = tribParamAddrLen, invalidbits = tribInvalidParams; invalidbits != 0; pal++, invalidbits >>= 1) { + if ((invalidbits & 0x01) != 0) { + uint8_t req = (*pal & TRIB_PAL_SYS_MASK) != 0 ? TRIB_PKT_READ_SYSTEM : TRIB_PKT_READ_SETTINGS; + uint8_t addr = (*pal & TRIB_PAL_ADDR_MASK) >> 8; + uint8_t len = *pal & TRIB_PAL_LEN_MASK; + tribBuildReq(req, addr, NULL, len, TRIB_PARAM_FRAME_PERIOD, TRIB_PARAM_READ_TIMEOUT); + return true; + } + } + + // ...or nothing pending + return false; +} + +static bool tribDecodeReadParamResp(uint8_t addr) +{ + uint8_t offset = 0; + for (uint8_t i = 0; i < ARRAYLEN(tribParamAddrLen); i++) { + const uint16_t *pal = tribParamAddrLen + i; + const uint8_t len = *pal & TRIB_PAL_LEN_MASK; + if ((*pal >> 8) == addr) { + // cache params by addr + if (addr == TRIB_PARAM_IQ22_ADDR) { + // convert IQ22 -> scaled uint32 + const uint32_t q22 = 1 << 22; + uint32_t *pr = (uint32_t*)(buffer + TRIB_HEADER_LENGTH); + uint32_t *pp = (uint32_t*)(paramPayload + offset); + *pp++ = roundf(((float)*pr++) / q22 * 100); + *pp = roundf(((float)*pr) / q22 * 100000); + } + else { + memcpy(paramPayload + offset, buffer + TRIB_HEADER_LENGTH, len); + } + // clear invalid bit + tribInvalidParams &= ~(1 << i); + + // make param payload available if all params cached + if (tribInvalidParams == 0 && paramPayloadLength == 0) + tribUncSetup = TRIB_UNCSETUP_PARAMSREADY; + return true; + } + offset += len; + } + return false; +} + +static bool tribDecodeWriteParamResp(uint8_t addr) +{ + for (uint8_t i = 0; i < ARRAYLEN(tribParamAddrLen); i++) { + const uint16_t *pal = tribParamAddrLen + i; + if ((*pal >> 8) == addr) { + // clear dirty bit and set invalid bit force read + const uint16_t addrbit = (1 << i); + tribDirtyParams &= ~addrbit; + tribInvalidParams |= addrbit; + return true; + } + } + return false; +} + +static bool tribValidateResponseHeader(void) +{ + // req and resp headers should match except for len ([4]) which may differ + for (int i = 0; i < TRIB_HEADER_LENGTH; i++) { + if (i == 4) + continue; + if (buffer[i] != reqbuffer[i]) + return false; + } + return true; +} + +static bool tribDecodeReadSettingResp(uint8_t sysbit) +{ + // validate header + if (!tribValidateResponseHeader()) + return false; + + const uint8_t addr = buffer[3]; + if (!tribDecodeReadParamResp(addr | sysbit) || !tribBuildNextParamReq()) + rrfsmInvalidateReq(); + + return true; +} + +static bool tribDecodeWriteSettingResp(void) +{ + // validate header + if (!tribValidateResponseHeader()) + return false; + + const uint8_t addr = buffer[3]; + if (!tribDecodeWriteParamResp(addr) || !tribBuildNextParamReq()) + rrfsmInvalidateReq(); + + return true; +} + +static bool tribDecodeLogRecord(uint8_t hl) +{ + // payload: 16 byte (Log_rec_t) + const uint16_t rpm = buffer[hl + 14] << 8 | buffer[hl + 13]; + const uint16_t temp = buffer[hl + 10]; + const uint16_t power = buffer[hl + 11]; + const uint16_t voltage = buffer[hl + 7] << 8 | buffer[hl + 6]; + const uint16_t current = buffer[hl + 5] << 8 | buffer[hl + 4]; + const uint16_t capacity = buffer[hl + 9] << 8 | buffer[hl + 8]; + const uint16_t status = buffer[hl + 15]; + const uint16_t voltBEC = buffer[hl + 12]; + + escSensorData[0].age = 0; + escSensorData[0].erpm = rpm * 5; + escSensorData[0].pwm = power * 5; + escSensorData[0].voltage = voltage * 100; + escSensorData[0].current = current * 100; + escSensorData[0].consumption = capacity; + escSensorData[0].temperature = temp * 10; + escSensorData[0].bec_voltage = voltBEC * 100; + + DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, rpm * 5); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, voltage * 100); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, current * 100); + + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, rpm); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, power); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, temp); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, voltage); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, current); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, capacity); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, status); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); + + return true; +} + +static bool tribDecodeReadStatusResp(void) +{ + // validate header (no CRC) + if (!tribValidateResponseHeader()) + return false; + + const uint8_t addr = buffer[3]; + if (tribUncSetup == TRIB_UNCSETUP_ABORTUNC && addr == 0) { + tribUncSetup = TRIB_UNCSETUP_ACTIVE; + paramPayloadLength = tribCalcParamBufferLength(); + rrfsmInvalidateReq(); + return true; + } + return false; +} + +static bool tribDecodeUNCFrame(void) +{ + // validate CRC, decode as 4 byte header + 16 byte (Log_rec_t) payload + const uint16_t crc = buffer[rrfsmFrameLength - 1] << 8 | buffer[rrfsmFrameLength - 2]; + if (calculateCRC16_CCITT(buffer, 20) != crc || !tribDecodeLogRecord(TRIB_UNC_HEADER_LENGTH)) + return false; + + rrfsmFrameTimeout = TRIB_UNC_FRAME_TIMEOUT; + return true; +} + +static bool tribDecode(timeMs_t currentTimeMs) +{ + UNUSED(currentTimeMs); + + const uint8_t req = buffer[0]; + switch (req) { + case TRIB_PKT_READ_STATUS: + return tribDecodeReadStatusResp(); + case TRIB_PKT_READ_SYSTEM: + return tribDecodeReadSettingResp(0x80); + case TRIB_PKT_READ_SETTINGS: + return tribDecodeReadSettingResp(0x00); + case TRIB_PKT_WRITE_SETTINGS: + return tribDecodeWriteSettingResp(); + case TRIB_PKT_UNSOLICITED: + return tribDecodeUNCFrame(); + default: + return false; + } +} + +static bool tribCrankUncSetup(timeMs_t currentTimeMs) +{ + switch(tribUncSetup) { + case TRIB_UNCSETUP_INACTIVE: + case TRIB_UNCSETUP_ABORTUNC: + // unexpected but required to keep compiler happy + break; + case TRIB_UNCSETUP_PARAMSREADY: + if (paramMspActive && currentTimeMs < rrfsmFrameTimestamp + 4000) { + // try to abort UNC mode + tribUncSetup = TRIB_UNCSETUP_ABORTUNC; + tribBuildReq(TRIB_PKT_READ_STATUS, 0, NULL, 0x10, TRIB_FRAME_PERIOD, TRIB_RESP_FRAME_TIMEOUT); + } + break; + case TRIB_UNCSETUP_ACTIVE: + if (tribBuildNextParamReq()) { + tribUncSetup = TRIB_UNCSETUP_WAIT; + } + break; + case TRIB_UNCSETUP_WAIT: + if (tribInvalidParams == 0 && tribDirtyParams == 0) { + tribUncSetup = TRIB_UNCSETUP_ACTIVE; + rrfsmInvalidateReq(); + } + break; + } + return true; +} + +static bool tribStart(timeMs_t currentTimeMs) +{ + UNUSED(currentTimeMs); + + tribBuildNextParamReq(); + return true; +} + +static int8_t tribAccept(uint16_t c) +{ + if (readBytes == 1) { + // req / singature + if ((c & TRIB_REQ_SIG) != TRIB_REQ_SIG) + return -1; + } + else if (buffer[0] == TRIB_PKT_UNSOLICITED && readBytes == 3) { + // UNC length + if (c > TELEMETRY_BUFFER_SIZE) { + // protect against buffer overflow + return -1; + } + else { + // new frame length for this frame (header + data) + rrfsmFrameLength = c; + return 1; + } + } + else if (buffer[0] != TRIB_PKT_UNSOLICITED && readBytes == 5) { + // STD length + if (c > TELEMETRY_BUFFER_SIZE) { + // protect against buffer overflow + return -1; + } + else if((c & 0x01) || c > 32) { + // invalid length (must not be more than 32 bytes and should be a multiple of 2 bytes) + return -1; + } + else { + // new frame length for this frame - read = (header + data), write = (header) + rrfsmFrameLength = (buffer[0] & TRIB_REQ_WRITE) == 0 ? TRIB_HEADER_LENGTH + c : TRIB_HEADER_LENGTH; + return 1; + } + } + return 0; +} + +static serialReceiveCallbackPtr tribSensorInit(bool bidirectional) +{ + rrfsmMinFrameLength = TRIB_HEADER_LENGTH; + rrfsmAccept = tribAccept; + rrfsmDecode = tribDecode; + + if (bidirectional) { + // request/response telemetry + rrfsmBootDelayMs = TRIB_REQ_BOOT_DELAY; + rrfsmStart = tribStart; + + // enable ESC parameter reads and writes, reset + paramSig = TRIB_PARAM_SIG; + paramVer = 0 | TRIB_PARAM_CAP_RESET; + paramCommit = tribParamCommit; + tribInvalidateParams(); + + // enable UNC setup FSM + rrfsmCrank = tribCrankUncSetup; + } + else { + // telemetry data only + rrfsmBootDelayMs = TRIB_BOOT_DELAY; + rrfsmFrameTimeout = TRIB_UNC_FRAME_TIMEOUT; + } + + return rrfsmDataReceive; +} + + /* * OpenYGE Telemetry * @@ -1556,61 +2129,75 @@ static void apdSensorProcess(timeUs_t currentTimeUs) * WARN_OVERTEMP = 0x20 // Fail if Motor Status == STATE_POWER_CUT * WARN_OVERAMP = 0x40 // Fail if Motor Status == STATE_POWER_CUT * WARN_SETPOINT_NOISE = 0xC0 // note this is special case (can never have OVERAMP w/ BEC hence reuse) - * - * Data Frame Format - * ――――――――――――――――――――――――――――――――――――――――――――――― - * Header... - * 0: sync; // sync, 0xA5 - * 1: version; // frame version - * 2: frame_type // telemetry data = 0 - * 3: frame_length; // frame length including header and CRC - * 4: reserved // reserved - * 5: reserved // reserved - * - * Payload... - * +0: reserved // reserved - * 1: temperature; // C degrees (0-> -40°C, 255->215°C) - * 2,3: voltage; // 0.01V Little endian! - * 4,5: current; // 0.01A Little endian! - * 6,7: consumption; // mAh Little endian! - * 8,9: rpm; // 0.1rpm Little endian! - * 10: pwm; // % - * 11: throttle; // % - * 12,13: bec_voltage; // 0.01V Little endian! - * 14,15: bec_current; // 0.01A Little endian! - * 16: bec_temp; // C degrees (0-> -40°C, 255->215°C) - * 17: status1; // see documentation - * 18: cap_temp; // C degrees (0-> -40°C, 255->215°C) - * 19: aux_temp; // C degrees (0-> -40°C, 255->215°C) - * 20: status2; // reserved - * 21: reserved1; // reserved - * 22,23: idx; // maybe future use - * 24,25: idx_data; // maybe future use - * - * 32,33: crc16; // CCITT, poly: 0x1021 - * */ -#define OPENYGE_SYNC 0xA5 // sync -#define OPENYGE_BOOT_DELAY 5000 // 5 seconds -#define OPENYGE_RAMP_INTERVAL 6000 // 6 seconds -#define OPENYGE_FRAME_PERIOD_INITIAL 900 // intially 800 w/ progressive decreasing frame-period during ramp time... -#define OPENYGE_FRAME_PERIOD_FINAL 60 // ...to the final 50ms -#define OPENYGE_FRAME_MIN_LENGTH 6 // assume minimum frame (header + CRC) until actual length of current frame known - -#define OPENYGE_TEMP_OFFSET 40 - -enum { - OPENYGE_FRAME_FAILED = 0, - OPENYGE_FRAME_PENDING = 1, - OPENYGE_FRAME_COMPLETE = 2, -}; - -static timeMs_t oygeRampTimer = 0; -static uint32_t oygeFrameTimestamp = 0; -static uint16_t oygeFramePeriod = OPENYGE_FRAME_PERIOD_INITIAL; -static volatile uint8_t oygeFrameLength = 0; - +#define OPENYGE_SYNC 0xA5 // sync +#define OPENYGE_VERSION 3 // protocol version +#define OPENYGE_HEADER_LENGTH 6 // header length +#define OPENYGE_HEADER_LENGTH_LEGACY 4 // legacy (pre-v3) header length +#define OPENYGE_CRC_LENGTH 2 // CRC length +#define OPENYGE_BOOT_DELAY 5000 // 5 seconds +#define OPENYGE_MIN_FRAME_LENGTH OPENYGE_HEADER_LENGTH_LEGACY + OPENYGE_CRC_LENGTH // assume minimum frame (legacy header + CRC) until actual length of current frame known +#define OPENYGE_AUTO_INITIAL_FRAME_TIMEOUT 900 // intially ~800ms w/ progressive decreasing frame-period... +#define OPENYGE_AUTO_MIN_FRAME_TIMEOUT 60U // ...to final ~50ms (no less) + +#define OPENYGE_FRAME_PERIOD_INIT 20 // delay before sending first master request after v3+ auto telemetry frame seen (possibly last chained) +#define OPENYGE_FRAME_PERIOD 38 // aim for approx. 50ms/20Hz +#define OPENYGE_PARAM_FRAME_PERIOD 4 // TBD ASAP? +#define OPENYGE_REQ_READ_TIMEOUT 200 // Response timeout for read requests TBD: confirm +#define OPENYGE_REQ_WRITE_TIMEOUT 400 // Response timeout for write requests TBD: confirm + +#define OPENYGE_PARAM_SIG 0xA5 // parameter payload signature for this ESC +#define OPENYGE_PARAM_CACHE_SIZE_MAX 64 // limited by use of uint64_t as bit array for oygeCachedParams + +#define OPENYGE_FTYPE_TELE_AUTO 0x00 // auto telemetry frame +#define OPENYGE_FTYPE_TELE_RESP 0x02 // telemetry response +#define OPENYGE_FTYPE_TELE_REQ 0x03 // telemetry request +#define OPENYGE_FTYPE_WRITE_PARAM_RESP 0x04 // write param response +#define OPENYGE_FTYPE_WRITE_PARAM_REQ 0x05 // write param request + +#define OPENYGE_DEV_MASTER 0x80 // device address master role bit + +#define OPENYGE_TEMP_OFFSET 40 + +typedef struct { + uint8_t sync; // sync 0xA5 + uint8_t version; // version + uint8_t frame_type; // high bit 0x00 for read, 0x80 for write, e.g 0x00 - telemetry frame data, 0x81 - update parameter + uint8_t frame_length; // frame length + uint8_t seq; // count for packet-control/assignment master counts, slave(ESC) sends this value back + uint8_t device; // ESC address, 0x80 for master and 7 bits for ESC address 0x01…0x7F where 0x00 might mean all ESCs +} OpenYGEHeader_t; + +typedef struct { + uint8_t reserved; // reserved + uint8_t temperature; // C degrees+40 (-40..215°C) + uint16_t voltage; // 0.01V + uint16_t current; // 0.01A + uint16_t consumption; // mAh + uint16_t rpm; // 0.1erpm + int8_t pwm; // % + int8_t throttle; // % + uint16_t bec_voltage; // 0.001V + uint16_t bec_current; // 0.001A + uint8_t bec_temp; // C degrees + uint8_t status1; // see documentation + uint8_t cap_temp; // C degrees+40 (-40..215°C) + uint8_t aux_temp; // C degrees+40 (-40..215°C) + uint8_t status2; // Debug/Reserved + uint8_t reserved1; // Debug/Reserved maybe consumption high-byte for more than 65Ah(industrial) + uint16_t pidx; // maybe future use + uint16_t pdata; // maybe future use +} OpenYGETelemetryFrame_t; + +typedef struct { + uint16_t index; // parameter addr + uint16_t param; // parameter value +} OpenYGEControlFrame_t; + +static uint16_t oygeAutoFrameTimeout = OPENYGE_AUTO_INITIAL_FRAME_TIMEOUT; +static uint64_t oygeCachedParams = 0; +static uint64_t oygeDirtyParams = 0; static uint16_t oygeCalculateCRC16_CCITT(const uint8_t *ptr, size_t len) { @@ -1631,165 +2218,278 @@ static uint16_t oygeCalculateCRC16_CCITT(const uint8_t *ptr, size_t len) return crc; } -static void oygeFrameSyncError(void) +static bool oygeParamCommit(uint8_t cmd) { - readBytes = 0; - syncCount = 0; - - totalSyncErrorCount++; + if (cmd == 0) { + // save page + // find dirty params, skip para[0] (parameter count) + uint16_t *ygeParams = (uint16_t*)paramPayload; + const uint16_t *ygeUpdParams = (uint16_t*)paramUpdPayload; + if (ygeParams[0] != ygeUpdParams[0]) + return false; + + const uint16_t ygeParamCount = ygeParams[0]; + for (uint8_t idx = 1; idx < ygeParamCount; idx++) { + // schedule writes for dirty params + if (ygeParams[idx] != ygeUpdParams[idx]) { + // set dirty bit + oygeDirtyParams |= (1ULL << idx); + // invalidate param + oygeCachedParams &= ~(1ULL << idx); + // invalidate param payload - will be available again when all params again cached + paramPayloadLength = 0; + } + } + return true; + } + else { + return false; + } } -static FAST_CODE void oygeDataReceive(uint16_t c, void *data) +static void oygeCacheParam(uint8_t pidx, uint16_t pdata) { - UNUSED(data); + const uint8_t maxParams = PARAM_BUFFER_SIZE / 2; + if (pidx >= maxParams || pidx >= OPENYGE_PARAM_CACHE_SIZE_MAX) + return; - totalByteCount++; + // don't accept params until pending writes cleared + if (oygeDirtyParams != 0) + return; - if (readBytes < oygeFrameLength) { - buffer[readBytes++] = c; + uint16_t *ygeParams = (uint16_t*)paramPayload; + ygeParams[pidx] = pdata; + oygeCachedParams |= (1ULL << pidx); - if (readBytes == 1) { - // sync - if (c != OPENYGE_SYNC) - oygeFrameSyncError(); - } - else if (readBytes == 3) { - if (c != 0) { - // unsupported frame type - oygeFrameSyncError(); - } - } - else if (readBytes == 4) { - // frame length - // protect against buffer overflow - if (c < OPENYGE_FRAME_MIN_LENGTH || c > TELEMETRY_BUFFER_SIZE) { - oygeFrameSyncError(); - } - else { - oygeFrameLength = c; - syncCount++; - } - } + // skip if count already known (parameter data ready) or count parameter not yet seen (param[0] for YGE) + if (paramPayloadLength > 0 || (oygeCachedParams & 0x01) == 0) + return; + + // make param payload available if all params cached + const uint16_t ygeParamCount = ygeParams[0]; + if (ygeParamCount > 0 && ygeParamCount <= OPENYGE_PARAM_CACHE_SIZE_MAX && + ~(~1ULL << (ygeParamCount - 1)) == oygeCachedParams) { + paramPayloadLength = ygeParamCount * 2; } } -static void oygeStartTelemetryFrame(timeMs_t currentTimeMs) +static void oygeBuildReq(uint8_t req, uint8_t device, void *payload, uint8_t len, uint16_t framePeriod, uint16_t frameTimeout) { - readBytes = 0; - oygeFrameLength = OPENYGE_FRAME_MIN_LENGTH; - oygeFrameTimestamp = currentTimeMs; + OpenYGEHeader_t *hdr = (OpenYGEHeader_t*)reqbuffer; + reqLength = sizeof(*hdr) + len + OPENYGE_CRC_LENGTH; // header + payload + crc + if (reqLength > REQUEST_BUFFER_SIZE) + return; + + hdr->sync = OPENYGE_SYNC; + hdr->version = OPENYGE_VERSION; + hdr->frame_type = req; + hdr->frame_length = reqLength; + hdr->seq++; // advance sequence number, overlapped operations not supported by this implementation + hdr->device = device | OPENYGE_DEV_MASTER; // as master + + if (payload != NULL) + memcpy(hdr + 1, payload, len); + + *((uint16_t*)(reqbuffer + reqLength - OPENYGE_CRC_LENGTH)) = oygeCalculateCRC16_CCITT(reqbuffer, reqLength - OPENYGE_CRC_LENGTH); + + rrfsmFramePeriod = framePeriod; + rrfsmFrameTimeout = frameTimeout; } -static uint8_t oygeDecodeTelemetryFrame(void) +static void oygeBuildNextReq(const OpenYGEHeader_t *hdr) { - // First, check the variables that can change in the interrupt - if (readBytes < oygeFrameLength) - return OPENYGE_FRAME_PENDING; - - // paranoid length check - uint8_t len = buffer[3]; - if (len < OPENYGE_FRAME_MIN_LENGTH || len > TELEMETRY_BUFFER_SIZE) { - totalCrcErrorCount++; - return OPENYGE_FRAME_FAILED; - } - // verify CRC16 checksum - uint16_t crc = buffer[len - 1] << 8 | buffer[len - 2]; - if (oygeCalculateCRC16_CCITT(buffer, len - 2) != crc) { - totalCrcErrorCount++; - return OPENYGE_FRAME_FAILED; + OpenYGEControlFrame_t ctl; + + // schedule pending write request... + const uint16_t *ygeUpdParams = (uint16_t*)paramUpdPayload; + for (uint8_t idx = 0; oygeDirtyParams != 0; idx++) { + uint64_t bit = 1ULL << idx; + // index dirty? + if ((oygeDirtyParams & bit) != 0) { + // clear dirty bit + oygeDirtyParams &= ~(bit); + + // schedule write request + ctl.index = idx; + ctl.param = ygeUpdParams[idx]; + oygeBuildReq(OPENYGE_FTYPE_WRITE_PARAM_REQ, 1, &ctl, sizeof(ctl), OPENYGE_PARAM_FRAME_PERIOD, OPENYGE_REQ_WRITE_TIMEOUT); + return; + } } - uint8_t version = buffer[1]; - uint8_t hl = (version >= 3) ? 6 : 4; + // ...or nothing pending, schedule read telemetry request + ctl.index = ctl.param = 0; + const uint8_t framePeriod = hdr->frame_type == OPENYGE_FTYPE_TELE_AUTO ? OPENYGE_FRAME_PERIOD_INIT : OPENYGE_FRAME_PERIOD; + oygeBuildReq(OPENYGE_FTYPE_TELE_REQ, 1, &ctl, sizeof(ctl), framePeriod, OPENYGE_REQ_READ_TIMEOUT); +} - int16_t temp = buffer[hl+1]; - uint16_t volt = buffer[hl+3] << 8 | buffer[hl+2]; - uint16_t curr = buffer[hl+5] << 8 | buffer[hl+4]; - uint16_t capa = buffer[hl+7] << 8 | buffer[hl+6]; - uint16_t erpm = buffer[hl+9] << 8 | buffer[hl+8]; - uint8_t pwm = buffer[hl+10]; - uint16_t voltBEC = buffer[hl+13] << 8 | buffer[hl+12]; - int16_t tempBEC = buffer[hl+16]; +static void oygeDecodeTelemetryFrame(void) +{ + const OpenYGEHeader_t *hdr = (OpenYGEHeader_t*)buffer; + const OpenYGETelemetryFrame_t *tele = (OpenYGETelemetryFrame_t*)(buffer + (hdr->version >= 3 ? OPENYGE_HEADER_LENGTH : OPENYGE_HEADER_LENGTH_LEGACY)); - if (version >= 2) { - // apply temperature mapping offsets - temp -= OPENYGE_TEMP_OFFSET; - tempBEC -= OPENYGE_TEMP_OFFSET; - } + int16_t temp = tele->temperature - OPENYGE_TEMP_OFFSET; + int16_t tempBEC = tele->bec_temp - OPENYGE_TEMP_OFFSET; escSensorData[0].age = 0; - escSensorData[0].erpm = erpm * 10; - escSensorData[0].pwm = pwm * 10; - escSensorData[0].voltage = volt * 10; - escSensorData[0].current = curr * 10; - escSensorData[0].consumption = capa; + escSensorData[0].erpm = tele->rpm * 10; + escSensorData[0].pwm = tele->pwm * 10; + escSensorData[0].throttle = tele->throttle * 10; + escSensorData[0].voltage = tele->voltage * 10; + escSensorData[0].current = tele->current * 10; + escSensorData[0].consumption = tele->consumption; escSensorData[0].temperature = temp * 10; escSensorData[0].temperature2 = tempBEC * 10; + escSensorData[0].bec_voltage = tele->bec_voltage; + escSensorData[0].bec_current = tele->bec_current; + escSensorData[0].status = tele->status1 | 0x0100; - totalFrameCount++; + oygeCacheParam(tele->pidx, tele->pdata); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, erpm * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, tele->rpm * 10); DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, temp * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, volt); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, curr); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, tele->voltage * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, tele->current * 10); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, erpm); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, pwm); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, tele->rpm); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_PWM, tele->pwm); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, temp); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, volt); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, curr); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, capa); - DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, voltBEC); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, tele->voltage); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, tele->current); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, tele->consumption); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, tele->status1); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); +} - return OPENYGE_FRAME_COMPLETE; +static const OpenYGEHeader_t *oygeGetHeaderWithCrcCheck() +{ + // get header (w/ paranoid buffer access) + const OpenYGEHeader_t *hdr = (OpenYGEHeader_t*)buffer; + const uint8_t len = hdr->frame_length; + if (len < OPENYGE_MIN_FRAME_LENGTH || len > TELEMETRY_BUFFER_SIZE) + return NULL; + + // check CRC + const uint16_t crc = *(uint16_t*)(buffer + len - OPENYGE_CRC_LENGTH); + if (oygeCalculateCRC16_CCITT(buffer, len - OPENYGE_CRC_LENGTH) != crc) + return NULL; + + return hdr; } -static void oygeSensorProcess(timeUs_t currentTimeUs) +static bool oygeDecodeAuto(timeMs_t currentTimeMs) { - const timeMs_t currentTimeMs = currentTimeUs / 1000; + // get header w/ CRC check + const OpenYGEHeader_t *hdr = oygeGetHeaderWithCrcCheck(); + if (hdr == NULL) + return false; - // wait before initializing - if (currentTimeMs < OPENYGE_BOOT_DELAY) - return; + // decode payload + oygeDecodeTelemetryFrame(); - // one time init - if (oygeFrameTimestamp == 0) { - oygeStartTelemetryFrame(currentTimeMs); - return; + // adjust auto frame timeout (10ms + last seen decreasing interval until min) + if (oygeAutoFrameTimeout > OPENYGE_AUTO_MIN_FRAME_TIMEOUT && currentTimeMs - rrfsmFrameTimestamp < oygeAutoFrameTimeout) { + oygeAutoFrameTimeout = MAX(OPENYGE_AUTO_MIN_FRAME_TIMEOUT, currentTimeMs - rrfsmFrameTimestamp + 10); } + rrfsmFrameTimeout = oygeAutoFrameTimeout; - // switch to final frame timeout if ramp time complete - if (oygeFramePeriod == OPENYGE_FRAME_PERIOD_INITIAL && oygeRampTimer != 0 && currentTimeMs > oygeRampTimer) - oygeFramePeriod = OPENYGE_FRAME_PERIOD_FINAL; + return true; +} - // timeout waiting for frame? - if (currentTimeMs > oygeFrameTimestamp + oygeFramePeriod) { - increaseDataAge(0); - oygeStartTelemetryFrame(currentTimeMs); - totalTimeoutCount++; - return; +static bool oygeDecodeTelemetry(const OpenYGEHeader_t *hdr, timeMs_t currentTimeMs) +{ + // switch to auto telemetry mode if ESC FW too old + if (hdr->version < 3) { + rrfsmDecode = oygeDecodeAuto; + paramCommit = NULL; + return oygeDecodeAuto(currentTimeMs); } - // attempt to decode frame - uint8_t state = oygeDecodeTelemetryFrame(); - switch (state) { - case OPENYGE_FRAME_PENDING: - // frame not ready yet - break; - case OPENYGE_FRAME_FAILED: - increaseDataAge(0); - // next frame - oygeStartTelemetryFrame(currentTimeMs); - break; - case OPENYGE_FRAME_COMPLETE: - // start ramp timer if first frame seen - if (oygeRampTimer == 0) - oygeRampTimer = currentTimeMs + OPENYGE_RAMP_INTERVAL; - // next frame - oygeStartTelemetryFrame(currentTimeMs); - break; + // response sequence number should match request (ignore auto telemetry) + const OpenYGEHeader_t *req = (OpenYGEHeader_t*)reqbuffer; + if (hdr->frame_type != OPENYGE_FTYPE_TELE_AUTO && hdr->seq != req->seq) + return false; + + // decode payload + oygeDecodeTelemetryFrame(); + + // schedule next request + oygeBuildNextReq(hdr); + + return true; +} + +static bool oygeDecode(timeMs_t currentTimeMs) +{ + // get header w/ CRC check + const OpenYGEHeader_t *hdr = oygeGetHeaderWithCrcCheck(); + if (hdr == NULL) + return false; + + switch (hdr->frame_type) { + case OPENYGE_FTYPE_TELE_AUTO: + case OPENYGE_FTYPE_TELE_RESP: + case OPENYGE_FTYPE_WRITE_PARAM_RESP: + return oygeDecodeTelemetry(hdr, currentTimeMs); + default: + return false; + } +} + +static int8_t oygeAccept(uint16_t c) +{ + if (readBytes == 1) { + // sync + if (c != OPENYGE_SYNC) + return -1; + } + else if (readBytes == 3) { + switch (c) { + case OPENYGE_FTYPE_TELE_AUTO: + case OPENYGE_FTYPE_TELE_REQ: + case OPENYGE_FTYPE_TELE_RESP: + case OPENYGE_FTYPE_WRITE_PARAM_REQ: + case OPENYGE_FTYPE_WRITE_PARAM_RESP: + break; + default: + // unsupported frame type + return -1; + } + } + else if (readBytes == 4) { + // frame length + if (c < OPENYGE_MIN_FRAME_LENGTH || c > TELEMETRY_BUFFER_SIZE) { + // protect against buffer underflow/overflow + return -1; + } + else { + // new frame length for this frame + rrfsmFrameLength = c; + return 1; + } } + return 0; +} + +static serialReceiveCallbackPtr oygeSensorInit(bool bidirectional) +{ + rrfsmBootDelayMs = OPENYGE_BOOT_DELAY; + rrfsmMinFrameLength = OPENYGE_MIN_FRAME_LENGTH; + rrfsmAccept = oygeAccept; + + paramSig = OPENYGE_PARAM_SIG; + + if (bidirectional) { + // use request/response telemetry mode, enable parameter writes to ESC + rrfsmDecode = oygeDecode; + paramCommit = oygeParamCommit; + } + else { + // use auto telemetry mode + rrfsmDecode = oygeDecodeAuto; + } + + return rrfsmDataReceive; } @@ -1829,7 +2529,7 @@ void escSensorProcess(timeUs_t currentTimeUs) hw5SensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_SCORPION: - uncSensorProcess(currentTimeUs); + rrfsmSensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_KONTRONIK: kontronikSensorProcess(currentTimeUs); @@ -1844,7 +2544,7 @@ void escSensorProcess(timeUs_t currentTimeUs) apdSensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_OPENYGE: - oygeSensorProcess(currentTimeUs); + rrfsmSensorProcess(currentTimeUs); break; case ESC_SENSOR_PROTO_RECORD: recordSensorProcess(currentTimeUs); @@ -1863,6 +2563,7 @@ void escSensorProcess(timeUs_t currentTimeUs) bool INIT_CODE escSensorInit(void) { + const bool escHalfDuplex = escSensorConfig()->halfDuplex; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR); serialReceiveCallbackPtr callback = NULL; portOptions_e options = 0; @@ -1872,7 +2573,7 @@ bool INIT_CODE escSensorInit(void) return false; } - options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | (escSensorConfig()->halfDuplex ? SERIAL_BIDIR : 0); + options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | (escHalfDuplex ? SERIAL_BIDIR : 0); switch (escSensorConfig()->protocol) { case ESC_SENSOR_PROTO_BLHELI32: @@ -1883,6 +2584,7 @@ bool INIT_CODE escSensorInit(void) baudrate = 19200; break; case ESC_SENSOR_PROTO_SCORPION: + callback = tribSensorInit(escHalfDuplex); baudrate = 38400; break; case ESC_SENSOR_PROTO_KONTRONIK: @@ -1896,7 +2598,7 @@ bool INIT_CODE escSensorInit(void) baudrate = 115200; break; case ESC_SENSOR_PROTO_OPENYGE: - callback = oygeDataReceive; + callback = oygeSensorInit(escHalfDuplex); baudrate = 115200; break; case ESC_SENSOR_PROTO_RECORD: @@ -1905,7 +2607,7 @@ bool INIT_CODE escSensorInit(void) } if (baudrate) { - escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, callback, NULL, baudrate, MODE_RX, options); + escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, callback, NULL, baudrate, escHalfDuplex ? MODE_RXTX : MODE_RX, options); } for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { @@ -1917,4 +2619,30 @@ bool INIT_CODE escSensorInit(void) return (escSensorPort != NULL); } + +uint8_t escGetParamBufferLength(void) +{ + paramMspActive = true; + return paramPayloadLength != 0 ? PARAM_HEADER_SIZE + paramPayloadLength : 0; +} + +uint8_t *escGetParamBuffer(void) +{ + paramBuffer[PARAM_HEADER_SIG] = paramSig; + paramBuffer[PARAM_HEADER_VER] = paramVer | (paramCommit == NULL ? PARAM_HEADER_RDONLY : 0); + return paramBuffer; +} + +uint8_t *escGetParamUpdBuffer(void) +{ + return paramUpdBuffer; +} + +bool escCommitParameters(void) +{ + return paramUpdBuffer[PARAM_HEADER_SIG] == paramBuffer[PARAM_HEADER_SIG] && + (paramUpdBuffer[PARAM_HEADER_VER] & PARAM_HEADER_VER_MASK) == (paramBuffer[PARAM_HEADER_VER] & PARAM_HEADER_VER_MASK) && + paramCommit != NULL && paramCommit(paramUpdBuffer[PARAM_HEADER_VER] & PARAM_HEADER_CMD_MASK); +} + #endif diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 91f03be064..32e96d2d09 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -56,14 +56,18 @@ typedef struct escSensorConfig_s { PG_DECLARE(escSensorConfig_t, escSensorConfig); typedef struct { - uint8_t age; // Data age - uint16_t pwm; // Output duty cycle 0.1% - uint32_t erpm; // eRPM - uint32_t voltage; // mV - uint32_t current; // mA - uint32_t consumption; // mAh - int16_t temperature; // 0.1°C - int16_t temperature2; // 0.1°C + uint8_t age; // Data age + uint16_t pwm; // Output duty cycle 0.1% + uint16_t throttle; // Input setpoint 0.1% + uint32_t erpm; // eRPM + uint32_t voltage; // mV + uint32_t current; // mA + uint32_t consumption; // mAh + int16_t temperature; // 0.1°C + int16_t temperature2; // 0.1°C + uint32_t bec_voltage; // mV + uint32_t bec_current; // mA + uint32_t status; // status / fault codes } escSensorData_t; #define ESC_DATA_INVALID 255 @@ -79,3 +83,8 @@ bool isEscSensorActive(void); uint32_t getEscSensorRPM(uint8_t motorNumber); escSensorData_t *getEscSensorData(uint8_t motorNumber); + +uint8_t escGetParamBufferLength(void); +uint8_t *escGetParamBuffer(void); +uint8_t *escGetParamUpdBuffer(void); +bool escCommitParameters(void); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 815de674bc..52360d1503 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -238,6 +238,17 @@ uint16 Altitude ( meter ­1000m offset ) uint8_t Satellites in use ( counter ) */ +static int getVoltageMeter(voltageMeterId_e id) +{ + voltageMeter_t meter; + + voltageMeterRead(id, &meter); + + // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions + // Max voltage 25.5V + return meter.voltage * 255 / 200; +} + static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) { escSensorData_t *escData; @@ -252,6 +263,27 @@ static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) case CRSF_GPS_REUSE_ESC_TEMP: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->temperature : 0; + case CRSF_GPS_REUSE_ESC_PWM: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->pwm : 0; + case CRSF_GPS_REUSE_ESC_THROTTLE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->throttle : 0; + case CRSF_GPS_REUSE_ESC_BEC_VOLTAGE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_voltage : 0; + case CRSF_GPS_REUSE_ESC_BEC_CURRENT: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_current : 0; + case CRSF_GPS_REUSE_ESC_BEC_TEMP: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->temperature2 : 0; + case CRSF_GPS_REUSE_ESC_STATUS: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status & 0xFFFF) : 0; + case CRSF_GPS_REUSE_ESC_STATUS2: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status >> 16) : 0; case CRSF_GPS_REUSE_MCU_TEMP: return getCoreTemperatureCelsius() * 10; case CRSF_GPS_REUSE_MCU_LOAD: @@ -260,6 +292,12 @@ static int16_t crsfGpsReuse(uint8_t reuse, int16_t value) return getAverageSystemLoad(); case CRSF_GPS_REUSE_RT_LOAD: return getMaxRealTimeLoad(); + case CRSF_GPS_REUSE_BEC_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_BEC); + case CRSF_GPS_REUSE_BUS_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_BUS); + case CRSF_GPS_REUSE_MCU_VOLTAGE: + return getVoltageMeter(VOLTAGE_METER_ID_MCU); } return 0; @@ -417,17 +455,6 @@ static int16_t decidegrees2Radians10000(int16_t angle_decidegree) return (int16_t)(RAD * 1000.0f * angle_decidegree); } -static int getVoltageMeter(voltageMeterId_e id) -{ - voltageMeter_t meter; - - voltageMeterRead(id, &meter); - - // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions - // Max voltage 25.5V - return meter.voltage * 255 / 200; -} - static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude) { escSensorData_t *escData; @@ -440,6 +467,24 @@ static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude) case CRSF_ATT_REUSE_ESC_TEMP: escData = getEscSensorData(ESC_SENSOR_COMBINED); return (escData) ? escData->temperature * 10 : 0; + case CRSF_ATT_REUSE_ESC_PWM: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->pwm : 0; + case CRSF_ATT_REUSE_ESC_BEC_VOLTAGE: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_voltage : 0; + case CRSF_ATT_REUSE_ESC_BEC_CURRENT: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->bec_current : 0; + case CRSF_ATT_REUSE_ESC_BEC_TEMP: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? escData->temperature2 * 10 : 0; + case CRSF_ATT_REUSE_ESC_STATUS: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status & 0xFFFF) : 0; + case CRSF_ATT_REUSE_ESC_STATUS2: + escData = getEscSensorData(ESC_SENSOR_COMBINED); + return (escData) ? (escData->status >> 16) : 0; case CRSF_ATT_REUSE_MCU_TEMP: return getCoreTemperatureCelsius() * 100; case CRSF_ATT_REUSE_MCU_LOAD: diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 58a4c7059a..ceaab554ff 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -49,6 +49,12 @@ enum { CRSF_ATT_REUSE_NONE = 0, CRSF_ATT_REUSE_THROTTLE, CRSF_ATT_REUSE_ESC_TEMP, + CRSF_ATT_REUSE_ESC_PWM, + CRSF_ATT_REUSE_ESC_BEC_VOLTAGE, + CRSF_ATT_REUSE_ESC_BEC_CURRENT, + CRSF_ATT_REUSE_ESC_BEC_TEMP, + CRSF_ATT_REUSE_ESC_STATUS, + CRSF_ATT_REUSE_ESC_STATUS2, CRSF_ATT_REUSE_MCU_TEMP, CRSF_ATT_REUSE_MCU_LOAD, CRSF_ATT_REUSE_SYS_LOAD, @@ -63,10 +69,20 @@ enum { CRSF_GPS_REUSE_HEADSPEED, CRSF_GPS_REUSE_THROTTLE, CRSF_GPS_REUSE_ESC_TEMP, + CRSF_GPS_REUSE_ESC_PWM, + CRSF_GPS_REUSE_ESC_THROTTLE, + CRSF_GPS_REUSE_ESC_BEC_VOLTAGE, + CRSF_GPS_REUSE_ESC_BEC_CURRENT, + CRSF_GPS_REUSE_ESC_BEC_TEMP, + CRSF_GPS_REUSE_ESC_STATUS, + CRSF_GPS_REUSE_ESC_STATUS2, CRSF_GPS_REUSE_MCU_TEMP, CRSF_GPS_REUSE_MCU_LOAD, CRSF_GPS_REUSE_SYS_LOAD, CRSF_GPS_REUSE_RT_LOAD, + CRSF_GPS_REUSE_BEC_VOLTAGE, + CRSF_GPS_REUSE_BUS_VOLTAGE, + CRSF_GPS_REUSE_MCU_VOLTAGE, }; enum {