From 86ab6e91770422da2ece5001670a10fe535b795e Mon Sep 17 00:00:00 2001 From: norbert-walter Date: Thu, 17 Mar 2022 22:04:23 +0100 Subject: [PATCH] Backup --- lib/obp60task/AS5600.cpp | 460 ++++++++++++++++++++++++++++++++++++ lib/obp60task/AS5600.h | 91 +++++++ lib/obp60task/Pagedata.h | 1 + lib/obp60task/config.json | 8 +- lib/obp60task/obp60task.cpp | 43 +++- 5 files changed, 598 insertions(+), 5 deletions(-) create mode 100644 lib/obp60task/AS5600.cpp create mode 100644 lib/obp60task/AS5600.h diff --git a/lib/obp60task/AS5600.cpp b/lib/obp60task/AS5600.cpp new file mode 100644 index 0000000..bc206dd --- /dev/null +++ b/lib/obp60task/AS5600.cpp @@ -0,0 +1,460 @@ + +/**************************************************** + AMS 5600 class for Arduino platform + Author: Tom Denton + Date: 15 Dec 2014 + File: AMS_5600.cpp + Version 1.00 + www.ams.com + + Description: This class has been designed to + access the AMS 5600 “potuino” shield. +*****************************************************/ + +// updated jan 2022 by isc - read two bytes together + +// datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf + +#include "Arduino.h" +#include "AS5600.h" +#include "Wire.h" + +/**************************************************** + Method: AMS_5600 + In: none + Out: none + Description: constructor class for AMS 5600 +*****************************************************/ +AMS_5600::AMS_5600() +{ +} + +/* mode = 0, output PWM, mode = 1 output analog (full range from 0% to 100% between GND and VDD */ +void AMS_5600::setOutPut(uint8_t mode) +{ + int _conf_lo = _addr_conf+1; // lower byte address + uint8_t config_status; + config_status = readOneByte(_conf_lo); + if (mode == 1) { + config_status = config_status & 0xcf; + } else { + uint8_t config_status; + config_status = readOneByte(_conf_lo); + if (mode == 1) + config_status = config_status & 0xcf; + else + config_status = config_status & 0xef; + writeOneByte(_conf_lo, lowByte(config_status)); + } +} + +/**************************************************** + Method: AMS_5600 + In: none + Out: i2c address of AMS 5600 + Description: returns i2c address of AMS 5600 +****************************************************/ +int AMS_5600::getAddress() +{ + return _ams5600_Address; +} + +/******************************************************* + Method: setMaxAngle + In: new maximum angle to set OR none + Out: value of max angle register + Description: sets a value in maximum angle register. + If no value is provided, method will read position of + magnet. Setting this register zeros out max position + register. +*******************************************************/ +word AMS_5600::setMaxAngle(word newMaxAngle) +{ + word _maxAngle; + if (newMaxAngle == -1) + _maxAngle = getRawAngle(); + else + _maxAngle = newMaxAngle; + + writeOneByte(_addr_mang, highByte(_maxAngle)); + delay(2); + writeOneByte(_addr_mang+1, lowByte(_maxAngle)); + delay(2); + + word retVal = readTwoBytesSeparately(_addr_mang); + return retVal; +} + +/******************************************************* + Method: getMaxAngle + In: none + Out: value of max angle register + Description: gets value of maximum angle register. +*******************************************************/ +word AMS_5600::getMaxAngle() +{ + return readTwoBytesSeparately(_addr_mang); +} + +/******************************************************* + Method: setStartPosition + In: new start angle position + Out: value of start position register + Description: sets a value in start position register. + If no value is provided, method will read position of + magnet. +*******************************************************/ +word AMS_5600::setStartPosition(word startAngle) +{ + word _rawStartAngle; + if (startAngle == -1) + _rawStartAngle = getRawAngle(); + else + _rawStartAngle = startAngle; + + writeOneByte(_addr_zpos, highByte(_rawStartAngle)); + delay(2); + writeOneByte(_addr_zpos+1, lowByte(_rawStartAngle)); + delay(2); + word _zPosition = readTwoBytesSeparately(_addr_zpos); + + return (_zPosition); +} + +/******************************************************* + Method: getStartPosition + In: none + Out: value of start position register + Description: gets value of start position register. +*******************************************************/ +word AMS_5600::getStartPosition() +{ + return readTwoBytesSeparately(_addr_zpos); +} + +/******************************************************* + Method: setEndtPosition + In: new end angle position + Out: value of end position register + Description: sets a value in end position register. + If no value is provided, method will read position of + magnet. +*******************************************************/ +word AMS_5600::setEndPosition(word endAngle) +{ + word _rawEndAngle; + if (endAngle == -1) + _rawEndAngle = getRawAngle(); + else + _rawEndAngle = endAngle; + + writeOneByte(_addr_mpos, highByte(_rawEndAngle)); + delay(2); + writeOneByte(_addr_mpos+1, lowByte(_rawEndAngle)); + delay(2); + word _mPosition = readTwoBytesSeparately(_addr_mpos); + + return (_mPosition); +} + +/******************************************************* + Method: getEndPosition + In: none + Out: value of end position register + Description: gets value of end position register. +*******************************************************/ +word AMS_5600::getEndPosition() +{ + word retVal = readTwoBytesSeparately(_addr_mpos); + return retVal; +} + +/******************************************************* + Method: getRawAngle + In: none + Out: value of raw angle register + Description: gets raw value of magnet position. + start, end, and max angle settings do not apply +*******************************************************/ +word AMS_5600::getRawAngle() +{ + return readTwoBytesTogether(_addr_raw_angle); +} + +/******************************************************* + Method: getScaledAngle + In: none + Out: value of scaled angle register + Description: gets scaled value of magnet position. + start, end, or max angle settings are used to + determine value +*******************************************************/ +word AMS_5600::getScaledAngle() +{ + return readTwoBytesTogether(_addr_angle); +} + +/******************************************************* + Method: detectMagnet + In: none + Out: 1 if magnet is detected, 0 if not + Description: reads status register and examines the + MH bit +*******************************************************/ +int AMS_5600::detectMagnet() +{ + int magStatus; + int retVal = 0; + /*0 0 MD ML MH 0 0 0*/ + /* MD high = magnet detected*/ + /* ML high = AGC Maximum overflow, magnet to weak*/ + /* MH high = AGC minimum overflow, Magnet to strong*/ + magStatus = readOneByte(_addr_status); + + if (magStatus & 0x20) + retVal = 1; + + return retVal; +} + +/******************************************************* + Method: getMagnetStrength + In: none + Out: 0 if no magnet is detected + 1 if magnet is to weak + 2 if magnet is just right + 3 if magnet is to strong + Description: reads status register andexamins the MH,ML,MD bits +*******************************************************/ +int AMS_5600::getMagnetStrength() +{ + int magStatus; + int retVal = 0; + /*0 0 MD ML MH 0 0 0*/ + /* MD high = magnet detected */ + /* ML high = AGC Maximum overflow, magnet to weak*/ + /* MH high = AGC minimum overflow, Magnet to strong*/ + magStatus = readOneByte(_addr_status); + if (detectMagnet() == 1) { + retVal = 2; /* just right */ + if (magStatus & 0x10) + retVal = 1; /* too weak */ + else if (magStatus & 0x08) + retVal = 3; /* too strong */ + } + + return retVal; +} + +/******************************************************* + Method: get Agc + In: none + Out: value of AGC register + Description: gets value of AGC register. +*******************************************************/ +int AMS_5600::getAgc() +{ + return readOneByte(_addr_agc); +} + +/******************************************************* + Method: getMagnitude + In: none + Out: value of magnitude register + Description: gets value of magnitude register. +*******************************************************/ +word AMS_5600::getMagnitude() +{ + return readTwoBytesTogether(_addr_magnitude); +} + +/******************************************************* + Method: getConf + In: none + Out: value of CONF register + Description: gets value of CONF register. +*******************************************************/ +word AMS_5600::getConf() +{ + return readTwoBytesSeparately(_addr_conf); +} + +/******************************************************* + Method: setConf + In: value of CONF register + Out: none + Description: sets value of CONF register. +*******************************************************/ +void AMS_5600::setConf(word _conf) +{ + writeOneByte(_addr_conf, highByte(_conf)); + delay(2); + writeOneByte(_addr_conf+1, lowByte(_conf)); + delay(2); +} + +/******************************************************* + Method: getBurnCount + In: none + Out: value of zmco register + Description: determines how many times chip has been + permanently written to. +*******************************************************/ +int AMS_5600::getBurnCount() +{ + return readOneByte(_addr_zmco); +} + +/******************************************************* + Method: burnAngle + In: none + Out: 1 success + -1 no magnet + -2 burn limit exceeded + -3 start and end positions not set (useless burn) + Description: burns start and end positions to chip. + THIS CAN ONLY BE DONE 3 TIMES +*******************************************************/ +int AMS_5600::burnAngle() +{ + word _zPosition = getStartPosition(); + word _mPosition = getEndPosition(); + word _maxAngle = getMaxAngle(); + + int retVal = 1; + if (detectMagnet() == 1) { + if (getBurnCount() < 3) { + if ((_zPosition == 0) && (_mPosition == 0)) + retVal = -3; + else + writeOneByte(_addr_burn, 0x80); + } + else + retVal = -2; + } else + retVal = -1; + + return retVal; +} + +/******************************************************* + Method: burnMaxAngleAndConfig + In: none + Out: 1 success + -1 burn limit exceeded + -2 max angle is to small, must be at or above 18 degrees + Description: burns max angle and config data to chip. + THIS CAN ONLY BE DONE 1 TIME +*******************************************************/ +int AMS_5600::burnMaxAngleAndConfig() +{ + word _maxAngle = getMaxAngle(); + + int retVal = 1; + if (getBurnCount() == 0) { + if (_maxAngle * 0.087 < 18) + retVal = -2; + else + writeOneByte(_addr_burn, 0x40); + } + else + retVal = -1; + + return retVal; +} + +/******************************************************* + Method: readOneByte + In: register to read + Out: data read from i2c + Description: reads one byte register from i2c +*******************************************************/ +int AMS_5600::readOneByte(int in_adr) +{ + int retVal = -1; + Wire.beginTransmission(_ams5600_Address); + Wire.write(in_adr); + Wire.endTransmission(); + Wire.requestFrom(_ams5600_Address, (uint8_t) 1); + while (Wire.available() == 0) + ; + retVal = Wire.read(); + + return retVal; +} + +/******************************************************* + Method: readTwoBytes + In: two registers to read + Out: data read from i2c as a word + Description: reads two bytes register from i2c +*******************************************************/ +word AMS_5600::readTwoBytesTogether(int addr_in) +{ + + // use only for Angle, Raw Angle and Magnitude + + // read 2 bytes together to prevent getting inconsistent + // data while the encoder is moving + // according to the datasheet the address is automatically incremented + // but only for Angle, Raw Angle and Magnitude + // the title says it's auto, but the paragraph after it + // says it does NOT + // tested and it does auto increment + + // PAGE 13: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf + // Automatic Increment of the Address Pointer for ANGLE, RAW ANGLE and MAGNITUDE Registers + // These are special registers which suppress the automatic + // increment of the address pointer on reads, so a re-read of these + // registers requires no I²C write command to reload the address + // pointer. This special treatment of the pointer is effective only if + // the address pointer is set to the high byte of the register. + + /* Read 2 Bytes */ + Wire.beginTransmission(_ams5600_Address); + Wire.write(addr_in); + Wire.endTransmission(); + Wire.requestFrom(_ams5600_Address, (uint8_t) 2); + while (Wire.available() < 2) + ; + + int highByte = Wire.read(); + int lowByte = Wire.read(); + + // in case newer version of IC used the same address to + // store something else, get only the 3 bits + //return ( ( highByte & 0b111 ) << 8 ) | lowByte; + + // but in case newer version has higher resolution + // we're good to go + return ( highByte << 8 ) | lowByte; +} + +/******************************************************* + Method: readTwoBytes + In: two registers to read + Out: data read from i2c as a word + Description: reads two bytes register from i2c +*******************************************************/ +word AMS_5600::readTwoBytesSeparately(int addr_in) +{ + int highByte = readOneByte(addr_in ); + int lowByte = readOneByte(addr_in+1); + return ( highByte << 8 ) | lowByte; +} + +/******************************************************* + Method: writeOneByte + In: address and data to write + Out: none + Description: writes one byte to a i2c register +*******************************************************/ +void AMS_5600::writeOneByte(int adr_in, int dat_in) +{ + Wire.beginTransmission(_ams5600_Address); + Wire.write(adr_in); + Wire.write(dat_in); + Wire.endTransmission(); +} + +/********** END OF AMS 5600 CALSS *****************/ diff --git a/lib/obp60task/AS5600.h b/lib/obp60task/AS5600.h new file mode 100644 index 0000000..7c8f5c2 --- /dev/null +++ b/lib/obp60task/AS5600.h @@ -0,0 +1,91 @@ + +/**************************************************** + AMS 5600 class for Arduino platform + Author: Tom Denton + Date: 15 Dec 2014 + File: AMS_5600.h + Version 1.00 + www.ams.com + + Description: This class has been designed to + access the AMS 5600 “potuino” shield. +***************************************************/ + +// updated jan 2022 by isc - read two bytes together + +// datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf + +#ifndef AMS_5600_h +#define AMS_5600_h + +#include + +class AMS_5600 +{ +public: + + AMS_5600(void); + int getAddress(); + + word setMaxAngle(word newMaxAngle = -1); + word getMaxAngle(); + + word setStartPosition(word startAngle = -1); + word getStartPosition(); + + word setEndPosition(word endAngle = -1); + word getEndPosition(); + + word getRawAngle(); + word getScaledAngle(); + + int detectMagnet(); + int getMagnetStrength(); + int getAgc(); + word getMagnitude(); + word getConf(); + void setConf(word _conf); + + int getBurnCount(); + int burnAngle(); + int burnMaxAngleAndConfig(); + void setOutPut(uint8_t mode); + +private: + + // i2c address + static const uint8_t _ams5600_Address = 0x36; + + // single byte registers + static const uint8_t _addr_status = 0x0b; // magnet status + static const uint8_t _addr_agc = 0x1a; // automatic gain control + static const uint8_t _addr_burn = 0xff; // permanent burning of configs (zpos, mpos, mang, conf) + static const uint8_t _addr_zmco = 0x00; // number of times zpos/mpos has been permanently burned + // zpos/mpos can be permanently burned 3x + // mang/conf can be burned only once + + // double byte registers, specify starting address (lower addr, but higher byte data) + // addr = upper byte of data (MSB), only bits 0:3 are used + // addr+1 = lower byte of data (LSB) + static const uint8_t _addr_zpos = 0x01; // zero position (start) + // 0x02 - lower byte + static const uint8_t _addr_mpos = 0x03; // maximum position (stop) + // 0x04 - lower byte + static const uint8_t _addr_mang = 0x05; // maximum angle + // 0x06 - lower byte + static const uint8_t _addr_conf = 0x07; // configuration + // 0x08 - lower byte + static const uint8_t _addr_raw_angle = 0x0c; // raw angle + // 0x0d - lower byte + static const uint8_t _addr_angle = 0x0e; // mapped angle + // 0x0f - lower byte + static const uint8_t _addr_magnitude = 0x1b; // magnitude of internal CORDIC + // 0x1c - lower byte + + int readOneByte(int in_adr); + word readTwoBytesSeparately(int addr_in); + word readTwoBytesTogether(int addr_in); + void writeOneByte(int adr_in, int dat_in); + +}; +#endif \ No newline at end of file diff --git a/lib/obp60task/Pagedata.h b/lib/obp60task/Pagedata.h index de054d8..99aa8c0 100644 --- a/lib/obp60task/Pagedata.h +++ b/lib/obp60task/Pagedata.h @@ -32,6 +32,7 @@ typedef struct{ double onewireTemp4 = 0; double onewireTemp5 = 0; double onewireTemp6 = 0; + int rotationAngle = 0; } SensorData; typedef struct{ diff --git a/lib/obp60task/config.json b/lib/obp60task/config.json index dc91c99..4179eb7 100644 --- a/lib/obp60task/config.json +++ b/lib/obp60task/config.json @@ -281,11 +281,15 @@ "label": "Rot. Function", "type": "list", "default": "off", - "description": "Function for rotation sensor [off|Rudder|Mast]", + "description": "Function for rotation sensor [off|Rudder|Wind|Mast|Keel|Trim|Boom]", "list": [ "off", "Rudder", - "Mast" + "Wind", + "Mast", + "Keel", + "Trim", + "Boom" ], "category": "OBP60 Hardware", "capabilities": { diff --git a/lib/obp60task/obp60task.cpp b/lib/obp60task/obp60task.cpp index cf7ea2e..8e0ac33 100644 --- a/lib/obp60task/obp60task.cpp +++ b/lib/obp60task/obp60task.cpp @@ -11,6 +11,7 @@ #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 @@ -39,6 +40,7 @@ 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 // Global vars bool initComplete = false; // Initialization complete @@ -48,9 +50,13 @@ 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; // Ais pressure 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(); @@ -241,7 +247,21 @@ void OBP60Init(GwApi *api){ SHT21_ready = true; } } + + // Settings for rotation sensors on I2C bus + String rotSensor=api->getConfig()->getConfigItem(api->getConfig()->useRotSensor,true)->asString(); + if(String(rotSensor) == "AS5600"){ + if (as5600.detectMagnet() == 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; + } + } } @@ -504,6 +524,8 @@ void OBP60Task(GwApi *api){ 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(); + // refreshmode defined in init section // displaycolor defined in init section // textcolor defined in init section @@ -534,6 +556,7 @@ void OBP60Task(GwApi *api){ 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 while (true){ delay(10); // Fixed the problem with NMEA0183 and GPS sentences @@ -638,7 +661,7 @@ void OBP60Task(GwApi *api){ display.update(); // Full update } - // Send supplay voltage value + // 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 @@ -650,7 +673,7 @@ void OBP60Task(GwApi *api){ } } - // Send data from environment sensor + // Send data from environment sensor all 1s if(millis() > starttime6 + 1000){ starttime6 = millis(); unsigned char TempSource = 2; // Inside temperature @@ -725,6 +748,20 @@ void OBP60Task(GwApi *api){ } } + // Send rotation angle all 100ms + if(millis() > starttime7 + 100){ + starttime7 = millis(); + if(String(rotsensor) == "AS5600" && AS5600_ready == true){ + rotationangle = as5600.getRawAngle() * 0.087; // 0...4095 segments = 0.087 degree + commonData.data.rotationAngle = rotationangle; // Data take over to page + // Send to NMEA200 bus + if(!isnan(rotationangle)){ + SetN2kRudder(N2kMsg, rotationangle, 0, N2kRDO_NoDirectionOrder, N2kDoubleNA); + api->sendN2kMessage(N2kMsg); + } + } + } + // Refresh display data all 1s if(millis() > starttime3 + 1000){ starttime3 = millis();