src/main/telemetry/sbus2.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19
  20 #include "platform.h"
  21
  22 #include "build/debug.h"
  23
  24 #include "common/utils.h"
  25 #include "common/time.h"
  26 #include "common/axis.h"
  27
  28 #include "config/feature.h"
  29
  30 #include "fc/config.h"
  31
  32 #include "telemetry/telemetry.h"
  33 #include "telemetry/sbus2.h"
  34 #include "telemetry/sbus2_sensors.h"
  35
  36 #include "rx/sbus.h"
  37
  38 #include "sensors/battery.h"
  39 #include "sensors/sensors.h"
  40 #include "sensors/temperature.h"
  41 #include "sensors/diagnostics.h"
  42
  43 #include "io/gps.h"
  44
  45 #include "navigation/navigation.h"
  46
  47 #ifdef USE_ESC_SENSOR
  48 #include "sensors/esc_sensor.h"
  49 #include "flight/mixer.h"
  50 #endif
  51
  52 #ifdef USE_TELEMETRY_SBUS2
  53
  54 const uint8_t sbus2SlotIds[SBUS2_SLOT_COUNT] = {
  55     0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3,
  56     0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3,
  57     0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB,
  58     0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB
  59 };
  60
  61 sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT] = {};
  62 uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT] = {};
  63 static uint8_t currentSlot = 0;
  64 static timeUs_t nextSlotTime = 0;
  65
  66 void handleSbus2Telemetry(timeUs_t currentTimeUs)
  67 {
  68     UNUSED(currentTimeUs);
  69
  70     float voltage = getBatteryVoltage() * 0.01f;
  71     float cellVoltage = getBatteryAverageCellVoltage() * 0.01f;
  72     float current = getAmperage() * 0.01f;
  73     float capacity = getMAhDrawn();
  74     float altitude = getEstimatedActualPosition(Z) * 0.01f;
  75     float vario = getEstimatedActualVelocity(Z);
  76     float temperature = 0;
  77     uint32_t rpm = 0;
  78
  79 #ifdef USE_ESC_SENSOR
  80     escSensorData_t * escSensor = escSensorGetData();
  81     if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
  82         rpm = escSensor->rpm;
  83         temperature = escSensor->temperature;
  84     } else {
  85         rpm = 0;
  86         temperature = 0;
  87     }
  88 #endif
  89
  90     //temperature = 42.16f;
  91
  92     DEBUG_SET(DEBUG_SBUS2, 0, voltage);
  93     DEBUG_SET(DEBUG_SBUS2, 1, cellVoltage);
  94     DEBUG_SET(DEBUG_SBUS2, 2, current);
  95     DEBUG_SET(DEBUG_SBUS2, 3, capacity);
  96     DEBUG_SET(DEBUG_SBUS2, 4, altitude);
  97     DEBUG_SET(DEBUG_SBUS2, 5, vario);
  98     DEBUG_SET(DEBUG_SBUS2, 6, rpm);
  99     DEBUG_SET(DEBUG_SBUS2, 7, temperature);
 100
 101     // 2 slots
 102     send_voltagef(1, voltage, cellVoltage);
 103     // 3 slots
 104     send_s1678_currentf(3, current, capacity, voltage);
 105     // 1 slot
 106     send_RPM(6, rpm);
 107     // 1 slot - esc temp
 108     static int change = 0;
 109     change++;
 110     int delta = change / 10;
 111     delta = delta % 20;
 112     send_SBS01T(7, temperature);
 113
 114     // 8 slots, gps
 115     uint16_t speed = 0;
 116     float latitude = 0;
 117     float longitude = 0;
 118
 119 #ifdef USE_GPS
 120     if (gpsSol.fixType >= GPS_FIX_2D) {
 121         speed = (CMSEC_TO_KPH(gpsSol.groundSpeed) + 0.5f);
 122         latitude = gpsSol.llh.lat * 1e-7;
 123         longitude = gpsSol.llh.lon * 1e-7;
 124     }
 125 #endif
 126
 127     send_F1675f(8, speed, altitude, vario, latitude, longitude);
 128     // imu 1 slot
 129     int16_t temp16;
 130     bool valid = getIMUTemperature(&temp16);
 131     send_SBS01T(16, valid ? temp16 / 10 : 0);
 132     // baro
 133     valid = 0;
 134     valid = getBaroTemperature(&temp16);
 135     send_SBS01T(17, valid ? temp16 / 10 : 0);
 136
 137     // temp sensors 18-25
 138 #ifdef USE_TEMPERATURE_SENSOR
 139     for(int i = 0; i < 8; i++) {
 140         temp16 = 0;
 141         valid = getSensorTemperature(0, &temp16);
 142         send_SBS01T(18 + i, valid ? temp16 / 10 : 0);
 143     }
 144 #endif
 145 }
 146
 147 uint8_t sbus2GetTelemetrySlot(timeUs_t elapsed)
 148 {
 149     UNUSED(elapsed);
 150     if (elapsed < SBUS2_DEADTIME) {
 151         currentSlot = 0;
 152         nextSlotTime = 0;
 153         return 0xFF; // skip it
 154     }
 155
 156     if(currentSlot < SBUS2_TELEMETRY_SLOTS) {
 157         return currentSlot;
 158     }
 159
 160     return 0xFF;
 161 }
 162
 163 void sbus2IncrementTelemetrySlot(timeUs_t currentTimeUs)
 164 {
 165     nextSlotTime = currentTimeUs + (SBUS2_TRANSMIT_TIME + SBUS2_SLOT_DELAY);
 166     currentSlot++;
 167 }
 168
 169 FAST_CODE void taskSendSbus2Telemetry(timeUs_t currentTimeUs)
 170 {
 171     if (!feature(FEATURE_TELEMETRY) || !telemetrySharedPort || rxConfig()->receiverType != RX_TYPE_SERIAL ||
 172         rxConfig()->serialrx_provider != SERIALRX_SBUS2) {
 173         return;
 174     }
 175
 176     timeUs_t elapsedTime = currentTimeUs - sbusGetLastFrameTime();
 177
 178     if (elapsedTime > MS2US(8)) {
 179         currentSlot = 0;
 180         nextSlotTime = 0;
 181         return;
 182     }
 183
 184     if (currentTimeUs < nextSlotTime) {
 185         return;
 186     }
 187
 188     uint8_t telemetryPage = sbusGetCurrentTelemetryPage();
 189
 190     uint8_t slot = sbus2GetTelemetrySlot(elapsedTime);
 191
 192     if(slot < SBUS2_TELEMETRY_SLOTS) {
 193         int slotIndex = (telemetryPage * SBUS2_TELEMETRY_SLOTS) + slot;
 194         if (slotIndex < SBUS2_SLOT_COUNT) {
 195             if (sbusTelemetryDataUsed[slotIndex] != 0) {
 196                 // send
 197                 serialWriteBuf(telemetrySharedPort,
 198                                (const uint8_t *)&sbusTelemetryData[slotIndex],
 199                                sizeof(sbus2_telemetry_frame_t));
 200             }
 201         }
 202         sbus2IncrementTelemetrySlot(currentTimeUs);
 203     }
 204 }
 205
 206
 207
 208
 209 #endif