/* * Authors (alphabetical order) * - Bertrand Songis * - Bryan J. Rentoul (Gruvin) * - Cameron Weeks * - Erez Raviv * - Jean-Pierre Parisy * - Karl Szmutny * - Michael Blandford * - Michal Hlavinka * - Pat Mackenzie * - Philip Moss * - Rob Thomson * - Romolo Manfredini * - Thomas Husterer * * open9x is based on code named * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, * er9x by Erez Raviv: http://code.google.com/p/er9x/, * and the original (and ongoing) project by * Thomas Husterer, th9x: http://code.google.com/p/th9x/ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * */ #include "open9x.h" #include "menus.h" // Enumerate FrSky packet codes #define LINKPKT 0xfe #define USRPKT 0xfd #define A11PKT 0xfc #define A12PKT 0xfb #define A21PKT 0xfa #define A22PKT 0xf9 #define ALRM_REQUEST 0xf8 #define RSSI1PKT 0xf7 #define RSSI2PKT 0xf6 #define RSSI_REQUEST 0xf1 #define START_STOP 0x7e #define BYTESTUFF 0x7d #define STUFF_MASK 0x20 uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE]; // Ditto for transmit buffer uint8_t frskyTxBufferCount = 0; uint8_t FrskyRxBufferReady = 0; int8_t frskyStreaming = -1; uint8_t frskyUsrStreaming = 0; FrskyData frskyTelemetry[2]; FrskyRSSI frskyRSSI[2]; struct FrskyAlarm { uint8_t level; // The alarm's 'urgency' level. 0=disabled, 1=yellow, 2=orange, 3=red uint8_t greater; // 1 = 'if greater than'. 0 = 'if less than' uint8_t value; // The threshold above or below which the alarm will sound }; struct FrskyAlarm frskyAlarms[4]; #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) FrskyHubData frskyHubData; enum BarThresholdIdx { THLD_ALT, THLD_RPM, THLD_FUEL, THLD_T1, THLD_T2, THLD_SPEED, THLD_CELL, THLD_DIST, THLD_MAX, }; uint8_t barsThresholds[THLD_MAX]; #endif void frskyPushValue(uint8_t *&ptr, uint8_t value) { // byte stuff the only byte than might need it if (value == START_STOP) { *ptr++ = 0x5e; *ptr++ = BYTESTUFF; } else if (value == BYTESTUFF) { *ptr++ = 0x5d; *ptr++ = BYTESTUFF; } else { *ptr++ = value; } } #ifdef DISPLAY_USER_DATA /* Copies all available bytes (up to max bufsize) from frskyUserData circular buffer into supplied *buffer. Returns number of bytes copied (or zero) */ uint8_t frskyGetUserData(char *buffer, uint8_t bufSize) { uint8_t i = 0; while (!frskyUserData.isEmpty()) { buffer[i] = frskyUserData.get(); i++; } return i; } #endif #ifdef FRSKY_HUB inline void getGpsPilotPosition() { frskyHubData.pilotLatitude = (((uint32_t)frskyHubData.gpsLatitude_bp / 100) * 1000000) + (((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3; frskyHubData.pilotLongitude = (((uint32_t)frskyHubData.gpsLongitude_bp / 100) * 1000000) + (((frskyHubData.gpsLongitude_bp % 100) * 10000 + frskyHubData.gpsLongitude_ap) * 5) / 3; uint32_t lat = ((uint32_t)frskyHubData.gpsLatitude_bp / 100) * 100+ (((uint32_t)frskyHubData.gpsLatitude_bp % 100) * 5) / 3; uint32_t angle2 = (lat*lat) / 10000; uint32_t angle4 = angle2 * angle2; frskyHubData.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); // printf("frskyHubData.distFromEarthAxis=%d\n", frskyHubData.distFromEarthAxis); fflush(stdout); } inline void getGpsDistance() { uint32_t lat = (((uint32_t)frskyHubData.gpsLatitude_bp / 100) * 1000000) + (((frskyHubData.gpsLatitude_bp % 100) * 10000 + frskyHubData.gpsLatitude_ap) * 5) / 3; uint32_t lng = (((uint32_t)frskyHubData.gpsLongitude_bp / 100) * 1000000) + (((frskyHubData.gpsLongitude_bp % 100) * 10000 + frskyHubData.gpsLongitude_ap) * 5) / 3; // printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyHubData.pilotLatitude), lng, abs(lng - frskyHubData.pilotLongitude)); uint32_t angle = (lat > frskyHubData.pilotLatitude) ? lat - frskyHubData.pilotLatitude : frskyHubData.pilotLatitude - lat; uint32_t dist = EARTH_RADIUS * angle / 1000000; uint32_t result = dist*dist; angle = (lng > frskyHubData.pilotLongitude) ? lng - frskyHubData.pilotLongitude : frskyHubData.pilotLongitude - lng; dist = frskyHubData.distFromEarthAxis * angle / 1000000; result += dist*dist; dist = abs(frskyHubData.baroAltitudeOffset ? frskyHubData.baroAltitude_bp : frskyHubData.gpsAltitude_bp); result += dist*dist; frskyHubData.gpsDistance = isqrt32(result); if (frskyHubData.gpsDistance > frskyHubData.maxGpsDistance) frskyHubData.maxGpsDistance = frskyHubData.gpsDistance; } typedef enum { TS_IDLE = 0, // waiting for 0x5e frame marker TS_DATA_ID, // waiting for dataID TS_DATA_LOW, // waiting for data low byte TS_DATA_HIGH, // waiting for data high byte TS_XOR = 0x80 // decode stuffed byte } TS_STATE; void parseTelemHubByte(uint8_t byte) { static int8_t structPos; static uint8_t lowByte; static TS_STATE state = TS_IDLE; if (byte == 0x5e) { state = TS_DATA_ID; return; } if (state == TS_IDLE) { return; } if (state & TS_XOR) { byte = byte ^ 0x60; state = (TS_STATE)(state - TS_XOR); } if (byte == 0x5d) { state = (TS_STATE)(state | TS_XOR); return; } if (state == TS_DATA_ID) { if (byte > 0x3b) { state = TS_IDLE; } else { structPos = byte*2; state = TS_DATA_LOW; } return; } if (state == TS_DATA_LOW) { lowByte = byte; state = TS_DATA_HIGH; return; } state = TS_IDLE; if ((uint8_t)structPos == offsetof(FrskyHubData, gpsLatitude_bp)) { if (lowByte || byte) frskyHubData.gpsFix = 1; else if (frskyHubData.gpsFix > 0 && frskyHubData.gpsLatitude_bp > 1) frskyHubData.gpsFix = 0; } else if ((uint8_t)structPos == offsetof(FrskyHubData, gpsLongitude_bp)) { if (lowByte || byte) frskyHubData.gpsFix = 1; else if (frskyHubData.gpsFix > 0 && frskyHubData.gpsLongitude_bp > 1) frskyHubData.gpsFix = 0; } if ((uint8_t)structPos == offsetof(FrskyHubData, gpsAltitude_bp) || ((uint8_t)structPos >= offsetof(FrskyHubData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskyHubData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskyHubData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskyHubData, baroAltitude_ap))) { // If we don't have a fix, we may discard the value if (frskyHubData.gpsFix <= 0) return; } ((uint8_t*)&frskyHubData)[structPos] = lowByte; ((uint8_t*)&frskyHubData)[structPos+1] = byte; switch ((uint8_t)structPos) { case offsetof(FrskyHubData, rpm): frskyHubData.rpm /= (g_model.frsky.blades+2); if (frskyHubData.rpm > frskyHubData.maxRpm) frskyHubData.maxRpm = frskyHubData.rpm; break; case offsetof(FrskyHubData, temperature1): if (frskyHubData.temperature1 > frskyHubData.maxTemperature1) frskyHubData.maxTemperature1 = frskyHubData.temperature1; break; case offsetof(FrskyHubData, temperature2): if (frskyHubData.temperature2 > frskyHubData.maxTemperature2) frskyHubData.maxTemperature2 = frskyHubData.temperature2; break; case offsetof(FrskyHubData, baroAltitude_bp): // First received barometer altitude => Altitude offset if (!frskyHubData.baroAltitudeOffset) frskyHubData.baroAltitudeOffset = -frskyHubData.baroAltitude_bp; frskyHubData.baroAltitude_bp += frskyHubData.baroAltitudeOffset; if (frskyHubData.baroAltitude_bp > frskyHubData.maxAltitude) frskyHubData.maxAltitude = frskyHubData.baroAltitude_bp; if (frskyHubData.baroAltitude_bp < frskyHubData.minAltitude) frskyHubData.minAltitude = frskyHubData.baroAltitude_bp; { int16_t actVario = frskyHubData.baroAltitude_bp - frskyHubData.lastBaroAltitude_bp; frskyHubData.varioAcc2 = frskyHubData.varioAcc2 - frskyHubData.varioQueue[frskyHubData.queuePointer]; frskyHubData.varioQueue[frskyHubData.queuePointer] = actVario; uint8_t tmp = frskyHubData.queuePointer + 5; if (tmp >= 10) tmp -= 10; tmp = (uint8_t)frskyHubData.varioQueue[tmp]; frskyHubData.varioAcc2 = frskyHubData.varioAcc2 + (int8_t)tmp; frskyHubData.varioAcc1 = frskyHubData.varioAcc1 + actVario - (int8_t)tmp; frskyHubData.varioSpeed = frskyHubData.varioAcc1 - frskyHubData.varioAcc2; if (++frskyHubData.queuePointer >= 10) frskyHubData.queuePointer = 0; frskyHubData.lastBaroAltitude_bp = frskyHubData.baroAltitude_bp; } break; case offsetof(FrskyHubData, gpsAltitude_ap): if (!frskyHubData.gpsAltitudeOffset) frskyHubData.gpsAltitudeOffset = -frskyHubData.gpsAltitude_bp; frskyHubData.gpsAltitude_bp += frskyHubData.gpsAltitudeOffset; if (frskyHubData.gpsAltitude_bp > frskyHubData.maxAltitude) frskyHubData.maxAltitude = frskyHubData.gpsAltitude_bp; if (frskyHubData.gpsAltitude_bp < frskyHubData.minAltitude) frskyHubData.minAltitude = frskyHubData.gpsAltitude_bp; if (!frskyHubData.pilotLatitude && !frskyHubData.pilotLongitude) { // First received GPS position => Pilot GPS position getGpsPilotPosition(); } else if (frskyHubData.gpsDistNeeded || g_menuStack[0] == menuProcFrsky) { getGpsDistance(); } break; case offsetof(FrskyHubData, gpsSpeed_bp): // Speed => Max speed if (frskyHubData.gpsSpeed_bp < frskyHubData.maxGpsSpeed) frskyHubData.maxGpsSpeed = frskyHubData.gpsSpeed_bp; break; case offsetof(FrskyHubData, volts): // Voltage => Cell number + Cell voltage { uint8_t battnumber = ((frskyHubData.volts & 0x00F0) >> 4); if (battnumber < 12) { if (frskyHubData.cellsCount < battnumber+1) { frskyHubData.cellsCount = battnumber+1; } uint8_t cellVolts = (uint8_t)(((((frskyHubData.volts & 0xFF00) >> 8) + ((frskyHubData.volts & 0x000F) << 8)))/10); frskyHubData.cellVolts[battnumber] = cellVolts; if (!frskyHubData.minCellVolts || cellVolts < frskyHubData.minCellVolts) frskyHubData.minCellVolts = cellVolts; } break; } case offsetof(FrskyHubData, hour): frskyHubData.hour = ((uint8_t)(frskyHubData.hour + g_eeGeneral.timezone + 24)) % 24; break; case offsetof(FrskyHubData, accelX): case offsetof(FrskyHubData, accelY): case offsetof(FrskyHubData, accelZ): *(int16_t*)(&((uint8_t*)&frskyHubData)[structPos]) /= 10; break; } } #endif #ifdef WS_HOW_HIGH void parseTelemWSHowHighByte(uint8_t byte) { if (frskyUsrStreaming < (FRSKY_TIMEOUT10ms*3 - 10)) // At least 100mS passed since last data received ((uint8_t*)&frskyHubData)[offsetof(FrskyHubData, baroAltitude_bp)] = byte; else ((uint8_t*)&frskyHubData)[offsetof(FrskyHubData, baroAltitude_bp)+1] = byte; frskyUsrStreaming = FRSKY_TIMEOUT10ms*3; // reset counter } #endif /* Called from somewhere in the main loop or a low priority interrupt routine perhaps. This function processes FrSky telemetry data packets assembled by he USART0_RX_vect) ISR function (below) and stores extracted data in global variables for use by other parts of the program. Packets can be any of the following: - A1/A2/RSSI telemetry data - Alarm level/mode/threshold settings for Ch1A, Ch1B, Ch2A, Ch2B - User Data packets */ void processFrskyPacket(uint8_t *packet) { // What type of packet? switch (packet[0]) { case A22PKT: case A21PKT: case A12PKT: case A11PKT: { struct FrskyAlarm *alarmptr ; alarmptr = &frskyAlarms[(packet[0]-A22PKT)] ; alarmptr->value = packet[1]; alarmptr->greater = packet[2] & 0x01; alarmptr->level = packet[3] & 0x03; } break; case LINKPKT: // A1/A2/RSSI values frskyTelemetry[0].set(packet[1]); frskyTelemetry[1].set(packet[2]); frskyRSSI[0].set(packet[3]); frskyRSSI[1].set(packet[4] / 2); frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected break; #if defined(FRSKY_HUB) || defined (WS_HOW_HIGH) case USRPKT: // User Data packet uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow for (uint8_t i=3; i g_model.frsky.channels[idx].alarms_value[i]) return true; } else { if (frskyTelemetry[idx].value < g_model.frsky.channels[idx].alarms_value[i]) return true; } } } return false; } inline void FRSKY_EnableTXD(void) { frskyTxBufferCount = 0; UCSR0B |= (1 << TXEN0); // enable TX } inline void FRSKY_EnableRXD(void) { UCSR0B |= (1 << RXEN0); // enable RX UCSR0B |= (1 << RXCIE0); // enable Interrupt } void FRSKY_Init(void) { // clear frsky variables memset(frskyAlarms, 0, sizeof(frskyAlarms)); resetTelemetry(); DDRE &= ~(1 << DDE0); // set RXD0 pin as input PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin #undef BAUD #define BAUD 9600 #ifndef SIMU #include UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE; UCSR0A &= ~(1 << U2X0); // disable double speed operation. // set 8N1 UCSR0B = 0 | (0 << RXCIE0) | (0 << TXCIE0) | (0 << UDRIE0) | (0 << RXEN0) | (0 << TXEN0) | (0 << UCSZ02); UCSR0C = 0 | (1 << UCSZ01) | (1 << UCSZ00); while (UCSR0A & (1 << RXC0)) UDR0; // flush receive buffer #endif // These should be running right from power up on a FrSky enabled '9X. FRSKY_EnableTXD(); // enable FrSky-Telemetry reception FRSKY_EnableRXD(); // enable FrSky-Telemetry reception } #if 0 // Send packet requesting all alarm settings be sent back to us void frskyAlarmsRefresh() { if (frskyTxBufferCount) return; // we only have one buffer. If it's in use, then we can't send. Sorry. { uint8_t *ptr ; ptr = &frskyTxBuffer[0] ; *ptr++ = START_STOP; // Start of packet *ptr++ = ALRM_REQUEST; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = 0x00 ; *ptr++ = START_STOP; // End of packet } frskyTxBufferCount = 11; frskyTransmitBuffer(); } #endif void FrskyRSSI::set(uint8_t value) { if (this->value == 0) this->value = value; else this->value = (((uint16_t)this->value * 7) + value + 4) / 8; if (!min || value < min) min = value; } void FrskyData::set(uint8_t value) { FrskyRSSI::set(value); if (!max || value > max) max = value; } void resetTelemetry() { memset(frskyTelemetry, 0, sizeof(frskyTelemetry)); memset(frskyRSSI, 0, sizeof(frskyRSSI)); memset(&frskyHubData, 0, sizeof(frskyHubData)); #if defined(FRSKY_HUB) frskyHubData.gpsLatitude_bp = 2; frskyHubData.gpsLongitude_bp = 2; frskyHubData.gpsFix = -1; #endif #ifdef SIMU frskyTelemetry[0].set(120); frskyRSSI[0].set(75); frskyHubData.fuelLevel = 75; frskyHubData.rpm = 12000; frskyHubData.gpsFix = 1; frskyHubData.gpsLatitude_bp = 4401; frskyHubData.gpsLatitude_ap = 7710; frskyHubData.gpsLongitude_bp = 1006; frskyHubData.gpsLongitude_ap = 8872; getGpsPilotPosition(); frskyHubData.gpsLatitude_bp = 4401; frskyHubData.gpsLatitude_ap = 7455; frskyHubData.gpsLongitude_bp = 1006; frskyHubData.gpsLongitude_ap = 9533; getGpsDistance(); frskyHubData.cellsCount = 6; frskyHubData.gpsAltitude_bp = 50; frskyHubData.baroAltitude_bp = 50; frskyHubData.minAltitude = 10; frskyHubData.maxAltitude = 500; frskyHubData.accelY = 100; frskyHubData.temperature1 = -30; frskyHubData.maxTemperature1 = 100; #endif } uint8_t maxTelemValue(uint8_t channel) { switch (channel) { case TELEM_FUEL: case TELEM_RSSI_TX: case TELEM_RSSI_RX: return 100; default: return 255; } } int16_t convertTelemValue(uint8_t channel, uint8_t value) { int16_t result; switch (channel) { case TELEM_TM1: case TELEM_TM2: result = value * 3; break; case TELEM_ALT: result = value * 4; break; case TELEM_RPM: result = value * 50; break; case TELEM_T1: case TELEM_T2: result = (int16_t)value - 30; break; case TELEM_SPEED: case TELEM_CELL: result = value * 2; break; case TELEM_DIST: result = value * 8; break; default: result = value; break; } return result; } void putsTelemetryValue(uint8_t x, uint8_t y, int16_t val, uint8_t unit, uint8_t att) { lcd_outdezAtt(x, (att & DBLSIZE ? y - FH : y), val, att & (~NO_UNIT)); // TODO we could add this test inside lcd_outdezAtt! if (~att & NO_UNIT && unit != UNIT_RAW) lcd_putsiAtt(lcd_lastPos+1, y, STR_VTELEMUNIT, unit, 0); } static const pm_uint8_t bchunit_ar[] PROGMEM = { UNIT_METERS, // Alt UNIT_RAW, // Rpm UNIT_PERCENT, // Fuel UNIT_DEGREES, // T1 UNIT_DEGREES, // T2 UNIT_KMH, // Speed UNIT_METERS, // Dist }; void putsTelemetryChannel(uint8_t x, uint8_t y, uint8_t channel, int16_t val, uint8_t att) { switch (channel) { case TELEM_TM1-1: case TELEM_TM2-1: putsTime(x-3*FW, y, val, att, att); break; case TELEM_MIN_A1-1: case TELEM_MIN_A2-1: channel -= TELEM_MIN_A1-1-MAX_TIMERS; // no break case TELEM_A1-1: case TELEM_A2-1: channel -= MAX_TIMERS; // A1 and A2 { // TODO optimize this, avoid int32_t int16_t converted_value = ((int32_t)val+g_model.frsky.channels[channel].offset) * (g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier) * 2 / 51; if (g_model.frsky.channels[channel].type >= UNIT_RAW) { converted_value /= 10; } else { if (converted_value < 1000) { att |= PREC2; } else { converted_value /= 10; att |= PREC1; } } putsTelemetryValue(x, y, converted_value, g_model.frsky.channels[channel].type, att); break; } case TELEM_CELL-1: putsTelemetryValue(x, y, val, UNIT_VOLTS, att|PREC2); break; case TELEM_ACCx-1: case TELEM_ACCy-1: case TELEM_ACCz-1: putsTelemetryValue(x, y, val, UNIT_RAW, att|PREC2); break; case TELEM_RSSI_TX-1: case TELEM_RSSI_RX-1: putsTelemetryValue(x, y, val, UNIT_RAW, att); break; default: putsTelemetryValue(x, y, val, pgm_read_byte(bchunit_ar+channel-6), att); break; } } enum FrskyViews { e_frsky_custom, e_frsky_bars, e_frsky_a1a2, e_frsky_after_flight, FRSKY_VIEW_MAX = e_frsky_after_flight }; static uint8_t s_frsky_view = e_frsky_custom; void displayRssiLine() { if (frskyStreaming > 0) { lcd_hline(0, 55, 128, 0); // separator lcd_putsLeft(7*FH+1, STR_TX); lcd_outdezNAtt(4*FW, 7*FH+1, frskyRSSI[1].value, LEADING0, 2); lcd_rect(25, 57, 38, 7); lcd_filled_rect(26, 58, 9*frskyRSSI[1].value/25, 5); lcd_puts(105, 7*FH+1, STR_RX); lcd_outdezNAtt(105+4*FW-1, 7*FH+1, frskyRSSI[0].value, LEADING0, 2); lcd_rect(65, 57, 38, 7); uint8_t v = 9*frskyRSSI[0].value/25; lcd_filled_rect(66+36-v, 58, v, 5); } else { lcd_putsAtt(7*FW, 7*FH+1, STR_NODATA, BLINK); lcd_status_line(); } } #if defined(FRSKY_HUB) void displayGpsTime() { #define TIME_LINE (7*FH+1) uint8_t att = (frskyStreaming > 0 ? LEFT|LEADING0 : LEFT|LEADING0|BLINK); lcd_outdezNAtt(6*FW+5, TIME_LINE, frskyHubData.hour, att, 2); lcd_putcAtt(8*FW+2, TIME_LINE, ':', att); lcd_outdezNAtt(9*FW+2, TIME_LINE, frskyHubData.min, att, 2); lcd_putcAtt(11*FW-1, TIME_LINE, ':', att); lcd_outdezNAtt(12*FW-1, TIME_LINE, frskyHubData.sec, att, 2); lcd_status_line(); } void displayGpsCoord(uint8_t y, char direction, int16_t bp, int16_t ap) { if (!direction) direction = '-'; lcd_outdezAtt(10*FW, y, bp / 100, LEFT); // ddd before '.' lcd_putc(lcd_lastPos, y, '@'); uint8_t mn = bp % 100; if (g_eeGeneral.gpsFormat == 0) { lcd_putc(lcd_lastPos+FWNUM, y, direction); lcd_outdezNAtt(lcd_lastPos+FW+FW+1, y, mn, LEFT|LEADING0, 2); // mm before '.' lcd_vline(lcd_lastPos, y, 2); uint16_t ss = ap * 6; lcd_outdezAtt(lcd_lastPos+3, y, ss / 1000, LEFT); // '' lcd_plot(lcd_lastPos, y+FH-2, 0); // small decimal point lcd_outdezAtt(lcd_lastPos+2, y, ss % 1000, LEFT); // '' lcd_vline(lcd_lastPos, y, 2); lcd_vline(lcd_lastPos+2, y, 2); } else { lcd_outdezNAtt(lcd_lastPos+FW, y, mn, LEFT|LEADING0, 2); // mm before '.' lcd_plot(lcd_lastPos, y+FH-2, 0); // small decimal point lcd_outdezNAtt(lcd_lastPos+2, y, ap, LEFT|UNSIGN|LEADING0, 4); // after '.' lcd_putc(lcd_lastPos+1, y, direction); } } #endif uint8_t getTelemCustomField(uint8_t line, uint8_t col) { uint8_t result = (col==0 ? (g_model.frskyLines[line] & 0x0f) : ((g_model.frskyLines[line] & 0xf0) / 16)); result += (((g_model.frskyLinesXtra >> (4*line+2*col)) & 0x03) * 16); return result; } void menuProcFrsky(uint8_t event) { switch (event) { case EVT_KEY_BREAK(KEY_UP): if (s_frsky_view-- == 0) s_frsky_view = FRSKY_VIEW_MAX; break; case EVT_KEY_BREAK(KEY_DOWN): if (s_frsky_view++ == FRSKY_VIEW_MAX) s_frsky_view = 0; break; case EVT_KEY_FIRST(KEY_EXIT): chainMenu(menuMainView); break; case EVT_KEY_FIRST(KEY_MENU): resetTelemetry(); break; } // The top black bar putsModelName(0, 0, g_model.name, g_eeGeneral.currModel, 0); uint8_t att = (g_vbat100mV < g_eeGeneral.vBatWarn ? BLINK : 0); putsVBat(14*FW,0,att); if (g_model.timers[0].mode) { att = (s_timerState[0]==TMR_BEEPING ? BLINK : 0); putsTime(17*FW, 0, s_timerVal[0], att, att); } lcd_filled_rect(0, 0, DISPLAY_W, 8); if (frskyStreaming >= 0) { if (s_frsky_view == e_frsky_custom) { // The custom view for (uint8_t i=0; i<4; i++) { for (uint8_t j=0; j<2; j++) { uint8_t field = getTelemCustomField(i, j); if (i==3 && j==0) { lcd_vline(63, 8, 48); if (frskyStreaming > 0) { if (field == TELEM_ACC) { lcd_putsLeft(7*FH+1, STR_ACCEL); lcd_outdezNAtt(4*FW, 7*FH+1, frskyHubData.accelX, LEFT|PREC2); lcd_outdezNAtt(10*FW, 7*FH+1, frskyHubData.accelY, LEFT|PREC2); lcd_outdezNAtt(16*FW, 7*FH+1, frskyHubData.accelZ, LEFT|PREC2); break; } else if (field == TELEM_GPS_TIME) { displayGpsTime(); return; } } else { displayRssiLine(); return; } } if (field) { int16_t value = getValue(CSW_CHOUT_BASE+NUM_CHNOUT+field-1); uint8_t att = (i==3 ? NO_UNIT : DBLSIZE|NO_UNIT); if (field <= TELEM_TM2) { uint8_t x = (i==3 ? j?80:20 : j?74:10); putsTime(x, 1+FH+2*FH*i, value, att, att); } else { putsTelemetryChannel(j ? 128 : 63, i==3 ? 1+7*FH : 1+2*FH+2*FH*i, field-1, value, att); lcd_putsiAtt(j*65, 1+FH+2*FH*i, STR_VTELEMCHNS, field, 0); } } } } lcd_status_line(); } else if (s_frsky_view == e_frsky_bars) { // The bars uint8_t bars_height = 5; for (int8_t i=3; i>=0; i--) { uint8_t source = g_model.frsky.bars[i].source; uint8_t bmin = g_model.frsky.bars[i].barMin * 5; uint8_t bmax = (51 - g_model.frsky.bars[i].barMax) * 5; if (source && bmax > bmin) { lcd_putsiAtt(0, bars_height+bars_height+1+i*(bars_height+6), STR_VTELEMCHNS, source, 0); lcd_rect(25, bars_height+6+i*(bars_height+6), 101, bars_height+2); int16_t value = getValue(CSW_CHOUT_BASE+NUM_CHNOUT+source-1); int16_t threshold = 0; uint8_t thresholdX = 0; if (source <= TELEM_TM1) threshold = 0; else if (source <= TELEM_A2) threshold = g_model.frsky.channels[source-TELEM_A1].alarms_value[0]; else if (source <= TELEM_RSSI_RX) threshold = 0; else threshold = convertTelemValue(source, barsThresholds[source-TELEM_ALT]); int16_t barMin = convertTelemValue(source, bmin); int16_t barMax = convertTelemValue(source, bmax); if (threshold) { thresholdX = (uint8_t)(((int32_t)(threshold - barMin) * (int32_t)100) / (barMax - barMin)); if (thresholdX > 100) thresholdX = 0; } uint8_t width = (uint8_t)limit((int16_t)0, (int16_t)(((int32_t)100 * (value - barMin)) / (barMax - barMin)), (int16_t)100); lcd_filled_rect(26, bars_height+6+1+i*(bars_height+6), width, bars_height, (threshold > value) ? DOTTED : SOLID); for (uint8_t j=50; j<125; j+=25) if (j>26+thresholdX || j>26+width) lcd_vline(j, bars_height+6+1+i*(bars_height+6), bars_height); if (thresholdX) { lcd_vlineStip(26+thresholdX, bars_height+4+i*(bars_height+6), bars_height+3, DOTTED); lcd_hline(25+thresholdX, bars_height+4+i*(bars_height+6), 3); } } else { bars_height += 2; } } if (bars_height == 13) { // No bars at all! s_frsky_view = ((event == EVT_KEY_BREAK(KEY_UP)) ? e_frsky_custom : e_frsky_a1a2); } displayRssiLine(); } else if (s_frsky_view == e_frsky_a1a2) { // Big A1 / A2 with min and max uint8_t blink; uint8_t y = 2*FH; for (uint8_t i=0; i<2; i++) { if (g_model.frsky.channels[i].ratio) { blink = (FRSKY_alarmRaised(i) ? INVERS : 0); putsStrIdx(0, y, STR_A, i+1, TWO_DOTS); putsTelemetryChannel(3*FW, y, i+MAX_TIMERS, frskyTelemetry[i].value, blink|DBLSIZE|LEFT); lcd_putc(12*FW-1, y-FH, '<'); putsTelemetryChannel(17*FW, y-FH, i+MAX_TIMERS, frskyTelemetry[i].min, NO_UNIT); lcd_putc(12*FW, y, '>'); putsTelemetryChannel(17*FW, y, i+MAX_TIMERS, frskyTelemetry[i].max, NO_UNIT); y += 3*FH; } } #ifdef FRSKY_HUB // Cells voltage if (frskyHubData.cellsCount > 0) { uint8_t y = 1*FH; for (uint8_t k=0; k