ID | Active | Freq | Launchsite | Mode |
");
strcat(ptr, "ID | Active | Freq | Launchsite | Mode |
");
for (int i = 0; i < sonde.config.maxsonde; i++) {
@@ -273,7 +274,7 @@ const char *createQRGForm() {
i + 1, i >= sonde.nSonde ? 400.000 : sonde.sondeList[i].freq,
i + 1, i >= sonde.nSonde ? " " : sonde.sondeList[i].launchsite,
i + 1, i >= sonde.nSonde ? 2 : sondeTypeChar[sonde.sondeList[i].type] );
- //i + 1, s.c_str());
+ //i + 1, s.c_str());
}
strcat(ptr, "
");
//
\n");
}
@@ -544,7 +545,7 @@ struct st_configitems config_list[] = {
{"tcp.port", "APRS TCP Port", 0, &sonde.config.tcpfeed.port},
{"tcp.idformat", "DFM ID Format", -2, &sonde.config.tcpfeed.idformat},
{"tcp.highrate", "Rate limit", 0, &sonde.config.tcpfeed.highrate},
-
+
/* MQTT */
{"mqtt.active", "MQTT Active (needs reboot)", 0, &sonde.config.mqtt.active},
{"mqtt.id", "MQTT client ID", 63, &sonde.config.mqtt.id},
@@ -564,6 +565,7 @@ struct st_configitems config_list[] = {
{"tft_rs", "TFT RS", 0, &sonde.config.tft_rs},
{"tft_cs", "TFT CS", 0, &sonde.config.tft_cs},
{"tft_orient", "TFT orientation (0/1/2/3), OLED flip: 3", 0, &sonde.config.tft_orient},
+ {"tft_modeflip", "TFT modeflip (usually 0)", 0, &sonde.config.tft_modeflip},
{"button_pin", "Button input port", -4, &sonde.config.button_pin},
{"button2_pin", "Button 2 input port", -4, &sonde.config.button2_pin},
{"button2_axp", "Use AXP192 PWR as Button 2", 0, &sonde.config.button2_axp},
@@ -740,7 +742,9 @@ const char *createControlForm() {
strcat(ptr, "\" value=\"");
strcat(ptr, ctrllabel[i]);
strcat(ptr, "\">");
- if(i==3) { strcat(ptr, "
"); }
+ if (i == 3) {
+ strcat(ptr, "
");
+ }
}
HTMLBODYEND(ptr);
Serial.printf("Control form: size=%d bytes\n", strlen(message));
@@ -790,6 +794,55 @@ const char *handleControlPost(AsyncWebServerRequest *request) {
return "";
}
+int streamEditForm(int &state, File &file, String filename, char *buffer, size_t maxlen, size_t index) {
+ Serial.printf("streamEdit: state=%d max:%d idx:%d\n", state, maxlen, index);
+ int i = 0;
+ switch (state) {
+ case 0: // header
+ {
+ // we optimistically assume that on first invocation, maxlen is large enough to handle the header.....
+ strncpy(buffer, "
EditorEdit: ", maxlen);
+ i = strlen(buffer);
+ strncpy(buffer + i, filename.c_str(), maxlen - i);
+ i += strlen(buffer + i);
+ strncpy(buffer + i, "
", maxlen);
+ state++;
+ return strlen(buffer);
+ case 3: // end
+ return 0;
+ }
+ return 0;
+}
+
// bad idea. prone to buffer overflow. use at your own risk...
const char *createEditForm(String filename) {
Serial.println("Creating edit form");
@@ -902,6 +955,48 @@ const char *handleUpdatePost(AsyncWebServerRequest *request) {
return "";
}
+const char *createKMLLive(const char *myIP) {
+ char *ptr = message;
+
+ strcpy(ptr, "
loads dynamic.kmlhttp://");
+ strcat(ptr, myIP);
+ strcat(ptr, "/dynamic.kmlonInterval10");
+
+ return message;
+}
+
+void addSondeStatusKML(char *ptr, int i)
+{
+ SondeInfo *s = &sonde.sondeList[i];
+
+ if (!s->validID)
+ {
+ return;
+ }
+
+ sprintf(ptr + strlen(ptr), "
%s%.6f,%.6f,%.0f%3.3f MHz, Type: %s, h=%.0fm",
+ s->id, s->id,
+ s->lon, s->lat, s->alt,
+ s->freq, sondeTypeStr[s->type], s->alt);
+}
+
+const char *createKMLDynamic() {
+ char *ptr = message;
+
+ strcpy(ptr, "
");
+
+ for (int i = 0; i < sonde.nSonde; i++) {
+ int snum = (i + sonde.currentSonde) % sonde.nSonde;
+ if (sonde.sondeList[snum].active) {
+ addSondeStatusKML(ptr, snum);
+ }
+ }
+
+ strcat(ptr, "");
+
+ return message;
+}
+
const char *sendGPX(AsyncWebServerRequest * request) {
Serial.println("\n\n\n********GPX request\n\n");
@@ -928,6 +1023,9 @@ const char *sendGPX(AsyncWebServerRequest * request) {
return message;
}
+#if 0
+// no longer supported
+// tcp socket / json for android app is used now
void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventType type, void * arg, uint8_t *data, size_t len) {
if (type == WS_EVT_CONNECT) {
Serial.println("Websocket client connection received");
@@ -936,6 +1034,7 @@ void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventT
Serial.println("Client disconnected");
}
}
+#endif
const char* PARAM_MESSAGE = "message";
@@ -999,14 +1098,27 @@ void SetupAsyncServer() {
});
server.on("/edit.html", HTTP_GET, [](AsyncWebServerRequest * request) {
- request->send(200, "text/html", createEditForm(request->getParam(0)->value()));
+ // new version:
+ // Open file
+ // store file object in request->_tempObject
+ //request->send(200, "text/html", createEditForm(request->getParam(0)->value()));
+ const String filename = request->getParam(0)->value();
+ File file = SPIFFS.open("/" + filename, "r");
+ int state = 0;
+ request->send("text/html", 0, [state, file, filename](uint8_t *buffer, size_t maxLen, size_t index) mutable -> size_t {
+ Serial.printf("******* send callback: %d %d %d\n", state, maxLen, index);
+ return streamEditForm(state, file, filename, (char *)buffer, maxLen, index);
+ });
});
server.on("/edit.html", HTTP_POST, [](AsyncWebServerRequest * request) {
const char *ret = handleEditPost(request);
if (ret == NULL)
request->send(200, "text/html", "ERROR
Something went wrong. Uploaded file is empty.
");
- else
- request->send(200, "text/html", createEditForm(request->getParam(0)->value()));
+ else {
+ String f = request->getParam(0)->value();
+ request->redirect("/edit.html?file=" + f);
+ //request->send(200, "text/html", createEditForm(request->getParam(0)->value()));
+ }
},
NULL,
[](AsyncWebServerRequest * request, uint8_t *data, size_t len, size_t index, size_t total) {
@@ -1016,7 +1128,7 @@ void SetupAsyncServer() {
// Route to load style.css file
server.on("/style.css", HTTP_GET, [](AsyncWebServerRequest * request) {
AsyncWebServerResponse *response = request->beginResponse(SPIFFS, "/style.css", "text/css");
- response->addHeader("Cache-Control","max-age=86400");
+ response->addHeader("Cache-Control", "max-age=86400");
request->send(response);
});
@@ -1025,6 +1137,14 @@ void SetupAsyncServer() {
request->send(SPIFFS, "/index.html", String(), false, processor);
});
+ server.on("/live.kml", HTTP_GET, [](AsyncWebServerRequest * request) {
+ request->send(200, "application/vnd.google-earth.kml+xml", createKMLLive(sonde.ipaddr.c_str()));
+ });
+
+ server.on("/dynamic.kml", HTTP_GET, [](AsyncWebServerRequest * request) {
+ request->send(200, "application/vnd.google-earth.kml+xml", createKMLDynamic());
+ });
+
server.onNotFound([](AsyncWebServerRequest * request) {
if (request->method() == HTTP_OPTIONS) {
request->send(200);
@@ -1040,10 +1160,6 @@ void SetupAsyncServer() {
}
});
- // Set up web socket
- ws.onEvent(onWsEvent);
- server.addHandler(&ws);
-
// Start server
server.begin();
}
@@ -1054,18 +1170,10 @@ int fetchWifiIndex(const char *id) {
Serial.printf("Match for %s at %d\n", id, i);
return i;
}
- Serial.printf("No match: '%s' vs '%s'\n", id, networks[i].id.c_str());
+ //Serial.printf("No match: '%s' vs '%s'\n", id, networks[i].id.c_str());
const char *cfgid = networks[i].id.c_str();
int len = strlen(cfgid);
if (strlen(id) > len) len = strlen(id);
- Serial.print("SSID: ");
- for (int i = 0; i < len; i++) {
- Serial.printf("%02x ", id[i]);
- } Serial.println("");
- Serial.print("Conf: ");
- for (int i = 0; i < len; i++) {
- Serial.printf("%02x ", cfgid[i]);
- } Serial.println("");
}
return -1;
}
@@ -1171,6 +1279,36 @@ void unkHandler(T nmea) {
if (*s == ',') return; /// no new course data
int lastCourse = nmea.parseFloat(s, 0, NULL);
Serial.printf("Course update: %d\n", lastCourse);
+ } else if (strcmp(nmea.getMessageID(), "GST") == 0) {
+ // get horizontal accuracy for android app on devices without gps
+ // GPGST,time,rms,-,-,-,stdlat,stdlon,stdalt,cs
+ const char *s = nmea.getSentence();
+ while (*s && *s != ',') s++; // #0: GST
+ if (*s == ',') s++; else return;
+ while (*s && *s != ',') s++; // #1: time: skip
+ if (*s == ',') s++; else return;
+ while (*s && *s != ',') s++; // #1: rms: skip
+ if (*s == ',') s++; else return;
+ while (*s && *s != ',') s++; // #1: (-): skip
+ if (*s == ',') s++; else return;
+ while (*s && *s != ',') s++; // #1: (-): skip
+ if (*s == ',') s++; else return;
+ while (*s && *s != ',') s++; // #1: (-): skip
+ if (*s == ',') s++; else return;
+ // stdlat
+ int stdlat = nmea.parseFloat(s, 1, NULL);
+ while (*s && *s != ',') s++;
+ if (*s == ',') s++; else return;
+ // stdlong
+ int stdlon = nmea.parseFloat(s, 1, NULL);
+ // calculate position error as 1-signma horizontal RMS
+ // I guess that is equivalent to Androids getAccurac()?
+ int poserr = 0;
+ if (stdlat < 10000 && stdlon < 10000) { // larger errors: no GPS fix, avoid overflow in *
+ poserr = (int)(sqrt(0.5 * (stdlat * stdlat + stdlon * stdlon)));
+ }
+ //Serial.printf("\nHorizontal accuracy: %d, %d => %.1fm\n", stdlat, stdlon, 0.1*poserr);
+ gpsPos.accuracy = poserr;
}
}
@@ -1186,22 +1324,22 @@ void gpsTask(void *parameter) {
//Serial.print(c);
if (nmea.process(c)) {
gpsPos.valid = nmea.isValid();
- if(gpsPos.valid) {
- gpsPos.lon = nmea.getLongitude()*0.000001;
- gpsPos.lat = nmea.getLatitude()*0.000001;
- long alt = 0;
- nmea.getAltitude(alt);
- gpsPos.alt=(int)(alt/1000);
- gpsPos.course = (int)(nmea.getCourse()/1000);
- gpsCourseOld = false;
- if(gpsPos.course==0) {
- // either north or not new
- if(lastCourse!=0) // use old value...
- {
- gpsCourseOld = true;
- gpsPos.course = lastCourse;
- }
- }
+ if (gpsPos.valid) {
+ gpsPos.lon = nmea.getLongitude() * 0.000001;
+ gpsPos.lat = nmea.getLatitude() * 0.000001;
+ long alt = 0;
+ nmea.getAltitude(alt);
+ gpsPos.alt = (int)(alt / 1000);
+ gpsPos.course = (int)(nmea.getCourse() / 1000);
+ gpsCourseOld = false;
+ if (gpsPos.course == 0) {
+ // either north or not new
+ if (lastCourse != 0) // use old value...
+ {
+ gpsCourseOld = true;
+ gpsPos.course = lastCourse;
+ }
+ }
}
#ifdef DEBUG_GPS
uint8_t hdop = nmea.getHDOP();
@@ -1215,25 +1353,41 @@ void gpsTask(void *parameter) {
#define UBX_SYNCH_1 0xB5
#define UBX_SYNCH_2 0x62
-uint8_t ubx_set9k6[]={UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x03, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8D, 0x8F};
-uint8_t ubx_factorydef[]={UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x09, 13, 0, 0xff, 0xff, 0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0xff, 0x17, 0x8A };
+uint8_t ubx_set9k6[] = {UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x03, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8D, 0x8F};
+uint8_t ubx_factorydef[] = {UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x09, 13, 0, 0xff, 0xff, 0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0xff, 0x17, 0x8A };
+uint8_t ubx_hardreset[] = {UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x04, 4, 0, 0xff, 0xff, 0, 0, 0x0C, 0x5D };
+// GPGST: Class 0xF0 Id 0x07
+uint8_t ubx_enable_gpgst[] = {UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x01, 3, 0, 0xF0, 0x07, 2, 0x03, 0x1F};
void initGPS() {
if (sonde.config.gps_rxd < 0) return; // GPS disabled
if (sonde.config.gps_txd >= 0) { // TX enable, thus try setting baud to 9600 and do a factory reset
- Serial.println("Trying to reset GPS...");
- Serial2.begin(115200, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
- Serial2.write(ubx_set9k6, sizeof(ubx_set9k6));
- delay(100);
- Serial2.begin(38400, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
- Serial2.write(ubx_set9k6, sizeof(ubx_set9k6));
- delay(100);
- Serial2.begin(19200, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
- Serial2.write(ubx_set9k6, sizeof(ubx_set9k6));
- delay(100);
- Serial2.begin(9600, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
- Serial2.write(ubx_factorydef, sizeof(ubx_factorydef));
- delay(1000);
+ File testfile = SPIFFS.open("/GPSRESET", FILE_READ);
+ if (testfile && !testfile.isDirectory()) {
+ testfile.close();
+ Serial.println("GPS factory reset...");
+ Serial2.begin(115200, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
+ Serial2.write(ubx_set9k6, sizeof(ubx_set9k6));
+ delay(100);
+ Serial2.begin(38400, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
+ Serial2.write(ubx_set9k6, sizeof(ubx_set9k6));
+ delay(100);
+ Serial2.begin(19200, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
+ Serial2.write(ubx_set9k6, sizeof(ubx_set9k6));
+ delay(100);
+ Serial2.begin(9600, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
+ Serial2.write(ubx_factorydef, sizeof(ubx_factorydef));
+ delay(100);
+ Serial2.write(ubx_hardreset, sizeof(ubx_hardreset));
+ delay(5000);
+ SPIFFS.remove("/GPSRESET");
+ } else if (testfile) {
+ Serial.println("GPS reset file: not found/isdir");
+ testfile.close();
+ Serial2.begin(9600, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
+ }
+ // Enable GPGST messages
+ Serial2.write(ubx_enable_gpgst, sizeof(ubx_enable_gpgst));
} else {
Serial2.begin(9600, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
}
@@ -1244,6 +1398,12 @@ void initGPS() {
NULL); /* task handle*/
}
+const char *getStateStr(int what) {
+ if(what<0 || what>=(sizeof(mainStateStr)/sizeof(const char *)))
+ return "--";
+ else
+ return mainStateStr[what];
+}
void sx1278Task(void *parameter) {
/* new strategy:
@@ -1259,13 +1419,12 @@ void sx1278Task(void *parameter) {
while (1) {
if (rxtask.activate >= 128) {
// activating sx1278 background task...
- Serial.printf("rx task: activate=%d mainstate=%d\n", rxtask.activate, rxtask.mainState);
+ Serial.printf("RXtask: start DECODER for sonde %d (was %s)\n", rxtask.activate&0x7f, getStateStr(rxtask.mainState));
rxtask.mainState = ST_DECODER;
rxtask.currentSonde = rxtask.activate & 0x7F;
- Serial.println("rx task: calling sonde.setup()");
sonde.setup();
} else if (rxtask.activate != -1) {
- Serial.printf("rx task: activate=%d mainstate=%d\n", rxtask.activate, rxtask.mainState);
+ Serial.printf("RXtask: start %s (was %s)\n", getStateStr(rxtask.activate), getStateStr(rxtask.mainState));
rxtask.mainState = rxtask.activate;
}
rxtask.activate = -1;
@@ -1513,7 +1672,7 @@ extern esp_err_t heap_caps_register_failed_alloc_callback(esp_alloc_failed_hook_
void heap_caps_alloc_failed_hook(size_t requested_size, uint32_t caps, const char *function_name)
{
- printf("%s was called but failed to allocate %d bytes with 0x%X capabilities. \n",function_name, requested_size, caps);
+ printf("%s was called but failed to allocate %d bytes with 0x%X capabilities. \n", function_name, requested_size, caps);
}
#endif
@@ -1574,7 +1733,7 @@ void setup()
// NOT TTGO v1 (fingerprint 64) or Heltec v1/v2 board (fingerprint 4)
// and NOT TTGO Lora32 v2.1_1.6 (fingerprint 31/63)
- if ( ( (sonde.fingerprint&(64+31)) != 31) && ((sonde.fingerprint & 16) == 16) ) {
+ if ( ( (sonde.fingerprint & (64 + 31)) != 31) && ((sonde.fingerprint & 16) == 16) ) {
// FOr T-Beam 1.0
for (int i = 0; i < 10; i++) { // try multiple times
Wire.begin(21, 22);
@@ -1598,12 +1757,12 @@ void setup()
axp.adc1Enable(AXP202_VBUS_VOL_ADC1, 1);
axp.adc1Enable(AXP202_VBUS_CUR_ADC1, 1);
axp.adc1Enable(AXP202_BATT_CUR_ADC1, 1);
- if(sonde.config.button2_axp) {
+ if (sonde.config.button2_axp) {
pinMode(PMU_IRQ, INPUT_PULLUP);
attachInterrupt(PMU_IRQ, [] {
pmu_irq = true;
}, FALLING);
- //axp.enableIRQ(AXP202_VBUS_REMOVED_IRQ | AXP202_VBUS_CONNECT_IRQ | AXP202_BATT_REMOVED_IRQ | AXP202_BATT_CONNECT_IRQ, 1);
+ //axp.enableIRQ(AXP202_VBUS_REMOVED_IRQ | AXP202_VBUS_CONNECT_IRQ | AXP202_BATT_REMOVED_IRQ | AXP202_BATT_CONNECT_IRQ, 1);
axp.enableIRQ( AXP202_PEK_LONGPRESS_IRQ | AXP202_PEK_SHORTPRESS_IRQ, 1 );
axp.clearIRQ();
}
@@ -1659,7 +1818,7 @@ void setup()
sonde.clearDisplay();
setupWifiList();
- Serial.printf("before disp.initFromFile... layouts is %p", disp.layouts);
+ Serial.printf("before disp.initFromFile... layouts is %p\n", disp.layouts);
disp.initFromFile(sonde.config.screenfile);
Serial.printf("disp.initFromFile... layouts is %p", disp.layouts);
@@ -1813,39 +1972,47 @@ static const char *action2text(uint8_t action) {
#define RDZ_DATA_LEN 128
void parseGpsJson(char *data) {
- char *key = NULL;
- char *value = NULL;
- // very simple json parser: look for ", then key, then ", then :, then number, then , or } or \0
- for(int i=0; i
%d [current: main=%d, rxtask=%d]\n", action2text(action), action, newact, sonde.currentSonde, rxtask.currentSonde);
+ action = newact;
if (action != 255) {
if (action == ACT_DISPLAY_SPECTRUM) {
enterMode(ST_SPECTRUM);
@@ -1872,10 +2039,7 @@ void loopDecoder() {
enterMode(ST_WIFISCAN);
return;
}
- // no... we are already in DECODER mode, so no need to do anything!?
- //else if (action == ACT_NEXTSONDE) enterMode(ST_DECODER); // update rx background task
}
- Serial.printf("current main is %d, current rxtask is %d\n", sonde.currentSonde, rxtask.currentSonde);
}
if (!tncclient.connected()) {
@@ -1892,33 +2056,24 @@ void loopDecoder() {
}
Serial.println("");
}
-#if 0
- if (!rdzclient.connected()) {
- rdzclient = rdzserver.available();
- if(rdzclient.connected()) {
- Serial.println("RDZ JSON socket: new connection");
- }
- }
-#else
if (rdzserver.hasClient()) {
Serial.println("TCP JSON socket: new connection");
- if(rdzclient) rdzclient.stop();
+ if (rdzclient) rdzclient.stop();
rdzclient = rdzserver.available();
}
-#endif
- if(rdzclient.available()) {
+ if (rdzclient.available()) {
Serial.print("RDZ JSON socket: received ");
- while(rdzclient.available()) {
+ while (rdzclient.available()) {
char c = (char)rdzclient.read();
Serial.print(c);
- if(c=='\n'||c=='}'||rdzDataPos>=RDZ_DATA_LEN) {
+ if (c == '\n' || c == '}' || rdzDataPos >= RDZ_DATA_LEN) {
// parse GPS position from phone
- rdzData[rdzDataPos] = c;
- if(rdzDataPos>2) parseGpsJson(rdzData);
- rdzDataPos = 0;
+ rdzData[rdzDataPos] = c;
+ if (rdzDataPos > 2) parseGpsJson(rdzData);
+ rdzDataPos = 0;
}
else {
- rdzData[rdzDataPos++] = c;
+ rdzData[rdzDataPos++] = c;
}
}
Serial.println("");
@@ -1948,89 +2103,101 @@ void loopDecoder() {
tncclient.write(raw, rawlen);
}
}
-
+
// send to MQTT if enabled
if (connected && mqttEnabled) {
Serial.println("Sending sonde info via MQTT");
mqttclient.publishPacket(s);
}
-
- // also send to web socket
- //TODO
}
// always send data, even if not valid....
if (rdzclient.connected()) {
- Serial.println("Sending position via TCP as rdzJSON");
- char raw[1024];
- const char *typestr = s->typestr;
- if(*typestr==0) typestr = sondeTypeStr[s->type];
- int len = snprintf(raw, 1024, "{"
- "\"res\": %d,"
- "\"type\": \"%s\","
- "\"active\": %d,"
- "\"freq\": %.2f,"
- "\"id\": \"%s\","
- "\"ser\": \"%s\","
- "\"validId\": %d,"
- "\"launchsite\": \"%s\","
- "\"lat\": %.5f,"
- "\"lon\": %.5f,"
- "\"alt\": %.1f,"
- "\"vs\": %.1f,"
- "\"hs\": %.1f,"
- "\"dir\": %.1f,"
- "\"sats\": %d,"
- "\"validPos\": %d,"
- "\"time\": %d,"
- "\"sec\": %d,"
- "\"frame\": %d,"
- "\"validTime\": %d,"
- "\"rssi\": %d,"
- "\"afc\": %d,"
- "\"launchKT\": %d,"
- "\"burstKT\": %d,"
- "\"countKT\": %d,"
- "\"crefKT\": %d"
- "}\n",
- res&0xff,
- typestr,
- (int)s->active,
- s->freq,
- s->id,
- s->ser,
- (int)s->validID,
- s->launchsite,
- s->lat,
- s->lon,
- s->alt,
- s->vs,
- s->hs,
- s->dir,
- s->sats,
- s->validPos,
- s->time,
- s->sec,
- s->frame,
- (int)s->validTime,
- s->rssi,
- s->afc,
- s->launchKT,
- s->burstKT,
- s->countKT,
- s->crefKT
- );
- //Serial.println("Writing rdzclient...");
- if(len>1024) len=1024;
- int wlen = rdzclient.write(raw, len);
- if(wlen != len) {
- Serial.println("Writing rdzClient not OK, closing connection");
- rdzclient.stop();
- rdzclient = NULL;
- }
- //Serial.println("Writing rdzclient OK");
+ Serial.println("Sending position via TCP as rdzJSON");
+ char raw[1024];
+ char gps[128];
+ const char *typestr = s->typestr;
+ if (*typestr == 0) typestr = sondeTypeStr[s->type];
+ // TODO: only if GPS is valid...
+ if (gpsPos.valid) {
+ snprintf(gps, 128, ", \"gpslat\": %f"
+ "\"gpslon\": %f,"
+ "\"gpsalt\": %d,"
+ "\"gpsacc\": %d,"
+ "\"gpsdir\": %d",
+ gpsPos.lat, gpsPos.lon, gpsPos.alt, gpsPos.accuracy, gpsPos.course);
+ } else {
+ *gps = 0;
+ }
+ //
+ int len = snprintf(raw, 1024, "{"
+ "\"res\": %d,"
+ "\"type\": \"%s\","
+ "\"active\": %d,"
+ "\"freq\": %.2f,"
+ "\"id\": \"%s\","
+ "\"ser\": \"%s\","
+ "\"validId\": %d,"
+ "\"launchsite\": \"%s\","
+ "\"lat\": %.5f,"
+ "\"lon\": %.5f,"
+ "\"alt\": %.1f,"
+ "\"vs\": %.1f,"
+ "\"hs\": %.1f,"
+ "\"dir\": %.1f,"
+ "\"sats\": %d,"
+ "\"validPos\": %d,"
+ "\"time\": %d,"
+ "\"sec\": %d,"
+ "\"frame\": %d,"
+ "\"validTime\": %d,"
+ "\"rssi\": %d,"
+ "\"afc\": %d,"
+ "\"launchKT\": %d,"
+ "\"burstKT\": %d,"
+ "\"countKT\": %d,"
+ "\"crefKT\": %d"
+ "%s"
+ "}\n",
+ res & 0xff,
+ typestr,
+ (int)s->active,
+ s->freq,
+ s->id,
+ s->ser,
+ (int)s->validID,
+ s->launchsite,
+ s->lat,
+ s->lon,
+ s->alt,
+ s->vs,
+ s->hs,
+ s->dir,
+ s->sats,
+ s->validPos,
+ s->time,
+ s->sec,
+ s->frame,
+ (int)s->validTime,
+ s->rssi,
+ s->afc,
+ s->launchKT,
+ s->burstKT,
+ s->countKT,
+ s->crefKT,
+ gps
+ );
+ //Serial.println("Writing rdzclient...");
+ if (len > 1024) len = 1024;
+ int wlen = rdzclient.write(raw, len);
+ if (wlen != len) {
+ Serial.println("Writing rdzClient not OK, closing connection");
+ rdzclient.stop();
+ rdzclient = NULL;
+ }
+ //Serial.println("Writing rdzclient OK");
}
- Serial.print("updateDisplay started... ");
+ Serial.print("MAIN: updateDisplay started\n");
if (forceReloadScreenConfig) {
disp.initFromFile(sonde.config.screenfile);
sonde.clearDisplay();
@@ -2038,7 +2205,7 @@ void loopDecoder() {
}
int t = millis();
sonde.updateDisplay();
- Serial.printf("updateDisplay done (after %d ms)\n", (int)(millis() - t));
+ Serial.printf("MAIN: updateDisplay done (after %d ms)\n", (int)(millis() - t));
}
void setCurrentDisplay(int value) {
@@ -2126,7 +2293,7 @@ void enableNetwork(bool enable) {
tncserver.begin();
rdzserver.begin();
}
-
+
if (sonde.config.mqtt.active && strlen(sonde.config.mqtt.host) > 0) {
mqttEnabled = true;
mqttclient.init(sonde.config.mqtt.host, sonde.config.mqtt.port, sonde.config.mqtt.id, sonde.config.mqtt.username, sonde.config.mqtt.password, sonde.config.mqtt.prefix);
@@ -2415,19 +2582,12 @@ void loopWifiScan() {
int index = -1;
int n = WiFi.scanNetworks();
for (int i = 0; i < n; i++) {
- Serial.print("Network name: ");
String ssid = WiFi.SSID(i);
- Serial.println(ssid);
disp.rdis->drawString(0, dispys * (1 + line), ssid.c_str());
line = (line + 1) % (disph / dispys);
- Serial.print("Signal strength: ");
- Serial.println(WiFi.RSSI(i));
- Serial.print("MAC address: ");
- Serial.println(WiFi.BSSIDstr(i));
- Serial.print("Encryption type: ");
+ String mac = WiFi.BSSIDstr(i);
String encryptionTypeDescription = translateEncryptionType(WiFi.encryptionType(i));
- Serial.println(encryptionTypeDescription);
- Serial.println("-----------------------");
+ Serial.printf("Network %s: RSSI %d, MAC %s, enc: %s\n", ssid.c_str(), WiFi.RSSI(i), mac.c_str(), encryptionTypeDescription.c_str());
int curidx = fetchWifiIndex(ssid.c_str());
if (curidx >= 0 && index == -1) {
index = curidx;
@@ -2665,8 +2825,8 @@ void execOTA() {
void loop() {
- Serial.printf("\nRunning main loop in state %d [currentDisp:%d, lastDiso:%d]. free heap: %d;\n",
- mainState, currentDisplay, lastDisplay, ESP.getFreeHeap());
+ Serial.printf("\nMAIN: Running loop in state %d [currentDisp:%d, lastDisp:%d]. free heap: %d, unused stack: %d\n",
+ mainState, currentDisplay, lastDisplay, ESP.getFreeHeap(), uxTaskGetStackHighWaterMark(0));
switch (mainState) {
case ST_DECODER:
#ifndef DISABLE_MAINRX
@@ -2703,5 +2863,4 @@ void loop() {
lastMqttUptime = now;
}
- Serial.printf("Unused stack: %d\n", uxTaskGetStackHighWaterMark(0));
}
diff --git a/RX_FSK/data/GPSRESET b/RX_FSK/data/GPSRESET
new file mode 100644
index 0000000..fd38861
--- /dev/null
+++ b/RX_FSK/data/GPSRESET
@@ -0,0 +1 @@
++
diff --git a/RX_FSK/data/config.txt b/RX_FSK/data/config.txt
index 4e8b2e8..3147cec 100644
--- a/RX_FSK/data/config.txt
+++ b/RX_FSK/data/config.txt
@@ -25,8 +25,9 @@
#tft_rs=2
#tft_cs=0
tft_orient=1
+#tft_modeflip=0
#gps_rxd=-1
-gps_txd=-1
+#gps_txd=-1
# Show AFC value (for RS41 and M10/M20, maybe also DFM, but not useful for RS92)
showafc=1
# Frequency correction, in Hz
@@ -112,7 +113,7 @@ tcp.idformat=0
# data not sanitized / quality checked, outliers not filtered out
mqtt.active=0
mqtt.id=rdz_sonde_server
-mqtt.ip=
+mqtt.host=
mqtt.port=1883
mqtt.username=
mqtt.password=
diff --git a/RX_FSK/version.h b/RX_FSK/version.h
index e8cc738..1a6e6e2 100644
--- a/RX_FSK/version.h
+++ b/RX_FSK/version.h
@@ -1,4 +1,4 @@
const char *version_name = "rdzTTGOsonde";
-const char *version_id = "devel20210213";
+const char *version_id = "devel20210315";
const int SPIFFS_MAJOR=2;
const int SPIFFS_MINOR=10;
diff --git a/libraries/SondeLib/DFM.cpp b/libraries/SondeLib/DFM.cpp
index 5c34975..bf9d204 100644
--- a/libraries/SondeLib/DFM.cpp
+++ b/libraries/SondeLib/DFM.cpp
@@ -412,7 +412,7 @@ void DFM::decodeDAT(uint8_t *dat)
case 2:
{
float lat, vh;
- lat = ((uint32_t)dat[0]<<24) + ((uint32_t)dat[1]<<16) + ((uint32_t)dat[2]<<8) + ((uint32_t)dat[3]);
+ lat = (int32_t)(((uint32_t)dat[0]<<24) + ((uint32_t)dat[1]<<16) + ((uint32_t)dat[2]<<8) + ((uint32_t)dat[3]));
vh = ((uint16_t)dat[4]<<8) + dat[5];
Serial.print("GPS-lat: "); Serial.print(lat*0.0000001);
Serial.print(", hor-V: "); Serial.print(vh*0.01);
@@ -424,7 +424,7 @@ void DFM::decodeDAT(uint8_t *dat)
case 3:
{
float lon, dir;
- lon = ((uint32_t)dat[0]<<24) + ((uint32_t)dat[1]<<16) + ((uint32_t)dat[2]<<8) + (uint32_t)dat[3];
+ lon = (int32_t)(((uint32_t)dat[0]<<24) + ((uint32_t)dat[1]<<16) + ((uint32_t)dat[2]<<8) + (uint32_t)dat[3]);
dir = ((uint16_t)dat[4]<<8) + dat[5];
Serial.print("GPS-lon: "); Serial.print(lon*0.0000001);
Serial.print(", dir: "); Serial.print(dir*0.01);
diff --git a/libraries/SondeLib/Display.cpp b/libraries/SondeLib/Display.cpp
index 00a261b..f4cbb40 100644
--- a/libraries/SondeLib/Display.cpp
+++ b/libraries/SondeLib/Display.cpp
@@ -339,6 +339,7 @@ static int ngfx = sizeof(gfl)/sizeof(GFXfont *);
void ILI9225Display::begin() {
tft = new MY_ILI9225(sonde.config.oled_rst, sonde.config.tft_rs, sonde.config.tft_cs,
sonde.config.oled_sda, sonde.config.oled_scl, TFT_LED, TFT_BRIGHTNESS);
+ tft->setModeFlip(sonde.config.tft_modeflip);
tft->begin(spiDisp);
tft->setOrientation(sonde.config.tft_orient);
}
@@ -665,7 +666,7 @@ int Display::allocDispInfo(int entries, DispInfo *d, char *label)
d->timeouts = (int16_t *)mem;
d->label = label;
- Serial.printf("allocated %d bytes (%d entries) for %p (addr=%p)\n", totalsize, entries, d, d->de);
+ Serial.printf("%s: alloc %d bytes (%d entries) for %p (addr=%p)\n", label, totalsize, entries, d, d->de);
return 0;
}
@@ -726,7 +727,7 @@ void Display::parseDispElement(char *text, DispEntry *de)
case 'f':
de->func = disp.drawFreq;
de->extra = strdup(text+1);
- Serial.printf("parsing 'f' entry: extra is '%s'\n", de->extra);
+ //Serial.printf("parsing 'f' entry: extra is '%s'\n", de->extra);
break;
case 'n':
// IP address / small always uses tiny font on TFT for backward compatibility
@@ -780,8 +781,8 @@ void Display::parseDispElement(char *text, DispEntry *de)
de->extra = (char *)circinfo;
} else {
de->extra = strdup(text+1);
+ //Serial.printf("parsing 'g' entry: extra is '%s'\n", de->extra);
}
- Serial.printf("parsing 'g' entry: extra is '%s'\n", de->extra);
break;
case 'r':
de->func = disp.drawRSSI; break;
@@ -839,7 +840,7 @@ int Display::countEntries(File f) {
break;
}
f.seek(pos, SeekSet);
- Serial.printf("Counted %d entries\n", n);
+ //Serial.printf("Counted %d entries\n", n);
return n;
}
@@ -848,7 +849,7 @@ void Display::initFromFile(int index) {
if(index>0) {
char file[20];
snprintf(file, 20, "/screens%d.txt", index);
- Serial.printf("Trying %i (%s)\n", index, file);
+ Serial.printf("Reading %s\n", file);
d = SPIFFS.open(file, "r");
if(!d || d.available()==0 ) { Serial.printf("%s not found, using /screens.txt\n", file); }
}
@@ -888,6 +889,7 @@ void Display::initFromFile(int index) {
case -1: // wait for start of screen (@)
{
if(*s != '@') {
+ if(*s==0 || *s==10 || *s==13) continue;
Serial.printf("Illegal start of screen: %s\n", s);
continue;
}
@@ -896,7 +898,6 @@ void Display::initFromFile(int index) {
DebugPrintf(DEBUG_SPARSER,"Reading entry with %d elements\n", entrysize);
idx++;
int res = allocDispInfo(entrysize, &newlayouts[idx], label);
- Serial.printf("allocDispInfo: idx %d: label is %p - %s\n",idx,newlayouts[idx].label, newlayouts[idx].label);
if(res<0) {
Serial.println("Error allocating memory for disp info");
continue;
@@ -917,7 +918,7 @@ void Display::initFromFile(int index) {
if(newlayouts[idx].timeouts[1]>0) newlayouts[idx].timeouts[1]*=1000;
if(newlayouts[idx].timeouts[2]>0) newlayouts[idx].timeouts[2]*=1000;
//sscanf(s+6, "%hd,%hd,%hd", newlayouts[idx].timeouts, newlayouts[idx].timeouts+1, newlayouts[idx].timeouts+2);
- Serial.printf("timer values: %d, %d, %d\n", newlayouts[idx].timeouts[0], newlayouts[idx].timeouts[1], newlayouts[idx].timeouts[2]);
+ //Serial.printf("timer values: %d, %d, %d\n", newlayouts[idx].timeouts[0], newlayouts[idx].timeouts[1], newlayouts[idx].timeouts[2]);
} else if(strncmp(s, "key1action=",11)==0) { // key 1 actions
char c1,c2,c3,c4;
sscanf(s+11, "%c,%c,%c,%c", &c1, &c2, &c3, &c4);
@@ -932,7 +933,7 @@ void Display::initFromFile(int index) {
newlayouts[idx].actions[6] = ACTION(c2);
newlayouts[idx].actions[7] = ACTION(c3);
newlayouts[idx].actions[8] = ACTION(c4);
- Serial.printf("parsing key2action: %c %c %c %c\n", c1, c2, c3, c4);
+ //Serial.printf("parsing key2action: %c %c %c %c\n", c1, c2, c3, c4);
} else if(strncmp(s, "timeaction=",11)==0) { // timer actions
char c1,c2,c3;
sscanf(s+11, "%c,%c,%c", &c1, &c2, &c3);
@@ -970,9 +971,11 @@ void Display::initFromFile(int index) {
what++;
newlayouts[idx].de[what].func = NULL;
} else {
+#if 0
for(int i=0; i<12; i++) {
Serial.printf("action %d: %d\n", i, (int)newlayouts[idx].actions[i]);
}
+#endif
what=-1;
}
break;
diff --git a/libraries/SondeLib/Display.h b/libraries/SondeLib/Display.h
index 0286f89..81852a8 100644
--- a/libraries/SondeLib/Display.h
+++ b/libraries/SondeLib/Display.h
@@ -15,6 +15,7 @@ struct GpsPos {
double lon;
int alt;
int course;
+ int accuracy;
int valid;
};
extern struct GpsPos gpsPos;
diff --git a/libraries/SondeLib/M10M20.cpp b/libraries/SondeLib/M10M20.cpp
index 0b2c38f..7f7b2c0 100644
--- a/libraries/SondeLib/M10M20.cpp
+++ b/libraries/SondeLib/M10M20.cpp
@@ -41,6 +41,8 @@ int M10M20::setup(float frequency)
Serial.print("M10/M20: setting RX frequency to ");
Serial.println(frequency);
int res = sx1278.setFrequency(frequency);
+ // Test: maybe fix issue after spectrum display?
+ sx1278.writeRegister(REG_PLL_HOP, 0);
if(sx1278.setAFCBandwidth(sonde.config.m10m20.agcbw)!=0) {
M10M20_DBG(Serial.printf("Setting AFC bandwidth %d Hz FAILED", sonde.config.m10m20.agcbw));
@@ -108,7 +110,7 @@ int M10M20::setup(float frequency)
}
///// Enable auto-AFC, auto-AGC, RX Trigger by preamble
- ///if(sx1278.setRxConf(0x1E)!=0) {
+ //if(sx1278.setRxConf(0x1E)!=0) {
// Disable auto-AFC, auto-AGC, RX Trigger by preamble
if(sx1278.setRxConf(0x00)!=0) {
M10M20_DBG(Serial.println("Setting RX Config FAILED"));
@@ -135,7 +137,6 @@ int M10M20::setup(float frequency)
// enable RX
sx1278.setPayloadLength(0); // infinite for now...
- //sx1278.setPayloadLength(292);
sx1278.setRxConf(0x20);
uint16_t afc = sx1278.getRawAFC();
sx1278.writeRegister(REG_OP_MODE, FSK_RX_MODE);
@@ -368,7 +369,7 @@ void M10M20::processM10data(uint8_t dt)
int rssi=sx1278.getRSSI();
int fei=sx1278.getFEI();
int afc=sx1278.getAFC();
- Serial.print("Test: RSSI="); Serial.print(rssi);
+ Serial.print("SYNC!!! Test: RSSI="); Serial.print(rssi);
Serial.print(" FEI="); Serial.print(fei);
Serial.print(" AFC="); Serial.println(afc);
sonde.si()->rssi = rssi;
diff --git a/libraries/SondeLib/RS41.cpp b/libraries/SondeLib/RS41.cpp
index 6047914..1b437db 100644
--- a/libraries/SondeLib/RS41.cpp
+++ b/libraries/SondeLib/RS41.cpp
@@ -17,6 +17,137 @@
static byte data[800];
static int dpos = 0;
+#if 0
+// subframe is never used?
+static byte subframe[51*16]; // 816 subframe bytes
+// this is moved to sondeInfo->extra
+static bool subframeReceived[51] = { false }; // do we have data for row
+static bool subframeComplete = false; // is the subframe complete
+// this is needed only locally, use a local variable on stack for that
+static bool validExternalTemperature = false; // have received all the calibration frames for the external temperature
+static bool validHumidity = false; // have received all the calibration frames for the humidity
+static bool validRAExternalTemperature = false; // have received all the calibration frames for the external temperature
+static bool validRAHumidity = false; // have received all the calibration frames for the humidity
+#endif
+
+// whole 51 row frame as C structure
+// taken from https://github.com/einergehtnochrein/ra-firmware
+struct subframeBuffer {
+ uint64_t valid; // bitmask for subframe valid; lsb=frame 0, etc.
+ union {
+ byte rawData[51*16];
+ struct __attribute__((__packed__)) {
+ uint16_t crc16; /* CRC16 CCITT Checksum over range 0x002...0x31F */
+ uint16_t frequency; /* 0x002: TX is on 400 MHz + (frequency / 64) * 10 kHz */
+ uint8_t startupTxPower; /* 0x004: TX power level at startup (1...7) */
+ uint8_t reserved005;
+ uint8_t reserved006;
+ uint16_t reserved007; /* 0x007: ?? (some bitfield) [0],[1],[2],[3]. Init value = 0xE */
+ uint16_t reserved009; /* 0x009: ? */
+ uint8_t reserved00B;
+ uint8_t reserved00C;
+ uint8_t serial[8]; /* 0x00D: Sonde ID, 8 char, not terminated */
+ uint16_t firmwareVersion; /* 0x015: 10000*major + 100*minor + patch*/
+ uint16_t reserved017;
+ uint16_t minHeight4Flight; /* 0x019: Height (meter above ground) where flight mode begins */
+ uint8_t lowBatteryThreshold100mV; /* 0x01B: (Default=18) Shutdown if battery voltage below this
+ threshold for some time (10s ?)
+ */
+ uint8_t nfcDetectorThreshold; /* 0x01C: NFC detector threshold [25mV] (Default: 0x05 = 125mV) */
+ uint8_t reserved01D; /* 0x01D: ?? (Init value = 0xB4) */
+ uint8_t reserved01E; /* 0x01E: ?? (Init value = 0x3C) */
+ uint16_t reserved01F;
+ int8_t refTemperatureThreshold; /* 0x021: Reference temperature threshold [°C] */
+ uint8_t reserved022;
+ uint16_t reserved023;
+ uint16_t reserved025;
+ int16_t flightKillFrames; /* 0x027: Number of frames in flight until kill (-1 = disabled) */
+ uint16_t reserved029; /* 0x029: ? (Init value = 0) */
+ uint8_t burstKill; /* 0x02B: Burst kill (0=disabled, 1=enabled) */
+ uint8_t reserved02C;
+ uint8_t reserved02D;
+ uint16_t reserved02E;
+ uint16_t reserved030;
+ uint8_t reserved032;
+ uint16_t reserved033;
+ uint16_t reserved035;
+ uint16_t reserved037;
+ uint16_t reserved039; /* 0x039: */
+ uint8_t reserved03B; /* 0x03B: */
+ uint8_t reserved03C; /* 0x03C: */
+ float refResistorLow; /* 0x03D: Reference resistor low (750 Ohms) */
+ float refResistorHigh; /* 0x041: Reference resistor high (1100 Ohms) */
+ float refCapLow; /* 0x045: Reference capacitance low (0) */
+ float refCapHigh; /* 0x049: Reference capacitance high (47 pF) */
+ float taylorT[3]; /* 0x04D: Tayor coefficients for main temperature calculation */
+ float calT; /* 0x059: Calibration factor for main sensor */
+ float polyT[6]; /* 0x05D: */
+ float calibU[2]; /* 0x075: Calibration coefficients for humidity sensor */
+
+ float matrixU[7][6]; /* 0x07D: Matrix for humidity sensor RH calculation */
+ float taylorTU[3]; /* 0x125: Coefficients for U sensor temperature calculation */
+ float calTU; /* 0x131: Calibration factor for U temperature sensor */
+ float polyTrh[6]; /* 0x135: */
+
+ uint8_t reserved14D; /* 0x14D: */
+ uint32_t reserved14E; /* 0x14E: */
+
+ float f152;
+ uint8_t u156;
+ float f157; /* 0x157: ?? (Initialized by same value as calibU) */
+ uint8_t reserved15B[0x160-0x15B];
+ float f160[35];
+ uint8_t startIWDG; /* 0x1EC: If ==0 or ==2: Watchdog IWDG will not be started */
+ uint8_t parameterSetupDone; /* 0x1ED: Set (!=0) if parameter setup was done */
+ uint8_t reserved1EE;
+ uint8_t reserved1EF;
+ float f1F0[8];
+ float pressureLaunchSite[2]; /* 0x210: Pressure [hPa] at launch site */
+ struct {
+ char variant[10]; /* 0x218: Sonde variant (e.g. "RS41-SG") */
+ uint8_t mainboard[10]; /* 0x222: Name of mainboard (e.g. "RSM412") */
+ } names;
+ struct {
+ uint8_t mainboard[9]; /* 0x22C: Serial number of mainboard (e.g. "L1123553") */
+ uint8_t text235[12]; /* 0x235: "0000000000" */
+ uint16_t reserved241; /* 0x241: */
+ uint8_t pressureSensor[8]; /* 0x243: Serial number of pressure sensor (e.g. "N1310487") */
+ uint16_t reserved24B; /* 0x24B: */
+ } serials;
+ uint16_t reserved24D; /* 0x24D: */
+ uint8_t reserved24F;
+ uint8_t reserved250;
+ uint16_t reserved251; /* 0x251: (Init value = 0x21A = 538) */
+ uint8_t xdataUartBaud; /* 0x253: 1=9k6, 2=19k2, 3=38k4, 4=57k6, 5=115k2 */
+ uint8_t reserved254;
+ float cpuTempSensorVoltageAt25deg; /* 0x255: CPU temperature sensor voltage at 25°C */
+ uint8_t reserved259;
+ uint8_t reserved25A[0x25E -0x25A];
+ float matrixP[18]; /* 0x25E: Coefficients for pressure sensor polynomial */
+ float f2A6[17];
+ uint8_t reserved2EA[0x2FA-0x2EA];
+ uint16_t halfword2FA[9];
+ float reserved30C;
+ float reserved310; /* 0x310: */
+ uint8_t reserved314; /* 0x314: */
+ uint8_t reserved315; /* 0x315: */
+ int16_t burstKillFrames; /* 0x316: Number of active frames after burst kill */
+ uint8_t reserved318[0x320-0x318];
+
+ /* This is fragment 50. It only uses 14 valid bytes! */
+ int16_t killCountdown; /* 0x320: Counts frames remaining until kill (-1 = inactive) */
+ uint8_t reserved322[6];
+ int8_t intTemperatureCpu; /* 0x328: Temperature [°C] of CPU */
+ int8_t intTemperatureRadio; /* 0x329: Temperature [°C] of radio chip */
+ int8_t reserved32A; /* 0x32A: */
+ uint8_t reserved32B; /* 0x32B: */
+ uint8_t reserved32C; /* 0x32C: ? (the sum of two slow 8-bit counters) */
+ uint8_t reserved32D; /* 0x32D: ? (the sum of two slow 8-bit counters) */
+ } value;
+ };
+};
+// moved global variable "calibration" to sondeInfo->extra
+
static uint16_t CRCTAB[256];
#define X2C_DIVR(a, b) ((b) != 0.0f ? (a)/(b) : (a))
@@ -135,8 +266,10 @@ int RS41::setup(float frequency)
RS41_DBG(Serial.println("Setting Packet config FAILED"));
return 1;
}
+#if RS41_DEBUG
Serial.print("RS41: setting RX frequency to ");
Serial.println(frequency);
+#endif
int retval = sx1278.setFrequency(frequency);
dpos = 0;
@@ -251,6 +384,11 @@ static int32_t getint32(const byte frame[], uint32_t frame_len,
return (int32_t)n;
} /* end getint32() */
+static uint32_t getint24(const byte frame[], uint32_t frame_len, uint32_t p) { // 24bit unsigned int
+ uint32_t val24 = 0;
+ val24 = frame[p] | (frame[p+1]<<8) | (frame[p+2]<<16);
+ return val24;
+}
static uint32_t getcard16(const byte frame[], uint32_t frame_len,
uint32_t p)
@@ -379,7 +517,131 @@ static void posrs41(const byte b[], uint32_t b_len, uint32_t p)
sonde.si()->validPos = 0x7f;
} /* end posrs41() */
+void ProcessSubframe( byte *subframeBytes, int subframeNumber ) {
+ // the total subframe consists of 51 rows, each row 16 bytes
+ // based on https://github.com/bazjo/RS41_Decoding/tree/master/RS41-SGP#Subframe
+ struct subframeBuffer *s = (struct subframeBuffer *)sonde.si()->extra;
+ // Allocate on demand
+ if(!s) {
+ s = (struct subframeBuffer *)malloc( sizeof(struct subframeBuffer) );
+ if(!s) { Serial.println("ProcessSubframe: out of memory"); return; }
+ sonde.si()->extra = s;
+ }
+ memcpy( s->rawData+16*subframeNumber, subframeBytes, 16);
+ s->valid |= (1ULL << subframeNumber);
+ // subframeReceived[subframeNumber] = true; // mark this row of the total subframe as complete
+ #if 0
+ Serial.printf("subframe number: 0x%02X\n", subframeNumber );
+ Serial.print("subframe values: ");
+ for ( int i = 0; i < 16; i++ ) {
+ Serial.printf( "%02X ", subframeBytes[i] );
+ }
+ Serial.println();
+
+ Serial.println("Full subframe");
+ for ( int j = 0; j<51; j++ ) {
+ Serial.printf("%03X ", j*16);
+ for ( int i = 0; i < 16; i++ ) {
+ Serial.printf( "%02X ", s.rawData[j*16+i] );
+ }
+ Serial.println();
+ }
+ Serial.println();
+ #endif
+}
+
+/* Find the water vapor saturation pressure for a given temperature.
+ * Uses the Hyland and Wexler equation with coefficients for T < 0°C.
+ */
+// taken from https://github.com/einergehtnochrein/ra-firmware
+static float _RS41_waterVaporSaturationPressure (float Tcelsius)
+{
+ /* Convert to Kelvin */
+ float T = Tcelsius + 273.15f;
+
+ /* Apply some correction magic */
+ T = 0
+ - 0.4931358f
+ + (1.0f + 4.61e-3f) * T
+ - 1.3746454e-5f * T * T
+ + 1.2743214e-8f * T * T * T
+ ;
+
+ /* Plug into H+W equation */
+ float p = expf(-5800.2206f / T
+ + 1.3914993f
+ + 6.5459673f * logf(T)
+ - 4.8640239e-2f * T
+ + 4.1764768e-5f * T * T
+ - 1.4452093e-8f * T * T * T
+ );
+
+ /* Scale result to hPa */
+ return p / 100.0f;
+}
+
+// taken from https://github.com/einergehtnochrein/ra-firmware
+float GetRATemp( uint32_t measuredCurrent, uint32_t refMin, uint32_t refMax, float calT, float taylorT[3], float polyT[6] ) {
+ struct subframeBuffer *calibration = (struct subframeBuffer *)sonde.si()->extra;
+ /* Reference values for temperature are two known resistors.
+ * From that we can derive the resistance of the sensor.
+ */
+ float current = ( float(measuredCurrent) - float(refMin) ) / float(refMax - refMin);
+ float res = calibration->value.refResistorLow
+ + (calibration->value.refResistorHigh - calibration->value.refResistorLow) * current;
+ float x = res * calT;
+
+ float Tuncal = 0
+ + taylorT[0]
+ + taylorT[1] * x
+ + taylorT[2] * x * x;
+
+ /* Apply calibration polynomial */
+ float temperature =
+ Tuncal + polyT[0]
+ + polyT[1] * Tuncal
+ + polyT[2] * Tuncal * Tuncal
+ + polyT[3] * Tuncal * Tuncal * Tuncal
+ + polyT[4] * Tuncal * Tuncal * Tuncal * Tuncal
+ + polyT[5] * Tuncal * Tuncal * Tuncal * Tuncal * Tuncal;
+
+ return temperature;
+}
+
+// taken from https://github.com/einergehtnochrein/ra-firmware
+float GetRAHumidity( uint32_t humCurrent, uint32_t humMin, uint32_t humMax, float sensorTemp, float externalTemp ) {
+ struct subframeBuffer *calibration = (struct subframeBuffer *)sonde.si()->extra;
+ float current = float( humCurrent - humMin) / float( humMax - humMin );
+ /* Compute absolute capacitance from the known references */
+ float C = calibration->value.refCapLow
+ + (calibration->value.refCapHigh - calibration->value.refCapLow) * current;
+ /* Apply calibration */
+ float Cp = ( C / calibration->value.calibU[0] - 1.0f) * calibration->value.calibU[1];
+
+ int j, k;
+ float sum = 0;
+ float xj = 1.0f;
+ for (j = 0; j < 7; j++) {
+ float yk = 1.0f;
+ for (k = 0; k < 6; k++) {
+ sum += xj * yk * calibration->value.matrixU[j][k];
+ yk *= ( sensorTemp - 20.0f) / 180.0f;
+ }
+ xj *= Cp;
+ }
+
+ /* Since there is always a small difference between the temperature readings for
+ * the atmospheric (main) tempoerature sensor and the temperature sensor inside
+ * the humidity sensor device, transform the humidity value to the atmospheric conditions
+ * with its different water vapor saturation pressure.
+ */
+ float RH = sum
+ * _RS41_waterVaporSaturationPressure(sensorTemp)
+ / _RS41_waterVaporSaturationPressure(externalTemp);
+
+ return RH;
+}
// returns: 0: ok, -1: rs or crc error
int RS41::decode41(byte *data, int maxlen)
@@ -457,6 +719,15 @@ int RS41::decode41(byte *data, int maxlen)
sonde.si()->countKT = cntdown;
sonde.si()->crefKT = fnr;
}
+#if 0
+ // process this subframe
+ int subframeOffset = 24; // 24 = 0x18, start of subframe data
+ byte receivedBytes[16];
+ memcpy( receivedBytes, data+p+subframeOffset, 16);
+ ProcessSubframe( receivedBytes, calnr );
+#endif
+ ProcessSubframe( data+p+24, calnr );
+
}
// TODO: some more data
break;
@@ -476,6 +747,61 @@ int RS41::decode41(byte *data, int maxlen)
case '{': // pos
posrs41(data+p, len, 0);
break;
+ case 'z': // 0x7a is character z - 7A-MEAS temperature and humidity frame
+ {
+ uint32_t tempMeasMain = getint24(data, 560, p+0);
+ uint32_t tempMeasRef1 = getint24(data, 560, p+3);
+ uint32_t tempMeasRef2 = getint24(data, 560, p+6);
+ uint32_t humidityMain = getint24(data, 560, p+9);
+ uint32_t humidityRef1 = getint24(data, 560, p+12);
+ uint32_t humidityRef2 = getint24(data, 560, p+15);
+ uint32_t tempHumiMain = getint24(data, 560, p+18);
+ uint32_t tempHumiRef1 = getint24(data, 560, p+21);
+ uint32_t tempHumiRef2 = getint24(data, 560, p+24);
+ uint32_t pressureMain = getint24(data, 560, p+27);
+ uint32_t pressureRef1 = getint24(data, 560, p+30);
+ uint32_t pressureRef2 = getint24(data, 560, p+33);
+ #if 0
+ Serial.printf( "External temp: tempMeasMain = %ld, tempMeasRef1 = %ld, tempMeasRef2 = %ld\n", tempMeasMain, tempMeasRef1, tempMeasRef2 );
+ Serial.printf( "Rel Humidity: humidityMain = %ld, humidityRef1 = %ld, humidityRef2 = %ld\n", humidityMain, humidityRef1, humidityRef2 );
+ Serial.printf( "Humid sensor: tempHumiMain = %ld, tempHumiRef1 = %ld, tempHumiRef2 = %ld\n", tempHumiMain, tempHumiRef1, tempHumiRef2 );
+ Serial.printf( "Pressure sens: pressureMain = %ld, pressureRef1 = %ld, pressureRef2 = %ld\n", pressureMain, pressureRef1, pressureRef2 );
+ #endif
+ struct subframeBuffer *calibration = (struct subframeBuffer *)sonde.si()->extra;
+#if 0
+ // for a valid temperature, require rLow rHigh (frames 3 4) TaylorT (frames 4 5), polyT (frames 5 6 7),
+ validExternalTemperature = subframeReceived[3] && subframeReceived[4] && subframeReceived[5]
+ && subframeReceived[6] && subframeReceived[7];
+
+ // for a valid RA humidity, valid temperature, calibU (frame 7) matrixU (frame 7-12), taylorTU (frame 12 13),
+ // calTU (frame 13) polyTrh (frame 13 14)
+ validHumidity = validExternalTemperature && subframeReceived[0x08] && subframeReceived[0x09] && subframeReceived[0x0A]
+ && subframeReceived[0x0B] && subframeReceived[0x0C] && subframeReceived[0x0D] && subframeReceived[0x0E]
+ && subframeReceived[0x0F] && subframeReceived[0x10] && subframeReceived[0x11] && subframeReceived[0x12];
+#else
+ // check for bits 3, 4, 5, 6, and 7 set
+ bool validExternalTemperature = calibration!=NULL && (calibration->valid & 0xF8) == 0xF8;
+ bool validHumidity = calibration!=NULL && (calibration->valid & 0x7FF8) == 0x7FF8;
+#endif
+
+ if ( validExternalTemperature ) {
+ sonde.si()->temperature = GetRATemp( tempMeasMain, tempMeasRef1, tempMeasRef2,
+ calibration->value.calT, calibration->value.taylorT, calibration->value.polyT );
+ Serial.printf("External temperature = %f\n", sonde.si()->temperature );
+ }
+
+ if ( validHumidity && validExternalTemperature ) {
+ sonde.si()->tempRHSensor = GetRATemp( tempHumiMain, tempHumiRef1, tempHumiRef2,
+ calibration->value.calTU, calibration->value.taylorTU, calibration->value.polyTrh );
+ Serial.printf("Humidity Sensor temperature = %f\n", sonde.si()->tempRHSensor );
+ sonde.si()->relativeHumidity = GetRAHumidity( humidityMain, humidityRef1, humidityRef2, sonde.si()->tempRHSensor, sonde.si()->temperature );
+ Serial.printf("Relative humidity = %f\n", sonde.si()->relativeHumidity );
+ }
+ }
+ break;
+
+
+
default:
break;
}}
@@ -524,7 +850,7 @@ static uint8_t scramble[64] = {150U,131U,62U,81U,177U,73U,8U,152U,50U,5U,89U,
int RS41::receive() {
sx1278.setPayloadLength(RS41MAXLEN-8);
int e = sx1278.receivePacketTimeout(1000, data+8);
- if(e) { Serial.println("TIMEOUT"); return RX_TIMEOUT; }
+ if(e) { /*Serial.println("TIMEOUT");*/ return RX_TIMEOUT; }
for(int i=0; i=nSonde) rxtask.currentSonde=0;
- }
- }
- sonde.currentSonde = rxtask.currentSonde;
- }
+ for(int i=0; i=nSonde) rxtask.currentSonde=0;
+ }
+ }
+ sonde.currentSonde = rxtask.currentSonde;
+ }
// update receiver config
- Serial.print("\nSonde::setup() on sonde index ");
+ Serial.print("Sonde.setup() on sonde index ");
Serial.println(rxtask.currentSonde);
switch(sondeList[rxtask.currentSonde].type) {
case STYPE_RS41:
@@ -464,9 +468,10 @@ void Sonde::setup() {
break;
}
// debug
- float afcbw = sx1278.getAFCBandwidth();
- float rxbw = sx1278.getRxBandwidth();
- Serial.printf("AFC BW: %f RX BW: %f\n", afcbw, rxbw);
+ int freq = (int)sx1278.getFrequency();
+ int afcbw = (int)sx1278.getAFCBandwidth();
+ int rxbw = (int)sx1278.getRxBandwidth();
+ Serial.printf("Sonde.setup(): Freq %d, AFC BW: %d, RX BW: %d\n", freq, afcbw, rxbw);
// reset rxtimer / norxtimer state
sonde.sondeList[sonde.currentSonde].lastState = -1;
@@ -504,7 +509,7 @@ void Sonde::receive() {
}
} else { // RX not ok
if(res==RX_ERROR) flashLed(100);
- Serial.printf("RX result %d, laststate was %d\n", res, si->lastState);
+ Serial.printf("RX result %d (%s), laststate was %d\n", res, (res<=3)?RXstr[res]:"?", si->lastState);
if(si->lastState != 0) {
si->norxStart = millis();
si->lastState = 0;
@@ -520,7 +525,7 @@ void Sonde::receive() {
int event = getKeyPressEvent();
if (!event) event = timeoutEvent(si);
int action = (event==EVT_NONE) ? ACT_NONE : disp.layout->actions[event];
- if(action!=ACT_NONE) { Serial.printf("event %x: action is %x\n", event, action); }
+ //if(action!=ACT_NONE) { Serial.printf("event %x: action is %x\n", event, action); }
// If action is to move to a different sonde index, we do update things here, set activate
// to force the sx1278 task to call sonde.setup(), and pass information about sonde to
// main loop (display update...)
@@ -542,7 +547,7 @@ void Sonde::receive() {
}
}
res = (action<<8) | (res&0xff);
- Serial.printf("receive(): Result is %04x (action %d, res %d)\n", res, action, res&0xff);
+ Serial.printf("Sonde:receive(): Event %02x: action %02x, res %02x => %04x\n", event, action, res&0xff, res);
// let waitRXcomplete resume...
rxtask.receiveResult = res;
}
@@ -598,22 +603,22 @@ rxloop:
uint8_t Sonde::timeoutEvent(SondeInfo *si) {
uint32_t now = millis();
-#if 1
+#if 0
Serial.printf("Timeout check: %d - %d vs %d; %d - %d vs %d; %d - %d vs %d; lastState: %d\n",
now, si->viewStart, disp.layout->timeouts[0],
now, si->rxStart, disp.layout->timeouts[1],
now, si->norxStart, disp.layout->timeouts[2], si->lastState);
#endif
if(disp.layout->timeouts[0]>=0 && now - si->viewStart >= disp.layout->timeouts[0]) {
- Serial.println("View timer expired");
+ Serial.println("Sonde.timeoutEvent: View");
return EVT_VIEWTO;
}
if(si->lastState==1 && disp.layout->timeouts[1]>=0 && now - si->rxStart >= disp.layout->timeouts[1]) {
- Serial.println("RX timer expired");
+ Serial.println("Sonde.timeoutEvent: RX");
return EVT_RXTO;
}
if(si->lastState==0 && disp.layout->timeouts[2]>=0 && now - si->norxStart >= disp.layout->timeouts[2]) {
- Serial.println("NORX timer expired");
+ Serial.println("Sonde.timeoutEvent: NORX");
return EVT_NORXTO;
}
return 0;
diff --git a/libraries/SondeLib/Sonde.h b/libraries/SondeLib/Sonde.h
index 189df28..c31ca9c 100644
--- a/libraries/SondeLib/Sonde.h
+++ b/libraries/SondeLib/Sonde.h
@@ -99,6 +99,11 @@ typedef struct st_sondeinfo {
// shut down timers, currently only for RS41; -1=disabled
int16_t launchKT, burstKT, countKT;
uint16_t crefKT; // frame number in which countKT was last sent
+ // sonde specific extra data, NULL if unused or not yet initialized, currently used for RS41 subframe data (calibration)
+ void *extra;
+ float temperature = -300.0; // platinum resistor temperature
+ float tempRHSensor = -300.0; // temperature of relative humidity sensor
+ float relativeHumidity = -1.0; // relative humidity
} SondeInfo;
// rxStat: 3=undef[empty] 1=timeout[.] 2=errro[E] 0=ok[|] 4=no valid position[°]
@@ -186,6 +191,7 @@ typedef struct st_rdzconfig {
int tft_rs; // TFT RS pin
int tft_cs; // TFT CS pin
int tft_orient; // TFT orientation (default: 1)
+ int tft_modeflip; // Hack for Joerg's strange display
int gps_rxd; // GPS module RXD pin. We expect 9600 baud NMEA data.
int gps_txd; // GPS module TXD pin
// software configuration
diff --git a/libraries/SondeLib/TFT22_ILI9225.cpp b/libraries/SondeLib/TFT22_ILI9225.cpp
index 1bd3762..45bc551 100644
--- a/libraries/SondeLib/TFT22_ILI9225.cpp
+++ b/libraries/SondeLib/TFT22_ILI9225.cpp
@@ -198,6 +198,7 @@ TFT22_ILI9225::TFT22_ILI9225(int8_t rst, int8_t rs, int8_t cs, int8_t sdi, int8_
hwSPI = false;
writeFunctionLevel = 0;
gfxFont = NULL;
+ _modeFlip = 0;
}
// Constructor when using software SPI. All output pins are configurable. Adds backlight brightness 0-255
@@ -212,6 +213,7 @@ TFT22_ILI9225::TFT22_ILI9225(int8_t rst, int8_t rs, int8_t cs, int8_t sdi, int8_
hwSPI = false;
writeFunctionLevel = 0;
gfxFont = NULL;
+ _modeFlip = 0;
}
// Constructor when using hardware SPI. Faster, but must use SPI pins
@@ -226,6 +228,7 @@ TFT22_ILI9225::TFT22_ILI9225(int8_t rst, int8_t rs, int8_t cs, int8_t led) {
hwSPI = true;
writeFunctionLevel = 0;
gfxFont = NULL;
+ _modeFlip = 0;
}
// Constructor when using hardware SPI. Faster, but must use SPI pins
@@ -241,6 +244,7 @@ TFT22_ILI9225::TFT22_ILI9225(int8_t rst, int8_t rs, int8_t cs, int8_t led, uint8
hwSPI = true;
writeFunctionLevel = 0;
gfxFont = NULL;
+ _modeFlip = 0;
}
@@ -342,10 +346,12 @@ void TFT22_ILI9225::begin()
endWrite();
delay(50);
+#define OCTLFLIP(m) ((m&0xff)<<8)
+#define EMODEFLIP(m) ((m>>8)<<3)
startWrite();
- _writeRegister(ILI9225_DRIVER_OUTPUT_CTRL, 0x011C); // set the display line number and display direction
+ _writeRegister(ILI9225_DRIVER_OUTPUT_CTRL, OCTLFLIP(_modeFlip) ^ 0x011C); // set the display line number and display direction
_writeRegister(ILI9225_LCD_AC_DRIVING_CTRL, 0x0100); // set 1 line inversion
- _writeRegister(ILI9225_ENTRY_MODE, 0x1038); // set GRAM write direction and BGR=1.
+ _writeRegister(ILI9225_ENTRY_MODE, EMODEFLIP(_modeFlip) ^ 0x1038); // set GRAM write direction and BGR=1.
_writeRegister(ILI9225_DISP_CTRL1, 0x0000); // Display off
_writeRegister(ILI9225_BLANK_PERIOD_CTRL1, 0x0808); // set the back porch and front porch
_writeRegister(ILI9225_FRAME_CYCLE_CTRL, 0x1100); // set the clocks number per line
@@ -482,7 +488,7 @@ void TFT22_ILI9225::_setWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y
startWrite();
// autoincrement mode
if ( _orientation > 0 ) mode = modeTab[_orientation-1][mode];
- _writeRegister(ILI9225_ENTRY_MODE, 0x1000 | ( mode<<3) );
+ _writeRegister(ILI9225_ENTRY_MODE, EMODEFLIP(_modeFlip) ^ (0x1000 | ( mode<<3)) );
_writeRegister(ILI9225_HORIZONTAL_WINDOW_ADDR1,x1);
_writeRegister(ILI9225_HORIZONTAL_WINDOW_ADDR2,x0);
@@ -582,6 +588,9 @@ void TFT22_ILI9225::setDisplay(boolean flag) {
}
}
+void TFT22_ILI9225::setModeFlip(uint16_t m) {
+ _modeFlip = m;
+}
void TFT22_ILI9225::setOrientation(uint8_t orientation) {
diff --git a/libraries/SondeLib/TFT22_ILI9225.h b/libraries/SondeLib/TFT22_ILI9225.h
index 2cdab23..ce4acd3 100644
--- a/libraries/SondeLib/TFT22_ILI9225.h
+++ b/libraries/SondeLib/TFT22_ILI9225.h
@@ -393,6 +393,8 @@ class TFT22_ILI9225 {
void getGFXCharExtent(uint8_t c, int16_t *gw, int16_t *gh, int16_t *xa);
+ void setModeFlip(uint16_t m);
+
private:
void _spiWrite(uint8_t v);
@@ -437,6 +439,7 @@ class TFT22_ILI9225 {
#endif
uint8_t _orientation, _brightness;
+ uint16_t _modeFlip;
// correspondig modes if orientation changed:
const autoIncMode_t modeTab [3][8] = {