kopia lustrzana https://github.com/dl9rdz/rdz_ttgo_sonde
gps init bugfix, less debug messages, gps for android w/o gps (+GPGST parser), merge kml live view
rodzic
db3f62c243
commit
8df0859f1c
|
@ -27,9 +27,9 @@ int e;
|
|||
|
||||
enum MainState { ST_DECODER, ST_SPECTRUM, ST_WIFISCAN, ST_UPDATE, ST_TOUCHCALIB };
|
||||
static MainState mainState = ST_WIFISCAN; // ST_WIFISCAN;
|
||||
const char *mainStateStr[5] = {"DECODER", "SPECTRUM", "WIFISCAN", "UPDATE", "TOUCHCALIB" };
|
||||
|
||||
AsyncWebServer server(80);
|
||||
AsyncWebSocket ws("/ws");
|
||||
|
||||
AXP20X_Class axp;
|
||||
#define PMU_IRQ 35
|
||||
|
@ -215,7 +215,7 @@ void setupChannelList() {
|
|||
file.close();
|
||||
}
|
||||
|
||||
const char *HTMLHEAD="<html><head> <meta charset=\"UTF-8\"> <link rel=\"stylesheet\" type=\"text/css\" href=\"style.css\">";
|
||||
const char *HTMLHEAD = "<html><head> <meta charset=\"UTF-8\"> <link rel=\"stylesheet\" type=\"text/css\" href=\"style.css\">";
|
||||
void HTMLBODY(char *ptr, const char *which) {
|
||||
strcat(ptr, "<body><form class=\"wrapper\" action=\"");
|
||||
strcat(ptr, which);
|
||||
|
@ -232,33 +232,33 @@ const char *createQRGForm() {
|
|||
char *ptr = message;
|
||||
strcpy(ptr, HTMLHEAD);
|
||||
strcat(ptr, "<script src=\"rdz.js\"/> <script> window.onload = prep; </script></head>");
|
||||
/*
|
||||
strcat(ptr, "<script type=\"text/javascript\">"
|
||||
"let stypes=new Map();"
|
||||
"stypes.set('4', 'RS41');"
|
||||
"stypes.set('R', 'RS92');"
|
||||
"stypes.set('9', 'DFM9 (old)');"
|
||||
"stypes.set('6', 'DFM6 (old)');"
|
||||
"stypes.set('D', 'DFM');"
|
||||
"stypes.set('M', 'M10');"
|
||||
"stypes.set('2', 'M20');"
|
||||
"function prep() {"
|
||||
" var stlist=document.querySelectorAll(\"input.stype\");"
|
||||
" for(txt of stlist){"
|
||||
" var val=txt.getAttribute('value'); var nam=txt.getAttribute('name'); "
|
||||
" var sel=document.createElement('select');"
|
||||
" sel.setAttribute('name',nam);"
|
||||
" for(stype of stypes) { "
|
||||
" var opt=document.createElement('option');"
|
||||
" opt.value=stype[0];"
|
||||
" opt.innerHTML=stype[1];"
|
||||
" if(stype[0]==val) { opt.setAttribute('selected','selected'); }"
|
||||
" sel.appendChild(opt);"
|
||||
" } txt.replaceWith(sel); } } "
|
||||
" window.onload = prep; "
|
||||
"</script>");
|
||||
*/
|
||||
HTMLBODY(ptr, "qrg.html");
|
||||
/*
|
||||
strcat(ptr, "<script type=\"text/javascript\">"
|
||||
"let stypes=new Map();"
|
||||
"stypes.set('4', 'RS41');"
|
||||
"stypes.set('R', 'RS92');"
|
||||
"stypes.set('9', 'DFM9 (old)');"
|
||||
"stypes.set('6', 'DFM6 (old)');"
|
||||
"stypes.set('D', 'DFM');"
|
||||
"stypes.set('M', 'M10');"
|
||||
"stypes.set('2', 'M20');"
|
||||
"function prep() {"
|
||||
" var stlist=document.querySelectorAll(\"input.stype\");"
|
||||
" for(txt of stlist){"
|
||||
" var val=txt.getAttribute('value'); var nam=txt.getAttribute('name'); "
|
||||
" var sel=document.createElement('select');"
|
||||
" sel.setAttribute('name',nam);"
|
||||
" for(stype of stypes) { "
|
||||
" var opt=document.createElement('option');"
|
||||
" opt.value=stype[0];"
|
||||
" opt.innerHTML=stype[1];"
|
||||
" if(stype[0]==val) { opt.setAttribute('selected','selected'); }"
|
||||
" sel.appendChild(opt);"
|
||||
" } txt.replaceWith(sel); } } "
|
||||
" window.onload = prep; "
|
||||
"</script>");
|
||||
*/
|
||||
HTMLBODY(ptr, "qrg.html");
|
||||
//strcat(ptr, "<body><form class=\"wrapper\" action=\"qrg.html\" method=\"post\"><div class=\"content\"><table><tr><th>ID</th><th>Active</th><th>Freq</th><th>Launchsite</th><th>Mode</th></tr>");
|
||||
strcat(ptr, "<table><tr><th>ID</th><th>Active</th><th>Freq</th><th>Launchsite</th><th>Mode</th></tr>");
|
||||
for (int i = 0; i < sonde.config.maxsonde; i++) {
|
||||
|
@ -274,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, "</table>");
|
||||
//</div><div class=\"footer\"><input type=\"submit\" class=\"update\" value=\"Update\"/>");
|
||||
|
@ -456,7 +456,7 @@ void addSondeStatus(char *ptr, int i)
|
|||
sprintf(ptr + strlen(ptr), "<a target=\"_empty\" href=\"https://radiosondy.info/sonde_archive.php?sondenumber=%s\">radiosondy.info</a> - ", s->id);
|
||||
sprintf(ptr + strlen(ptr), "<a target=\"_empty\" href=\"https://www.openstreetmap.org/?mlat=%.6f&mlon=%.6f&zoom=14\">OSM</a> - ", s->lat, s->lon);
|
||||
sprintf(ptr + strlen(ptr), "<a target=\"_empty\" href=\"https://www.google.com/maps/search/?api=1&query=%.6f,%.6f\">Google</a></td></tr>", s->lat, s->lon);
|
||||
|
||||
|
||||
strcat(ptr, "</table><p/>\n");
|
||||
}
|
||||
|
||||
|
@ -545,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},
|
||||
|
@ -742,7 +742,9 @@ const char *createControlForm() {
|
|||
strcat(ptr, "\" value=\"");
|
||||
strcat(ptr, ctrllabel[i]);
|
||||
strcat(ptr, "\"></input>");
|
||||
if(i==3) { strcat(ptr, "<p></p>"); }
|
||||
if (i == 3) {
|
||||
strcat(ptr, "<p></p>");
|
||||
}
|
||||
}
|
||||
HTMLBODYEND(ptr);
|
||||
Serial.printf("Control form: size=%d bytes\n", strlen(message));
|
||||
|
@ -794,40 +796,40 @@ const char *handleControlPost(AsyncWebServerRequest *request) {
|
|||
|
||||
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) {
|
||||
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, "<html><head><title>Editor</title></head><body><p>Edit: ", maxlen);
|
||||
i = strlen(buffer);
|
||||
strncpy(buffer+i, filename.c_str(), maxlen-i);
|
||||
i += strlen(buffer+i);
|
||||
strncpy(buffer+i, "</p><form action=\"edit.html?file=", maxlen-i);
|
||||
i += strlen(buffer+i);
|
||||
strncpy(buffer+i, filename.c_str(), maxlen-i);
|
||||
i += strlen(buffer+i);
|
||||
strncpy(buffer+i, "\" method=\"post\" enctype=\"multipart/form-data\"><textarea name=\"text\" cols=\"80\" rows=\"40\">", maxlen-i);
|
||||
i += strlen(buffer+i);
|
||||
if(i>=maxlen) {
|
||||
strncpy(buffer, "Out of memory", maxlen);
|
||||
state = 3;
|
||||
return strlen(buffer);
|
||||
{
|
||||
// we optimistically assume that on first invocation, maxlen is large enough to handle the header.....
|
||||
strncpy(buffer, "<html><head><title>Editor</title></head><body><p>Edit: ", maxlen);
|
||||
i = strlen(buffer);
|
||||
strncpy(buffer + i, filename.c_str(), maxlen - i);
|
||||
i += strlen(buffer + i);
|
||||
strncpy(buffer + i, "</p><form action=\"edit.html?file=", maxlen - i);
|
||||
i += strlen(buffer + i);
|
||||
strncpy(buffer + i, filename.c_str(), maxlen - i);
|
||||
i += strlen(buffer + i);
|
||||
strncpy(buffer + i, "\" method=\"post\" enctype=\"multipart/form-data\"><textarea name=\"text\" cols=\"80\" rows=\"40\">", maxlen - i);
|
||||
i += strlen(buffer + i);
|
||||
if (i >= maxlen) {
|
||||
strncpy(buffer, "Out of memory", maxlen);
|
||||
state = 3;
|
||||
return strlen(buffer);
|
||||
}
|
||||
state++;
|
||||
Serial.printf("Wrote %d bytes. Header finished", i);
|
||||
return i;
|
||||
break;
|
||||
}
|
||||
state++;
|
||||
Serial.printf("Wrote %d bytes. Header finished", i);
|
||||
return i;
|
||||
break;
|
||||
}
|
||||
case 1: // file content
|
||||
while(file.available()) {
|
||||
int cnt = readLine(file, buffer+i, maxlen-i-1);
|
||||
while (file.available()) {
|
||||
int cnt = readLine(file, buffer + i, maxlen - i - 1);
|
||||
i += cnt;
|
||||
buffer[i++] = '\n';
|
||||
buffer[i] = 0;
|
||||
if(i+256 > maxlen) break; // max line length in file 256 chars
|
||||
if (i + 256 > maxlen) break; // max line length in file 256 chars
|
||||
}
|
||||
if(i>0) return i;
|
||||
if (i > 0) return i;
|
||||
file.close();
|
||||
state++; // intentional fall-through
|
||||
case 2: // footer
|
||||
|
@ -837,7 +839,7 @@ int streamEditForm(int &state, File &file, String filename, char *buffer, size_t
|
|||
return strlen(buffer);
|
||||
case 3: // end
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
@ -1021,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");
|
||||
|
@ -1029,6 +1034,7 @@ void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventT
|
|||
Serial.println("Client disconnected");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
const char* PARAM_MESSAGE = "message";
|
||||
|
@ -1099,7 +1105,7 @@ void SetupAsyncServer() {
|
|||
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 {
|
||||
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);
|
||||
});
|
||||
|
@ -1110,7 +1116,7 @@ void SetupAsyncServer() {
|
|||
request->send(200, "text/html", "<html><head>ERROR</head><body><p>Something went wrong. Uploaded file is empty.</p></body></hhtml>");
|
||||
else {
|
||||
String f = request->getParam(0)->value();
|
||||
request->redirect("/edit.html?file="+f);
|
||||
request->redirect("/edit.html?file=" + f);
|
||||
//request->send(200, "text/html", createEditForm(request->getParam(0)->value()));
|
||||
}
|
||||
},
|
||||
|
@ -1122,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);
|
||||
});
|
||||
|
||||
|
@ -1154,10 +1160,6 @@ void SetupAsyncServer() {
|
|||
}
|
||||
});
|
||||
|
||||
// Set up web socket
|
||||
ws.onEvent(onWsEvent);
|
||||
server.addHandler(&ws);
|
||||
|
||||
// Start server
|
||||
server.begin();
|
||||
}
|
||||
|
@ -1168,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;
|
||||
}
|
||||
|
@ -1285,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;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1300,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();
|
||||
|
@ -1329,15 +1353,17 @@ 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_hardreset[]={UBX_SYNCH_1, UBX_SYNCH_2, 0x06, 0x04, 4, 0, 0xff, 0xff, 0, 0, 0x0C, 0x5D };
|
||||
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
|
||||
File testfile = SPIFFS.open("/GPSRESET", FILE_READ);
|
||||
if(testfile && !testfile.isDirectory()) {
|
||||
if (testfile && !testfile.isDirectory()) {
|
||||
testfile.close();
|
||||
Serial.println("GPS factory reset...");
|
||||
Serial2.begin(115200, SERIAL_8N1, sonde.config.gps_rxd, sonde.config.gps_txd);
|
||||
|
@ -1353,12 +1379,15 @@ void initGPS() {
|
|||
Serial2.write(ubx_factorydef, sizeof(ubx_factorydef));
|
||||
delay(100);
|
||||
Serial2.write(ubx_hardreset, sizeof(ubx_hardreset));
|
||||
//delay(5000);
|
||||
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);
|
||||
}
|
||||
|
@ -1369,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:
|
||||
|
@ -1384,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;
|
||||
|
@ -1638,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
|
||||
|
||||
|
@ -1699,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);
|
||||
|
@ -1723,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();
|
||||
}
|
||||
|
@ -1784,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);
|
||||
|
@ -1938,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<RDZ_DATA_LEN; i++) {
|
||||
if(key==NULL) {
|
||||
if(data[i]!='"') continue;
|
||||
key=data+i+1;
|
||||
i+=2;
|
||||
continue;
|
||||
}
|
||||
if(value==NULL) {
|
||||
if(data[i]!=':') continue;
|
||||
value = data+i+1;
|
||||
i += 2;
|
||||
continue;
|
||||
}
|
||||
if(data[i]==',' || data[i]=='}' || data[i]==0) {
|
||||
// get value
|
||||
double val = strtod(value,NULL);
|
||||
// get data
|
||||
if(strncmp(key,"lat",3)==0) { gpsPos.lat = val; }
|
||||
else if(strncmp(key,"lon",3)==0) { gpsPos.lon = val; }
|
||||
else if(strncmp(key,"alt",3)==0) { gpsPos.alt = (int)val; }
|
||||
else if(strncmp(key,"course",6)==0) { gpsPos.course = (int)val; }
|
||||
gpsPos.valid = true;
|
||||
|
||||
// next item:
|
||||
if(data[i]!=',') break;
|
||||
key = NULL;
|
||||
value = NULL;
|
||||
}
|
||||
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 < RDZ_DATA_LEN; i++) {
|
||||
if (key == NULL) {
|
||||
if (data[i] != '"') continue;
|
||||
key = data + i + 1;
|
||||
i += 2;
|
||||
continue;
|
||||
}
|
||||
Serial.printf("Parse result: lat=%f, lon=%f, alt=%d, valid=%d\n", gpsPos.lat, gpsPos.lon, gpsPos.alt, gpsPos.valid);
|
||||
if (value == NULL) {
|
||||
if (data[i] != ':') continue;
|
||||
value = data + i + 1;
|
||||
i += 2;
|
||||
continue;
|
||||
}
|
||||
if (data[i] == ',' || data[i] == '}' || data[i] == 0) {
|
||||
// get value
|
||||
double val = strtod(value, NULL);
|
||||
// get data
|
||||
if (strncmp(key, "lat", 3) == 0) {
|
||||
gpsPos.lat = val;
|
||||
}
|
||||
else if (strncmp(key, "lon", 3) == 0) {
|
||||
gpsPos.lon = val;
|
||||
}
|
||||
else if (strncmp(key, "alt", 3) == 0) {
|
||||
gpsPos.alt = (int)val;
|
||||
}
|
||||
else if (strncmp(key, "course", 6) == 0) {
|
||||
gpsPos.course = (int)val;
|
||||
}
|
||||
gpsPos.valid = true;
|
||||
|
||||
// next item:
|
||||
if (data[i] != ',') break;
|
||||
key = NULL;
|
||||
value = NULL;
|
||||
}
|
||||
}
|
||||
Serial.printf("Parse result: lat=%f, lon=%f, alt=%d, valid=%d\n", gpsPos.lat, gpsPos.lon, gpsPos.alt, gpsPos.valid);
|
||||
}
|
||||
|
||||
static char rdzData[RDZ_DATA_LEN];
|
||||
|
@ -1985,9 +2027,9 @@ void loopDecoder() {
|
|||
// TODO: update displayed sonde?
|
||||
|
||||
if (action != ACT_NONE) {
|
||||
Serial.printf("Loop: triggering action %s (%d)\n", action2text(action), action);
|
||||
action = sonde.updateState(action);
|
||||
Serial.printf("Loop: action is %d, sonde index is %d\n", action, sonde.currentSonde);
|
||||
int newact = sonde.updateState(action);
|
||||
Serial.printf("MAIN: loopDecoder: action %s (%d) => %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);
|
||||
|
@ -1997,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()) {
|
||||
|
@ -2017,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("");
|
||||
|
@ -2073,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();
|
||||
|
@ -2163,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) {
|
||||
|
@ -2251,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);
|
||||
|
@ -2540,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;
|
||||
|
@ -2790,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
|
||||
|
@ -2828,5 +2863,4 @@ void loop() {
|
|||
lastMqttUptime = now;
|
||||
}
|
||||
|
||||
Serial.printf("Unused stack: %d\n", uxTaskGetStackHighWaterMark(0));
|
||||
}
|
||||
|
|
|
@ -113,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=
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
const char *version_name = "rdzTTGOsonde";
|
||||
const char *version_id = "devel20210302";
|
||||
const char *version_id = "devel20210314";
|
||||
const int SPIFFS_MAJOR=2;
|
||||
const int SPIFFS_MINOR=10;
|
||||
|
|
|
@ -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,7 +781,7 @@ 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':
|
||||
|
@ -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);
|
||||
|
|
|
@ -15,6 +15,7 @@ struct GpsPos {
|
|||
double lon;
|
||||
int alt;
|
||||
int course;
|
||||
int accuracy;
|
||||
int valid;
|
||||
};
|
||||
extern struct GpsPos gpsPos;
|
||||
|
|
|
@ -135,8 +135,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;
|
||||
|
||||
|
@ -524,7 +526,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<RS41MAXLEN; i++) { data[i] = reverse(data[i]); }
|
||||
for(int i=0; i<RS41MAXLEN; i++) { data[i] = data[i] ^ scramble[i&0x3F]; }
|
||||
|
|
|
@ -438,17 +438,17 @@ void Sonde::setup() {
|
|||
Serial.print("Invalid rxtask.currentSonde: ");
|
||||
Serial.println(rxtask.currentSonde);
|
||||
rxtask.currentSonde = 0;
|
||||
for(int i=0; i<config.maxsonde - 1; i++) {
|
||||
if(!sondeList[rxtask.currentSonde].active) {
|
||||
rxtask.currentSonde++;
|
||||
if(rxtask.currentSonde>=nSonde) rxtask.currentSonde=0;
|
||||
}
|
||||
}
|
||||
sonde.currentSonde = rxtask.currentSonde;
|
||||
}
|
||||
for(int i=0; i<config.maxsonde - 1; i++) {
|
||||
if(!sondeList[rxtask.currentSonde].active) {
|
||||
rxtask.currentSonde++;
|
||||
if(rxtask.currentSonde>=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:
|
||||
|
@ -468,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;
|
||||
|
@ -508,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;
|
||||
|
@ -524,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...)
|
||||
|
@ -546,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;
|
||||
}
|
||||
|
@ -602,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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
@ -584,7 +588,7 @@ void TFT22_ILI9225::setDisplay(boolean flag) {
|
|||
}
|
||||
}
|
||||
|
||||
void TFT22_ILI9225::setModeFlip(uint8_t m) {
|
||||
void TFT22_ILI9225::setModeFlip(uint16_t m) {
|
||||
_modeFlip = m;
|
||||
}
|
||||
|
||||
|
|
|
@ -393,7 +393,7 @@ class TFT22_ILI9225 {
|
|||
|
||||
void getGFXCharExtent(uint8_t c, int16_t *gw, int16_t *gh, int16_t *xa);
|
||||
|
||||
void setModeFlip(uint8_t m);
|
||||
void setModeFlip(uint16_t m);
|
||||
|
||||
private:
|
||||
|
||||
|
@ -438,7 +438,8 @@ class TFT22_ILI9225 {
|
|||
int8_t _rst, _rs, _cs, _sdi, _clk, _led;
|
||||
#endif
|
||||
|
||||
uint8_t _orientation, _brightness, _modeflip;
|
||||
uint8_t _orientation, _brightness;
|
||||
uint16_t _modeFlip;
|
||||
|
||||
// correspondig modes if orientation changed:
|
||||
const autoIncMode_t modeTab [3][8] = {
|
||||
|
|
Ładowanie…
Reference in New Issue