diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 014a1e24ab..287526d826 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -85,6 +85,7 @@ #include "sensors/rangefinder.h" #include "telemetry/frsky_hub.h" +#include "telemetry/ibus_shared.h" #include "telemetry/telemetry.h" // Sensor names (used in lookup tables for *_hardware settings and in status command output) @@ -707,6 +708,7 @@ const clivalue_t valueTable[] = { { "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pidValuesAsTelemetry) }, #if defined(USE_TELEMETRY_IBUS) { "report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, report_cell_voltage) }, + { "ibus_sensor", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = IBUS_SENSOR_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, flysky_sensors)}, #endif #endif // USE_TELEMETRY diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index b4879b3a03..d8590f0fe8 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -316,6 +316,8 @@ uint32_t baroUpdate(void) baro.dev.get_up(&baro.dev); baro.dev.start_ut(&baro.dev); baro.dev.calculate(&baroPressure, &baroTemperature); + baro.baroPressure = baroPressure; + baro.baroTemperature = baroTemperature; baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure); state = BAROMETER_NEEDS_SAMPLES; return baro.dev.ut_delay; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 83911c1c0f..de3c12ec17 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -50,6 +50,7 @@ typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; int32_t baroTemperature; // Use temperature for telemetry + int32_t baroPressure; // Use pressure for telemetry } baro_t; extern baro_t baro; diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 8144e00ad5..efc4200412 100644 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -54,6 +54,8 @@ #define REMAP_TIM16_DMA #define REMAP_TIM17_DMA +#undef USE_TELEMETRY_IBUS + #define USE_VCP #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 41226f8e49..625d64016f 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -170,7 +170,8 @@ #define USE_OSD_ADJUSTMENTS #define USE_NAV #define USE_TELEMETRY_IBUS +#define USE_TELEMETRY_IBUS_EXTENDED #define USE_UNCOMMON_MIXERS // #define USE_GYRO_FAST_KALMAN #define USE_GYRO_BIQUAD_RC_FIR2 -#endif +#endif \ No newline at end of file diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 716a439289..f63fed83be 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -23,182 +23,540 @@ */ -#include -#include -#include -// #include + #include + #include + #include + // #include -#include "platform.h" -//#include "common/utils.h" -#include "telemetry/telemetry.h" -#include "telemetry/ibus_shared.h" + #include "platform.h" + //#include "common/utils.h" + #include "telemetry/telemetry.h" + #include "telemetry/ibus_shared.h" -static uint16_t calculateChecksum(const uint8_t *ibusPacket, size_t packetLength); + static uint16_t calculateChecksum(const uint8_t *ibusPacket); + + #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) + #include "config/feature.h" + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "sensors/battery.h" + #include "fc/rc_controls.h" + #include "fc/config.h" + #include "sensors/gyro.h" + #include "drivers/accgyro/accgyro.h" + #include "fc/runtime_config.h" + #include "sensors/acceleration.h" + #include "sensors/sensors.h" + #include "sensors/barometer.h" + #include "flight/imu.h" + #include "flight/altitude.h" + #include "flight/navigation.h" + #include "io/gps.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) + #define IBUS_TEMPERATURE_OFFSET 400 + #define INVALID_IBUS_ADDRESS 0 + #define IBUS_BUFFSIZE 33 // biggest iBus message seen so far + 1 + #define IBUS_HEADER_FOOTER_SIZE 4 + #define IBUS_2BYTE_SESNSOR 2 + #define IBUS_4BYTE_SESNSOR 4 -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "sensors/battery.h" -#include "fc/rc_controls.h" -#include "sensors/gyro.h" + typedef uint8_t ibusAddress_t; + + typedef enum { + IBUS_COMMAND_DISCOVER_SENSOR = 0x80, + IBUS_COMMAND_SENSOR_TYPE = 0x90, + IBUS_COMMAND_MEASUREMENT = 0xA0 + } ibusCommand_e; + + typedef enum { + IBUS_SENSOR_TYPE_NONE = 0x00, + IBUS_SENSOR_TYPE_TEMPERATURE = 0x01, + IBUS_SENSOR_TYPE_RPM_FLYSKY = 0x02, + IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03, + IBUS_SENSOR_TYPE_CELL = 0x04, // Avg Cell voltage + IBUS_SENSOR_TYPE_BAT_CURR = 0x05, // battery current A * 100 + IBUS_SENSOR_TYPE_FUEL = 0x06, // remaining battery percentage / mah drawn otherwise or fuel level no unit! + IBUS_SENSOR_TYPE_RPM = 0x07, // throttle value / battery capacity + IBUS_SENSOR_TYPE_CMP_HEAD = 0x08, //Heading 0..360 deg, 0=north 2bytes + IBUS_SENSOR_TYPE_CLIMB_RATE = 0x09, //2 bytes m/s *100 + IBUS_SENSOR_TYPE_COG = 0x0a, //2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint + IBUS_SENSOR_TYPE_GPS_STATUS = 0x0b, //2 bytes + IBUS_SENSOR_TYPE_ACC_X = 0x0c, //2 bytes m/s *100 signed + IBUS_SENSOR_TYPE_ACC_Y = 0x0d, //2 bytes m/s *100 signed + IBUS_SENSOR_TYPE_ACC_Z = 0x0e, //2 bytes m/s *100 signed + IBUS_SENSOR_TYPE_ROLL = 0x0f, //2 bytes deg *100 signed + IBUS_SENSOR_TYPE_PITCH = 0x10, //2 bytes deg *100 signed + IBUS_SENSOR_TYPE_YAW = 0x11, //2 bytes deg *100 signed + IBUS_SENSOR_TYPE_VERTICAL_SPEED = 0x12, //2 bytes m/s *100 + IBUS_SENSOR_TYPE_GROUND_SPEED = 0x13, //2 bytes m/s *100 different unit than build-in sensor + IBUS_SENSOR_TYPE_GPS_DIST = 0x14, //2 bytes dist from home m unsigned + IBUS_SENSOR_TYPE_ARMED = 0x15, //2 bytes + IBUS_SENSOR_TYPE_FLIGHT_MODE = 0x16, //2 bytes + IBUS_SENSOR_TYPE_PRES = 0x41, // Pressure + IBUS_SENSOR_TYPE_ODO1 = 0x7c, // Odometer1 + IBUS_SENSOR_TYPE_ODO2 = 0x7d, // Odometer2 + IBUS_SENSOR_TYPE_SPE = 0x7e, // Speed 2bytes km/h + + IBUS_SENSOR_TYPE_GPS_LAT = 0x80, //4bytes signed WGS84 in degrees * 1E7 + IBUS_SENSOR_TYPE_GPS_LON = 0x81, //4bytes signed WGS84 in degrees * 1E7 + IBUS_SENSOR_TYPE_GPS_ALT = 0x82, //4bytes signed!!! GPS alt m*100 + IBUS_SENSOR_TYPE_ALT = 0x83, //4bytes signed!!! Alt m*100 + IBUS_SENSOR_TYPE_ALT_MAX = 0x84, //4bytes signed MaxAlt m*100 + + IBUS_SENSOR_TYPE_ALT_FLYSKY = 0xf9, // Altitude 2 bytes signed in m + #if defined(USE_TELEMETRY_IBUS_EXTENDED) + IBUS_SENSOR_TYPE_GPS_FULL = 0xfd, + IBUS_SENSOR_TYPE_VOLT_FULL = 0xf0, + IBUS_SENSOR_TYPE_ACC_FULL = 0xef, + #endif //defined(TELEMETRY_IBUS_EXTENDED) + IBUS_SENSOR_TYPE_UNKNOWN = 0xff + } ibusSensorType_e; + + typedef union ibusTelemetry { + uint16_t uint16; + uint32_t uint32; + int16_t int16; + int32_t int32; + uint8_t byte[4]; + } ibusTelemetry_s; + + #if defined(USE_GPS) + + const uint8_t GPS_IDS[] = { + IBUS_SENSOR_TYPE_GPS_STATUS, + IBUS_SENSOR_TYPE_SPE, + IBUS_SENSOR_TYPE_GPS_LAT, + IBUS_SENSOR_TYPE_GPS_LON, + IBUS_SENSOR_TYPE_GPS_ALT, + IBUS_SENSOR_TYPE_GROUND_SPEED, + IBUS_SENSOR_TYPE_ODO1, + IBUS_SENSOR_TYPE_ODO2, + IBUS_SENSOR_TYPE_GPS_DIST, + IBUS_SENSOR_TYPE_COG, + }; + #endif + + #if defined(USE_TELEMETRY_IBUS_EXTENDED) + + const uint8_t FULL_GPS_IDS[] = { + IBUS_SENSOR_TYPE_GPS_STATUS, + IBUS_SENSOR_TYPE_GPS_LAT, + IBUS_SENSOR_TYPE_GPS_LON, + IBUS_SENSOR_TYPE_GPS_ALT, + }; + + const uint8_t FULL_VOLT_IDS[] = { + IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, + IBUS_SENSOR_TYPE_CELL, + IBUS_SENSOR_TYPE_BAT_CURR, + IBUS_SENSOR_TYPE_FUEL, + IBUS_SENSOR_TYPE_RPM, + }; + + const uint8_t FULL_ACC_IDS[] = { + IBUS_SENSOR_TYPE_ACC_X, + IBUS_SENSOR_TYPE_ACC_Y, + IBUS_SENSOR_TYPE_ACC_Z, + IBUS_SENSOR_TYPE_ROLL, + IBUS_SENSOR_TYPE_PITCH, + IBUS_SENSOR_TYPE_YAW, + }; + + #endif //defined(USE_TELEMETRY_IBUS_EXTENDED) + + static serialPort_t *ibusSerialPort = NULL; + static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS; + static uint8_t sendBuffer[IBUS_BUFFSIZE]; -#define IBUS_TEMPERATURE_OFFSET (400) + static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length); -typedef uint8_t ibusAddress_t; + static uint8_t getSensorID(ibusAddress_t address) + { + //all checks are done in theAddressIsWithinOurRange + uint32_t index = address - ibusBaseAddress; + return telemetryConfig()->flysky_sensors[index]; + } -typedef enum { - IBUS_COMMAND_DISCOVER_SENSOR = 0x80, - IBUS_COMMAND_SENSOR_TYPE = 0x90, - IBUS_COMMAND_MEASUREMENT = 0xA0 -} ibusCommand_e; + static uint8_t getSensorLength(uint8_t sensorID) + { + if (sensorID == IBUS_SENSOR_TYPE_PRES || (sensorID >= IBUS_SENSOR_TYPE_GPS_LAT && sensorID <= IBUS_SENSOR_TYPE_ALT_MAX)) { + return IBUS_4BYTE_SESNSOR; + } + #if defined(USE_TELEMETRY_IBUS_EXTENDED) + if (sensorID == IBUS_SENSOR_TYPE_GPS_FULL) { + return 14; + } + if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) { + return 10; + } + if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) { + return 12; + } + #endif + return IBUS_2BYTE_SESNSOR; + } -typedef enum { - IBUS_SENSOR_TYPE_TEMPERATURE = 0x01, - IBUS_SENSOR_TYPE_RPM = 0x02, - IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03 -} ibusSensorType_e; + static uint8_t transmitIbusPacket() + { + unsigned frameLength = sendBuffer[0]; + if (frameLength == INVALID_IBUS_ADDRESS) { + return 0; + } + unsigned payloadLength = frameLength - IBUS_CHECKSUM_SIZE; + uint16_t checksum = calculateChecksum(sendBuffer); + for (unsigned i = 0; i < payloadLength; i++) { + serialWrite(ibusSerialPort, sendBuffer[i]); + } + serialWrite(ibusSerialPort, checksum & 0xFF); + serialWrite(ibusSerialPort, checksum >> 8); + return frameLength; + } -/* Address lookup relative to the sensor base address which is the lowest address seen by the FC - The actual lowest value is likely to change when sensors are daisy chained */ -static const uint8_t sensorAddressTypeLookup[] = { - IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, - IBUS_SENSOR_TYPE_TEMPERATURE, - IBUS_SENSOR_TYPE_RPM -}; + static void setIbusDiscoverSensorReply(ibusAddress_t address) + { + sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE; + sendBuffer[1] = IBUS_COMMAND_DISCOVER_SENSOR | address; + } -static serialPort_t *ibusSerialPort = NULL; + static void setIbusSensorType(ibusAddress_t address) + { + uint8_t sensorID = getSensorID(address); + uint8_t sensorLength = getSensorLength(sensorID); + sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + 2; + sendBuffer[1] = IBUS_COMMAND_SENSOR_TYPE | address; + sendBuffer[2] = sensorID; + sendBuffer[3] = sensorLength; + } -#define INVALID_IBUS_ADDRESS 0 -static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS; + static uint16_t getVoltage() + { + uint16_t voltage = getBatteryVoltage() *10; + if (telemetryConfig()->report_cell_voltage) { + voltage /= getBatteryCellCount(); + } + return voltage; + } + + static uint16_t getTemperature() + { + uint16_t temperature = gyroGetTemperature() * 10; + #if defined(BARO) + if (sensors(SENSOR_BARO)) { + temperature = (uint16_t) ((baro.baroTemperature + 50) / 10); + } + #endif + return temperature + IBUS_TEMPERATURE_OFFSET; + } + + + static uint16_t getFuel() + { + uint16_t fuel = 0; + if (batteryConfig()->batteryCapacity > 0) { + fuel = (uint16_t)calculateBatteryPercentageRemaining(); + } else { + fuel = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF); + } + return fuel; + } + + static uint16_t getRPM() + { + uint16_t rpm = 0; + if (ARMING_FLAG(ARMED)) { + const throttleStatus_e throttleStatus = calculateThrottleStatus(); + rpm = rcCommand[THROTTLE]; // / BLADE_NUMBER_DIVIDER; + if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) rpm = 0; + } else { + rpm = (uint16_t)(batteryConfig()->batteryCapacity); // / BLADE_NUMBER_DIVIDER + } + return rpm; + } + + static uint16_t getMode() + { + uint16_t flightMode = 1; //Acro + if (FLIGHT_MODE(ANGLE_MODE)) { + flightMode = 0; //Stab + } + if (FLIGHT_MODE(BARO_MODE)) { + flightMode = 2; //AltHold + } + if (FLIGHT_MODE(PASSTHRU_MODE)) { + flightMode = 3; //Auto + } + if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE)) { + flightMode = 4; //Guided! (there in no HEAD, MAG so use Guided) + } + if (FLIGHT_MODE(GPS_HOLD_MODE) && FLIGHT_MODE(BARO_MODE)) { + flightMode = 5; //Loiter + } + if (FLIGHT_MODE(GPS_HOME_MODE)) { + flightMode = 6; //RTL + } + if (FLIGHT_MODE(HORIZON_MODE)) { + flightMode = 7; //Circle! (there in no horizon so use Circle) + } + if (FLIGHT_MODE(GPS_HOLD_MODE)) { + flightMode = 8; //PosHold + } + if (FLIGHT_MODE(FAILSAFE_MODE)) { + flightMode = 9; //Land + } + return flightMode; + } + + static int16_t getACC(uint8_t index) + { + return (int16_t)(((float)acc.accADC[index] / acc.dev.acc_1G) * 1000); + } + + #if defined(USE_TELEMETRY_IBUS_EXTENDED) + static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount) + { + uint8_t offset = 0; + uint8_t size = 0; + for (unsigned i = 0; i < itemCount; i++) { + size = getSensorLength(structure[i]); + setValue(bufferPtr + offset, structure[i], size); + offset += size; + } + } + #endif -static uint8_t transmitIbusPacket(uint8_t *ibusPacket, size_t payloadLength) -{ - uint16_t checksum = calculateChecksum(ibusPacket, payloadLength + IBUS_CHECKSUM_SIZE); - for (size_t i = 0; i < payloadLength; i++) { - serialWrite(ibusSerialPort, ibusPacket[i]); - } - serialWrite(ibusSerialPort, checksum & 0xFF); - serialWrite(ibusSerialPort, checksum >> 8); - return payloadLength + IBUS_CHECKSUM_SIZE; -} + #if defined(USE_GPS) + static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value) + { + bool result = false; + for (unsigned i = 0; i < sizeof(GPS_IDS); i++) { + if (sensorType == GPS_IDS[i]) { + result = true; + break; + } + } + if (!result) return result; -static uint8_t sendIbusDiscoverSensorReply(ibusAddress_t address) -{ - uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address}; - return transmitIbusPacket(sendBuffer, sizeof(sendBuffer)); -} + uint16_t gpsFixType = 0; + uint16_t sats = 0; + if (sensors(SENSOR_GPS)) { + gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < 5 ? 2 : 3); + sats = gpsSol.numSat; + if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) { + result = true; + switch (sensorType) { + case IBUS_SENSOR_TYPE_SPE: + value->uint16 = gpsSol.groundSpeed * 36 / 100; + break; + case IBUS_SENSOR_TYPE_GPS_LAT: + value->int32 = gpsSol.llh.lat; + break; + case IBUS_SENSOR_TYPE_GPS_LON: + value->int32 = gpsSol.llh.lon; + break; + case IBUS_SENSOR_TYPE_GPS_ALT: + value->int32 = (int32_t)gpsSol.llh.alt; + break; + case IBUS_SENSOR_TYPE_GROUND_SPEED: + value->uint16 = gpsSol.groundSpeed; + break; + case IBUS_SENSOR_TYPE_ODO1: + case IBUS_SENSOR_TYPE_ODO2: + case IBUS_SENSOR_TYPE_GPS_DIST: + value->uint16 = GPS_distanceToHome; + break; + case IBUS_SENSOR_TYPE_COG: + value->uint16 = gpsSol.groundCourse * 100; + break; + case IBUS_SENSOR_TYPE_GPS_STATUS: + value->byte[0] = gpsFixType; + value->byte[1] = sats; + break; + } + } + } + return result; + } + #endif //defined(USE_GPS) -static uint8_t sendIbusSensorType(ibusAddress_t address) -{ - uint8_t sendBuffer[] = {0x06, - IBUS_COMMAND_SENSOR_TYPE | address, - sensorAddressTypeLookup[address - ibusBaseAddress], - 0x02 - }; - return transmitIbusPacket(sendBuffer, sizeof(sendBuffer)); -} + static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length) + { + ibusTelemetry_s value; -static uint8_t sendIbusMeasurement(ibusAddress_t address, uint16_t measurement) -{ - uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_MEASUREMENT | address, measurement & 0xFF, measurement >> 8}; - return transmitIbusPacket(sendBuffer, sizeof(sendBuffer)); -} + #if defined(USE_TELEMETRY_IBUS_EXTENDED) + const uint8_t* structure = 0; + uint8_t itemCount; + if (sensorType == IBUS_SENSOR_TYPE_GPS_FULL) { + structure = FULL_GPS_IDS; + itemCount = sizeof(FULL_GPS_IDS); + } + if (sensorType == IBUS_SENSOR_TYPE_VOLT_FULL) { + structure = FULL_VOLT_IDS; + itemCount = sizeof(FULL_VOLT_IDS); + } + if (sensorType == IBUS_SENSOR_TYPE_ACC_FULL) { + structure = FULL_ACC_IDS; + itemCount = sizeof(FULL_ACC_IDS); + } + if (structure != 0) { + setCombinedFrame(bufferPtr, structure, sizeof(itemCount)); + return; + } + #endif //defined(USE_TELEMETRY_IBUS_EXTENDED) -static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket) -{ - return (ibusPacket[1] & 0xF0) == expected; -} + #if defined(USE_GPS) + if (setGPS(sensorType, &value)) { + return; + } + #endif //defined(USE_TELEMETRY_IBUS_EXTENDED) -static ibusAddress_t getAddress(const uint8_t *ibusPacket) -{ - return (ibusPacket[1] & 0x0F); -} + for (unsigned i = 0; i < length; i++) { + bufferPtr[i] = value.byte[i] = 0; + } + switch (sensorType) { + case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: + value.uint16 = getVoltage(); + break; + case IBUS_SENSOR_TYPE_TEMPERATURE: + value.uint16 = getTemperature(); + break; + case IBUS_SENSOR_TYPE_RPM_FLYSKY: + value.int16 = (int16_t)rcCommand[THROTTLE]; + break; + case IBUS_SENSOR_TYPE_FUEL: + value.uint16 = getFuel(); + break; + case IBUS_SENSOR_TYPE_RPM: + value.uint16 = getRPM(); + break; + case IBUS_SENSOR_TYPE_FLIGHT_MODE: + value.uint16 = getMode(); + break; + case IBUS_SENSOR_TYPE_CELL: + value.uint16 = (uint16_t)(getBatteryAverageCellVoltage() *10); + break; + case IBUS_SENSOR_TYPE_BAT_CURR: + value.uint16 = (uint16_t)getAmperage(); + break; + case IBUS_SENSOR_TYPE_ACC_X: + case IBUS_SENSOR_TYPE_ACC_Y: + case IBUS_SENSOR_TYPE_ACC_Z: + value.int16 = getACC(sensorType - IBUS_SENSOR_TYPE_ACC_X); + break; + case IBUS_SENSOR_TYPE_ROLL: + case IBUS_SENSOR_TYPE_PITCH: + case IBUS_SENSOR_TYPE_YAW: + value.int16 = attitude.raw[sensorType - IBUS_SENSOR_TYPE_ROLL] *10; + break; + case IBUS_SENSOR_TYPE_ARMED: + value.uint16 = ARMING_FLAG(ARMED) ? 0 : 1; + break; + #if defined(USE_TELEMETRY_IBUS_EXTENDED) + case IBUS_SENSOR_TYPE_CMP_HEAD: + value.uint16 = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + break; + case IBUS_SENSOR_TYPE_VERTICAL_SPEED: + case IBUS_SENSOR_TYPE_CLIMB_RATE: + if(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) { + value.int16 = (int16_t)getEstimatedVario(); + } + break; + case IBUS_SENSOR_TYPE_ALT: + case IBUS_SENSOR_TYPE_ALT_MAX: + value.int32 = baro.BaroAlt; + break; + case IBUS_SENSOR_TYPE_PRES: + value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19); + break; + #endif //defined(TELEMETRY_IBUS_EXTENDED) + } + for (unsigned i = 0; i < length; i++) { + bufferPtr[i] = value.byte[i]; + } + } + static void setIbusMeasurement(ibusAddress_t address) + { + uint8_t sensorID = getSensorID(address); + uint8_t sensorLength = getSensorLength(sensorID); + sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + sensorLength; + sendBuffer[1] = IBUS_COMMAND_MEASUREMENT | address; + setValue(sendBuffer + 2, sensorID, sensorLength); + } -static uint8_t dispatchMeasurementReply(ibusAddress_t address) -{ - int value; + static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket) + { + return (ibusPacket[1] & 0xF0) == expected; + } - switch (sensorAddressTypeLookup[address - ibusBaseAddress]) { - case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE: - value = getBatteryVoltage() * 10; - if (telemetryConfig()->report_cell_voltage) { - value /= getBatteryCellCount(); - } - return sendIbusMeasurement(address, value); + static ibusAddress_t getAddress(const uint8_t *ibusPacket) + { + return (ibusPacket[1] & 0x0F); + } - case IBUS_SENSOR_TYPE_TEMPERATURE: - value = gyroGetTemperature() * 10; - return sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET); + static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress) + { + if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) && + (INVALID_IBUS_ADDRESS != returnAddress)) { + ibusBaseAddress = returnAddress; + } + } - case IBUS_SENSOR_TYPE_RPM: - return sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]); - } - return 0; -} + static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress) + { + return (returnAddress >= ibusBaseAddress) && + (ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(telemetryConfig()->flysky_sensors) && + telemetryConfig()->flysky_sensors[(returnAddress - ibusBaseAddress)] != IBUS_SENSOR_TYPE_NONE; + } -static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress) -{ - if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) && - (INVALID_IBUS_ADDRESS != returnAddress)) { - ibusBaseAddress = returnAddress; - } -} + uint8_t respondToIbusRequest(uint8_t const * const ibusPacket) + { + ibusAddress_t returnAddress = getAddress(ibusPacket); + autodetectFirstReceivedAddressAsBaseAddress(returnAddress); + //set buffer to invalid + sendBuffer[0] = INVALID_IBUS_ADDRESS; -static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress) -{ - return (returnAddress >= ibusBaseAddress) && - (ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(sensorAddressTypeLookup); -} - -uint8_t respondToIbusRequest(uint8_t const * const ibusPacket) -{ - ibusAddress_t returnAddress = getAddress(ibusPacket); - autodetectFirstReceivedAddressAsBaseAddress(returnAddress); - - if (theAddressIsWithinOurRange(returnAddress)) { - if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) { - return sendIbusDiscoverSensorReply(returnAddress); - } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) { - return sendIbusSensorType(returnAddress); - } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) { - return dispatchMeasurementReply(returnAddress); - } - } - - return 0; -} + if (theAddressIsWithinOurRange(returnAddress)) { + if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) { + setIbusDiscoverSensorReply(returnAddress); + } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) { + setIbusSensorType(returnAddress); + } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) { + setIbusMeasurement(returnAddress); + } + } + //transmit if content was set + return transmitIbusPacket(); + } -void initSharedIbusTelemetry(serialPort_t *port) -{ - ibusSerialPort = port; - ibusBaseAddress = INVALID_IBUS_ADDRESS; -} + void initSharedIbusTelemetry(serialPort_t *port) + { + ibusSerialPort = port; + ibusBaseAddress = INVALID_IBUS_ADDRESS; + } -#endif //defined(TELEMETRY) && defined(TELEMETRY_IBUS) + #endif //defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) -static uint16_t calculateChecksum(const uint8_t *ibusPacket, size_t packetLength) -{ - uint16_t checksum = 0xFFFF; - for (size_t i = 0; i < packetLength - IBUS_CHECKSUM_SIZE; i++) { - checksum -= ibusPacket[i]; - } + static uint16_t calculateChecksum(const uint8_t *ibusPacket) + { + uint16_t checksum = 0xFFFF; + uint8_t dataSize = ibusPacket[0] - IBUS_CHECKSUM_SIZE; + for (unsigned i = 0; i < dataSize; i++) { + checksum -= ibusPacket[i]; + } - return checksum; -} + return checksum; + } -bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length) -{ - uint16_t calculatedChecksum = calculateChecksum(ibusPacket, length); - - // Note that there's a byte order swap to little endian here - return (calculatedChecksum >> 8) == ibusPacket[length - 1] - && (calculatedChecksum & 0xFF) == ibusPacket[length - 2]; -} + bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length) + { + uint16_t calculatedChecksum = calculateChecksum(ibusPacket); + // Note that there's a byte order swap to little endian here + return (calculatedChecksum >> 8) == ibusPacket[length - 1] + && (calculatedChecksum & 0xFF) == ibusPacket[length - 2]; + } diff --git a/src/main/telemetry/ibus_shared.h b/src/main/telemetry/ibus_shared.h index f9d0a83384..e6f68b5627 100644 --- a/src/main/telemetry/ibus_shared.h +++ b/src/main/telemetry/ibus_shared.h @@ -29,7 +29,7 @@ #include "drivers/serial.h" #define IBUS_CHECKSUM_SIZE (2) - +#define IBUS_SENSOR_COUNT 15 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) uint8_t respondToIbusRequest(uint8_t const * const ibusPacket); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 61a075421a..e5d6b3e182 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -26,6 +26,7 @@ #include "pg/pg.h" #include "io/serial.h" +#include "telemetry/ibus_shared.h" typedef enum { FRSKY_FORMAT_DMS = 0, @@ -49,6 +50,7 @@ typedef struct telemetryConfig_s { uint8_t hottAlarmSoundInterval; uint8_t pidValuesAsTelemetry; uint8_t report_cell_voltage; + uint8_t flysky_sensors[IBUS_SENSOR_COUNT]; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index d343c298ac..416763294b 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -24,11 +24,16 @@ extern "C" { #include "pg/pg.h" #include "drivers/serial.h" #include "io/serial.h" +#include "io/gps.h" +#include "flight/imu.h" +#include "fc/config.h" #include "fc/rc_controls.h" #include "telemetry/telemetry.h" #include "telemetry/ibus.h" #include "sensors/gyro.h" #include "sensors/battery.h" +#include "sensors/barometer.h" +#include "sensors/acceleration.h" #include "scheduler/scheduler.h" #include "fc/fc_tasks.h" } @@ -38,9 +43,18 @@ extern "C" { extern "C" { + uint8_t armingFlags = 0; + uint8_t stateFlags = 0; + uint16_t flightModeFlags = 0; uint8_t testBatteryCellCount =3; float rcCommand[4] = {0, 0, 0, 0}; telemetryConfig_t telemetryConfig_System; + batteryConfig_s batteryConfig_System; + attitudeEulerAngles_t attitude = EULER_INITIALIZE; + acc_t acc; + baro_t baro; + gpsSolutionData_t gpsSol; + uint16_t GPS_distanceToHome; } static int16_t gyroTemperature; @@ -54,6 +68,57 @@ uint16_t getVbat(void) return vbat; } +extern "C" { +static int32_t amperage = 100; +static int32_t estimatedVario = 0; +static uint8_t batteryRemaining = 0; +static uint16_t avgCellVoltage = vbat/testBatteryCellCount; +static throttleStatus_e throttleStatus = THROTTLE_HIGH; +static uint32_t definedFeatures = 0; +static uint32_t definedSensors = SENSOR_GYRO | SENSOR_ACC | SENSOR_MAG | SENSOR_SONAR | SENSOR_GPS | SENSOR_GPSMAG; + + +int32_t getAmperage(void) +{ + return amperage; +} + +int32_t getEstimatedVario(void) +{ + return estimatedVario; +} + +uint8_t calculateBatteryPercentageRemaining(void) +{ + return batteryRemaining; +} + +uint16_t getBatteryAverageCellVoltage(void) +{ + return avgCellVoltage; +} + +int32_t getMAhDrawn(void) +{ + return 0; +} + +throttleStatus_e calculateThrottleStatus(void) +{ + return throttleStatus; +} + +bool feature(uint32_t mask) +{ + return (definedFeatures & mask) != 0; +} + +bool sensors(sensors_e sensor) +{ + return (definedSensors & sensor) != 0; +} +} + #define SERIAL_BUFFER_SIZE 256 typedef struct serialPortStub_s { @@ -202,6 +267,13 @@ void serialTestResetBuffers() memset(&serialWriteStub, 0, sizeof(serialWriteStub)); } +void setTestSensors() +{ + telemetryConfig_System.flysky_sensors[0] = 0x03; + telemetryConfig_System.flysky_sensors[1] = 0x01; + telemetryConfig_System.flysky_sensors[2] = 0x02; + telemetryConfig_System.flysky_sensors[3] = 0x00; +} void serialTestResetPort() { @@ -221,6 +293,7 @@ protected: virtual void SetUp() { serialTestResetPort(); + setTestSensors(); } };