DSM Telemetry support

static void parse_telemetry_packet()
{
    static s32 altitude;
    static const u8 update7f[] = {
                 TELEM_DSM_FLOG_FADESA, TELEM_DSM_FLOG_FADESB,
                 TELEM_DSM_FLOG_FADESL, TELEM_DSM_FLOG_FADESR,
                 TELEM_DSM_FLOG_FRAMELOSS, TELEM_DSM_FLOG_HOLDS,
                 TELEM_DSM_FLOG_VOLT2, 0};
    static const u8 update7e[] = {
                TELEM_DSM_FLOG_RPM1, TELEM_DSM_FLOG_VOLT1, TELEM_DSM_FLOG_TEMP1, 0};
    static const u8 update16[] = { TELEM_GPS_ALT, TELEM_GPS_LAT, TELEM_GPS_LONG, 0};
    static const u8 update17[] = { TELEM_GPS_SPEED, TELEM_GPS_TIME, 0};
    const u8 *update = NULL;
    switch(packet[0]) {
        case 0x7f: //TM1000 Flight log
        case 0xff: //TM1100 Flight log
            update = update7f;
            Telemetry.p.dsm.flog.fades[0] = pkt16_to_u8(packet+2); //FadesA 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.fades[1] = pkt16_to_u8(packet+4); //FadesB 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.fades[2] = pkt16_to_u8(packet+6); //FadesL 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.fades[3] = pkt16_to_u8(packet+8); //FadesR 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.frameloss = pkt16_to_u8(packet+10);
            Telemetry.p.dsm.flog.holds = pkt16_to_u8(packet+12);
            Telemetry.p.dsm.flog.volt[1] = pkt16_to_volt(packet+14);
            break;
        case 0x7e: //TM1000
        case 0xfe: //TM1100
            update = update7e;
            Telemetry.p.dsm.flog.rpm = pkt16_to_rpm(packet+2);
            Telemetry.p.dsm.flog.volt[0] = pkt16_to_volt(packet+4);  //In 1/10 of Volts
            Telemetry.p.dsm.flog.temp = pkt16_to_temp(packet+6);
        case 0x03: //High Current sensor
            //Telemetry.current = (s32)((s16)(packet[2] << 8) | packet[3]) * 196791 / 100000; //In 1/10 of Amps (16bit signed integer, 1 unit is 0.196791A)
            break;
        case 0x0a: //Powerbox sensor
            //Telemetry.pwb.volt1 = (((s32)packet[2] << 8) | packet[3] + 5) /10; //In 1/10 of Volts
            //Telemetry.pwb.volt1 = (((s32)packet[4] << 8) | packet[5] + 5) /10; //In 1/10 of Volts
            //Telemetry.pwb.capacity1 = ((s32)packet[6] << 8) | packet[7]; //In mAh
            //Telemetry.pwb.capacity2 = ((s32)packet[8] << 8) | packet[9]; //In mAh
            //Telemetry.pwb.alarm_v1 = packet[15] & 0x01; //0 = disable, 1 = enable
            //Telemetry.pwb.alarm_v2 = (packet[15] >> 1) & 0x01; //0 = disable, 1 = enable
            //Telemetry.pwb.alarm_c1 = (packet[15] >> 2) & 0x01; //0 = disable, 1 = enable
            //Telemetry.pwb.alarm_c2 = (packet[15] >> 3) & 0x01; //0 = disable, 1 = enable
            break;
        case 0x11: //AirSpeed sensor
            //Telemetry.airspeed = ((s32)packet[2] << 8) | packet[3]; //In km/h (16Bit value, 1 unit is 1 km/h)
            break;
        case 0x12: //Altimeter sensor
            //Telemetry.altitude = (s16)(packet[2] << 8) | packet[3]; //In 0.1 meters (16Bit signed integer, 1 unit is 0.1m)
            //Telemetry.maxaltitude = (s16)(packet[4] << 8) | packet[5]; //In 0.1 meters (16Bit signed integer, 1 unit is 0.1m)
            break;
        case 0x14: //G-Force sensor
            //Telemetry.gforce.x = (s16)(packet[2] << 8) | packet[3]; //In 0.01g (16Bit signed integers, unit is 0.01g)
            //Telemetry.gforce.y = (s16)(packet[4] << 8) | packet[5];
            //Telemetry.gforce.z = (s16)(packet[6] << 8) | packet[7];
            //Telemetry.gforce.xmax = (s16)(packet[8] << 8) | packet[9];
            //Telemetry.gforce.ymax = (s16)(packet[10] << 8) | packet[11];
            //Telemetry.gforce.zmax = (s16)(packet[12] << 8) | packet[13];
            //Telemetry.gforce.zmin = (s16)(packet[14] << 8) | packet[15];
            break;
        case 0x15: //JetCat sensor
            //Telemetry.jc.status = packet[2];
                //Possible messages for status:
                //0x00:OFF
                //0x01:WAIT FOR RPM
                //0x02:IGNITE
                //0x03;ACCELERATE
                //0x04:STABILIZE
                //0x05:LEARN HIGH
                //0x06:LEARN LOW
                //0x07:undef
                //0x08:SLOW DOWN
                //0x09:MANUAL
                //0x0a,0x10:AUTO OFF
                //0x0b,0x11:RUN
                //0x0c,0x12:ACCELERATION DELAY
                //0x0d,0x13:SPEED REG
                //0x0e,0x14:TWO SHAFT REGULATE
                //0x0f,0x15:PRE HEAT
                //0x16:PRE HEAT 2
                //0x17:MAIN F START
                //0x18:not used
                //0x19:KERO FULL ON
                //0x1a:MAX STATE
            //Telemetry.jc.throttle = (packet[3] >> 4) * 10 + (packet[3] & 0x0f); //up to 159% (the upper nibble is 0-f, the lower nibble 0-9)
            //Telemetry.jc.pack_volt = (((packet[5] >> 4) * 10 + (packet[5] & 0x0f)) * 100 
            //                         + (packet[4] >> 4) * 10 + (packet[4] & 0x0f) + 5) / 10; //In 1/10 of Volts
            //Telemetry.jc.pump_volt = (((packet[7] >> 6) * 10 + (packet[7] & 0x0f)) * 100 
            //                         + (packet[6] >> 4) * 10 + (packet[6] & 0x0f) + 5) / 10; //In 1/10 of Volts (low voltage)
            //Telemetry.jc.rpm = ((packet[10] >> 4) * 10 + (packet[10] & 0x0f)) * 10000 
            //                 + ((packet[9] >> 4) * 10 + (packet[9] & 0x0f)) * 100 
            //                 + ((packet[8] >> 4) * 10 + (packet[8] & 0x0f)); //RPM up to 999999
            //Telemetry.jc.tempEGT = (packet[13] & 0x0f) * 100 + (packet[12] >> 4) * 10 + (packet[12] & 0x0f); //EGT temp up to 999В°C
            //Telemetry.jc.off_condition = packet[14];
                //Messages for Off_Condition:
                //0x00:NA
                //0x01:OFF BY RC
                //0x02:OVER TEMPERATURE
                //0x03:IGNITION TIMEOUT
                //0x04:ACCELERATION TIMEOUT
                //0x05:ACCELERATION TOO SLOW
                //0x06:OVER RPM
                //0x07:LOW RPM OFF
                //0x08:LOW BATTERY
                //0x09:AUTO OFF
                //0x0a,0x10:LOW TEMP OFF
                //0x0b,0x11:HIGH TEMP OFF
                //0x0c,0x12:GLOW PLUG DEFECTIVE
                //0x0d,0x13:WATCH DOG TIMER
                //0x0e,0x14:FAIL SAFE OFF
                //0x0f,0x15:MANUAL OFF
                //0x16:POWER BATT FAIL
                //0x17:TEMP SENSOR FAIL
                //0x18:FUEL FAIL
                //0x19:PROP FAIL
                //0x1a:2nd ENGINE FAIL
                //0x1b:2nd ENGINE DIFFERENTIAL TOO HIGH
                //0x1c:2nd ENGINE NO COMMUNICATION
                //0x1d:MAX OFF CONDITION
            break;
        case 0x16: //GPS sensor (always second GPS packet)
            update = update16;
            altitude += (((packet[3] >> 4) * 10 + (packet[3] & 0x0f)) * 100 
                       + ((packet[2] >> 4) * 10 + (packet[2] & 0x0f))) * 100; //In meters * 1000 (16Bit decimal, 1 unit is 0.1m)
            Telemetry.gps.altitude = altitude;
            Telemetry.gps.latitude = ((packet[7] >> 4) * 10 + (packet[7] & 0x0f)) * 3600000 
                                   + ((packet[6] >> 4) * 10 + (packet[6] & 0x0f)) * 60000 
                                   + ((packet[5] >> 4) * 10 + (packet[5] & 0x0f)) * 600 
                                   + ((packet[4] >> 4) * 10 + (packet[4] & 0x0f)) * 6; // (decimal, format DD MM.SSSS)
            if ((packet[15] & 0x01)  == 0)
                Telemetry.gps.latitude *= -1; //1=N(+), 0=S(-)
            Telemetry.gps.longitude = ((packet[11] >> 4) * 10 + (packet[11] & 0x0f)) * 3600000 
                                    + ((packet[10] >> 4) * 10 + (packet[10] & 0x0f)) * 60000 
                                    + ((packet[9] >> 4) * 10 + (packet[9] & 0x0f)) * 600 
                                    + ((packet[8] >> 4) * 10 + (packet[8] & 0x0f)) * 6; // (decimal, format DD MM.SSSS)
            if ((packet[15] & 0x04) == 4)
                Telemetry.gps.longitude += 360000000; //1=+100 degrees
            if ((packet[15] & 0x02) == 0)
                Telemetry.gps.longitude *= -1; //1=E(+), 0=W(-)
            //Telemetry.gps.heading = ((packet[13] >> 4) * 10 + (packet[13] & 0x0f)) * 10 
            //                      + ((packet[12] >> 4) * 10 + (packet[12] & 0x0f)) / 10; //In degrees (16Bit decimal, 1 unit is 0.1 degree)
            break;
        case 0x17: //GPS sensor (always first GPS packet)
            update = update17;
            Telemetry.gps.velocity = (((packet[3] >> 4) * 10 + (packet[3] & 0x0f)) * 100 
                                    + ((packet[2] >> 4) * 10 + (packet[2] & 0x0f))) * 5556 / 108; //In m/s * 1000
            u8 hour  = (packet[7] >> 4) * 10 + (packet[7] & 0x0f);
            u8 min   = (packet[6] >> 4) * 10 + (packet[6] & 0x0f);
            u8 sec   = (packet[5] >> 4) * 10 + (packet[5] & 0x0f);
            //u8 ssec   = (packet[4] >> 4) * 10 + (packet[4] & 0x0f);
            u8 day   = 0;
            u8 month = 0;
            u8 year  = 0; // + 2000
            Telemetry.gps.time = ((year & 0x3F) << 26)
                               | ((month & 0x0F) << 22)
                               | ((day & 0x1F) << 17)
                               | ((hour & 0x1F) << 12)
                               | ((min & 0x3F) << 6)
                               | ((sec & 0x3F) << 0);
            //Telemetry.gps.sats = ((packet[8] >> 4) * 10 + (packet[8] & 0x0f));
            altitude = ((packet[9] >> 4) * 10 + (packet[9] & 0x0f)) * 1000000; //In 1000 meters * 1000 (8Bit decimal, 1 unit is 1000m)
            break;
    }
    if (update) {
        while(*update) {
            TELEMETRY_SetUpdated(*update++);
        }
    }
}

        case 0x7f: //TM1000 Flight log
        case 0xff: //TM1100 Flight log
            update = update7f;
            Telemetry.p.dsm.flog.fades[0] = pkt16_to_u8(packet+2); //FadesA 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.fades[1] = pkt16_to_u8(packet+4); //FadesB 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.fades[2] = pkt16_to_u8(packet+6); //FadesL 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.fades[3] = pkt16_to_u8(packet+8); //FadesR 0xFFFF = (not connected)
            Telemetry.p.dsm.flog.frameloss = pkt16_to_u8(packet+10);
            Telemetry.p.dsm.flog.holds = pkt16_to_u8(packet+12);
            Telemetry.p.dsm.flog.volt[1] = pkt16_to_volt(packet+14);
            break;