From 60a085280663ff31a500a15659019ba0298ec744 Mon Sep 17 00:00:00 2001 From: badevguru Date: Tue, 9 Dec 2014 20:01:32 -0800 Subject: [PATCH 1/9] Upgrade master to vnext --- package.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.json b/package.json index 4ce68cc..af0d997 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "openrov-software-arduino", - "version": "2.5.1", + "version": "2.5.2", "description": "Suite of OpenROV projects that are deployed to the ROV", "main": "OpenROV/OpenROV.ino", "scripts": { From e59b0d60a50a47c03c0aa0ebee48bbe978e58a52 Mon Sep 17 00:00:00 2001 From: badevguru Date: Tue, 9 Dec 2014 20:03:42 -0800 Subject: [PATCH 2/9] Update README.md --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 7169827..a8785b7 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,10 @@ [![Views in the last 24 hours](https://sourcegraph.com/api/repos/github.com/OpenROV/openrov-software-arduino/counters/views-24h.png)](https://sourcegraph.com/github.com/OpenROV/openrov-software-arduino) -[![Build Status](https://secure.travis-ci.org/OpenROV/openrov-software-arduino.png?branch=master)](http://travis-ci.org/OpenROV/openrov-software-arduino) + +Master: [![Build Status](https://secure.travis-ci.org/OpenROV/openrov-software-arduino.png?branch=master)](http://travis-ci.org/OpenROV/openrov-software-arduino) + +Pre-Release: [![Build Status](https://secure.travis-ci.org/OpenROV/openrov-software-arduino.png?branch=pre-release)](http://travis-ci.org/OpenROV/openrov-software-arduino) + +Stable: [![Build Status](https://secure.travis-ci.org/OpenROV/openrov-software-arduino.png?branch=stable)](http://travis-ci.org/OpenROV/openrov-software-arduino) openrov-software-arduino ======================== From 4d1dcb147fe391e043c8a9716b5f8667828883c1 Mon Sep 17 00:00:00 2001 From: badevguru Date: Sat, 14 Mar 2015 23:41:39 -0700 Subject: [PATCH 3/9] OpenROV/openrov-software#339; Added routine reporting of mtrmod --- OpenROV/Thrusters2X1.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/OpenROV/Thrusters2X1.cpp b/OpenROV/Thrusters2X1.cpp index 6aad86f..d98688f 100644 --- a/OpenROV/Thrusters2X1.cpp +++ b/OpenROV/Thrusters2X1.cpp @@ -224,7 +224,19 @@ void Thrusters::device_loop(Command command){ Serial.print(s); Serial.println(';'); thrusterdata::MATC = port_motor.attached() || port_motor.attached() || port_motor.attached(); - + Serial.print(F("mtrmod:")); + Serial.print(port_motor.motor_positive_modifer); + Serial.print (","); + Serial.print(vertical_motor.motor_positive_modifer); + Serial.print (","); + Serial.print(starboard_motor.motor_positive_modifer); + Serial.print (","); + Serial.print(port_motor.motor_negative_modifer); + Serial.print (","); + Serial.print(vertical_motor.motor_negative_modifer); + Serial.print (","); + Serial.print(starboard_motor.motor_negative_modifer); + Serial.println (";"); } } #endif From 1f97d8f66ff244528bcab86548556ef6b06b801d Mon Sep 17 00:00:00 2001 From: badevguru Date: Mon, 16 Mar 2015 23:55:22 -0700 Subject: [PATCH 4/9] OpenROV/openrov-software#339; Added CRC check to Serial Msgs --- OpenROV/Command.cpp | 51 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 49 insertions(+), 2 deletions(-) diff --git a/OpenROV/Command.cpp b/OpenROV/Command.cpp index 64921e7..5f9e54e 100644 --- a/OpenROV/Command.cpp +++ b/OpenROV/Command.cpp @@ -11,8 +11,30 @@ static int internalCommandBuffer_head =0; static int internalCommandBuffer_tail =0; + + +//CRC-8 - based on the CRC8 formulas by Dallas/Maxim +//code released under the therms of the GNU GPL 3.0 license +byte CRC8(byte start, char *data, byte len) { + byte crc = 0x00; + for(byte j = 0; j>= 1; + if (sum) { + crc ^= 0x8C; + } + extract >>= 1; + } + } + return crc; +} + + boolean getSerialString(){ - static byte dataBufferIndex = 0; + // static byte dataBufferIndex = 0; while(Serial.available()>0){ char incomingbyte = Serial.read(); // if(incomingbyte==startChar){ @@ -30,6 +52,7 @@ boolean getSerialString(){ break; } if(incomingbyte==endChar){ + dataBuffer[dataBufferIndex++] = incomingbyte; dataBuffer[dataBufferIndex] = 0; //null terminate the C string storeString = false; //Our data string is complete. return true @@ -145,8 +168,32 @@ void Command::reset(){ // get 'arguments' from command void Command::parse(){ char * pch; + byte crc = 0; byte i = 0; - pch = strtok (dataBuffer," ,();"); + crc = CRC8(1,dataBuffer,dataBufferIndex-1); + + Serial.print(F("rawcmd:")); + byte tt = 0; + while (tt Date: Tue, 17 Mar 2015 03:04:31 -0700 Subject: [PATCH 5/9] Fixed version number scheme --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index df432ef..a59db0d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,7 +24,7 @@ after_success: - echo real_get_branch $REAL_GIT_BRANCH - rm -rf OpenROV/build-* - 'fpm -f -m info@openrov.com -s dir -t deb -a armhf -n openrov-arduino-firmware -v - $VERSION_NUMBER-$REAL_GIT_BRANCH.$BUILD_NUMBER.`git rev-parse --short HEAD` --description ''OpenROV Arduino Firmware'' .=/opt/openrov/arduino ' + $VERSION_NUMBER~~$REAL_GIT_BRANCH.$BUILD_NUMBER.`git rev-parse --short HEAD` --description ''OpenROV Arduino Firmware'' .=/opt/openrov/arduino ' - mkdir build - DEBFILE="`ls *.deb`" - cp *.deb build/openrov-arduino-firmware_latest-${REAL_GIT_BRANCH}_armhf.deb From f496c77e45a67ab16a050914933118a4160465ae Mon Sep 17 00:00:00 2001 From: badevguru Date: Wed, 1 Apr 2015 22:51:01 -0700 Subject: [PATCH 6/9] OpenROV/openrov-software#382; Dropped branch name from package version --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index a59db0d..4d94029 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,7 +24,7 @@ after_success: - echo real_get_branch $REAL_GIT_BRANCH - rm -rf OpenROV/build-* - 'fpm -f -m info@openrov.com -s dir -t deb -a armhf -n openrov-arduino-firmware -v - $VERSION_NUMBER~~$REAL_GIT_BRANCH.$BUILD_NUMBER.`git rev-parse --short HEAD` --description ''OpenROV Arduino Firmware'' .=/opt/openrov/arduino ' + $VERSION_NUMBER~$BUILD_NUMBER.`git rev-parse --short HEAD` --description ''OpenROV Arduino Firmware'' .=/opt/openrov/arduino ' - mkdir build - DEBFILE="`ls *.deb`" - cp *.deb build/openrov-arduino-firmware_latest-${REAL_GIT_BRANCH}_armhf.deb @@ -45,8 +45,6 @@ deploy: repo: OpenROV/openrov-software-arduino all_branches: true condition: "($REAL_GIT_BRANCH = master )||($REAL_GIT_BRANCH = pre-release )||($REAL_GIT_BRANCH = stable)" -after_deploy: -- curl -g http://build.openrov.com:8080/job/OpenROV-generic-upload-deb-to-repo/buildWithParameters -d token=$JENKINS_TOKEN_NAME -d urlToDebPackage=http://openrov-software-nightlies.s3-website-us-west-2.amazonaws.com/arduino-firmware%2F$DEBFILE -d branch=$REAL_GIT_BRANCH notifications: webhooks: From 5a87bee58b154324d31041d6c12b2076b45a3b5b Mon Sep 17 00:00:00 2001 From: BrianAdams Date: Wed, 1 Apr 2015 23:01:03 -0700 Subject: [PATCH 7/9] bumped version number --- package.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.json b/package.json index af0d997..34aa4ee 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "openrov-software-arduino", - "version": "2.5.2", + "version": "30.0.0", "description": "Suite of OpenROV projects that are deployed to the ROV", "main": "OpenROV/OpenROV.ino", "scripts": { From 17135899425a5d54d373697b66ae8daa9a35d659 Mon Sep 17 00:00:00 2001 From: badevguru Date: Wed, 1 Apr 2015 23:06:42 -0700 Subject: [PATCH 8/9] Support larger version numbers --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 4d94029..cb247e0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,7 @@ script: env: global: - ARDUINO_IDE_VERSION='1.0.5' - - VERSION_NUMBER="`cat package.json | grep version | grep -o '[0-9]\.[0-9]\.[0-9]\+'`" + - VERSION_NUMBER="`cat package.json | grep version | grep -o '.[0-9]\.[0-9]\.[0-9]\+'`" - BUILD_NUMBER=$TRAVIS_BUILD_NUMBER - secure: "P4i5RD2+chpGAPtkn1JYChOc+TVEHdHVVWA40uVAaHqRGm0I/kAvA2i4xXun0VAFFJJjQgC7BSdzQs9kP6IVu4x7VVXyOt3OYMql8ehsH6LU3Q476Rb+HIOzX30qYx+7F+cHUPFp/BTiyEJ+IOjU6sDTWLmSDpjUvVD2Tzg+PLI=" - REAL_GIT_BRANCH="`git for-each-ref --format='%(objectname) %(refname:short)' refs/heads From ed5b7d27ea12de0dbc30eb257de3cd43fc8c5bb0 Mon Sep 17 00:00:00 2001 From: BrianAdams Date: Mon, 31 Aug 2015 13:57:17 -0700 Subject: [PATCH 9/9] 30.0.2 final changes --- OpenROV/AConfig.h | 8 +- OpenROV/AltServo.cpp | 45 +++ OpenROV/AltServo.h | 34 ++ OpenROV/BNO055.cpp | 576 ++++++++++++++++++++++++++++++++++ OpenROV/BNO055.h | 17 + OpenROV/CalLib.cpp | 4 +- OpenROV/Device.cpp | 5 + OpenROV/Device.h | 4 +- OpenROV/Lights.cpp | 15 + OpenROV/MPU9150.cpp | 47 ++- OpenROV/MS5803_14BA.cpp | 78 +---- OpenROV/MS5803_14BA.h | 2 + OpenROV/MS5803_14BALib.cpp | 200 ++++++++++++ OpenROV/MS5803_14BALib.h | 15 + OpenROV/OpenROV.ino | 18 +- OpenROV/controllerboard25.cpp | 7 +- OpenROV/controllerboard25.h | 3 + 17 files changed, 991 insertions(+), 87 deletions(-) create mode 100644 OpenROV/AltServo.cpp create mode 100755 OpenROV/AltServo.h create mode 100644 OpenROV/BNO055.cpp create mode 100644 OpenROV/BNO055.h mode change 100755 => 100644 OpenROV/Lights.cpp create mode 100644 OpenROV/MS5803_14BALib.cpp create mode 100644 OpenROV/MS5803_14BALib.h diff --git a/OpenROV/AConfig.h b/OpenROV/AConfig.h index 73655e2..a8bfb9e 100644 --- a/OpenROV/AConfig.h +++ b/OpenROV/AConfig.h @@ -33,9 +33,15 @@ //After Market: #define HAS_POLOLU_MINIMUV (0) #define HAS_MS5803_14BA (1) -#define MS5803_14BA_I2C_ADDRESS 0x76 +#define HAS_MS5803_30BA (0) +#define MS5803___BA_I2C_ADDRESS 0x76 #define HAS_MPU9150 (1) +#define HAS_BNO055 (1) #define MPU9150_EEPROM_START 2 +#define HAS_ALT_SERVO (1) +#define ALTS_MIDPOINT 1500 +#define ALTS_MINPOINT 1000 +#define ALTS_MAXPOINT 2000 #if !(HAS_OROV_CONTROLLERBOARD_25) && !(HAS_STD_CAPE) # error "You must select either standard cape or controllerboard25 in the AConfig.h file as they have predefined pin values required by other libraries." diff --git a/OpenROV/AltServo.cpp b/OpenROV/AltServo.cpp new file mode 100644 index 0000000..c3f4a5d --- /dev/null +++ b/OpenROV/AltServo.cpp @@ -0,0 +1,45 @@ +#include "AConfig.h" +#if(HAS_ALT_SERVO) +#include "Device.h" +#include "Pin.h" +#include "AltServo.h" +#include "Settings.h" +#include +#include "openrov_servo.h" +Servo _altservo; +int alts_val = ALTS_MIDPOINT; +int new_alts = ALTS_MIDPOINT; +int altsrate = 1; + +int smoothAdjustedServo(int target, int current){ + double x = target - current; + int sign = (x>0) - (x<0); + int adjustedVal = current + sign * (min(abs(target - current), altsrate)); + return (adjustedVal); +} + +void AltServo::device_setup(){ + _altservo.attach(ALTSERVO_PIN); + _altservo.writeMicroseconds(ALTS_MIDPOINT); + Settings::capability_bitarray |= (1 << ALT_SERVO_CAPABLE); + +} + +void AltServo::device_loop(Command command){ + if (command.cmp("asrt")) { + int ms = map(command.args[1],-100,100,ALTS_MINPOINT,ALTS_MAXPOINT); + if ((ms >= ALTS_MINPOINT) && (ms <= ALTS_MAXPOINT)){ + alts_val = ms; + Serial.print("asr.t:");Serial.print(alts_val);Serial.println(";"); + } + } + if (alts_val != new_alts){ + new_alts = smoothAdjustedServo(alts_val,new_alts); + _altservo.writeMicroseconds(new_alts); + Serial.print("asr.v:");Serial.print(new_alts);Serial.println(";"); + } + + +} + +#endif diff --git a/OpenROV/AltServo.h b/OpenROV/AltServo.h new file mode 100755 index 0000000..a6f2dbb --- /dev/null +++ b/OpenROV/AltServo.h @@ -0,0 +1,34 @@ + +#ifndef __ALTSERVO_H_ +#define __ALTSERVO_H_ +#include "AConfig.h" +#if(HAS_ALT_SERVO) +#include +#include "Device.h" +#include "Pin.h" + +#ifndef ALTS_MIDPOINT +#define ALTS_MIDPOINT 1500 +#endif +#ifndef ALTS_MINPOINT +#define ALTS_MINPOINT 1000 +#endif +#ifndef ALTS_MAXPOINT +#define ALTS_MAXPOINT 2000 +#endif +#if(HAS_STD_CAPE) + #include "Cape.h" +#endif + +#if(HAS_OROV_CONTROLLERBOARD_25) + #include "controllerboard25.h" +#endif + +class AltServo : public Device { + public: + AltServo():Device(){}; + void device_setup(); + void device_loop(Command cmd); +}; +#endif +#endif diff --git a/OpenROV/BNO055.cpp b/OpenROV/BNO055.cpp new file mode 100644 index 0000000..9b6da7b --- /dev/null +++ b/OpenROV/BNO055.cpp @@ -0,0 +1,576 @@ +#include "AConfig.h" +#if(HAS_BNO055) + +#include "BNO055.h" +#include "Timer.h" + + +/* BNO055 OpenROV AHRS Module + By Brian Adams + Adopted from code by Kris Winer + */ +#include + +// BNO055 Register Map +// http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_10_Release.pdf +// +// BNO055 Page 0 +#define BNO055_CHIP_ID 0x00 // should be 0xA0 +#define BNO055_ACC_ID 0x01 // should be 0xFB +#define BNO055_MAG_ID 0x02 // should be 0x32 +#define BNO055_GYRO_ID 0x03 // should be 0x0F +#define BNO055_SW_REV_ID_LSB 0x04 +#define BNO055_SW_REV_ID_MSB 0x05 +#define BNO055_BL_REV_ID 0x06 +#define BNO055_PAGE_ID 0x07 +#define BNO055_ACC_DATA_X_LSB 0x08 +#define BNO055_ACC_DATA_X_MSB 0x09 +#define BNO055_ACC_DATA_Y_LSB 0x0A +#define BNO055_ACC_DATA_Y_MSB 0x0B +#define BNO055_ACC_DATA_Z_LSB 0x0C +#define BNO055_ACC_DATA_Z_MSB 0x0D +#define BNO055_MAG_DATA_X_LSB 0x0E +#define BNO055_MAG_DATA_X_MSB 0x0F +#define BNO055_MAG_DATA_Y_LSB 0x10 +#define BNO055_MAG_DATA_Y_MSB 0x11 +#define BNO055_MAG_DATA_Z_LSB 0x12 +#define BNO055_MAG_DATA_Z_MSB 0x13 +#define BNO055_GYR_DATA_X_LSB 0x14 +#define BNO055_GYR_DATA_X_MSB 0x15 +#define BNO055_GYR_DATA_Y_LSB 0x16 +#define BNO055_GYR_DATA_Y_MSB 0x17 +#define BNO055_GYR_DATA_Z_LSB 0x18 +#define BNO055_GYR_DATA_Z_MSB 0x19 +#define BNO055_EUL_HEADING_LSB 0x1A +#define BNO055_EUL_HEADING_MSB 0x1B +#define BNO055_EUL_ROLL_LSB 0x1C +#define BNO055_EUL_ROLL_MSB 0x1D +#define BNO055_EUL_PITCH_LSB 0x1E +#define BNO055_EUL_PITCH_MSB 0x1F +#define BNO055_QUA_DATA_W_LSB 0x20 +#define BNO055_QUA_DATA_W_MSB 0x21 +#define BNO055_QUA_DATA_X_LSB 0x22 +#define BNO055_QUA_DATA_X_MSB 0x23 +#define BNO055_QUA_DATA_Y_LSB 0x24 +#define BNO055_QUA_DATA_Y_MSB 0x25 +#define BNO055_QUA_DATA_Z_LSB 0x26 +#define BNO055_QUA_DATA_Z_MSB 0x27 +#define BNO055_LIA_DATA_X_LSB 0x28 +#define BNO055_LIA_DATA_X_MSB 0x29 +#define BNO055_LIA_DATA_Y_LSB 0x2A +#define BNO055_LIA_DATA_Y_MSB 0x2B +#define BNO055_LIA_DATA_Z_LSB 0x2C +#define BNO055_LIA_DATA_Z_MSB 0x2D +#define BNO055_GRV_DATA_X_LSB 0x2E +#define BNO055_GRV_DATA_X_MSB 0x2F +#define BNO055_GRV_DATA_Y_LSB 0x30 +#define BNO055_GRV_DATA_Y_MSB 0x31 +#define BNO055_GRV_DATA_Z_LSB 0x32 +#define BNO055_GRV_DATA_Z_MSB 0x33 +#define BNO055_TEMP 0x34 +#define BNO055_CALIB_STAT 0x35 +#define BNO055_ST_RESULT 0x36 +#define BNO055_INT_STATUS 0x37 +#define BNO055_SYS_CLK_STATUS 0x38 +#define BNO055_SYS_STATUS 0x39 +#define BNO055_SYS_ERR 0x3A +#define BNO055_UNIT_SEL 0x3B +#define BNO055_OPR_MODE 0x3D +#define BNO055_PWR_MODE 0x3E +#define BNO055_SYS_TRIGGER 0x3F +#define BNO055_TEMP_SOURCE 0x40 +#define BNO055_AXIS_MAP_CONFIG 0x41 +#define BNO055_AXIS_MAP_SIGN 0x42 +#define BNO055_ACC_OFFSET_X_LSB 0x55 +#define BNO055_ACC_OFFSET_X_MSB 0x56 +#define BNO055_ACC_OFFSET_Y_LSB 0x57 +#define BNO055_ACC_OFFSET_Y_MSB 0x58 +#define BNO055_ACC_OFFSET_Z_LSB 0x59 +#define BNO055_ACC_OFFSET_Z_MSB 0x5A +#define BNO055_MAG_OFFSET_X_LSB 0x5B +#define BNO055_MAG_OFFSET_X_MSB 0x5C +#define BNO055_MAG_OFFSET_Y_LSB 0x5D +#define BNO055_MAG_OFFSET_Y_MSB 0x5E +#define BNO055_MAG_OFFSET_Z_LSB 0x5F +#define BNO055_MAG_OFFSET_Z_MSB 0x60 +#define BNO055_GYR_OFFSET_X_LSB 0x61 +#define BNO055_GYR_OFFSET_X_MSB 0x62 +#define BNO055_GYR_OFFSET_Y_LSB 0x63 +#define BNO055_GYR_OFFSET_Y_MSB 0x64 +#define BNO055_GYR_OFFSET_Z_LSB 0x65 +#define BNO055_GYR_OFFSET_Z_MSB 0x66 +#define BNO055_ACC_RADIUS_LSB 0x67 +#define BNO055_ACC_RADIUS_MSB 0x68 +#define BNO055_MAG_RADIUS_LSB 0x69 +#define BNO055_MAG_RADIUS_MSB 0x6A +// +// BNO055 Page 1 +#define BNO055_PAGE_ID 0x07 +#define BNO055_ACC_CONFIG 0x08 +#define BNO055_MAG_CONFIG 0x09 +#define BNO055_GYRO_CONFIG_0 0x0A +#define BNO055_GYRO_CONFIG_1 0x0B +#define BNO055_ACC_SLEEP_CONFIG 0x0C +#define BNO055_GYR_SLEEP_CONFIG 0x0D +#define BNO055_INT_MSK 0x0F +#define BNO055_INT_EN 0x10 +#define BNO055_ACC_AM_THRES 0x11 +#define BNO055_ACC_INT_SETTINGS 0x12 +#define BNO055_ACC_HG_DURATION 0x13 +#define BNO055_ACC_HG_THRESH 0x14 +#define BNO055_ACC_NM_THRESH 0x15 +#define BNO055_ACC_NM_SET 0x16 +#define BNO055_GYR_INT_SETTINGS 0x17 +#define BNO055_GYR_HR_X_SET 0x18 +#define BNO055_GYR_DUR_X 0x19 +#define BNO055_GYR_HR_Y_SET 0x1A +#define BNO055_GYR_DUR_Y 0x1B +#define BNO055_GYR_HR_Z_SET 0x1C +#define BNO055_GYR_DUR_Z 0x1D +#define BNO055_GYR_AM_THRESH 0x1E +#define BNO055_GYR_AM_SET 0x1F + + +//#define BNO055_ADDRESS 0x29 +#define BNO055_ADDRESS 0x28 + +// Set initial input parameters +enum Ascale { // ACC Full Scale + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_18G +}; + +enum Abw { // ACC Bandwidth + ABW_7_81Hz = 0, + ABW_15_63Hz, + ABW_31_25Hz, + ABW_62_5Hz, + ABW_125Hz, + ABW_250Hz, + ABW_500Hz, + ABW_1000Hz, //0x07 +}; + +enum APwrMode { // ACC Pwr Mode + NormalA = 0, + SuspendA, + LowPower1A, + StandbyA, + LowPower2A, + DeepSuspendA +}; + +enum Gscale { // gyro full scale + GFS_2000DPS = 0, + GFS_1000DPS, + GFS_500DPS, + GFS_250DPS, + GFS_125DPS // 0x04 +}; + +enum GPwrMode { // GYR Pwr Mode + NormalG = 0, + FastPowerUpG, + DeepSuspendedG, + SuspendG, + AdvancedPowerSaveG +}; + +enum Gbw { // gyro bandwidth + GBW_523Hz = 0, + GBW_230Hz, + GBW_116Hz, + GBW_47Hz, + GBW_23Hz, + GBW_12Hz, + GBW_64Hz, + GBW_32Hz +}; + +enum OPRMode { // BNO-55 operation modes + CONFIGMODE = 0x00, +// Sensor Mode + ACCONLY, + MAGONLY, + GYROONLY, + ACCMAG, + ACCGYRO, + MAGGYRO, + AMG, // 0x07 +// Fusion Mode + IMU, + COMPASS, + M4G, + NDOF_FMC_OFF, + NDOF // 0x0C +}; + +enum PWRMode { + Normalpwr = 0, + Lowpower, + Suspendpwr +}; + +enum Modr { // magnetometer output data rate + MODR_2Hz = 0, + MODR_6Hz, + MODR_8Hz, + MODR_10Hz, + MODR_15Hz, + MODR_20Hz, + MODR_25Hz, + MODR_30Hz +}; + +enum MOpMode { // MAG Op Mode + LowPower = 0, + Regular, + EnhancedRegular, + HighAccuracy +}; + +enum MPwrMode { // MAG power mode + Normal = 0, + Sleep, + Suspend, + ForceMode +}; + +static uint8_t PWRMode = Normal ; // Select BNO055 power mode +static uint8_t OPRMode = NDOF; // specify operation mode for sensors + +static uint8_t status; // BNO055 data status register +static int16_t EulCount[3]; // Stores the 16-bit signed Euler angle output + +static float bPitch, bYaw, bRoll; +static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion +static float quat[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion + + + +// I2C read/write functions for the BNO055 sensor + +void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) +{ + Wire.beginTransmission(address); // Initialize the Tx buffer + Wire.write(subAddress); // Put slave register address in Tx buffer + Wire.write(data); // Put data in Tx buffer + Wire.endTransmission(); // Send the Tx buffer +} + +uint8_t readByte(uint8_t address, uint8_t subAddress) +{ + uint8_t data; // `data` will store the register data + Wire.beginTransmission(address); // Initialize the Tx buffer + Wire.write(subAddress); // Put slave register address in Tx buffer +// Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive + Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive + Wire.requestFrom(address, 1); // Read one byte from slave register address + Wire.requestFrom(address, (size_t) 1); // Read one byte from slave register address + data = Wire.read(); // Fill Rx buffer with result + return data; // Return data read from slave register +} + +void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) +{ + Wire.beginTransmission(address); // Initialize the Tx buffer + Wire.write(subAddress); // Put slave register address in Tx buffer +// Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive + Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive + uint8_t i = 0; + Wire.requestFrom(address, count); // Read bytes from slave register address + Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address + unsigned long timeout = 0; + while ((i>2 & 0x03); Serial.println(';'); + Serial.print("BNO055.CALIB_GYR:"); Serial.print(magCal>>4 & 0x03); Serial.println(';'); + Serial.print("BNO055.CALIB_SYS:"); Serial.print(magCal>>6 & 0x03); Serial.println(';'); + + byte mode = readByte(BNO055_ADDRESS, BNO055_OPR_MODE); + Serial.print("BNO055.MODE:"); Serial.print(mode); Serial.println(';'); + + if (mode != OPRMode){ + initalized=false; + } + } + + readEulData(EulCount); // Read the x/y/z adc values + // Calculate the Euler angles values in degrees + bYaw = (float)EulCount[0]/16.; + bRoll = -(float)EulCount[1]/16.; + bPitch = (float)EulCount[2]/16.; + + + navdata::PITC = bPitch; + navdata::ROLL = bRoll; + navdata::YAW = bYaw; + navdata::HDGD = bYaw; + } +} +#endif diff --git a/OpenROV/BNO055.h b/OpenROV/BNO055.h new file mode 100644 index 0000000..7ff9a9d --- /dev/null +++ b/OpenROV/BNO055.h @@ -0,0 +1,17 @@ + +#include "AConfig.h" +#if(HAS_BNO055) +#ifndef __ASBNO055_H_ +#define __ASBNO055_H_ +#include +#include "Device.h" + +class BNO055 : public Device { + public: + BNO055():Device(){}; + void device_setup(); + void device_loop(Command cmd); +}; +#endif +#endif + diff --git a/OpenROV/CalLib.cpp b/OpenROV/CalLib.cpp index f358344..0168bd9 100755 --- a/OpenROV/CalLib.cpp +++ b/OpenROV/CalLib.cpp @@ -30,7 +30,7 @@ #include "DueFlash.h" -DueFlash flash; +static DueFlash flash; void calLibErase(byte device) { @@ -93,4 +93,4 @@ boolean calLibRead(byte device, CALLIB_DATA *calData) return true; } #endif -#endif \ No newline at end of file +#endif diff --git a/OpenROV/Device.cpp b/OpenROV/Device.cpp index 3f20415..c05f285 100755 --- a/OpenROV/Device.cpp +++ b/OpenROV/Device.cpp @@ -1,5 +1,6 @@ #include "Device.h" #include "Settings.h" +#include // watchdog timer Device::Device(){ DeviceManager::registerDevice(this); @@ -31,6 +32,10 @@ void DeviceManager::doDeviceLoops(Command cmd){ void DeviceManager::doDeviceSetups(){ for(int i=0;idevice_setup(); } } diff --git a/OpenROV/Device.h b/OpenROV/Device.h index 3a38022..6a961c3 100644 --- a/OpenROV/Device.h +++ b/OpenROV/Device.h @@ -13,10 +13,10 @@ #define COMPASS_CAPABLE 4 #define ORIENTATION_CAPABLE 5 #define DEAPTH_CAPABLE 6 +#define ALT_SERVO_CAPABLE 7 - -#define MAX_DEVICES 10 +#define MAX_DEVICES 12 // Be sure to initialize any storage variables added to these shared data classes in the device.cpp file. // These shared storage classes are always available and should be device independent. Data that is // unque to a particular implimenatation of a device (for example details related to a 3 thruster configuration vs diff --git a/OpenROV/Lights.cpp b/OpenROV/Lights.cpp old mode 100755 new mode 100644 index 0183e71..10f606d --- a/OpenROV/Lights.cpp +++ b/OpenROV/Lights.cpp @@ -7,11 +7,14 @@ #include Pin light("light", LIGHTS_PIN, light.analog, light.out); +Pin elight("elight", ELIGHTS_PIN, elight.analog, elight.out); void Lights::device_setup(){ Settings::capability_bitarray |= (1 << LIGHTS_CAPABLE); light.reset(); light.write(0); + elight.reset(); + elight.write(0); } @@ -29,5 +32,17 @@ void Lights::device_loop(Command command){ Serial.print(command.args[1]/255.0); Serial.println(';'); } + if( command.cmp("eligt")){ + float percentvalue = command.args[1]/100.0; //0 - 255 + int value = 255*percentvalue; + elight.write(value); + + Serial.print(F("LIGTE:")); + Serial.print(value); + Serial.print(';'); + Serial.print(F("LIGPE:")); + Serial.print(percentvalue); + Serial.println(';'); + } } #endif diff --git a/OpenROV/MPU9150.cpp b/OpenROV/MPU9150.cpp index 991450e..631b2c0 100755 --- a/OpenROV/MPU9150.cpp +++ b/OpenROV/MPU9150.cpp @@ -15,14 +15,14 @@ #include "Timer.h" MPU9150Lib MPU; // the MPU object -int MPUDeviceId = 1; -boolean DidInit = false; -boolean InCallibrationMode = false; -Timer MPU9150ReInit; -Timer mpu_update_timer; -CALLIB_DATA calData; -Timer calibration_timer; -int counter = 0; +static int MPUDeviceId = 1; +static boolean DidInit = false; +static boolean InCallibrationMode = false; +static Timer MPU9150ReInit; +static Timer mpu_update_timer; +static CALLIB_DATA calData; +static Timer calibration_timer; +static int counter = 0; // MPU_UPDATE_RATE defines the rate (in Hz) at which the MPU updates the sensor data and DMP output // MPU_UPDATE_RATE defines the rate (in Hz) at which the MPU updates the sensor data and DMP output @@ -49,15 +49,35 @@ int counter = 0; void MPU9150::device_setup(){ + //Todo: Read calibration values from EPROM - Wire.begin(); + bool canConnect = true; + //Wire.begin(); + MPU9150ReInit.reset(); + mpu_update_timer.reset(); + + Wire.beginTransmission(0x68); + byte error = Wire.endTransmission(); + if (error != 0) { + canConnect = false; + Wire.beginTransmission(0x69); + error = Wire.endTransmission(); + if (error != 0) { + Serial.print(F("MPU9150.status:"));Serial.print("Not found");Serial.println(";"); + canConnect = false; + } else { + canConnect = true; + } + } + + if (canConnect){ MPU.selectDevice(MPUDeviceId); if (!MPU.init(MPU_UPDATE_RATE, MPU_MAG_MIX_GYRO_ONLY, MAG_UPDATE_RATE, MPU_LPF_RATE)){ Serial.println(F("log:Trying other MPU9150 address to init;")); - Serial.print(F("log:IMU Address was :")); + Serial.print(F("log:IMU Address was ")); Serial.print(1); MPUDeviceId = !MPUDeviceId; - Serial.print(F(" but is now:")); + Serial.print(F(" but is now ")); Serial.print(MPUDeviceId); Serial.println(";"); MPU.selectDevice(MPUDeviceId); @@ -73,8 +93,9 @@ void MPU9150::device_setup(){ } // start the MPU Settings::capability_bitarray |= (1 << COMPASS_CAPABLE); Settings::capability_bitarray |= (1 << ORIENTATION_CAPABLE); - MPU9150ReInit.reset(); - mpu_update_timer.reset(); +// MPU9150ReInit.reset(); +// mpu_update_timer.reset(); + } } void MPU9150::device_loop(Command command){ diff --git a/OpenROV/MS5803_14BA.cpp b/OpenROV/MS5803_14BA.cpp index b8548e9..8cb1436 100644 --- a/OpenROV/MS5803_14BA.cpp +++ b/OpenROV/MS5803_14BA.cpp @@ -6,6 +6,7 @@ #include "Settings.h" #include "Timer.h" #include +#include "MS5803_14BALib.h" /* Sketch to read a MS5803-14BA pressure sensor, written from scratch. @@ -18,7 +19,7 @@ Rev 1 12 Oct 2013 -- Implements 2nd order temperature compensation -const int DevAddress = MS5803_14BA_I2C_ADDRESS; // 7-bit I2C address of the MS5803 +const int DevAddress = MS5803___BA_I2C_ADDRESS; // 7-bit I2C address of the MS5803 // Here are the commands that can be sent to the 5803 // Page 6 of the data sheet @@ -62,19 +63,21 @@ void sendCommand(byte command){ Wire.endTransmission(); } + + void MS5803_14BA::device_setup(){ Settings::capability_bitarray |= (1 << DEAPTH_CAPABLE); - Serial.println("Depth Sensor setup."); + Serial.println("log:Depth Sensor setup;"); Wire.begin(); - Serial.println("Depth Sensor: initialized I2C"); + Serial.println("MS5803.status: initialized I2C;"); delay(10); - + unsigned int cal; // Reset the device and check for device presence sendCommand(Reset); delay(10); - Serial.println("Depth Sensor is reset"); + Serial.println("MS5803.status: reset;"); // Get the calibration constants and store in array @@ -93,13 +96,16 @@ void MS5803_14BA::device_setup(){ for (byte i=0; i < 8; i++) { - log(CalConstant[i]); + cal = CalConstant[i]; + Serial.print("Depth.C");Serial.print(i);Serial.print(":");Serial.print(cal);Serial.println(";"); + //log(CalConstant[i]); } DepthSensorSamples.reset(); } + void MS5803_14BA::device_loop(Command command){ if (command.cmp("dzer")){ DepthOffset=Depth; @@ -158,63 +164,9 @@ void MS5803_14BA::device_loop(Command command){ // log("D2 is: "); // log(AdcTemperature); + envdata::TEMP = CorrectedTemperature(AdcTemperature, CalConstant); - // Calculate the Temperature (first-order computation) - - TempDifference = (float)(AdcTemperature - ((long)CalConstant[5] << 8)); - Temperature = (TempDifference * (float)CalConstant[6])/ pow(2, 23); - Temperature = Temperature + 2000; // This is the temperature in hundredths of a degree C - - // Calculate the second-order offsets - - if (Temperature < 2000.0) // Is temperature below or above 20.00 deg C ? - { - T2 = 3 * pow(TempDifference, 2) / pow(2, 33); - Off2 = 1.5 * pow((Temperature - 2000.0), 2); - Sens2 = 0.625 * pow((Temperature - 2000.0), 2); - } - else - { - T2 = (TempDifference * TempDifference) * 7 / pow(2, 37); - Off2 = 0.0625 * pow((Temperature - 2000.0), 2); - Sens2 = 0.0; - } - - // Check print the offsets - - //log("Second-order offsets are:"); - //log(T2); - //log(Off2); - //log(Sens2); - - - // Print the temperature results - - Temperature = Temperature / 100; // Convert to degrees C - Serial.print("First-Order Temperature in Degrees C is "); - Serial.println(Temperature); - Serial.print("Second-Order Temperature in Degrees C is "); - Serial.println(Temperature - (T2 / 100)); - envdata::TEMP = Temperature- (T2 / 100); - // Calculate the pressure parameters - - Offset = (float)CalConstant[2] * pow(2,16); - Offset = Offset + ((float)CalConstant[4] * TempDifference / pow(2, 7)); - - Sensitivity = (float)CalConstant[1] * pow(2, 15); - Sensitivity = Sensitivity + ((float)CalConstant[3] * TempDifference / pow(2, 8)); - - // Add second-order corrections - - Offset = Offset - Off2; - Sensitivity = Sensitivity - Sens2; - - // Calculate absolute pressure in bars - - Pressure = (float)AdcPressure * Sensitivity / pow(2, 21); - Pressure = Pressure - Offset; - Pressure = Pressure / pow(2, 15); - Pressure = Pressure / 10; // Set output to mbars = hectopascal; + Pressure = TemperatureCorrectedPressure(AdcPressure, AdcTemperature, CalConstant); envdata::PRES = Pressure; @@ -244,5 +196,3 @@ void MS5803_14BA::device_loop(Command command){ #endif - - diff --git a/OpenROV/MS5803_14BA.h b/OpenROV/MS5803_14BA.h index ca01552..9e482a9 100755 --- a/OpenROV/MS5803_14BA.h +++ b/OpenROV/MS5803_14BA.h @@ -9,6 +9,8 @@ class MS5803_14BA : public Device { MS5803_14BA():Device(){}; void device_setup(); void device_loop(Command cmd); +// float TemperatureCorrectedPressure(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ); + }; #endif diff --git a/OpenROV/MS5803_14BALib.cpp b/OpenROV/MS5803_14BALib.cpp new file mode 100644 index 0000000..3a42b6c --- /dev/null +++ b/OpenROV/MS5803_14BALib.cpp @@ -0,0 +1,200 @@ +#include "MS5803_14BALib.h" +#include + +/* It appears that the gen 2 of the MS5803 series is embedding + the pow(2, X) used in the final calculation that is unique + to the sensor in the vendor reserved space. 8192 & CalConstant[0] + seems to mostly = 8192. Not reliable enough to pull yet. But + all of the new sensors do have values in the CalConstant[0] whereas + the 14bar sensor we were using does not, so I will use that to auto + detect which sensor is used at the moment +*/ +static unsigned int MS5803_Model_Varient(unsigned int CalConstant[8]){ + if (CalConstant[0]!=0) return 8192; // (2^13) + return 32768;// (2^15) +} + +float CorrectedTemperature(long AdcTemperature, unsigned int CalConstant[8]){ + float T2,Off2,Sens2,TempDifference,Temperature; + // Calculate the Temperature (first-order computation) + + TempDifference = (float)(AdcTemperature - ((long)CalConstant[5] << 8)); + Temperature = (TempDifference * (float)CalConstant[6])/ pow(2, 23); + Temperature = Temperature + 2000; // This is the temperature in hundredths of a degree C + + // Calculate the second-order offsets + + if (Temperature < 2000.0) // Is temperature below or above 20.00 deg C ? + { + T2 = 3 * pow(TempDifference, 2) / pow(2, 33); + Off2 = 1.5 * pow((Temperature - 2000.0), 2); + Sens2 = 0.625 * pow((Temperature - 2000.0), 2); + } + else + { + T2 = (TempDifference * TempDifference) * 7 / pow(2, 37); + Off2 = 0.0625 * pow((Temperature - 2000.0), 2); + Sens2 = 0.0; + } + + // Print the temperature results + + Temperature = Temperature / 100; // Convert to degrees C + // Calculate the pressure parameters + return Temperature - (T2 / 100); +} + +float TemperatureCorrectedPressure(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ){ + return TemperatureCorrectedPressure_2(AdcPressure, AdcTemperature, CalConstant); +} + + +float TemperatureCorrectedPressure_1(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ){ + float T2,Off2,Sens2,TempDifference,Temperature; + // Calculate the Temperature (first-order computation) + + TempDifference = (float)(AdcTemperature - ((long)CalConstant[5] << 8)); + Temperature = (TempDifference * (float)CalConstant[6])/ pow(2, 23); + Temperature = Temperature + 2000; // This is the temperature in hundredths of a degree C + + // Calculate the second-order offsets + + if (Temperature < 2000.0) // Is temperature below or above 20.00 deg C ? + { + T2 = 3 * pow(TempDifference, 2) / pow(2, 33); + Off2 = 1.5 * pow((Temperature - 2000.0), 2); + Sens2 = 0.625 * pow((Temperature - 2000.0), 2); + } + else + { + T2 = (TempDifference * TempDifference) * 7 / pow(2, 37); + Off2 = 0.0625 * pow((Temperature - 2000.0), 2); + Sens2 = 0.0; + } + + float Offset, Sensitivity, Pressure; + + TempDifference = (float)(AdcTemperature - ((long)CalConstant[5] << 8)); + + Offset = (float)CalConstant[2] * pow(2,16); + Offset = Offset + ((float)CalConstant[4] * TempDifference / pow(2, 7)); + + Sensitivity = (float)CalConstant[1] * pow(2, 15); + Sensitivity = Sensitivity + ((float)CalConstant[3] * TempDifference / pow(2, 8)); + + // Add second-order corrections + + Offset = Offset - Off2; + Sensitivity = Sensitivity - Sens2; + + // Calculate absolute pressure in bars + + Pressure = (float)AdcPressure * Sensitivity / pow(2, 21); + Pressure = Pressure - Offset; + Pressure = Pressure / pow(2, 15); + Pressure = Pressure / 10; // Set output to mbars = hectopascal; + return Pressure; +} + +float TemperatureCorrectedPressure_2(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ){ + uint32_t D1 = AdcPressure; // Store uncompensated pressure value + uint32_t D2 = AdcTemperature; // Store uncompensated temperature value + // These three variables are used for the conversion steps + // They should be signed 32-bit integer initially + // i.e. signed long from -2147483648 to 2147483647 + int32_t dT = 0; + int32_t TEMP = 0; + // These values need to be signed 64 bit integers + // (long long = int64_t) + int64_t Offset = 0; + int64_t Sensitivity = 0; + int64_t T2 = 0; + int64_t OFF2 = 0; + int64_t Sens2 = 0; + // Some constants used in calculations below + const uint64_t POW_2_33 = 8589934592ULL; // 2^33 = 8589934592 + const uint64_t POW_2_37 = 137438953472ULL; // 2^37 = 137438953472 + float mbar; // Store pressure in mbar. + float tempC; // Store temperature in degrees Celsius + int32_t mbarInt; // pressure in mbar, initially as a signed long integer + + dT = (int32_t)D2 - ( (int32_t)CalConstant[5] * 256 ); + // Use integer division to calculate TEMP. It is necessary to cast + // one of the operands as a signed 64-bit integer (int64_t) so there's no + // rollover issues in the numerator. + TEMP = 2000 + ((int64_t)dT * CalConstant[6]) / 8388608LL; + // Recast TEMP as a signed 32-bit integer + TEMP = (int32_t)TEMP; + + + // All operations from here down are done as integer math until we make + // the final calculation of pressure in mbar. + + + // Do 2nd order temperature compensation (see pg 9 of MS5803 data sheet) + // I have tried to insert the fixed values wherever possible + // (i.e. 2^31 is hard coded as 2147483648). + if (TEMP < 2000) { + // For 30 bar model + T2 = 3 * ((int64_t)dT * dT) / POW_2_33 ; + T2 = (int32_t)T2; // recast as signed 32bit integer + OFF2 = 3 * ((TEMP-2000) * (TEMP-2000)) / 2 ; + Sens2 = 5 * ((TEMP-2000) * (TEMP-2000)) / 8 ; + } else { // if TEMP is > 2000 (20.0C) + // For 30 bar model + T2 = 7 * ((int64_t)dT * dT) / POW_2_37; + T2 = (int32_t)T2; // recast as signed 32bit integer + OFF2 = 1 * ((TEMP-2000) * (TEMP-2000)) / 16; + Sens2 = 0; + } + // Additional compensation for very low temperatures (< -15C) + if (TEMP < -1500) { + // For 30 bar model + OFF2 = OFF2 + 7 * ((TEMP+1500)*(TEMP+1500)); + Sens2 = Sens2 + 4 * ((TEMP+1500)*(TEMP+1500)); + } + + // Calculate initial Offset and Sensitivity + // Notice lots of casts to int64_t to ensure that the + // multiplication operations don't overflow the original 16 bit and 32 bit + // integers +// For 30 bar sensor +Offset = (int64_t)CalConstant[2] * 65536 + (CalConstant[4] * (int64_t)dT) / 128; +Sensitivity = (int64_t)CalConstant[1] * 32768 + (CalConstant[3] * (int64_t)dT) / 256; + + // Adjust TEMP, Offset, Sensitivity values based on the 2nd order + // temperature correction above. + TEMP = TEMP - T2; // both should be int32_t + Offset = Offset - OFF2; // both should be int64_t + Sensitivity = Sensitivity - Sens2; // both should be int64_t + + // Final compensated pressure calculation. We first calculate the pressure + // as a signed 32-bit integer (mbarInt), then convert that value to a + // float (mbar). + // For 30 bar sensor + mbarInt = ((D1 * Sensitivity) / 2097152 - Offset) / MS5803_Model_Varient(CalConstant); + +// Serial.print("D1:");Serial.println(D1); +// Serial.print("D2:");Serial.println(D2); +// Serial.print("Sensitivity:");Serial.println(Sensitivity); +// Serial.print("Offset:");Serial.println(Offset); + mbar = (float)mbarInt / 10; + + // Calculate the human-readable temperature in Celsius + tempC = (float)TEMP / 100; + + + // // Start other temperature conversions by converting mbar to psi absolute + // psiAbs = mbar * 0.0145038; + // // Convert psi absolute to inches of mercury + // inHgPress = psiAbs * 2.03625; + // // Convert psi absolute to gauge pressure + // psiGauge = psiAbs - 14.7; + // // Convert mbar to mm Hg + // mmHgPress = mbar * 0.7500617; + // // Convert temperature to Fahrenheit + // tempF = (tempC * 1.8) + 32; + return mbar; + +} + diff --git a/OpenROV/MS5803_14BALib.h b/OpenROV/MS5803_14BALib.h new file mode 100644 index 0000000..d8887df --- /dev/null +++ b/OpenROV/MS5803_14BALib.h @@ -0,0 +1,15 @@ +#ifndef __MS5803_14BALIB_H_ +#define __MS5803_14BALIB_H_ + + +float CorrectedTemperature(long AdcTemperature, unsigned int CalConstant[8]); + +float TemperatureCorrectedPressure(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ); + + +float TemperatureCorrectedPressure_1(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ); + +float TemperatureCorrectedPressure_2(long AdcPressure, long AdcTemperature, unsigned int CalConstant[8] ); + +#endif + diff --git a/OpenROV/OpenROV.ino b/OpenROV/OpenROV.ino index ff163ae..81b2f49 100644 --- a/OpenROV/OpenROV.ino +++ b/OpenROV/OpenROV.ino @@ -52,7 +52,10 @@ Settings settings; CameraMount cameramount; #endif - +#if(HAS_ALT_SERVO) + #include "AltServo.h" + AltServo altservo1; +#endif #if(HAS_POLOLU_MINIMUV) #define COMPASS_ENABLED 1 @@ -73,7 +76,7 @@ Settings settings; MPU9150 IMU; #endif -#if(HAS_MS5803_14BA) +#if(HAS_MS5803_14BA | HAS_MS5803_30BA) #define DEAPTH_ENABLED 1 #include "MS5803_14BA.h" #include //required to force the Arduino IDE to include the library in the path for the I2C code @@ -86,6 +89,15 @@ Settings settings; DeadmanSwitch DMS; #endif +#if(HAS_BNO055) + #define COMPASS_ENABLED 1 + #define GYRO_ENABLED 1 + #define ACCELEROMETER_ENABLED 1 + #include "BNO055.h" + #include //required to force the Arduino IDE to include the library in the path for the I2C code + BNO055 IMU2; +#endif + Command cmd; volatile byte wdt_resets = 0; //watchdog resets @@ -124,6 +136,7 @@ void setup(){ enableWatchdog(); Serial.begin(115200); //watchdogOn(); + Wire.begin(); check = EEPROM.read(0); @@ -141,7 +154,6 @@ void setup(){ pinMode(13, OUTPUT); Output1000ms.reset(); Output100ms.reset(); - DeviceManager::doDeviceSetups(); Serial.println(F("boot:1;")); } diff --git a/OpenROV/controllerboard25.cpp b/OpenROV/controllerboard25.cpp index 7b740a0..e26e9d7 100755 --- a/OpenROV/controllerboard25.cpp +++ b/OpenROV/controllerboard25.cpp @@ -26,7 +26,7 @@ float celsiusTempRead; //Pin vout("vout", CAPE_VOLTAGE_PIN, vout.analog, vout.in); //Pin iout("iout", CAPE_CURRENT_PIN, iout.analog, iout.in); - +Pin i2cpower("i2cpower", I2CPOWER_PIN, i2cpower.digital, i2cpower.out); double GetTemp(void) { @@ -123,7 +123,10 @@ void Controller25::device_setup(){ time.reset(); statustime2.reset(); onesecondtimer.reset(); - + i2cpower.reset(); + i2cpower.write(0); + delay(10); + i2cpower.write(1); // initialize all the readings to 0: for (int thisReading = 0; thisReading < numReadings; thisReading++) readings[thisReading] = 0; diff --git a/OpenROV/controllerboard25.h b/OpenROV/controllerboard25.h index 78c68c6..be6b71d 100755 --- a/OpenROV/controllerboard25.h +++ b/OpenROV/controllerboard25.h @@ -5,12 +5,15 @@ #include "Pin.h" #define LIGHTS_PIN 44 +#define ELIGHTS_PIN 46 #define CAMERAMOUNT_PIN 11 #define CALIBRATIONLASERS_PIN 45 #define PORT_PIN 6 #define VERTICAL_PIN 7 #define STARBOARD_PIN 8 #define ESCPOWER_PIN 16 +#define I2CPOWER_PIN 48 +#define ALTSERVO_PIN 9 //prewired on 2.8 controllerboard to ext lines class Controller25 : public Device { private: