diff --git a/lib/obp60task/OBP60Formater.cpp b/lib/obp60task/OBP60Formater.cpp index 8545088..6e7e56e 100644 --- a/lib/obp60task/OBP60Formater.cpp +++ b/lib/obp60task/OBP60Formater.cpp @@ -14,7 +14,7 @@ FormatedData formatValue(GwApi::BoatValue *value, CommonData &commondata){ static int dayoffset = 0; // Load configuration values - int timeZone = commondata.config->getInt(commondata.config->timeZone); // [UTC -12...+12] + int timeZone = commondata.config->getInt(commondata.config->timeZone); // [UTC -12...+14] String lengthFormat = commondata.config->getString(commondata.config->lengthFormat); // [m|ft] String distanceFormat = commondata.config->getString(commondata.config->distanceFormat); // [m|km|nm] String speedFormat = commondata.config->getString(commondata.config->speedFormat); // [m/s|km/h|kn] diff --git a/lib/obp60task/OBPSensorTask.cpp b/lib/obp60task/OBPSensorTask.cpp new file mode 100644 index 0000000..a31bccf --- /dev/null +++ b/lib/obp60task/OBPSensorTask.cpp @@ -0,0 +1,371 @@ +#ifdef BOARD_NODEMCU32S_OBP60 +#include // Adafruit Lib for sensors +#include // Adafruit Lib for BME280 +#include // Adafruit Lib for BMP280 +#include // Adafruit Lib for BMP085 and BMP180 +#include // Lib for SHT21/HTU21 +#include +#include // Timer Lib for timer interrupts +#include "OBPSensorTask.h" +#include "OBP60Hardware.h" +#include "N2kMessages.h" +#include "NMEA0183.h" +#include "ObpNmea0183.h" +#include "OBP60ExtensionPort.h" + +// Timer Interrupts for hardware functions +void underVoltageDetection(); +Ticker Timer1(underVoltageDetection, 1); // Start Timer1 with maximum speed with 1ms +Ticker Timer2(blinkingFlashLED, 500); // Satrt Timer2 for flash LED all 500ms + +// Undervoltage function for shutdown display +void underVoltageDetection(){ + float actVoltage = (float(analogRead(OBP_ANALOG0)) * 3.3 / 4096 + 0.17) * 20; // V = 1/20 * Vin + long starttime = 0; + static bool undervoltage = false; + + if(actVoltage < MIN_VOLTAGE){ + if(undervoltage == false){ + starttime = millis(); + undervoltage = true; + } + if(millis() > starttime + POWER_FAIL_TIME){ + Timer1.stop(); // Stop Timer1 + setPortPin(OBP_BACKLIGHT_LED, false); // Backlight Off + setPortPin(OBP_FLASH_LED, false); // Flash LED Off + setPortPin(OBP_POWER_33, false); // Power rail 3.3V Off + buzzer(TONE4, 20); // Buzzer tone 4kHz 20ms + setPortPin(OBP_POWER_50, false); // Power rail 5.0V Off + // Shutdown EInk display + display.fillRect(0, 0, GxEPD_WIDTH, GxEPD_HEIGHT, GxEPD_WHITE); // Draw white sreen + display.updateWindow(0, 0, GxEPD_WIDTH, GxEPD_HEIGHT, false); // Partial update + display.update(); + // Stop system + while(true){ + esp_deep_sleep_start(); // Deep Sleep without weakup. Weakup only after power cycle (restart). + } + } + } + else{ + undervoltage = false; + } +} + +// Initialization for all sensors (RS232, I2C, 1Wire, IOs) +//#################################################################################### + +void sensorTask(void *param){ + SharedData *shared = (SharedData *)param; + GwApi *api = shared->api; + GwLog *logger = api->getLogger(); + LOG_DEBUG(GwLog::LOG, "Sensor task started"); + SensorData sensors; + ObpNmea0183 NMEA0183; + + Adafruit_BME280 bme280; // Evironment sensor BME280 + Adafruit_BMP280 bmp280; // Evironment sensor BMP280 + Adafruit_BMP085 bmp085; // Evironment sensor BMP085 and BMP180 + HTU21D sht21(HTU21D_RES_RH12_TEMP14); // Environment sensor SHT21 and HTU21 + AMS_5600 as5600; // Rotation sensor AS5600 + + // Init sensor stuff + bool gps_ready = false; // GPS initialized and ready to use + bool BME280_ready = false; // BME280 initialized and ready to use + bool BMP280_ready = false; // BMP280 initialized and ready to use + bool BMP180_ready = false; // BMP180 initialized and ready to use + bool SHT21_ready = false; // SHT21 initialized and ready to use + bool AS5600_ready = false; // AS5600 initialized and ready to use + + // Start timer interrupts + bool uvoltage = api->getConfig()->getConfigItem(api->getConfig()->underVoltage,true)->asBoolean(); + if(uvoltage == true){ + Timer1.start(); // Start Timer1 for undervoltage detection + } + Timer2.start(); // Start Timer2 for blinking LED + + // Settings for NMEA0183 + String nmea0183Mode = api->getConfig()->getConfigItem(api->getConfig()->serialDirection, true)->asString(); + api->getLogger()->logDebug(GwLog::DEBUG, "NMEA0183 Mode is: %s", nmea0183Mode); + pinMode(OBP_DIRECTION_PIN, OUTPUT); + if (String(nmea0183Mode) == "receive" || String(nmea0183Mode) == "off") + { + digitalWrite(OBP_DIRECTION_PIN, false); + } + if (String(nmea0183Mode) == "send") + { + digitalWrite(OBP_DIRECTION_PIN, true); + } + + // Setting for GPS sensors + String gpsOn=api->getConfig()->getConfigItem(api->getConfig()->useGPS,true)->asString(); + if(String(gpsOn) == "NEO-6M"){ + Serial2.begin(9600, SERIAL_8N1, OBP_GPS_TX, -1); // GPS RX unused in hardware (-1) + if (!Serial2) { + api->getLogger()->logDebug(GwLog::ERROR,"GPS modul NEO-6M not found, check wiring"); + gps_ready = false; + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"GPS modul NEO-M6 found"); + NMEA0183.SetMessageStream(&Serial2); + NMEA0183.Open(); + gps_ready = true; + } + } + if(String(gpsOn) == "NEO-M8N"){ + Serial2.begin(9600, SERIAL_8N1, OBP_GPS_TX, -1); // GPS RX unused in hardware (-1) + if (!Serial2) { + api->getLogger()->logDebug(GwLog::ERROR,"GPS modul NEO-M8N not found, check wiring"); + gps_ready = false; + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"GPS modul NEO-M8N found"); + NMEA0183.SetMessageStream(&Serial2); + NMEA0183.Open(); + gps_ready = true; + } + } + + // Settings for temperature sensors + String tempSensor = api->getConfig()->getConfigItem(api->getConfig()->useTempSensor,true)->asString(); + if(String(tempSensor) == "DS18B20"){ + api->getLogger()->logDebug(GwLog::DEBUG,"1Wire Mode is On"); + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"1Wire Mode is Off"); + } + + // Settings for environment sensors on I2C bus + String envSensors=api->getConfig()->getConfigItem(api->getConfig()->useEnvSensor,true)->asString(); + + if(String(envSensors) == "BME280"){ + if (!bme280.begin(BME280_I2C_ADDR)) { + api->getLogger()->logDebug(GwLog::ERROR,"Modul BME280 not found, check wiring"); + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"Modul BME280 found"); + sensors.airTemperature = bme280.readTemperature(); + sensors.airPressure = bme280.readPressure()/100; + sensors.airHumidity = bme280.readHumidity(); + BME280_ready = true; + } + } + else if(String(envSensors) == "BMP280"){ + if (!bmp280.begin(BMP280_I2C_ADDR)) { + api->getLogger()->logDebug(GwLog::ERROR,"Modul BMP280 not found, check wiring"); + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"Modul BMP280 found"); + sensors.airTemperature = bmp280.readTemperature(); + sensors.airPressure =bmp280.readPressure()/100; + BMP280_ready = true; + } + } + else if(String(envSensors) == "BMP085" || String(envSensors) == "BMP180"){ + if (!bmp085.begin()) { + api->getLogger()->logDebug(GwLog::ERROR,"Modul BMP085/BMP180 not found, check wiring"); + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"Modul BMP085/BMP180 found"); + sensors.airTemperature = bmp085.readTemperature(); + sensors.airPressure =bmp085.readPressure()/100; + BMP180_ready = true; + } + } + else if(String(envSensors) == "HTU21" || String(envSensors) == "SHT21"){ + if (!sht21.begin()) { + api->getLogger()->logDebug(GwLog::ERROR,"Modul HTU21/SHT21 not found, check wiring"); + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"Modul HTU21/SHT21 found"); + sensors.airHumidity = sht21.readCompensatedHumidity(); + sensors.airTemperature = sht21.readTemperature(); + SHT21_ready = true; + } + } + + // Settings for rotation sensors on I2C bus + String envsensor = api->getConfig()->getConfigItem(api->getConfig()->useEnvSensor, true)->asString(); + String rotsensor = api->getConfig()->getConfigItem(api->getConfig()->useRotSensor, true)->asString(); + String rotfunction = api->getConfig()->getConfigItem(api->getConfig()->rotFunction, true)->asString(); + String rotSensor=api->getConfig()->getConfigItem(api->getConfig()->useRotSensor,true)->asString(); + + if(String(rotSensor) == "AS5600"){ + Wire.beginTransmission(AS5600_I2C_ADDR); + if (Wire.endTransmission() != 0) { + api->getLogger()->logDebug(GwLog::ERROR,"Modul AS5600 not found, check wiring"); + } + else{ + api->getLogger()->logDebug(GwLog::DEBUG,"Modul AS5600 found"); + sensors.rotationAngle = DegToRad(as5600.getRawAngle() * 0.087); // 0...4095 segments = 0.087 degree + //sensors.magnitude = as5600.getMagnitude(); // Magnetic magnitude in [mT] + AS5600_ready = true; + } + } + + + int rotoffset = api->getConfig()->getConfigItem(api->getConfig()->rotOffset,true)->asInt(); + long starttime0 = millis(); + long starttime5 = millis(); // Voltage update all 1s + long starttime6 = millis(); // Environment sensor update all 1s + long starttime7 = millis(); // Rotation sensor update all 100ms + tN2kMsg N2kMsg; + shared->setSensorData(sensors); //set initially read values + + // Sensor task loop runs with 10ms + //#################################################################################### + + while (true){ + delay(10); + Timer1.update(); // Update for Timer1 + Timer2.update(); // Update for Timer2 + if (millis() > starttime0 + 100) + { + starttime0 = millis(); + // Send NMEA0183 GPS data on several bus systems all 1000ms + if (gps_ready == true) + { + SNMEA0183Msg NMEA0183Msg; + while (NMEA0183.GetMessageCor(NMEA0183Msg)) + { + api->sendNMEA0183Message(NMEA0183Msg); + } + } + + } + // Read sensors and set values in sensor data + // Send supplay voltage value all 1s + if(millis() > starttime5 + 1000){ + starttime5 = millis(); + sensors.batteryVoltage = (float(analogRead(OBP_ANALOG0)) * 3.3 / 4096 + 0.17) * 20; // Vin = 1/20 + // Send to NMEA200 bus + if(!isnan(sensors.batteryVoltage)){ + SetN2kDCBatStatus(N2kMsg, 0, sensors.batteryVoltage, N2kDoubleNA, N2kDoubleNA, 1); + api->sendN2kMessage(N2kMsg); + } + } + + // Send data from environment sensor all 1s + if(millis() > starttime6 + 2000){ + starttime6 = millis(); + unsigned char TempSource = 2; // Inside temperature + unsigned char PressureSource = 0; // Atmospheric pressure + unsigned char HumiditySource=0; // Inside humidity + LOG_DEBUG(GwLog::LOG,"Ready status BME280 %d", BME280_ready); + if(envsensor == "BME280" && BME280_ready == true){ + sensors.airTemperature = bme280.readTemperature(); + sensors.airPressure = bme280.readPressure()/100; + sensors.airHumidity = bme280.readHumidity(); + // Send to NMEA200 bus + if(!isnan(sensors.airTemperature)){ + SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(sensors.airTemperature), N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + if(!isnan(sensors.airHumidity)){ + SetN2kPGN130313(N2kMsg, 0, 0,(tN2kHumiditySource) HumiditySource, sensors.airHumidity, N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + if(!isnan(sensors.airPressure)){ + SetN2kPGN130314(N2kMsg, 0, 0, (tN2kPressureSource) mBarToPascal(PressureSource), sensors.airPressure); + api->sendN2kMessage(N2kMsg); + } + } + else if(envsensor == "BMP280" && BMP280_ready == true){ + sensors.airTemperature = bmp280.readTemperature(); + sensors.airPressure =bmp280.readPressure()/100; + // Send to NMEA200 bus + if(!isnan(sensors.airTemperature)){ + SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(sensors.airTemperature), N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + if(!isnan(sensors.airPressure)){ + SetN2kPGN130314(N2kMsg, 0, 0, (tN2kPressureSource) mBarToPascal(PressureSource), sensors.airPressure); + api->sendN2kMessage(N2kMsg); + } + } + else if((envsensor == "BMP085" || envsensor == "BMP180") && BMP180_ready == true){ + sensors.airTemperature = bmp085.readTemperature(); + sensors.airPressure =bmp085.readPressure()/100; + // Send to NMEA200 bus + if(!isnan(sensors.airTemperature)){ + SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(sensors.airTemperature), N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + if(!isnan(sensors.airPressure)){ + SetN2kPGN130314(N2kMsg, 0, 0, (tN2kPressureSource) mBarToPascal(PressureSource), sensors.airPressure); + api->sendN2kMessage(N2kMsg); + } + } + else if((envsensor == "SHT21" || envsensor == "HTU21") && SHT21_ready == true){ + sensors.airHumidity = sht21.readCompensatedHumidity(); + sensors.airHumidity = sht21.readTemperature(); + // Send to NMEA200 bus + if(!isnan(sensors.airTemperature)){ + SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(sensors.airTemperature), N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + if(!isnan(sensors.airHumidity)){ + SetN2kPGN130313(N2kMsg, 0, 0,(tN2kHumiditySource) HumiditySource, sensors.airHumidity, N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + } + } + + // Send rotation angle all 1000ms + if(millis() > starttime7 + 500){ + starttime7 = millis(); + LOG_DEBUG(GwLog::LOG,"Rotation sensor"); + double rotationAngle=0; + if(String(rotsensor) == "AS5600" && AS5600_ready == true && as5600.detectMagnet() == 1){ + rotationAngle = as5600.getRawAngle() * 0.087; // 0...4095 segments = 0.087 degree + // Offset correction + if(rotoffset >= 0){ + rotationAngle = rotationAngle + rotoffset; + rotationAngle = int(rotationAngle) % 360; + } + else{ + rotationAngle = rotationAngle + 360 + rotoffset; + rotationAngle = int(rotationAngle) % 360; + } + // Send to NMEA200 bus as rudder angle values + if(!isnan(rotationAngle) && String(rotfunction) == "Rudder"){ + double rudder = rotationAngle - 180; // Center position is 180° + // Rudder limits to +/-45° + if(rudder < -45){ + rudder = -45; + } + if(rudder > 45){ + rudder = 45; + } + SetN2kRudder(N2kMsg, DegToRad(rudder), 0, N2kRDO_NoDirectionOrder, PI); + api->sendN2kMessage(N2kMsg); + } + // Send to NMEA200 bus as wind angle values + if(!isnan(rotationAngle) && String(rotfunction) == "Wind"){ + SetN2kWindSpeed(N2kMsg, 1, 0, DegToRad(rotationAngle), N2kWind_Apprent); + api->sendN2kMessage(N2kMsg); + } + // Send to NMEA200 bus as trim angle values in [%] + if(!isnan(rotationAngle) && (String(rotfunction) == "Mast" || String(rotfunction) == "Keel" || String(rotfunction) == "Trim" || String(rotfunction) == "Boom")){ + int trim = rotationAngle * 100 / 360; // 0...360° -> 0...100% + SetN2kTrimTab(N2kMsg, trim, trim); + api->sendN2kMessage(N2kMsg); + } + sensors.rotationAngle = DegToRad(rotationAngle); // Data take over to page + sensors.validRotAngle = true; // Valid true, magnet present + } + else{ + sensors.rotationAngle = 0; // Center position 0° + sensors.validRotAngle = false; // Valid false, magnet missing + } + } + + shared->setSensorData(sensors); + } + vTaskDelete(NULL); +} + + +void createSensorTask(SharedData *shared){ + xTaskCreate(sensorTask,"readSensors",4000,shared,3,NULL); +} +#endif \ No newline at end of file diff --git a/lib/obp60task/OBPSensorTask.h b/lib/obp60task/OBPSensorTask.h new file mode 100644 index 0000000..69e8f54 --- /dev/null +++ b/lib/obp60task/OBPSensorTask.h @@ -0,0 +1,28 @@ +#pragma once +#include "GwSynchronized.h" +#include "GwApi.h" +#include "freertos/semphr.h" +#include "Pagedata.h" + +class SharedData{ + private: + SemaphoreHandle_t locker; + SensorData sensors; + public: + GwApi *api=NULL; + SharedData(GwApi *api){ + locker=xSemaphoreCreateMutex(); + this->api=api; + } + void setSensorData(SensorData &values){ + GWSYNCHRONIZED(&locker); + sensors=values; + } + SensorData getSensorData(){ + GWSYNCHRONIZED(&locker); + return sensors; + } +}; + +void createSensorTask(SharedData *shared); + diff --git a/lib/obp60task/PageVoltage.cpp b/lib/obp60task/PageVoltage.cpp index 8610535..7f0b1a9 100644 --- a/lib/obp60task/PageVoltage.cpp +++ b/lib/obp60task/PageVoltage.cpp @@ -35,7 +35,7 @@ public: // Get voltage value String name1 = "VBat"; - double value1 = (float(analogRead(OBP_ANALOG0)) * 3.3 / 4096 + 0.17) * 20; // Vin = 1/20 + double value1 = commonData.data.batteryVoltage; // Live supplay voltage bool valid1 = true; // Optical warning by limit violation diff --git a/lib/obp60task/obp60task.cpp b/lib/obp60task/obp60task.cpp index 8b00ed8..d2eda8c 100644 --- a/lib/obp60task/obp60task.cpp +++ b/lib/obp60task/obp60task.cpp @@ -2,16 +2,8 @@ #include "obp60task.h" #include "Pagedata.h" #include "OBP60Hardware.h" // PIN definitions - -#include // Timer Lib for timer interrupts #include // I2C connections #include // MCP23017 extension Port -#include // Adafruit Lib for sensors -#include // Adafruit Lib for BME280 -#include // Adafruit Lib for BMP280 -#include // Adafruit Lib for BMP085 and BMP180 -#include // Lib for SHT21/HTU21 -#include // Lib for AS5600 magnetic rotation sensor #include // NMEA2000 #include #include // NMEA0183 @@ -29,85 +21,17 @@ #include "MFD_OBP60_400x300_sw.h" // MFD with logo #include "Logo_OBP_400x300_sw.h" // OBP Logo #include "OBP60QRWiFi.h" // Functions lib for WiFi QR code - -#include "ObpNmea0183.h" // Bugfix lib for NMEA0183 -#include "GwNmea0183Msg.h" // Bugfix lib for NMEA0183 - -ObpNmea0183 NMEA0183; // Use new Lib for NMEA0183 -// tNMEA0183 NMEA0183; // Use old lib with problems for NMEA0183 - -Adafruit_BME280 bme280; // Evironment sensor BME280 -Adafruit_BMP280 bmp280; // Evironment sensor BMP280 -Adafruit_BMP085 bmp085; //Evironment sensor BMP085 and BMP180 -HTU21D sht21(HTU21D_RES_RH12_TEMP14); // Environment sensor SHT21 and HTU21 -AMS_5600 as5600; // Magnetic rotation sensor +#include "OBPSensorTask.h" // Functions lib for sensor data // Global vars bool initComplete = false; // Initialization complete int taskRunCounter = 0; // Task couter for loop section -bool gps_ready = false; // GPS initialized and ready to use -bool BME280_ready = false; // BME280 initialized and ready to use -bool BMP280_ready = false; // BMP280 initialized and ready to use -bool BMP180_ready = false; // BMP180 initialized and ready to use -bool SHT21_ready = false; // SHT21 initialized and ready to use -bool AS5600_ready = false; // AS5600 initialized and ready to use - -double airhumidity = 0; // Air Humitity value from environment sensor -double airtemperature = 0; // Air Temperature value from environment sensor -double airpressure = 0; // Air pressure value from environment sensor -double rotationangle = 0; // Rotaion sensor angle -double magnitude = 0; // Rotation sensor magnetic magnitude in [mT] - -// Timer Interrupts for hardware functions -void underVoltageDetection(); -Ticker Timer1(underVoltageDetection, 1); // Start Timer1 with maximum speed with 1ms -Ticker Timer2(blinkingFlashLED, 500); // Satrt Timer2 for flash LED all 500ms - -// Undervoltage function for shutdown display -void underVoltageDetection(){ - float actVoltage = (float(analogRead(OBP_ANALOG0)) * 3.3 / 4096 + 0.17) * 20; // V = 1/20 * Vin - long starttime = 0; - static bool undervoltage = false; - - if(actVoltage < MIN_VOLTAGE){ - if(undervoltage == false){ - starttime = millis(); - undervoltage = true; - } - if(millis() > starttime + POWER_FAIL_TIME){ - Timer1.stop(); // Stop Timer1 - setPortPin(OBP_BACKLIGHT_LED, false); // Backlight Off - setPortPin(OBP_FLASH_LED, false); // Flash LED Off - setPortPin(OBP_POWER_33, false); // Power rail 3.3V Off - buzzer(TONE4, 20); // Buzzer tone 4kHz 20ms - setPortPin(OBP_POWER_50, false); // Power rail 5.0V Off - // Shutdown EInk display - display.fillRect(0, 0, GxEPD_WIDTH, GxEPD_HEIGHT, GxEPD_WHITE); // Draw white sreen - display.updateWindow(0, 0, GxEPD_WIDTH, GxEPD_HEIGHT, false); // Partial update - display.update(); - // Stop system - while(true){ - esp_deep_sleep_start(); // Deep Sleep without weakup. Weakup only after power cycle (restart). - } - } - } - else{ - undervoltage = false; - } -} // Hardware initialization before start all services -//################################################## +//#################################################################################### void OBP60Init(GwApi *api){ api->getLogger()->logDebug(GwLog::LOG,"obp60init running"); - - // Start timer interrupts - bool uvoltage = api->getConfig()->getConfigItem(api->getConfig()->underVoltage,true)->asBoolean(); - if(uvoltage == true){ - Timer1.start(); // Start Timer1 for undervoltage detection - } - Timer2.start(); // Start Timer2 for blinking LED - + // Extension port MCP23017 // Check I2C devices MCP23017 Wire.begin(OBP_I2C_SDA, OBP_I2C_SCL); @@ -119,17 +43,6 @@ void OBP60Init(GwApi *api){ else{ // Init extension port MCP23017Init(); - - // Settings for NMEA0183 - String nmea0183Mode = api->getConfig()->getConfigItem(api->getConfig()->serialDirection,true)->asString(); - api->getLogger()->logDebug(GwLog::DEBUG,"NMEA0183 Mode is: %s", nmea0183Mode); - pinMode(OBP_DIRECTION_PIN, OUTPUT); - if(String(nmea0183Mode) == "receive" || String(nmea0183Mode) == "off"){ - digitalWrite(OBP_DIRECTION_PIN, false); - } - if(String(nmea0183Mode) == "send"){ - digitalWrite(OBP_DIRECTION_PIN, true); - } // Settings for backlight String backlightMode = api->getConfig()->getConfigItem(api->getConfig()->backlight,true)->asString(); @@ -150,36 +63,6 @@ void OBP60Init(GwApi *api){ if(String(ledMode) == "Off"){ setBlinkingLED(false); } - - // Start serial stream and take over GPS data stream form internal GPS - String gpsOn=api->getConfig()->getConfigItem(api->getConfig()->useGPS,true)->asString(); - if(String(gpsOn) == "NEO-6M"){ - Serial2.begin(9600, SERIAL_8N1, OBP_GPS_TX, -1); // GPS RX unused in hardware (-1) - if (!Serial2) { - api->getLogger()->logDebug(GwLog::ERROR,"GPS modul NEO-6M not found, check wiring"); - gps_ready = false; - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"GPS modul NEO-M6 found"); - NMEA0183.SetMessageStream(&Serial2); - NMEA0183.Open(); - gps_ready = true; - } - } - if(String(gpsOn) == "NEO-M8N"){ - Serial2.begin(9600, SERIAL_8N1, OBP_GPS_TX, -1); // GPS RX unused in hardware (-1) - if (!Serial2) { - api->getLogger()->logDebug(GwLog::ERROR,"GPS modul NEO-M8N not found, check wiring"); - gps_ready = false; - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"GPS modul NEO-M8N found"); - NMEA0183.SetMessageStream(&Serial2); - NMEA0183.Open(); - gps_ready = true; - } - } - // Marker for init complete // Used in OBP60Task() initComplete = true; @@ -189,81 +72,6 @@ void OBP60Init(GwApi *api){ buzzer(TONE4, 500); } - - // Settings for temperature sensors - String tempSensor = api->getConfig()->getConfigItem(api->getConfig()->useTempSensor,true)->asString(); - if(String(tempSensor) == "DS18B20"){ - api->getLogger()->logDebug(GwLog::DEBUG,"1Wire Mode is On"); - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"1Wire Mode is Off"); - } - - // Settings for environment sensors on I2C bus - String envSensors=api->getConfig()->getConfigItem(api->getConfig()->useEnvSensor,true)->asString(); - - if(String(envSensors) == "BME280"){ - if (!bme280.begin(BME280_I2C_ADDR)) { - api->getLogger()->logDebug(GwLog::ERROR,"Modul BME280 not found, check wiring"); - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"Modul BME280 found"); - airtemperature = bme280.readTemperature(); - airpressure = bme280.readPressure()/100; - airhumidity = bme280.readHumidity(); - BME280_ready = true; - } - } - else if(String(envSensors) == "BMP280"){ - if (!bmp280.begin(BMP280_I2C_ADDR)) { - api->getLogger()->logDebug(GwLog::ERROR,"Modul BMP280 not found, check wiring"); - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"Modul BMP280 found"); - airtemperature = bmp280.readTemperature(); - airpressure =bmp280.readPressure()/100; - BMP280_ready = true; - } - } - else if(String(envSensors) == "BMP085" || String(envSensors) == "BMP180"){ - if (!bmp085.begin()) { - api->getLogger()->logDebug(GwLog::ERROR,"Modul BMP085/BMP180 not found, check wiring"); - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"Modul BMP085/BMP180 found"); - airtemperature = bmp085.readTemperature(); - airpressure =bmp085.readPressure()/100; - BMP180_ready = true; - } - } - else if(String(envSensors) == "HTU21" || String(envSensors) == "SHT21"){ - if (!sht21.begin()) { - api->getLogger()->logDebug(GwLog::ERROR,"Modul HTU21/SHT21 not found, check wiring"); - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"Modul HTU21/SHT21 found"); - airhumidity = sht21.readCompensatedHumidity(); - airtemperature = sht21.readTemperature(); - SHT21_ready = true; - } - } - - // Settings for rotation sensors on I2C bus - String rotSensor=api->getConfig()->getConfigItem(api->getConfig()->useRotSensor,true)->asString(); - - if(String(rotSensor) == "AS5600"){ - Wire.beginTransmission(AS5600_I2C_ADDR); - if (Wire.endTransmission() != 0) { - api->getLogger()->logDebug(GwLog::ERROR,"Modul AS5600 not found, check wiring"); - } - else{ - api->getLogger()->logDebug(GwLog::DEBUG,"Modul AS5600 found"); - rotationangle = as5600.getRawAngle() * 0.087; // 0...4095 segments = 0.087 degree - magnitude = as5600.getMagnitude(); // Magnetic magnitude in [mT] - AS5600_ready = true; - } - } - } typedef struct { @@ -273,7 +81,7 @@ typedef struct { } MyData; // Keyboard Task -//####################################### +//#################################################################################### int readKeypad(); void keyboardTask(void *param){ @@ -400,7 +208,7 @@ void registerAllPages(PageList &list){ } // OBP60 Task -//####################################### +//#################################################################################### void OBP60Task(GwApi *api){ GwLog *logger=api->getLogger(); GwConfigHandler *config=api->getConfig(); @@ -517,19 +325,15 @@ void OBP60Task(GwApi *api){ allParameters.page0=3; allParameters.queue=xQueueCreate(10,sizeof(int)); xTaskCreate(keyboardTask,"keyboard",2000,&allParameters,configMAX_PRIORITIES-1,NULL); - + SharedData *shared=new SharedData(api); + createSensorTask(shared); // Task Loop - //############################### + //#################################################################################### // Configuration values for main loop String gpsFix = api->getConfig()->getConfigItem(api->getConfig()->flashLED,true)->asString(); - String gps = api->getConfig()->getConfigItem(api->getConfig()->useGPS,true)->asString(); String backlight = api->getConfig()->getConfigItem(api->getConfig()->backlight,true)->asString(); - String envsensor = api->getConfig()->getConfigItem(api->getConfig()->useEnvSensor,true)->asString(); - String rotsensor = api->getConfig()->getConfigItem(api->getConfig()->useRotSensor,true)->asString(); - String rotfunction = api->getConfig()->getConfigItem(api->getConfig()->rotFunction,true)->asString(); - int rotoffset = api->getConfig()->getConfigItem(api->getConfig()->rotOffset,true)->asInt(); // refreshmode defined in init section // displaycolor defined in init section @@ -542,14 +346,10 @@ void OBP60Task(GwApi *api){ GwApi::BoatValue *date = boatValues.findValueOrCreate("GPSD"); // Load GpsDate GwApi::BoatValue *time = boatValues.findValueOrCreate("GPST"); // Load GpsTime - // Internal sensor values - double batteryVoltage = 0; - LOG_DEBUG(GwLog::LOG,"obp60task: start mainloop"); int pageNumber=0; int lastPage=pageNumber; - commonData.data.actpage = pageNumber + 1; - commonData.data.maxpage = numPages; + commonData.time = boatValues.findValueOrCreate("GPST"); // Load GpsTime commonData.date = boatValues.findValueOrCreate("GPSD"); // Load GpsTime bool delayedDisplayUpdate = false; // If select a new pages then make a delayed full display update @@ -559,29 +359,20 @@ void OBP60Task(GwApi *api){ long starttime2 = millis(); // Full display refresh after 5 min long starttime3 = millis(); // Display update all 1s long starttime4 = millis(); // Delayed display update after 4s when select a new page - long starttime5 = millis(); // Voltage update all 1s - long starttime6 = millis(); // Environment sensor update all 1s - long starttime7 = millis(); // Rotation sensor update all 100ms + + // Main loop runs with 100ms + //#################################################################################### + while (true){ - delay(100); // Fix the problem with NMEA0183 and GPS sentences - Timer1.update(); // Update for Timer1 - Timer2.update(); // Update for Timer2 - LOG_DEBUG(GwLog::LOG,"Loop"); + delay(100); // Delay 100ms (loop time) if(millis() > starttime0 + 100){ starttime0 = millis(); - - // Send NMEA0183 GPS data on several bus systems all 1000ms - if(String(gps) == "NEO-6M" || String(gps) == "NEO-M8N"){ // If config enabled - if(gps_ready == true){ - SNMEA0183Msg NMEA0183Msg; - while(NMEA0183.GetMessageCor(NMEA0183Msg)){ - api->sendNMEA0183Message(NMEA0183Msg); - } - } - } - + commonData.data=shared->getSensorData(); + commonData.data.actpage = pageNumber + 1; + commonData.data.maxpage = numPages; + // If GPS fix then LED off (HDOP) if(String(gpsFix) == "GPS Fix Lost" && hdop->valid == true && int(hdop->value) <= 50){ setPortPin(OBP_FLASH_LED, false); @@ -630,14 +421,7 @@ void OBP60Task(GwApi *api){ commonData.data.actpage = pageNumber + 1; commonData.data.maxpage = numPages; } -/* - // #9 or #10 Refresh display befor start a new page if reshresh is enabled - if(refreshmode == true && (keyboardMessage == 9 || keyboardMessage == 10)){ - display.fillRect(0, 0, GxEPD_WIDTH, GxEPD_HEIGHT, GxEPD_WHITE); // Draw white sreen - display.updateWindow(0, 0, GxEPD_WIDTH, GxEPD_HEIGHT, true); // Needs partial update before full update to refresh the frame buffer - display.update(); // Full update - } -*/ + // #9 or #10 Refresh display after a new page after 4s waiting time and if refresh is disabled if(refreshmode == true && (keyboardMessage == 9 || keyboardMessage == 10)){ starttime4 = millis(); @@ -668,141 +452,7 @@ void OBP60Task(GwApi *api){ display.update(); // Full update } - // Send supplay voltage value all 1s - if(millis() > starttime5 + 1000){ - starttime5 = millis(); - batteryVoltage = (float(analogRead(OBP_ANALOG0)) * 3.3 / 4096 + 0.17) * 20; // Vin = 1/20 - commonData.data.batteryVoltage = batteryVoltage; // Data take over to page - // Send to NMEA200 bus - if(!isnan(batteryVoltage)){ - SetN2kDCBatStatus(N2kMsg, 0, batteryVoltage, N2kDoubleNA, N2kDoubleNA, 1); - api->sendN2kMessage(N2kMsg); - } - } - - // Send data from environment sensor all 1s - if(millis() > starttime6 + 2000){ - starttime6 = millis(); - unsigned char TempSource = 2; // Inside temperature - unsigned char PressureSource = 0; // Atmospheric pressure - unsigned char HumiditySource=0; // Inside humidity - LOG_DEBUG(GwLog::LOG,"Ready status BME280 %d", BME280_ready); - if(envsensor == "BME280" && BME280_ready == true){ - airtemperature = bme280.readTemperature(); - commonData.data.airTemperature = airtemperature; // Data take over to page - airpressure = bme280.readPressure()/100; - commonData.data.airPressure = airpressure; // Data take over to page - airhumidity = bme280.readHumidity(); - commonData.data.airHumidity = airhumidity; // Data take over to page - // Send to NMEA200 bus - if(!isnan(airtemperature)){ - SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(airtemperature), N2kDoubleNA); - api->sendN2kMessage(N2kMsg); - } - if(!isnan(airhumidity)){ - SetN2kPGN130313(N2kMsg, 0, 0,(tN2kHumiditySource) HumiditySource, airhumidity, N2kDoubleNA); - api->sendN2kMessage(N2kMsg); - } - if(!isnan(airpressure)){ - SetN2kPGN130314(N2kMsg, 0, 0, (tN2kPressureSource) mBarToPascal(PressureSource), airpressure); - api->sendN2kMessage(N2kMsg); - } - } - else if(envsensor == "BMP280" && BMP280_ready == true){ - airtemperature = bmp280.readTemperature(); - commonData.data.airTemperature = airtemperature; // Data take over to page - airpressure =bmp280.readPressure()/100; - commonData.data.airPressure = airpressure; // Data take over to page - // Send to NMEA200 bus - if(!isnan(airtemperature)){ - SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(airtemperature), N2kDoubleNA); - api->sendN2kMessage(N2kMsg); - } - if(!isnan(airpressure)){ - SetN2kPGN130314(N2kMsg, 0, 0, (tN2kPressureSource) mBarToPascal(PressureSource), airpressure); - api->sendN2kMessage(N2kMsg); - } - } - else if((envsensor == "BMP085" || envsensor == "BMP180") && BMP180_ready == true){ - airtemperature = bmp085.readTemperature(); - commonData.data.airTemperature = airtemperature; // Data take over to page - airpressure =bmp085.readPressure()/100; - commonData.data.airPressure = airpressure; // Data take over to page - // Send to NMEA200 bus - if(!isnan(airtemperature)){ - SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(airtemperature), N2kDoubleNA); - api->sendN2kMessage(N2kMsg); - } - if(!isnan(airpressure)){ - SetN2kPGN130314(N2kMsg, 0, 0, (tN2kPressureSource) mBarToPascal(PressureSource), airpressure); - api->sendN2kMessage(N2kMsg); - } - } - else if((envsensor == "SHT21" || envsensor == "HTU21") && SHT21_ready == true){ - airhumidity = sht21.readCompensatedHumidity(); - commonData.data.airHumidity = airhumidity; // Data take over to page - airtemperature = sht21.readTemperature(); - commonData.data.airTemperature = airtemperature; // Data take over to page - // Send to NMEA200 bus - if(!isnan(airtemperature)){ - SetN2kPGN130312(N2kMsg, 0, 0,(tN2kTempSource) TempSource, CToKelvin(airtemperature), N2kDoubleNA); - api->sendN2kMessage(N2kMsg); - } - if(!isnan(airhumidity)){ - SetN2kPGN130313(N2kMsg, 0, 0,(tN2kHumiditySource) HumiditySource, airhumidity, N2kDoubleNA); - api->sendN2kMessage(N2kMsg); - } - } - } - - // Send rotation angle all 1000ms - if(millis() > starttime7 + 500){ - starttime7 = millis(); - LOG_DEBUG(GwLog::LOG,"Rotation sensor"); - if(String(rotsensor) == "AS5600" && AS5600_ready == true && as5600.detectMagnet() == 1){ - rotationangle = as5600.getRawAngle() * 0.087; // 0...4095 segments = 0.087 degree - // Offset correction - if(rotoffset >= 0){ - rotationangle = rotationangle + rotoffset; - rotationangle = int(rotationangle) % 360; - } - else{ - rotationangle = rotationangle + 360 + rotoffset; - rotationangle = int(rotationangle) % 360; - } - commonData.data.rotationAngle = DegToRad(rotationangle); // Data take over to page - commonData.data.validRotAngle = true; // Valid true, magnet present - // Send to NMEA200 bus as rudder angle values - if(!isnan(rotationangle) && String(rotfunction) == "Rudder"){ - double rudder = rotationangle - 180; // Center position is 180° - // Rudder limits to +/-45° - if(rudder < -45){ - rudder = -45; - } - if(rudder > 45){ - rudder = 45; - } - SetN2kRudder(N2kMsg, DegToRad(rudder), 0, N2kRDO_NoDirectionOrder, PI); - api->sendN2kMessage(N2kMsg); - } - // Send to NMEA200 bus as wind angle values - if(!isnan(rotationangle) && String(rotfunction) == "Wind"){ - SetN2kWindSpeed(N2kMsg, 1, 0, DegToRad(rotationangle), N2kWind_Apprent); - api->sendN2kMessage(N2kMsg); - } - // Send to NMEA200 bus as trim angle values in [%] - if(!isnan(rotationangle) && (String(rotfunction) == "Mast" || String(rotfunction) == "Keel" || String(rotfunction) == "Trim" || String(rotfunction) == "Boom")){ - int trim = rotationangle * 100 / 360; // 0...360° -> 0...100% - SetN2kTrimTab(N2kMsg, trim, trim); - api->sendN2kMessage(N2kMsg); - } - } - else{ - commonData.data.rotationAngle = 0; // Center position 0° - commonData.data.validRotAngle = false; // Valid false, magnet missing - } - } - + // Refresh display data all 1s if(millis() > starttime3 + 1000){ starttime3 = millis();