├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md ├── bin ├── afro │ └── afro_nfet.hex └── pac5220 │ └── esc.bin ├── libraries ├── BNO055 │ ├── BNO055.cpp │ ├── BNO055.h │ ├── BNO055_Def.h │ ├── BNO055_MathLib.h │ ├── BNO055_Matrix.h │ ├── BNO055_Quaternion.h │ ├── BNO055_Vector.h │ └── library.properties ├── I2C │ ├── library.properties │ └── src │ │ ├── I2C.h │ │ ├── I2C_Shared.cpp │ │ ├── I2C_Shared.h │ │ ├── avr │ │ ├── I2C.cpp │ │ ├── I2C_Driver.cpp │ │ ├── I2C_Driver.h │ │ └── I2C_Properties.h │ │ └── samd │ │ ├── I2C.cpp │ │ └── I2C_Properties.h ├── MPL3115A2 │ ├── MPL3115A2.cpp │ ├── MPL3115A2.h │ └── library.properties ├── MPU9150 │ ├── MPU9150.cpp │ ├── MPU9150.h │ ├── MPU9150_Calibration.cpp │ ├── MPU9150_Calibration.h │ ├── MPU9150_DMPDriver.cpp │ ├── MPU9150_DMPDriver.h │ ├── MPU9150_DMPKey.h │ ├── MPU9150_DMPMap.h │ ├── MPU9150_Def.h │ ├── MPU9150_DriverLayer.cpp │ ├── MPU9150_DriverLayer.h │ ├── MPU9150_Quaternion.cpp │ ├── MPU9150_Quaternion.h │ ├── MPU9150_Vector3.cpp │ ├── MPU9150_Vector3.h │ └── library.properties ├── MS5803_14BA │ ├── MS5803_14BA.cpp │ ├── MS5803_14BA.h │ ├── MS5803_14BA_Def.h │ └── library.properties ├── MS5837_30BA │ ├── MS5837_30BA.cpp │ ├── MS5837_30BA.h │ ├── MS5837_30BA_Def.h │ └── library.properties ├── ORUtil │ ├── library.properties │ ├── orutil.cpp │ └── orutil.h ├── PCA9539 │ ├── PCA9539.cpp │ ├── PCA9539.h │ └── library.properties └── Servo │ ├── library.properties │ └── src │ ├── Servo.h │ ├── avr │ ├── Servo.cpp │ └── ServoTimers.h │ └── samd │ ├── Servo.cpp │ └── ServoTimers.h ├── scripts ├── astyle-file.sh └── astyle-source.sh └── sketches ├── ArduinoUSBLinker └── ArduinoUSBLinker.ino └── OpenROV2x ├── CAltServo.cpp ├── CAltServo.h ├── CAutopilot.h ├── CAutopilot_STD.cpp ├── CBNO055.cpp ├── CBNO055.h ├── CCalibrationLaser.cpp ├── CCalibrationLaser.h ├── CCameraServo.cpp ├── CCameraServo.h ├── CCommand.cpp ├── CCommand.h ├── CControllerBoard.cpp ├── CControllerBoard.h ├── CDeadManSwitch.cpp ├── CDeadManSwitch.h ├── CExternalLights.cpp ├── CExternalLights.h ├── CLights.cpp ├── CLights.h ├── CMPU9150.cpp ├── CMPU9150.h ├── CMS5803_14BA.cpp ├── CMS5803_14BA.h ├── CMS5837_30BA.cpp ├── CMS5837_30BA.h ├── CModule.cpp ├── CModule.h ├── CMotor.cpp ├── CMotor.h ├── CPin.cpp ├── CPin.h ├── CThrusters.h ├── CThrusters_2X1.cpp ├── CThrusters_2Xv2.cpp ├── CThrusters_v2X1Xv2.cpp ├── CompileOptions.h ├── NArduinoManager.cpp ├── NArduinoManager.h ├── NCommManager.cpp ├── NCommManager.h ├── NDataManager.cpp ├── NDataManager.h ├── NModuleManager.cpp ├── NModuleManager.h ├── NVehicleManager.cpp ├── NVehicleManager.h ├── OpenROV2x.ino ├── PinDefinitions.h ├── Plugins.h ├── SysConfig.h ├── SysModules.h └── open_source_attribution.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled binary files 2 | build/* 3 | bin/* 4 | .vscode/ 5 | !bin/afro/afro_nfet.hex 6 | !bin/pac5220/esc.bin -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: 2 | install: 3 | before_script: 4 | script: 5 | env: 6 | global: 7 | after_success: 8 | deploy: 9 | notifications: 10 | webhooks: 11 | urls: 12 | - https://webhooks.gitter.im/e/75083394acfa5b6df091 13 | on_success: change # options: [always|never|change] default: always 14 | on_failure: always # options: [always|never|change] default: always 15 | on_start: false # default: false 16 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 OpenROV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | V2.0 of the OpenROV Firmware Project 2 | ======================== 3 | 4 | #### Important: 5 | This version of the firmware no longer works with Cockpit versions 30.0.3 or earlier. 6 | The infrastructure to build and flash this code only exists in cockpit 31.0.1 and later. 7 | 8 | #### Description 9 | Contains sketches, configuration files, and build/upload scripts for the OpenROV Controller Board. 10 | The sketches are built automatically by the ArduinoBuilder library in the cockpit project and can currently only be easily built using that process. 11 | 12 | Eventually this project will become a GCC Makefile project to facilitate easy, cross-platform builds and automated testing. 13 | 14 | The code is organized in the following manner: 15 | - bin: contains pre-compiled binaries distributed with our images, such as ESC firmware. Built firmware also gets installed here. 16 | - libraries: contains libraries and modules shared across MCU architectures and sketches 17 | - libraries_wip: libraries that still need to be made cross-architecture 18 | - scripts: helper scripts for working with the source. Currently contains astyle presets for the project 19 | - sketches: MCU application source. These are not guaranteed to be cross-architecture/board compatible. 20 | 21 | Currently, the firmware that runs on the OpenROV 2X controllerboard can be found in sketches/OpenROV2x/ 22 | 23 | #### Style 24 | Please use one of the astyle scripts in the scripts directory to automatically format your source before submitting PRs. 25 | 26 | #### Issues 27 | For submitting issues use the primary software repository: https://github.com/OpenROV/openrov-software/issues 28 | -------------------------------------------------------------------------------- /bin/pac5220/esc.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenROV/openrov-software-arduino/49dd52b44d0a6cac6f51b90b338ceb0385e2e672/bin/pac5220/esc.bin -------------------------------------------------------------------------------- /libraries/BNO055/BNO055.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Driver definitions and helpers 4 | #include "BNO055_Def.h" 5 | 6 | namespace bno055 7 | { 8 | class BNO055 9 | { 10 | public: 11 | // Attributes 12 | TData m_data; 13 | TCalibration m_calibration; 14 | 15 | // Methods 16 | BNO055( I2C *i2cInterfaceIn, EAddress addressIn ); 17 | 18 | // State machine functions 19 | void Tick(); 20 | void HardReset(); 21 | void FullReset(); 22 | void Disable(); 23 | 24 | bool IsEnabled(); 25 | 26 | // Configuration 27 | EResult SetMode( EMode modeIn ); 28 | EMode GetMode(); 29 | 30 | uint32_t GetUpdatePeriod(); 31 | 32 | // Statistics 33 | uint32_t GetResultCount( EResult resultTypeIn ); 34 | void ClearResultCount( EResult resultTypeIn ); 35 | 36 | private: 37 | // I2C info 38 | I2C *m_pI2C; 39 | uint8_t m_address; 40 | 41 | // Configuration 42 | EMode m_mode = EMode::GYRO; 43 | 44 | // Status 45 | bool m_enabled = true; 46 | 47 | EOpMode m_opMode; 48 | uint8_t m_chipID; 49 | uint8_t m_postResults; 50 | ESystemStatus m_sysStatus; 51 | ESystemError m_sysError; 52 | TRevisionInfo m_revInfo; 53 | 54 | // State management 55 | EState m_state = EState::POWER_ON_RESET; 56 | TTransitionDelay m_delay; 57 | 58 | orutil::CTimer m_calibTimer; 59 | orutil::CTimer m_dataTimer; 60 | 61 | // Result statistics 62 | orutil::TResultCount m_results; 63 | 64 | // Methods 65 | void Transition( EState nextState ); 66 | void DelayedTransition( EState nextState, uint32_t millisIn ); 67 | 68 | EResult Cmd_Reset(); 69 | 70 | EResult Cmd_VerifyChipID(); 71 | EResult Cmd_VerifyPostResults(); 72 | 73 | EResult Cmd_ReadSystemStatus(); // 74 | EResult Cmd_ReadSystemError(); // 75 | 76 | EResult Cmd_ReadRevisionInfo(); 77 | EResult Cmd_ReadOperatingMode(); 78 | EResult Cmd_ReadCalibrationState(); 79 | 80 | EResult Cmd_ReadEulerData(); 81 | 82 | EResult Cmd_ConfigureSensor(); 83 | EResult Cmd_SetOperatingMode( EOpMode opModeIn ); 84 | 85 | // I2C Methods 86 | i2c::EI2CResult WriteRegisterByte( uint8_t registerIn, uint8_t dataIn ); 87 | i2c::EI2CResult ReadRegisterByte( uint8_t registerIn, uint8_t *dataOut ); 88 | i2c::EI2CResult ReadRegisterBytes( uint8_t registerIn, uint8_t *dataOut, uint8_t numBytesIn ); 89 | }; 90 | } -------------------------------------------------------------------------------- /libraries/BNO055/BNO055_MathLib.h: -------------------------------------------------------------------------------- 1 | /* 2 | Inertial Measurement Unit Maths Library 3 | Copyright (C) 2013-2014 Samuel Cowen 4 | www.camelsoftware.com 5 | 6 | This program is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program. If not, see . 18 | */ 19 | 20 | #pragma once 21 | 22 | // Includes all helper libraries 23 | #include "BNO055_Vector.h" 24 | #include "BNO055_Matrix.h" 25 | #include "BNO055_Quaternion.h" 26 | 27 | -------------------------------------------------------------------------------- /libraries/BNO055/BNO055_Matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | Inertial Measurement Unit Maths Library 3 | Copyright (C) 2013-2014 Samuel Cowen 4 | www.camelsoftware.com 5 | 6 | This program is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program. If not, see . 18 | */ 19 | 20 | #ifndef IMUMATH_MATRIX_HPP 21 | #define IMUMATH_MATRIX_HPP 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace bno055 29 | { 30 | template class Matrix 31 | { 32 | public: 33 | Matrix() 34 | { 35 | int r = sizeof(double)*N; 36 | _cell = &_cell_data[0]; 37 | memset(_cell, 0, r*r); 38 | } 39 | 40 | Matrix(const Matrix &v) 41 | { 42 | int r = sizeof(double)*N; 43 | _cell = &_cell_data[0]; 44 | memset(_cell, 0, r*r); 45 | for (int x = 0; x < N; x++ ) 46 | { 47 | for(int y = 0; y < N; y++) 48 | { 49 | _cell[x*N+y] = v._cell[x*N+y]; 50 | } 51 | } 52 | } 53 | 54 | ~Matrix() 55 | { 56 | } 57 | 58 | void operator = (Matrix m) 59 | { 60 | for(int x = 0; x < N; x++) 61 | { 62 | for(int y = 0; y < N; y++) 63 | { 64 | cell(x, y) = m.cell(x, y); 65 | } 66 | } 67 | } 68 | 69 | Vector row_to_vector(int y) 70 | { 71 | Vector ret; 72 | for(int i = 0; i < N; i++) 73 | { 74 | ret[i] = _cell[y*N+i]; 75 | } 76 | return ret; 77 | } 78 | 79 | Vector col_to_vector(int x) 80 | { 81 | Vector ret; 82 | for(int i = 0; i < N; i++) 83 | { 84 | ret[i] = _cell[i*N+x]; 85 | } 86 | return ret; 87 | } 88 | 89 | void vector_to_row(Vector v, int row) 90 | { 91 | for(int i = 0; i < N; i++) 92 | { 93 | cell(row, i) = v(i); 94 | } 95 | } 96 | 97 | void vector_to_col(Vector v, int col) 98 | { 99 | for(int i = 0; i < N; i++) 100 | { 101 | cell(i, col) = v(i); 102 | } 103 | } 104 | 105 | double& operator ()(int x, int y) 106 | { 107 | return _cell[x*N+y]; 108 | } 109 | 110 | double& cell(int x, int y) 111 | { 112 | return _cell[x*N+y]; 113 | } 114 | 115 | 116 | Matrix operator + (Matrix m) 117 | { 118 | Matrix ret; 119 | for(int x = 0; x < N; x++) 120 | { 121 | for(int y = 0; y < N; y++) 122 | { 123 | ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y]; 124 | } 125 | } 126 | return ret; 127 | } 128 | 129 | Matrix operator - (Matrix m) 130 | { 131 | Matrix ret; 132 | for(int x = 0; x < N; x++) 133 | { 134 | for(int y = 0; y < N; y++) 135 | { 136 | ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y]; 137 | } 138 | } 139 | return ret; 140 | } 141 | 142 | Matrix operator * (double scalar) 143 | { 144 | Matrix ret; 145 | for(int x = 0; x < N; x++) 146 | { 147 | for(int y = 0; y < N; y++) 148 | { 149 | ret._cell[x*N+y] = _cell[x*N+y] * scalar; 150 | } 151 | } 152 | return ret; 153 | } 154 | 155 | Matrix operator * (Matrix m) 156 | { 157 | Matrix ret; 158 | for(int x = 0; x < N; x++) 159 | { 160 | for(int y = 0; y < N; y++) 161 | { 162 | Vector row = row_to_vector(x); 163 | Vector col = m.col_to_vector(y); 164 | ret.cell(x, y) = row.dot(col); 165 | } 166 | } 167 | return ret; 168 | } 169 | 170 | Matrix transpose() 171 | { 172 | Matrix ret; 173 | for(int x = 0; x < N; x++) 174 | { 175 | for(int y = 0; y < N; y++) 176 | { 177 | ret.cell(y, x) = cell(x, y); 178 | } 179 | } 180 | return ret; 181 | } 182 | 183 | Matrix minor_matrix(int row, int col) 184 | { 185 | int colCount = 0, rowCount = 0; 186 | Matrix ret; 187 | for(int i = 0; i < N; i++ ) 188 | { 189 | if( i != row ) 190 | { 191 | for(int j = 0; j < N; j++ ) 192 | { 193 | if( j != col ) 194 | { 195 | ret(rowCount, colCount) = cell(i, j); 196 | colCount++; 197 | } 198 | } 199 | rowCount++; 200 | } 201 | } 202 | return ret; 203 | } 204 | 205 | double determinant() 206 | { 207 | if(N == 1) 208 | return cell(0, 0); 209 | 210 | float det = 0.0; 211 | for(int i = 0; i < N; i++ ) 212 | { 213 | Matrix minor = minor_matrix(0, i); 214 | det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant(); 215 | } 216 | return det; 217 | } 218 | 219 | Matrix invert() 220 | { 221 | Matrix ret; 222 | float det = determinant(); 223 | 224 | for(int x = 0; x < N; x++) 225 | { 226 | for(int y = 0; y < N; y++) 227 | { 228 | Matrix minor = minor_matrix(y, x); 229 | ret(x, y) = det*minor.determinant(); 230 | if( (x+y)%2 == 1) 231 | ret(x, y) = -ret(x, y); 232 | } 233 | } 234 | return ret; 235 | } 236 | 237 | private: 238 | double* _cell; 239 | double _cell_data[N*N]; 240 | }; 241 | } 242 | 243 | #endif 244 | 245 | -------------------------------------------------------------------------------- /libraries/BNO055/BNO055_Vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | Inertial Measurement Unit Maths Library 3 | Copyright (C) 2013-2014 Samuel Cowen 4 | www.camelsoftware.com 5 | 6 | This program is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program. If not, see . 18 | */ 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace bno055 27 | { 28 | 29 | template class Vector 30 | { 31 | public: 32 | Vector() 33 | { 34 | memset(p_vec, 0, sizeof(double)*N); 35 | } 36 | 37 | Vector(double a) 38 | { 39 | memset(p_vec, 0, sizeof(double)*N); 40 | p_vec[0] = a; 41 | } 42 | 43 | Vector(double a, double b) 44 | { 45 | memset(p_vec, 0, sizeof(double)*N); 46 | p_vec[0] = a; 47 | p_vec[1] = b; 48 | } 49 | 50 | Vector(double a, double b, double c) 51 | { 52 | memset(p_vec, 0, sizeof(double)*N); 53 | p_vec[0] = a; 54 | p_vec[1] = b; 55 | p_vec[2] = c; 56 | } 57 | 58 | Vector(double a, double b, double c, double d) 59 | { 60 | memset(p_vec, 0, sizeof(double)*N); 61 | p_vec[0] = a; 62 | p_vec[1] = b; 63 | p_vec[2] = c; 64 | p_vec[3] = d; 65 | } 66 | 67 | Vector(const Vector &v) 68 | { 69 | for (int x = 0; x < N; x++) 70 | p_vec[x] = v.p_vec[x]; 71 | } 72 | 73 | ~Vector() 74 | { 75 | } 76 | 77 | uint8_t n() { return N; } 78 | 79 | double magnitude() 80 | { 81 | double res = 0; 82 | int i; 83 | for(i = 0; i < N; i++) 84 | res += (p_vec[i] * p_vec[i]); 85 | 86 | if(isnan(res)) 87 | return 0; 88 | if((fabs(res-1)) >= 0.000001) // Avoid a sqrt if possible. 89 | return sqrt(res); 90 | return 1; 91 | } 92 | 93 | void normalize() 94 | { 95 | double mag = magnitude(); 96 | if(abs(mag) <= 0.0001) 97 | return; 98 | 99 | int i; 100 | for(i = 0; i < N; i++) 101 | p_vec[i] = p_vec[i]/mag; 102 | } 103 | 104 | double dot(Vector v) 105 | { 106 | double ret = 0; 107 | int i; 108 | for(i = 0; i < N; i++) 109 | ret += p_vec[i] * v.p_vec[i]; 110 | 111 | return ret; 112 | } 113 | 114 | Vector cross(Vector v) 115 | { 116 | Vector ret; 117 | 118 | // The cross product is only valid for vectors with 3 dimensions, 119 | // with the exception of higher dimensional stuff that is beyond the intended scope of this library 120 | if(N != 3) 121 | return ret; 122 | 123 | ret.p_vec[0] = (p_vec[1] * v.p_vec[2]) - (p_vec[2] * v.p_vec[1]); 124 | ret.p_vec[1] = (p_vec[2] * v.p_vec[0]) - (p_vec[0] * v.p_vec[2]); 125 | ret.p_vec[2] = (p_vec[0] * v.p_vec[1]) - (p_vec[1] * v.p_vec[0]); 126 | return ret; 127 | } 128 | 129 | Vector scale(double scalar) const 130 | { 131 | Vector ret; 132 | for(int i = 0; i < N; i++) 133 | ret.p_vec[i] = p_vec[i] * scalar; 134 | return ret; 135 | } 136 | 137 | Vector invert() const 138 | { 139 | Vector ret; 140 | for(int i = 0; i < N; i++) 141 | ret.p_vec[i] = -p_vec[i]; 142 | return ret; 143 | } 144 | 145 | Vector operator = (Vector v) 146 | { 147 | for (int x = 0; x < N; x++ ) 148 | p_vec[x] = v.p_vec[x]; 149 | return *this; 150 | } 151 | 152 | double& operator [](int n) 153 | { 154 | return p_vec[n]; 155 | } 156 | 157 | double operator [](int n) const 158 | { 159 | return p_vec[n]; 160 | } 161 | 162 | double& operator ()(int n) 163 | { 164 | return p_vec[n]; 165 | } 166 | 167 | double operator ()(int n) const 168 | { 169 | return p_vec[n]; 170 | } 171 | 172 | Vector operator + (Vector v) const 173 | { 174 | Vector ret; 175 | for(int i = 0; i < N; i++) 176 | ret.p_vec[i] = p_vec[i] + v.p_vec[i]; 177 | return ret; 178 | } 179 | 180 | Vector operator - (Vector v) const 181 | { 182 | Vector ret; 183 | for(int i = 0; i < N; i++) 184 | ret.p_vec[i] = p_vec[i] - v.p_vec[i]; 185 | return ret; 186 | } 187 | 188 | Vector operator * (double scalar) const 189 | { 190 | return scale(scalar); 191 | } 192 | 193 | Vector operator / (double scalar) const 194 | { 195 | Vector ret; 196 | for(int i = 0; i < N; i++) 197 | ret.p_vec[i] = p_vec[i] / scalar; 198 | return ret; 199 | } 200 | 201 | void toDegrees() 202 | { 203 | for(int i = 0; i < N; i++) 204 | p_vec[i] *= 57.2957795131; //180/pi 205 | } 206 | 207 | void toRadians() 208 | { 209 | for(int i = 0; i < N; i++) 210 | p_vec[i] *= 0.01745329251; //pi/180 211 | } 212 | 213 | double& x() { return p_vec[0]; } 214 | double& y() { return p_vec[1]; } 215 | double& z() { return p_vec[2]; } 216 | double x() const { return p_vec[0]; } 217 | double y() const { return p_vec[1]; } 218 | double z() const { return p_vec[2]; } 219 | 220 | 221 | private: 222 | double p_vec[N]; 223 | }; 224 | } 225 | -------------------------------------------------------------------------------- /libraries/BNO055/library.properties: -------------------------------------------------------------------------------- 1 | name=BNO055 2 | version=1.0 3 | author=OpenROV, Charles Cross 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/I2C/library.properties: -------------------------------------------------------------------------------- 1 | name=I2C 2 | version=1.0 3 | author=OpenROV, Charles Cross 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/I2C/src/I2C.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | // Architecture specific definitions 6 | #if defined( ARDUINO_ARCH_AVR ) 7 | #include "avr/I2C_Properties.h" 8 | #elif defined( ARDUINO_ARCH_SAMD ) 9 | #include "samd/I2C_Properties.h" 10 | #else 11 | #error "I2C library used on unsupported architecture!" 12 | #endif 13 | 14 | // Shared I2C definitions 15 | #include "I2C_Shared.h" 16 | 17 | // Pass this in at build time to enable error counting 18 | #define ENABLE_I2C_DEBUG 1 19 | 20 | class I2C 21 | { 22 | public: 23 | // Attributes 24 | #ifdef ENABLE_I2C_DEBUG 25 | i2c::TI2CStats m_statistics; 26 | #endif 27 | 28 | i2c::EI2CResult m_result; 29 | 30 | // Methods 31 | I2C(const I2C&) = delete; // Delete copy constructor 32 | I2C& operator=(const I2C &) = delete; // Delete copy assignment operator 33 | 34 | // Arch specific constructors 35 | #if defined( ARDUINO_ARCH_AVR ) 36 | I2C(); // AVR only has one I2C interface on predetermined pins 37 | #elif defined( ARDUINO_ARCH_SAMD ) 38 | I2C( SERCOM *sercomInstance, uint8_t pinSDA, uint8_t pinSCL ); // SAMD requires a specific SERCOM interface and pins 39 | I2C() = delete; // Delete default constructor 40 | #endif 41 | 42 | i2c::EI2CResult Enable(); 43 | i2c::EI2CResult Disable(); 44 | i2c::EI2CResult Reset(); 45 | 46 | bool LockAddress( uint8_t addressIn ); 47 | void FreeAddress( uint8_t addressIn ); 48 | 49 | bool IsAvailable(); 50 | i2c::EI2CResult SetBaudRate( i2c::EI2CBaudRate baudRateIn ); 51 | 52 | #ifdef ENABLE_I2C_DEBUG 53 | uint32_t GetResultCount( i2c::EI2CResult resultIn ) 54 | { 55 | return m_statistics.GetResultCount( resultIn ); 56 | } 57 | #endif 58 | 59 | // Write operations 60 | i2c::EI2CResult WriteByte( uint8_t slaveAddressIn, uint8_t dataIn, bool issueRepeatedStart = false ); 61 | i2c::EI2CResult WriteRegisterByte( uint8_t slaveAddressIn, uint8_t registerIn, uint8_t dataIn, bool issueRepeatedStart = false ); 62 | i2c::EI2CResult WriteBytes( uint8_t slaveAddressIn, uint8_t *dataIn, uint8_t numberBytesIn, bool issueRepeatedStart = false ); 63 | i2c::EI2CResult WriteRegisterBytes( uint8_t slaveAddressIn, uint8_t registerIn, uint8_t *dataIn, uint8_t numberBytesIn, bool issueRepeatedStart = false ); 64 | 65 | // Read operations 66 | i2c::EI2CResult ReadRegisterByte( uint8_t slaveAddressIn, uint8_t registerIn, uint8_t *dataOut, bool issueRepeatedStart = false ); 67 | i2c::EI2CResult ReadRegisterBytes( uint8_t slaveAddressIn, uint8_t registerIn, uint8_t *dataOut, uint8_t numberBytesIn, bool issueRepeatedStart = false ); 68 | 69 | private: 70 | // Attributes 71 | bool m_isEnabled = false; 72 | i2c::EI2CBaudRate m_baudRate = i2c::EI2CBaudRate::BAUDRATE_400K; 73 | 74 | uint8_t m_addressLocks[ 127 ] = { 0 }; 75 | 76 | // Custom properties defined for specific architectures 77 | TI2CProperties m_customProperties; 78 | }; 79 | 80 | // Arch specific instantiation 81 | #if defined( ARDUINO_ARCH_AVR ) 82 | // AVR 83 | extern I2C I2C0; 84 | 85 | #elif defined( ARDUINO_ARCH_SAMD ) 86 | // SAMD 87 | #if WIRE_INTERFACES_COUNT > 0 88 | extern I2C I2C0; 89 | #endif 90 | #if WIRE_INTERFACES_COUNT > 1 91 | extern I2C I2C1; 92 | #endif 93 | #if WIRE_INTERFACES_COUNT > 2 94 | extern I2C I2C2; 95 | #endif 96 | #if WIRE_INTERFACES_COUNT > 3 97 | extern I2C I2C3; 98 | #endif 99 | #if WIRE_INTERFACES_COUNT > 4 100 | extern I2C I2C4; 101 | #endif 102 | #if WIRE_INTERFACES_COUNT > 5 103 | extern I2C I2C5; 104 | #endif 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /libraries/I2C/src/I2C_Shared.cpp: -------------------------------------------------------------------------------- 1 | #include "I2C_Shared.h" 2 | 3 | namespace i2c 4 | { 5 | TI2CStats::TI2CStats() 6 | : results{0} 7 | { 8 | } 9 | 10 | EI2CResult TI2CStats::AddResult( EI2CResult resultIn ) 11 | { 12 | results[ resultIn ] += 1; 13 | return resultIn; 14 | } 15 | 16 | uint32_t TI2CStats::GetResultCount( EI2CResult resultIn ) 17 | { 18 | return results[ resultIn ]; 19 | } 20 | } -------------------------------------------------------------------------------- /libraries/I2C/src/I2C_Shared.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace i2c 6 | { 7 | enum class EI2CBaudRate 8 | { 9 | BAUDRATE_100K, 10 | BAUDRATE_400K 11 | }; 12 | 13 | enum EI2CAction: uint8_t 14 | { 15 | I2C_WRITE = 0x0, 16 | I2C_READ = 0x1 17 | }; 18 | 19 | struct TI2CTransfer 20 | { 21 | uint8_t slaveAddress; 22 | uint32_t length; 23 | uint8_t *buffer; 24 | 25 | EI2CAction action; 26 | bool issueRepeatedStart; 27 | }; 28 | 29 | // I2C Operation Result Codes 30 | enum EI2CResult : uint8_t 31 | { 32 | // Normal results 33 | RESULT_SUCCESS = 0, // Operation successful 34 | RESULT_NACK, // Transaction was denied or there was no response 35 | 36 | // Errors 37 | RESULT_ERR_TIMEOUT, // Operation timed out 38 | RESULT_ERR_FAILED, // Operation failed 39 | RESULT_ERR_ALREADY_INITIALIZED, // Interface already initialized 40 | RESULT_ERR_INVALID_BAUD, // Invalid baud rate specified 41 | RESULT_ERR_LOST_ARBITRATION, // Lost arbitration during transaction 42 | RESULT_ERR_BAD_ADDRESS, // Invalid I2C slave address specified 43 | 44 | // Counter 45 | RESULT_TYPES 46 | }; 47 | 48 | // Helper struct for collecting error statistics 49 | struct TI2CStats 50 | { 51 | TI2CStats(); 52 | 53 | uint32_t results[ EI2CResult::RESULT_TYPES ]; 54 | 55 | EI2CResult AddResult( EI2CResult resultIn ); 56 | 57 | uint32_t GetResultCount( EI2CResult resultIn ); 58 | }; 59 | } -------------------------------------------------------------------------------- /libraries/I2C/src/avr/I2C_Driver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace i2c 6 | { 7 | namespace avr 8 | { 9 | i2c::EI2CResult Enable( i2c::EI2CBaudRate baudRateIn ); 10 | i2c::EI2CResult Disable(); 11 | i2c::EI2CResult SetBaudRate( i2c::EI2CBaudRate baudRateIn ); 12 | 13 | i2c::EI2CResult StartTransaction(); 14 | i2c::EI2CResult SendAddress( uint8_t slaveAddressIn, i2c::EI2CAction actionIn ); 15 | i2c::EI2CResult WriteByte( uint8_t byteIn ); 16 | i2c::EI2CResult ReadByte( uint8_t* dataOut, bool sendNack ); 17 | i2c::EI2CResult StopTransaction(); 18 | i2c::EI2CResult WaitForInterrupt( uint8_t &twiStatusOut ); 19 | i2c::EI2CResult WaitForStop(); 20 | 21 | void Reset(); 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /libraries/I2C/src/avr/I2C_Properties.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | struct TI2CProperties 4 | { 5 | // No special properties for the AVR I2C interface 6 | }; -------------------------------------------------------------------------------- /libraries/I2C/src/samd/I2C.cpp: -------------------------------------------------------------------------------- 1 | #if defined( ARDUINO_ARCH_SAMD ) 2 | 3 | 4 | 5 | #endif -------------------------------------------------------------------------------- /libraries/I2C/src/samd/I2C_Properties.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace samd 7 | { 8 | namespace i2c 9 | { 10 | constexpr uint8_t kI2CTxBufferSize_bytes = 32; 11 | constexpr uint32_t kI2CDefaultTimeout_ms = 25; 12 | 13 | struct TI2CProperties 14 | { 15 | SERCOM *m_pSercom; 16 | uint8_t m_pinSDA; 17 | uint8_t m_pinSCL; 18 | 19 | TOptions m_options; 20 | TI2CTransfer m_transfer; 21 | uint8_t m_pTransferBuffer[ kI2CTxBufferSize_bytes ]; // Internal buffer used for single byte reads and writes, and buffered reads 22 | uint32_t m_timeout_ms = kI2CDefaultTimeout_ms; 23 | }; 24 | } 25 | } -------------------------------------------------------------------------------- /libraries/MPL3115A2/MPL3115A2.h: -------------------------------------------------------------------------------- 1 | /* 2 | MPL3115A2 Barometric Pressure Sensor Library 3 | 4 | Get pressure, altitude and temperature from the MPL3115A2 sensor. 5 | 6 | September 6, 2016: Modified for use in OpenROV's Software 7 | 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | 15 | namespace mpl3115a2 16 | { 17 | // Unshifted 7-bit I2C address for sensor 18 | const uint8_t MPL3115A2_ADDRESS = 0x60; 19 | 20 | //TODO 21 | //We should really have a system wide firmware ret code framework 22 | enum ERetCode : int32_t 23 | { 24 | SUCCESS, 25 | FAILED, 26 | FAILED_ONESHOT, 27 | FAILED_PRESSURE_READ, 28 | FAILED_TEMP_READ, 29 | TIMED_OUT, 30 | UNKNOWN 31 | }; 32 | 33 | enum EMode 34 | { 35 | BAROMETER, 36 | ALTIMETER, 37 | TEMPERATURE, 38 | STANDBY, 39 | ACTIVE 40 | }; 41 | 42 | enum class EOversampleRatio : uint8_t 43 | { 44 | OSR_1 = 0, 45 | OSR_2 = 1, 46 | OSR_4 = 2, 47 | OSR_8 = 3, 48 | OSR_16 = 4, 49 | OSR_32 = 5, 50 | OSR_64 = 6, 51 | OSR_128 = 7 52 | }; 53 | 54 | class MPL3115A2 55 | { 56 | 57 | public: 58 | 59 | MPL3115A2( I2C *i2cInterfaceIn ); 60 | 61 | //Public member functions 62 | ERetCode Initialize(); 63 | bool IsInitialized() const { return m_isInitialized; }; 64 | 65 | ERetCode ReadPressure( float& pressureOut ); 66 | ERetCode ReadAltitude( float& altitudeOut ); 67 | ERetCode ReadPressureAndTemp( float& pressureOut, float& tempOut ); 68 | 69 | ERetCode SetMode( EMode modeIn ); 70 | ERetCode SetOversampleRatio( EOversampleRatio osrIn ); 71 | ERetCode EnableEventFlags(); 72 | 73 | private: 74 | 75 | //TODO 76 | //Add bit fields 77 | 78 | enum class MPL3115A2_REGISTER : uint8_t 79 | { 80 | STATUS = 0x00, 81 | 82 | PRESSURE_OUT_MSB = 0x01, 83 | PRESSURE_OUT_CSB = 0x02, 84 | PRESSURE_OUT_LSB = 0x03, 85 | 86 | TEMP_OUT_MSB = 0x04, 87 | TEMP_OUT_LSB = 0x05, 88 | 89 | DR_STATUS = 0x06, 90 | 91 | PRESSURE_OUT_DELTA_MSB = 0x07, 92 | PRESSURE_OUT_DELTA_CSB = 0x08, 93 | PRESSURE_OUT_DELTA_LSB = 0x09, 94 | 95 | TEMP_OUT_DELTA_MSB = 0x0A, 96 | TEMP_OUT_DELTA_LSB = 0x0B, 97 | 98 | WHO_AM_I = 0x0C, 99 | 100 | FIFO_STATUS = 0x0D, 101 | FIFO_DATA = 0x0E, 102 | FIFO_SETUP = 0x0F, 103 | 104 | TIME_DELAY = 0x10, 105 | 106 | SYSMOD = 0x11, 107 | 108 | INT_SOURCE = 0x12, 109 | 110 | PT_DATA_CFG = 0x13, 111 | 112 | BAR_INPUT_MSB = 0x14, 113 | BAR_INPUT_LSB = 0x15, 114 | 115 | PRESSURE_TARGET_MSB = 0x16, 116 | PRESSURE_TARGET_LSB = 0x17, 117 | 118 | TEMP_TARGET = 0x18, 119 | 120 | PRESSURE_WINDOW_MSB = 0x19, 121 | PRESSURE_WINDOW_LSB = 0x1A, 122 | 123 | TEMP_WINDOW = 0x1B, 124 | 125 | MIN_PRESSURE_DATA_OUT_MSB = 0x1C, 126 | MIN_PRESSURE_DATA_OUT_CSB = 0x1D, 127 | MIN_PRESSURE_DATA_OUT_LSB = 0x1E, 128 | 129 | MIN_TEMP_DATA_OUT_MSB = 0x1F, 130 | MIN_TEMP_DATA_OUT_LSB = 0x20, 131 | 132 | MAX_PRESSURE_DATA_OUT_MSB = 0x21, 133 | MAX_PRESSURE_DATA_OUT_CSB = 0x22, 134 | MAX_PRESSURE_DATA_OUT_LSB = 0x23, 135 | 136 | MAX_TEMP_DATA_OUT_MSB = 0x24, 137 | MAX_TEMP_DATA_OUT_LSB = 0x25, 138 | 139 | CONTROL_REGISTER_1 = 0x26, 140 | CONTROL_REGISTER_2 = 0x27, 141 | CONTROL_REGISTER_3 = 0x28, 142 | CONTROL_REGISTER_4 = 0x29, 143 | CONTROL_REGISTER_5 = 0x2A, 144 | 145 | PRESSURE_DATA_OFFSET = 0x2B, 146 | TEMP_DATA_OFFSET = 0x2C, 147 | ALT_DATA_OFFSET = 0x2D 148 | }; 149 | 150 | //Private member functions 151 | ERetCode VerifyChipId(); 152 | ERetCode ToggleOneShot(); 153 | 154 | ERetCode SetModeActive(); 155 | ERetCode SetModeAltimeter(); 156 | ERetCode SetModeBarometer(); 157 | ERetCode SetModeStandby(); 158 | 159 | int32_t ReadByte( MPL3115A2_REGISTER addressIn, uint8_t& dataOut ); 160 | int32_t ReadNBytes( MPL3115A2_REGISTER addressIn, uint8_t* dataOut, uint8_t byteCountIn ); 161 | int32_t WriteByte( MPL3115A2_REGISTER addressIn, uint8_t dataIn ); 162 | 163 | //Private member Attributes 164 | uint8_t m_i2cAddress; 165 | I2C *m_pI2C; 166 | 167 | bool m_isInitialized = false; 168 | }; 169 | } 170 | 171 | 172 | 173 | -------------------------------------------------------------------------------- /libraries/MPL3115A2/library.properties: -------------------------------------------------------------------------------- 1 | name=MPL3115A2 2 | version=1.0 3 | author=OpenROV 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of MPU9150 4 | // 5 | // Copyright (c) 2013 Pansenti, LLC 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _MPU9150_H_ 25 | #define _MPU9150_H_ 26 | 27 | #include "MPU9150_Def.h" 28 | #include "MPU9150_Quaternion.h" 29 | #include "MPU9150_Calibration.h" 30 | 31 | // This symbol defines the scaled range for mag and accel values 32 | #define SENSOR_RANGE 4096 33 | 34 | namespace mpu9150 35 | { 36 | class MPU9150 37 | { 38 | public: 39 | // Attributes 40 | long m_rawQuaternion[4]; // the quaternion output from the DMP 41 | short m_rawGyro[3]; // calibrated gyro output from the sensor 42 | short m_rawAccel[3]; // raw accel data 43 | short m_rawMag[3]; // raw mag data 44 | 45 | float m_dmpQuaternion[4]; // float and normalized version of the dmp quaternion 46 | float m_dmpEulerPose[3]; // Euler angles from the DMP quaternion 47 | short m_calAccel[3]; // calibrated and scaled accel data 48 | short m_calMag[3]; // calibrated mag data 49 | 50 | float m_fusedEulerPose[3]; // the fused Euler angles 51 | float m_fusedQuaternion[4]; // the fused quaternion 52 | 53 | // Constructor 54 | MPU9150( EAddress addressIn ); 55 | 56 | void useAccelCal( bool useCal ); 57 | void useMagCal( bool useCal ); 58 | 59 | // init must be called to setup the MPU chip. 60 | // mpuRate is the update rate in Hz. 61 | // magMix controls the amoutn of influence that the magnetometer has on yaw: 62 | // 0 = just use MPU gyros (will not be referenced to north) 63 | // 1 = just use magnetometer with no input from gyros 64 | // 2-n = mix the two. Higher numbers decrease correction from magnetometer 65 | // It returns false if something went wrong with the initialization. 66 | // magRate is the magnetometer update rate in Hz. magRate <= mpuRate. 67 | // Also, magRate must be <= 100Hz. 68 | // lpf is the low pass filter setting - can be between 5Hz and 188Hz. 69 | // 0 means let the MotionDriver library decide. 70 | 71 | bool init( int mpuRate, int magMix = 5, int magRate = 10, int lpf = 0 ); 72 | bool read(); 73 | void disableAccelCal(); 74 | 75 | bool isMagCal(); 76 | bool isAccelCal(); 77 | 78 | // Statistics 79 | void AddResult( EResult resultTypeIn ); 80 | uint32_t GetResultCount( EResult resultTypeIn ); 81 | void ClearResultCount( EResult resultTypeIn ); 82 | 83 | private: 84 | // Result statistics 85 | orutil::TResultCount m_results; 86 | 87 | CALLIB_DATA m_calData; // calibration data 88 | bool m_useMagCalibration; // true if use mag calibration 89 | bool m_useAccelCalibration; // true if use mag calibration 90 | uint8_t m_device; // IMU device index 91 | int m_magMix; // controls gyro and magnetometer mixing for yaw 92 | unsigned long m_magInterval; // interval between mag reads in mS 93 | unsigned long m_lastMagSample; // last time mag was read 94 | 95 | float m_lastDMPYaw; // the last yaw from the DMP gyros 96 | float m_lastYaw; // last calculated output yaw 97 | 98 | // Calibration data in processed form 99 | short m_magXOffset; // offset to be structed for mag X 100 | short m_magXRange; // range of mag X 101 | short m_magYOffset; // offset to be structed for mag Y 102 | short m_magYRange; // range of mag Y 103 | short m_magZOffset; // offset to be structed for mag Z 104 | short m_magZRange; // range of mag Z 105 | 106 | short m_accelXRange; // range of accel X 107 | short m_accelYRange; // range of accel Y 108 | short m_accelZRange; // range of accel Z 109 | long m_accelOffset[3]; // offsets for accel 110 | 111 | // Fuse mag data with the dmp quaternion 112 | void dataFusion(); 113 | 114 | }; 115 | } 116 | 117 | #endif // _MPU9150_H_ 118 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Calibration.cpp: -------------------------------------------------------------------------------- 1 | 2 | //////////////////////////////////////////////////////////////////////////// 3 | // 4 | // This file is part of MPU9150Lib 5 | // 6 | // Copyright (c) 2013 Pansenti, LLC 7 | // 8 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 9 | // this software and associated documentation files (the "Software"), to deal in 10 | // the Software without restriction, including without limitation the rights to use, 11 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 12 | // Software, and to permit persons to whom the Software is furnished to do so, 13 | // subject to the following conditions: 14 | // 15 | // The above copyright notice and this permission notice shall be included in all 16 | // copies or substantial portions of the Software. 17 | // 18 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 19 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 20 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 21 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 22 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 23 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24 | 25 | #include "MPU9150_Calibration.h" 26 | 27 | // AVR version 28 | 29 | #include 30 | 31 | void calLibErase( uint8_t device ) 32 | { 33 | EEPROM.write( CALLIB_START, 0 ); // just destroy the valid byte 34 | } 35 | 36 | void calLibWrite( uint8_t device, CALLIB_DATA* calData ) 37 | { 38 | uint8_t* ptr = ( uint8_t* )calData; 39 | uint8_t length = sizeof( CALLIB_DATA ); 40 | int eeprom = CALLIB_START; 41 | 42 | calData->valid = CALLIB_DATA_VALID; 43 | 44 | for( uint8_t i = 0; i < length; i++ ) 45 | { 46 | EEPROM.write( eeprom + i, *ptr++ ); 47 | } 48 | } 49 | 50 | bool calLibRead( uint8_t device, CALLIB_DATA* calData ) 51 | { 52 | uint8_t* ptr = ( uint8_t* )calData; 53 | uint8_t length = sizeof( CALLIB_DATA ); 54 | int eeprom = CALLIB_START; 55 | 56 | calData->magValid = false; 57 | calData->accelValid = false; 58 | 59 | if( ( EEPROM.read( eeprom ) != CALLIB_DATA_VALID_LOW ) || 60 | ( EEPROM.read( eeprom + 1 ) != CALLIB_DATA_VALID_HIGH ) ) 61 | { 62 | return false; // invalid data 63 | } 64 | 65 | for( uint8_t i = 0; i < length; i++ ) 66 | { 67 | *ptr++ = EEPROM.read( eeprom + i ); 68 | } 69 | 70 | return true; 71 | } -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Calibration.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of MPU9150Lib 4 | // 5 | // Copyright (c) 2013 Pansenti, LLC 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef _CALLIB_H_ 25 | #define _CALLIB_H_ 26 | 27 | #include 28 | 29 | #define CALLIB_DATA_VALID 0x15fc 30 | #define CALLIB_DATA_VALID_LOW 0xfc // pattern to detect valid config - low byte 31 | #define CALLIB_DATA_VALID_HIGH 0x15 // pattern to detect valid config - high byte 32 | 33 | #define MPU9150_EEPROM_START 2 34 | #define CALLIB_START MPU9150_EEPROM_START 35 | 36 | typedef struct 37 | { 38 | short valid; // should contain the valid pattern if a good config 39 | short magValid; // true if mag data valid 40 | short magMinX; // mag min x value 41 | short magMaxX; // mag max x value 42 | short magMinY; // mag min y value 43 | short magMaxY; // mag max y value 44 | short magMinZ; // mag min z value 45 | short magMaxZ; // mag max z value 46 | short accelValid; // true if accel data valid 47 | short accelMinX; // mag min x value 48 | short accelMaxX; // mag max x value 49 | short accelMinY; // mag min y value 50 | short accelMaxY; // mag max y value 51 | short accelMinZ; // mag min z value 52 | short accelMaxZ; // mag max z value 53 | short unused; // must be multiple of 32 bits for Due 54 | } CALLIB_DATA; 55 | 56 | // calLibErase() erases any current data in the EEPROM 57 | 58 | void calLibErase(uint8_t device); 59 | 60 | // calLibWrite() writes new data to the EEPROM 61 | 62 | void calLibWrite(uint8_t device, CALLIB_DATA * calData); 63 | 64 | // calLibRead() reads existing data and returns true if valid else false in not. 65 | 66 | bool calLibRead(uint8_t device, CALLIB_DATA * calData); 67 | 68 | #endif // _CALLIB_H_ 69 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_DMPDriver.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu_dmp_motion_driver.h 13 | * @brief DMP image and interface functions. 14 | * @details All functions are preceded by the dmp_ prefix to 15 | * differentiate among MPL and general driver function calls. 16 | */ 17 | #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ 18 | #define _INV_MPU_DMP_MOTION_DRIVER_H_ 19 | 20 | #define TAP_X (0x01) 21 | #define TAP_Y (0x02) 22 | #define TAP_Z (0x04) 23 | #define TAP_XYZ (0x07) 24 | 25 | #define TAP_X_UP (0x01) 26 | #define TAP_X_DOWN (0x02) 27 | #define TAP_Y_UP (0x03) 28 | #define TAP_Y_DOWN (0x04) 29 | #define TAP_Z_UP (0x05) 30 | #define TAP_Z_DOWN (0x06) 31 | 32 | #define ANDROID_ORIENT_PORTRAIT (0x00) 33 | #define ANDROID_ORIENT_LANDSCAPE (0x01) 34 | #define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02) 35 | #define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03) 36 | 37 | #define DMP_INT_GESTURE (0x01) 38 | #define DMP_INT_CONTINUOUS (0x02) 39 | 40 | #define DMP_FEATURE_TAP (0x001) 41 | #define DMP_FEATURE_ANDROID_ORIENT (0x002) 42 | #define DMP_FEATURE_LP_QUAT (0x004) 43 | #define DMP_FEATURE_PEDOMETER (0x008) 44 | #define DMP_FEATURE_6X_LP_QUAT (0x010) 45 | #define DMP_FEATURE_GYRO_CAL (0x020) 46 | #define DMP_FEATURE_SEND_RAW_ACCEL (0x040) 47 | #define DMP_FEATURE_SEND_RAW_GYRO (0x080) 48 | #define DMP_FEATURE_SEND_CAL_GYRO (0x100) 49 | 50 | #define INV_WXYZ_QUAT (0x100) 51 | 52 | /* Set up functions. */ 53 | void dmp_init_structures(); 54 | int dmp_select_device(int device); 55 | int dmp_load_motion_driver_firmware(void); 56 | int dmp_set_fifo_rate(unsigned short rate); 57 | int dmp_get_fifo_rate(unsigned short *rate); 58 | int dmp_enable_feature(unsigned short mask); 59 | int dmp_get_enabled_features(unsigned short *mask); 60 | int dmp_set_interrupt_mode(unsigned char mode); 61 | int dmp_set_orientation(unsigned short orient); 62 | int dmp_set_gyro_bias(long *bias); 63 | int dmp_set_accel_bias(long *bias); 64 | 65 | #ifdef MPU_MAXIMAL 66 | /* Tap functions. */ 67 | int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)); 68 | int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh); 69 | int dmp_set_tap_axes(unsigned char axis); 70 | int dmp_set_tap_count(unsigned char min_taps); 71 | int dmp_set_tap_time(unsigned short time); 72 | int dmp_set_tap_time_multi(unsigned short time); 73 | int dmp_set_shake_reject_thresh(long sf, unsigned short thresh); 74 | int dmp_set_shake_reject_time(unsigned short time); 75 | int dmp_set_shake_reject_timeout(unsigned short time); 76 | 77 | /* Android orientation functions. */ 78 | int dmp_register_android_orient_cb(void (*func)(unsigned char)); 79 | #endif // MPU_MAXIMAL 80 | 81 | /* LP quaternion functions. */ 82 | int dmp_enable_lp_quat(unsigned char enable); 83 | int dmp_enable_6x_lp_quat(unsigned char enable); 84 | 85 | #ifdef MPU_MAXIMAL 86 | /* Pedometer functions. */ 87 | int dmp_get_pedometer_step_count(unsigned long *count); 88 | int dmp_set_pedometer_step_count(unsigned long count); 89 | int dmp_get_pedometer_walk_time(unsigned long *time); 90 | int dmp_set_pedometer_walk_time(unsigned long time); 91 | #endif // MPU_MAXIMAL 92 | 93 | /* DMP gyro calibration functions. */ 94 | int dmp_enable_gyro_cal(unsigned char enable); 95 | 96 | /* Read function. This function should be called whenever the MPU interrupt is 97 | * detected. 98 | */ 99 | int dmp_read_fifo(short *gyro, short *accel, long *quat, 100 | unsigned long *timestamp, short *sensors, unsigned char *more); 101 | 102 | #endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */ 103 | 104 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_DMPMap.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. 4 | $ 5 | */ 6 | #ifndef DMPMAP_H 7 | #define DMPMAP_H 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | #define DMP_PTAT 0 15 | #define DMP_XGYR 2 16 | #define DMP_YGYR 4 17 | #define DMP_ZGYR 6 18 | #define DMP_XACC 8 19 | #define DMP_YACC 10 20 | #define DMP_ZACC 12 21 | #define DMP_ADC1 14 22 | #define DMP_ADC2 16 23 | #define DMP_ADC3 18 24 | #define DMP_BIASUNC 20 25 | #define DMP_FIFORT 22 26 | #define DMP_INVGSFH 24 27 | #define DMP_INVGSFL 26 28 | #define DMP_1H 28 29 | #define DMP_1L 30 30 | #define DMP_BLPFSTCH 32 31 | #define DMP_BLPFSTCL 34 32 | #define DMP_BLPFSXH 36 33 | #define DMP_BLPFSXL 38 34 | #define DMP_BLPFSYH 40 35 | #define DMP_BLPFSYL 42 36 | #define DMP_BLPFSZH 44 37 | #define DMP_BLPFSZL 46 38 | #define DMP_BLPFMTC 48 39 | #define DMP_SMC 50 40 | #define DMP_BLPFMXH 52 41 | #define DMP_BLPFMXL 54 42 | #define DMP_BLPFMYH 56 43 | #define DMP_BLPFMYL 58 44 | #define DMP_BLPFMZH 60 45 | #define DMP_BLPFMZL 62 46 | #define DMP_BLPFC 64 47 | #define DMP_SMCTH 66 48 | #define DMP_0H2 68 49 | #define DMP_0L2 70 50 | #define DMP_BERR2H 72 51 | #define DMP_BERR2L 74 52 | #define DMP_BERR2NH 76 53 | #define DMP_SMCINC 78 54 | #define DMP_ANGVBXH 80 55 | #define DMP_ANGVBXL 82 56 | #define DMP_ANGVBYH 84 57 | #define DMP_ANGVBYL 86 58 | #define DMP_ANGVBZH 88 59 | #define DMP_ANGVBZL 90 60 | #define DMP_BERR1H 92 61 | #define DMP_BERR1L 94 62 | #define DMP_ATCH 96 63 | #define DMP_BIASUNCSF 98 64 | #define DMP_ACT2H 100 65 | #define DMP_ACT2L 102 66 | #define DMP_GSFH 104 67 | #define DMP_GSFL 106 68 | #define DMP_GH 108 69 | #define DMP_GL 110 70 | #define DMP_0_5H 112 71 | #define DMP_0_5L 114 72 | #define DMP_0_0H 116 73 | #define DMP_0_0L 118 74 | #define DMP_1_0H 120 75 | #define DMP_1_0L 122 76 | #define DMP_1_5H 124 77 | #define DMP_1_5L 126 78 | #define DMP_TMP1AH 128 79 | #define DMP_TMP1AL 130 80 | #define DMP_TMP2AH 132 81 | #define DMP_TMP2AL 134 82 | #define DMP_TMP3AH 136 83 | #define DMP_TMP3AL 138 84 | #define DMP_TMP4AH 140 85 | #define DMP_TMP4AL 142 86 | #define DMP_XACCW 144 87 | #define DMP_TMP5 146 88 | #define DMP_XACCB 148 89 | #define DMP_TMP8 150 90 | #define DMP_YACCB 152 91 | #define DMP_TMP9 154 92 | #define DMP_ZACCB 156 93 | #define DMP_TMP10 158 94 | #define DMP_DZH 160 95 | #define DMP_DZL 162 96 | #define DMP_XGCH 164 97 | #define DMP_XGCL 166 98 | #define DMP_YGCH 168 99 | #define DMP_YGCL 170 100 | #define DMP_ZGCH 172 101 | #define DMP_ZGCL 174 102 | #define DMP_YACCW 176 103 | #define DMP_TMP7 178 104 | #define DMP_AFB1H 180 105 | #define DMP_AFB1L 182 106 | #define DMP_AFB2H 184 107 | #define DMP_AFB2L 186 108 | #define DMP_MAGFBH 188 109 | #define DMP_MAGFBL 190 110 | #define DMP_QT1H 192 111 | #define DMP_QT1L 194 112 | #define DMP_QT2H 196 113 | #define DMP_QT2L 198 114 | #define DMP_QT3H 200 115 | #define DMP_QT3L 202 116 | #define DMP_QT4H 204 117 | #define DMP_QT4L 206 118 | #define DMP_CTRL1H 208 119 | #define DMP_CTRL1L 210 120 | #define DMP_CTRL2H 212 121 | #define DMP_CTRL2L 214 122 | #define DMP_CTRL3H 216 123 | #define DMP_CTRL3L 218 124 | #define DMP_CTRL4H 220 125 | #define DMP_CTRL4L 222 126 | #define DMP_CTRLS1 224 127 | #define DMP_CTRLSF1 226 128 | #define DMP_CTRLS2 228 129 | #define DMP_CTRLSF2 230 130 | #define DMP_CTRLS3 232 131 | #define DMP_CTRLSFNLL 234 132 | #define DMP_CTRLS4 236 133 | #define DMP_CTRLSFNL2 238 134 | #define DMP_CTRLSFNL 240 135 | #define DMP_TMP30 242 136 | #define DMP_CTRLSFJT 244 137 | #define DMP_TMP31 246 138 | #define DMP_TMP11 248 139 | #define DMP_CTRLSF2_2 250 140 | #define DMP_TMP12 252 141 | #define DMP_CTRLSF1_2 254 142 | #define DMP_PREVPTAT 256 143 | #define DMP_ACCZB 258 144 | #define DMP_ACCXB 264 145 | #define DMP_ACCYB 266 146 | #define DMP_1HB 272 147 | #define DMP_1LB 274 148 | #define DMP_0H 276 149 | #define DMP_0L 278 150 | #define DMP_ASR22H 280 151 | #define DMP_ASR22L 282 152 | #define DMP_ASR6H 284 153 | #define DMP_ASR6L 286 154 | #define DMP_TMP13 288 155 | #define DMP_TMP14 290 156 | #define DMP_FINTXH 292 157 | #define DMP_FINTXL 294 158 | #define DMP_FINTYH 296 159 | #define DMP_FINTYL 298 160 | #define DMP_FINTZH 300 161 | #define DMP_FINTZL 302 162 | #define DMP_TMP1BH 304 163 | #define DMP_TMP1BL 306 164 | #define DMP_TMP2BH 308 165 | #define DMP_TMP2BL 310 166 | #define DMP_TMP3BH 312 167 | #define DMP_TMP3BL 314 168 | #define DMP_TMP4BH 316 169 | #define DMP_TMP4BL 318 170 | #define DMP_STXG 320 171 | #define DMP_ZCTXG 322 172 | #define DMP_STYG 324 173 | #define DMP_ZCTYG 326 174 | #define DMP_STZG 328 175 | #define DMP_ZCTZG 330 176 | #define DMP_CTRLSFJT2 332 177 | #define DMP_CTRLSFJTCNT 334 178 | #define DMP_PVXG 336 179 | #define DMP_TMP15 338 180 | #define DMP_PVYG 340 181 | #define DMP_TMP16 342 182 | #define DMP_PVZG 344 183 | #define DMP_TMP17 346 184 | #define DMP_MNMFLAGH 352 185 | #define DMP_MNMFLAGL 354 186 | #define DMP_MNMTMH 356 187 | #define DMP_MNMTML 358 188 | #define DMP_MNMTMTHRH 360 189 | #define DMP_MNMTMTHRL 362 190 | #define DMP_MNMTHRH 364 191 | #define DMP_MNMTHRL 366 192 | #define DMP_ACCQD4H 368 193 | #define DMP_ACCQD4L 370 194 | #define DMP_ACCQD5H 372 195 | #define DMP_ACCQD5L 374 196 | #define DMP_ACCQD6H 376 197 | #define DMP_ACCQD6L 378 198 | #define DMP_ACCQD7H 380 199 | #define DMP_ACCQD7L 382 200 | #define DMP_ACCQD0H 384 201 | #define DMP_ACCQD0L 386 202 | #define DMP_ACCQD1H 388 203 | #define DMP_ACCQD1L 390 204 | #define DMP_ACCQD2H 392 205 | #define DMP_ACCQD2L 394 206 | #define DMP_ACCQD3H 396 207 | #define DMP_ACCQD3L 398 208 | #define DMP_XN2H 400 209 | #define DMP_XN2L 402 210 | #define DMP_XN1H 404 211 | #define DMP_XN1L 406 212 | #define DMP_YN2H 408 213 | #define DMP_YN2L 410 214 | #define DMP_YN1H 412 215 | #define DMP_YN1L 414 216 | #define DMP_YH 416 217 | #define DMP_YL 418 218 | #define DMP_B0H 420 219 | #define DMP_B0L 422 220 | #define DMP_A1H 424 221 | #define DMP_A1L 426 222 | #define DMP_A2H 428 223 | #define DMP_A2L 430 224 | #define DMP_SEM1 432 225 | #define DMP_FIFOCNT 434 226 | #define DMP_SH_TH_X 436 227 | #define DMP_PACKET 438 228 | #define DMP_SH_TH_Y 440 229 | #define DMP_FOOTER 442 230 | #define DMP_SH_TH_Z 444 231 | #define DMP_TEMP29 448 232 | #define DMP_TEMP30 450 233 | #define DMP_XACCB_PRE 452 234 | #define DMP_XACCB_PREL 454 235 | #define DMP_YACCB_PRE 456 236 | #define DMP_YACCB_PREL 458 237 | #define DMP_ZACCB_PRE 460 238 | #define DMP_ZACCB_PREL 462 239 | #define DMP_TMP22 464 240 | #define DMP_TAP_TIMER 466 241 | #define DMP_TAP_THX 468 242 | #define DMP_TAP_THY 472 243 | #define DMP_TAP_THZ 476 244 | #define DMP_TAPW_MIN 478 245 | #define DMP_TMP25 480 246 | #define DMP_TMP26 482 247 | #define DMP_TMP27 484 248 | #define DMP_TMP28 486 249 | #define DMP_ORIENT 488 250 | #define DMP_THRSH 490 251 | #define DMP_ENDIANH 492 252 | #define DMP_ENDIANL 494 253 | #define DMP_BLPFNMTCH 496 254 | #define DMP_BLPFNMTCL 498 255 | #define DMP_BLPFNMXH 500 256 | #define DMP_BLPFNMXL 502 257 | #define DMP_BLPFNMYH 504 258 | #define DMP_BLPFNMYL 506 259 | #define DMP_BLPFNMZH 508 260 | #define DMP_BLPFNMZL 510 261 | #ifdef __cplusplus 262 | } 263 | #endif 264 | #endif // DMPMAP_H 265 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Def.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace mpu9150 8 | { 9 | // I2C Slave Addresses 10 | constexpr uint8_t DEVICE_A = 0; 11 | constexpr uint8_t DEVICE_B = 1; 12 | 13 | enum class EAddress 14 | { 15 | ADDRESS_A, 16 | ADDRESS_B 17 | }; 18 | 19 | enum EResult : uint32_t 20 | { 21 | // Successes 22 | RESULT_SUCCESS, 23 | 24 | // Errors 25 | RESULT_ERR_HARD_RESET, 26 | RESULT_ERR_READ_ERROR, 27 | 28 | // Counter 29 | _RESULT_COUNT 30 | }; 31 | } -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_DriverLayer.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu.h 13 | * @brief An I2C-based driver for Invensense gyroscopes. 14 | * @details This driver currently works for the following devices: 15 | * MPU6050 16 | * MPU6500 17 | * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) 18 | * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) 19 | */ 20 | 21 | #ifndef _INV_MPU_H_ 22 | #define _INV_MPU_H_ 23 | 24 | #include 25 | 26 | // Define this symbol to get debug messages 27 | 28 | #define MPU_DEBUG 29 | 30 | // Define this symbol to enable rarely-used functions from library 31 | 32 | //#define MPU_MAXIMAL 33 | 34 | // This symbol defines how many devices are supported 35 | 36 | #define MPU_MAX_DEVICES 2 37 | 38 | // Call this function before using the MPU to select the correct device 39 | 40 | int mpu_select_device( int device ); 41 | 42 | inline void get_ms( long unsigned int* timestamp ) 43 | { 44 | *timestamp = millis(); 45 | } 46 | 47 | // IMU hardware device defines 48 | 49 | #define USING_MPU9150 50 | 51 | #define INV_X_GYRO (0x40) 52 | #define INV_Y_GYRO (0x20) 53 | #define INV_Z_GYRO (0x10) 54 | #define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) 55 | #define INV_XYZ_ACCEL (0x08) 56 | #define INV_XYZ_COMPASS (0x01) 57 | 58 | struct int_param_s 59 | { 60 | void ( *cb )( void ); 61 | unsigned short pin; 62 | unsigned char lp_exit; 63 | unsigned char active_low; 64 | }; 65 | 66 | #define MPU_INT_STATUS_DATA_READY (0x0001) 67 | #define MPU_INT_STATUS_DMP (0x0002) 68 | #define MPU_INT_STATUS_PLL_READY (0x0004) 69 | #define MPU_INT_STATUS_I2C_MST (0x0008) 70 | #define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010) 71 | #define MPU_INT_STATUS_ZMOT (0x0020) 72 | #define MPU_INT_STATUS_MOT (0x0040) 73 | #define MPU_INT_STATUS_FREE_FALL (0x0080) 74 | #define MPU_INT_STATUS_DMP_0 (0x0100) 75 | #define MPU_INT_STATUS_DMP_1 (0x0200) 76 | #define MPU_INT_STATUS_DMP_2 (0x0400) 77 | #define MPU_INT_STATUS_DMP_3 (0x0800) 78 | #define MPU_INT_STATUS_DMP_4 (0x1000) 79 | #define MPU_INT_STATUS_DMP_5 (0x2000) 80 | 81 | /* Set up APIs */ 82 | void mpu_init_structures(); 83 | 84 | int mpu_init( struct int_param_s* int_param ); 85 | int mpu_init_slave( void ); 86 | int mpu_set_bypass( unsigned char bypass_on ); 87 | 88 | /* Configuration APIs */ 89 | int mpu_lp_accel_mode( unsigned char rate ); 90 | int mpu_lp_motion_interrupt( unsigned short thresh, unsigned char time, 91 | unsigned char lpa_freq ); 92 | int mpu_set_int_level( unsigned char active_low ); 93 | int mpu_set_int_latched( unsigned char enable ); 94 | 95 | int mpu_set_dmp_state( unsigned char enable ); 96 | int mpu_get_dmp_state( unsigned char* enabled ); 97 | 98 | int mpu_get_lpf( unsigned short* lpf ); 99 | int mpu_set_lpf( unsigned short lpf ); 100 | 101 | int mpu_get_gyro_fsr( unsigned short* fsr ); 102 | int mpu_set_gyro_fsr( unsigned short fsr ); 103 | 104 | int mpu_get_accel_fsr( unsigned char* fsr ); 105 | int mpu_set_accel_fsr( unsigned char fsr ); 106 | 107 | int mpu_get_compass_fsr( unsigned short* fsr ); 108 | 109 | int mpu_get_gyro_sens( float* sens ); 110 | int mpu_get_accel_sens( unsigned short* sens ); 111 | 112 | int mpu_get_sample_rate( unsigned short* rate ); 113 | int mpu_set_sample_rate( unsigned short rate ); 114 | int mpu_get_compass_sample_rate( unsigned short* rate ); 115 | int mpu_set_compass_sample_rate( unsigned short rate ); 116 | 117 | int mpu_get_fifo_config( unsigned char* sensors ); 118 | int mpu_configure_fifo( unsigned char sensors ); 119 | 120 | int mpu_get_power_state( unsigned char* power_on ); 121 | int mpu_set_sensors( unsigned char sensors ); 122 | 123 | int mpu_set_accel_bias( const long* accel_bias ); 124 | 125 | /* Data getter/setter APIs */ 126 | int mpu_get_gyro_reg( short* data, unsigned long* timestamp ); 127 | int mpu_get_accel_reg( short* data, unsigned long* timestamp ); 128 | int mpu_get_compass_reg( short* data, unsigned long* timestamp ); 129 | int mpu_get_temperature( long* data, unsigned long* timestamp ); 130 | 131 | int mpu_get_int_status( short* status ); 132 | int mpu_read_fifo( short* gyro, short* accel, unsigned long* timestamp, 133 | unsigned char* sensors, unsigned char* more ); 134 | int mpu_read_fifo_stream( unsigned short length, unsigned char* data, 135 | unsigned char* more ); 136 | int mpu_reset_fifo( void ); 137 | 138 | int mpu_write_mem( unsigned short mem_addr, unsigned short length, 139 | unsigned char* data ); 140 | int mpu_read_mem( unsigned short mem_addr, unsigned short length, 141 | unsigned char* data ); 142 | int mpu_load_firmware( unsigned short length, const unsigned char* firmware, 143 | unsigned short start_addr, unsigned short sample_rate ); 144 | 145 | int mpu_reg_dump( void ); 146 | int mpu_read_reg( unsigned char reg, unsigned char* data ); 147 | 148 | #ifdef MPU_MAXIMAL 149 | int mpu_run_self_test( long* gyro, long* accel ); 150 | int mpu_register_tap_cb( void ( *func )( unsigned char, unsigned char ) ); 151 | #endif // MPU_MAXIMAL 152 | 153 | #endif /* #ifndef _INV_MPU_H_ */ 154 | 155 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Quaternion.cpp: -------------------------------------------------------------------------------- 1 | 2 | //////////////////////////////////////////////////////////////////////////// 3 | // 4 | // This file is part of MPU9150Lib 5 | // 6 | // Copyright (c) 2013 Pansenti, LLC 7 | // 8 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 9 | // this software and associated documentation files (the "Software"), to deal in 10 | // the Software without restriction, including without limitation the rights to use, 11 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 12 | // Software, and to permit persons to whom the Software is furnished to do so, 13 | // subject to the following conditions: 14 | // 15 | // The above copyright notice and this permission notice shall be included in all 16 | // copies or substantial portions of the Software. 17 | // 18 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 19 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 20 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 21 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 22 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 23 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24 | 25 | #include "MPU9150_Quaternion.h" 26 | 27 | void MPUQuaternionNormalize( MPUQuaternion q ) 28 | { 29 | float length = MPUQuaternionNorm( q ); 30 | 31 | if( length == 0 ) 32 | { 33 | return; 34 | } 35 | 36 | q[QUAT_W] /= length; 37 | q[QUAT_X] /= length; 38 | q[QUAT_Y] /= length; 39 | q[QUAT_Z] /= length; 40 | } 41 | 42 | void MPUQuaternionQuaternionToEuler( const MPUQuaternion q, MPUVector3 v ) 43 | { 44 | float pole = M_PI / 2.0f - 0.05f; // fix roll near poles with this tolerance 45 | 46 | v[VEC3_Y] = asin( 2.0f * ( q[QUAT_W] * q[QUAT_Y] - q[QUAT_X] * q[QUAT_Z] ) ); 47 | 48 | if( ( v[VEC3_Y] < pole ) && ( v[VEC3_Y] > -pole ) ) 49 | v[VEC3_X] = atan2( 2.0f * ( q[QUAT_Y] * q[QUAT_Z] + q[QUAT_W] * q[QUAT_X] ), 50 | 1.0f - 2.0f * ( q[QUAT_X] * q[QUAT_X] + q[QUAT_Y] * q[QUAT_Y] ) ); 51 | 52 | v[VEC3_Z] = atan2( 2.0f * ( q[QUAT_X] * q[QUAT_Y] + q[QUAT_W] * q[QUAT_Z] ), 53 | 1.0f - 2.0f * ( q[QUAT_Y] * q[QUAT_Y] + q[QUAT_Z] * q[QUAT_Z] ) ); 54 | } 55 | 56 | void MPUQuaternionEulerToQuaternion( const MPUVector3 v, MPUQuaternion q ) 57 | { 58 | float cosX2 = cos( v[VEC3_X] / 2.0f ); 59 | float sinX2 = sin( v[VEC3_X] / 2.0f ); 60 | float cosY2 = cos( v[VEC3_Y] / 2.0f ); 61 | float sinY2 = sin( v[VEC3_Y] / 2.0f ); 62 | float cosZ2 = cos( v[VEC3_Z] / 2.0f ); 63 | float sinZ2 = sin( v[VEC3_Z] / 2.0f ); 64 | 65 | q[QUAT_W] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2; 66 | q[QUAT_X] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2; 67 | q[QUAT_Y] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2; 68 | q[QUAT_Z] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2; 69 | MPUQuaternionNormalize( q ); 70 | } 71 | 72 | void MPUQuaternionConjugate( const MPUQuaternion s, MPUQuaternion d ) 73 | { 74 | d[QUAT_W] = s[QUAT_W]; 75 | d[QUAT_X] = -s[QUAT_X]; 76 | d[QUAT_Y] = -s[QUAT_Y]; 77 | d[QUAT_Z] = -s[QUAT_Z]; 78 | } 79 | 80 | void MPUQuaternionMultiply( const MPUQuaternion qa, const MPUQuaternion qb, MPUQuaternion qd ) 81 | { 82 | MPUVector3 va; 83 | MPUVector3 vb; 84 | float dotAB; 85 | MPUVector3 crossAB; 86 | 87 | va[VEC3_X] = qa[QUAT_X]; 88 | va[VEC3_Y] = qa[QUAT_Y]; 89 | va[VEC3_Z] = qa[QUAT_Z]; 90 | 91 | vb[VEC3_X] = qb[QUAT_X]; 92 | vb[VEC3_Y] = qb[QUAT_Y]; 93 | vb[VEC3_Z] = qb[QUAT_Z]; 94 | 95 | MPUVector3DotProduct( va, vb, &dotAB ); 96 | MPUVector3CrossProduct( va, vb, crossAB ); 97 | 98 | qd[QUAT_W] = qa[QUAT_W] * qb[QUAT_W] - dotAB; 99 | qd[QUAT_X] = qa[QUAT_W] * vb[VEC3_X] + qb[QUAT_W] * va[VEC3_X] + crossAB[VEC3_X]; 100 | qd[QUAT_Y] = qa[QUAT_W] * vb[VEC3_Y] + qb[QUAT_W] * va[VEC3_Y] + crossAB[VEC3_Y]; 101 | qd[QUAT_Z] = qa[QUAT_W] * vb[VEC3_Z] + qb[QUAT_W] * va[VEC3_Z] + crossAB[VEC3_Z]; 102 | } 103 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Quaternion.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of MPU9150Lib 4 | // 5 | // Copyright (c) 2013 Pansenti, LLC 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef MPUQUATERNION_H_ 25 | #define MPUQUATERNION_H_ 26 | 27 | #include 28 | #include "MPU9150_Vector3.h" 29 | 30 | #define QUAT_W 0 // scalar offset 31 | #define QUAT_X 1 // x offset 32 | #define QUAT_Y 2 // y offset 33 | #define QUAT_Z 3 // z offset 34 | 35 | typedef float MPUQuaternion[4]; 36 | 37 | void MPUQuaternionNormalize( MPUQuaternion q ); 38 | void MPUQuaternionQuaternionToEuler( const MPUQuaternion q, MPUVector3 v ); 39 | void MPUQuaternionEulerToQuaternion( const MPUVector3 v, MPUQuaternion q ); 40 | void MPUQuaternionConjugate( const MPUQuaternion s, MPUQuaternion d ); 41 | void MPUQuaternionMultiply( const MPUQuaternion qa, const MPUQuaternion qb, MPUQuaternion qd ); 42 | 43 | inline float MPUQuaternionNorm( MPUQuaternion q ) 44 | { 45 | return sqrt( q[QUAT_W] * q[QUAT_W] + q[QUAT_X] * q[QUAT_X] + 46 | q[QUAT_Y] * q[QUAT_Y] + q[QUAT_Z] * q[QUAT_Z] ); 47 | } 48 | 49 | 50 | #endif /* MPUQUATERNION_H_ */ 51 | -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Vector3.cpp: -------------------------------------------------------------------------------- 1 | 2 | //////////////////////////////////////////////////////////////////////////// 3 | // 4 | // This file is part of MPU9150Lib 5 | // 6 | // Copyright (c) 2013 Pansenti, LLC 7 | // 8 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 9 | // this software and associated documentation files (the "Software"), to deal in 10 | // the Software without restriction, including without limitation the rights to use, 11 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 12 | // Software, and to permit persons to whom the Software is furnished to do so, 13 | // subject to the following conditions: 14 | // 15 | // The above copyright notice and this permission notice shall be included in all 16 | // copies or substantial portions of the Software. 17 | // 18 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 19 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 20 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 21 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 22 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 23 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 24 | 25 | #include "MPU9150_Vector3.h" 26 | 27 | void MPUVector3DotProduct( MPUVector3 a, MPUVector3 b, float* d ) 28 | { 29 | *d = a[VEC3_X] * b[VEC3_X] + a[VEC3_Y] * b[VEC3_Y] + a[VEC3_Z] * b[VEC3_Z]; 30 | } 31 | 32 | void MPUVector3CrossProduct( MPUVector3 a, MPUVector3 b, MPUVector3 d ) 33 | { 34 | d[VEC3_X] = a[VEC3_Y] * b[VEC3_Z] - a[VEC3_Z] * b[VEC3_Y]; 35 | d[VEC3_Y] = a[VEC3_Z] * b[VEC3_X] - a[VEC3_X] * b[VEC3_Z]; 36 | d[VEC3_Z] = a[VEC3_X] * b[VEC3_Y] - a[VEC3_Y] * b[VEC3_X]; 37 | } -------------------------------------------------------------------------------- /libraries/MPU9150/MPU9150_Vector3.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of MPU9150Lib 4 | // 5 | // Copyright (c) 2013 Pansenti, LLC 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef MPUVECTOR3_H_ 25 | #define MPUVECTOR3_H_ 26 | 27 | #include 28 | #include 29 | 30 | 31 | #define DEGREE_TO_RAD (float)M_PI / 180.0f) 32 | #define RAD_TO_DEGREE (180.0f / (float)M_PI) 33 | 34 | #define VEC3_X 0 // x offset 35 | #define VEC3_Y 1 // y offset 36 | #define VEC3_Z 2 // z offset 37 | 38 | typedef float MPUVector3[3]; 39 | 40 | void MPUVector3DotProduct(MPUVector3 a, MPUVector3 b, float *d); 41 | void MPUVector3CrossProduct(MPUVector3 a, MPUVector3 b, MPUVector3 d); 42 | 43 | 44 | #endif /* MPUVECTOR3_H_ */ 45 | -------------------------------------------------------------------------------- /libraries/MPU9150/library.properties: -------------------------------------------------------------------------------- 1 | name=MPU9150 2 | version=1.0 3 | author=OpenROV, Pansenti LLC 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/MS5803_14BA/MS5803_14BA.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Driver definitions and helpers 4 | #include "MS5803_14BA_Def.h" 5 | 6 | namespace ms5803_14ba 7 | { 8 | class MS5803_14BA 9 | { 10 | public: 11 | // Attributes 12 | TData m_data; 13 | 14 | // Methods 15 | MS5803_14BA( I2C *i2cInterfaceIn, ms5803_14ba::EAddress addressIn ); 16 | 17 | // State machine functions 18 | void Tick(); 19 | void HardReset(); 20 | void FullReset(); 21 | void Disable(); 22 | 23 | bool IsEnabled(); 24 | 25 | bool GetLock(); 26 | void FreeLock(); 27 | 28 | // Configuration 29 | EResult SetOversampleRate( EOversampleRate rateIn ); 30 | EResult SetWaterType( EWaterType typeIn ); 31 | 32 | uint32_t GetUpdatePeriod(); 33 | 34 | // Statistics 35 | uint32_t GetResultCount( EResult resultTypeIn ); 36 | void ClearResultCount( EResult resultTypeIn ); 37 | 38 | private: 39 | // I2C info 40 | I2C *m_pI2C; 41 | uint8_t m_address; 42 | 43 | // Configuration 44 | EOversampleRate m_osr = EOversampleRate::OSR_256_SAMPLES; 45 | EWaterType m_waterType = EWaterType::FRESH; 46 | float m_waterMod = kWaterModFresh; 47 | 48 | // State management 49 | bool m_enabled = true; 50 | EState m_state = EState::UNINITIALIZED; 51 | TTransitionDelay m_delay; 52 | 53 | // Result statistics 54 | orutil::TResultCount m_results; 55 | 56 | // Computation parameters 57 | uint16_t m_coeffs[8] = {0}; // unsigned 16-bit integer (0-65535) 58 | 59 | uint32_t m_D1 = 0; // Digital pressure value 60 | uint32_t m_D2 = 0; // Digital temperature value 61 | 62 | int32_t m_dT = 0; // Difference between actual and reference temperature 63 | int32_t m_TEMP = 0; // Actual temperature 64 | 65 | int64_t m_OFF = 0; // Offset at actual temperature 66 | int64_t m_SENS = 0; // Sensitivity at actual temperature 67 | 68 | int64_t m_Ti = 0; // Second order temperature component 69 | int64_t m_OFFi = 0; // Second order offset component 70 | int64_t m_SENSi = 0; // Second order sens component 71 | 72 | int64_t m_OFF2 = 0; // Second order final off 73 | int64_t m_SENS2 = 0; // Second order final sens 74 | int64_t m_TEMP2 = 0; // Second order final temp 75 | 76 | int32_t m_P = 0; // Temperature compensated pressure 77 | 78 | // Bytes to hold the results from I2C communications with the sensor 79 | uint8_t m_highByte = 0; 80 | uint8_t m_midByte = 0; 81 | uint8_t m_lowByte = 0; 82 | 83 | // Methods 84 | void Transition( EState nextState ); 85 | void DelayedTransition( EState nextState, uint32_t millisIn ); 86 | 87 | EResult Cmd_Reset(); 88 | EResult Cmd_ReadCalibrationData(); 89 | EResult Cmd_StartTempConversion(); 90 | EResult Cmd_StartPresConversion(); 91 | EResult Cmd_ReadTemperature(); 92 | EResult Cmd_ReadPressure(); 93 | 94 | uint8_t CalculateCRC4(); 95 | void ProcessData(); 96 | 97 | // I2C Methods 98 | i2c::EI2CResult WriteByte( uint8_t registerIn ); 99 | i2c::EI2CResult ReadRegisterBytes( uint8_t registerIn, uint8_t *dataOut, uint8_t numBytesIn ); 100 | }; 101 | } -------------------------------------------------------------------------------- /libraries/MS5803_14BA/MS5803_14BA_Def.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace ms5803_14ba 8 | { 9 | // I2C Slave Addresses 10 | constexpr uint8_t I2C_ADDRESS_A = 0x76; 11 | constexpr uint8_t I2C_ADDRESS_B = 0x77; 12 | 13 | // Commands 14 | constexpr uint8_t CMD_ADC_READ = 0x00; 15 | constexpr uint8_t CMD_RESET = 0x1E; 16 | constexpr uint8_t CMD_PROM_READ_BASE = 0xA0; // Add coefficient index modifier 17 | constexpr uint8_t CMD_PRES_CONV_BASE = 0x40; // Add OSR modifier 18 | constexpr uint8_t CMD_TEMP_CONV_BASE = 0x40 + 0x10; // Add OSR modifier 19 | 20 | constexpr float kWaterModFresh = 1.019716f; 21 | constexpr float kWaterModSalt = 0.9945f; 22 | 23 | constexpr uint32_t kRetryDelay_ms = 1000; 24 | constexpr uint32_t kResetDelay_ms = 10; 25 | 26 | // Computation constants 27 | constexpr int32_t POW_2_7 = 128; // 2^7 28 | constexpr int32_t POW_2_8 = 256; // 2^8 29 | constexpr int32_t POW_2_13 = 8192; // 2^13 30 | constexpr int32_t POW_2_15 = 32768; // 2^15 31 | constexpr int32_t POW_2_16 = 65536; // 2^16 32 | constexpr int32_t POW_2_21 = 2097152; // 2^21 33 | constexpr int32_t POW_2_23 = 8388608; // 2^23 34 | constexpr int64_t POW_2_33 = 8589934592; // 2^33 35 | constexpr int64_t POW_2_37 = 137438953472; // 2^37 36 | 37 | //typedef void (*TStateCallback)(); 38 | 39 | enum class EAddress 40 | { 41 | ADDRESS_A, 42 | ADDRESS_B 43 | }; 44 | 45 | enum class EWaterType 46 | { 47 | FRESH = 0, 48 | SALT = 1 49 | }; 50 | 51 | enum EState : uint8_t 52 | { 53 | UNINITIALIZED, 54 | DISABLED, 55 | DELAY, 56 | READING_CALIB_DATA, 57 | CONVERTING_PRESSURE, 58 | CONVERTING_TEMPERATURE, 59 | PROCESSING_DATA, 60 | 61 | _STATE_COUNT 62 | }; 63 | 64 | enum EResult : uint32_t 65 | { 66 | RESULT_SUCCESS, 67 | 68 | RESULT_ERR_HARD_RESET, 69 | RESULT_ERR_FAILED_SEQUENCE, 70 | RESULT_ERR_I2C_TRANSACTION, 71 | RESULT_ERR_CRC_MISMATCH, 72 | 73 | _RESULT_COUNT 74 | }; 75 | 76 | // Data structure 77 | struct TData 78 | { 79 | public: 80 | float temperature_c = 0.0f; 81 | float pressure_mbar = 0.0f; 82 | float depth_m = 0.0f; 83 | 84 | bool SampleAvailable() 85 | { 86 | if( isAvailable ) 87 | { 88 | isAvailable = false; 89 | return true; 90 | } 91 | else 92 | { 93 | return false; 94 | } 95 | } 96 | 97 | private: 98 | void Update( float tempIn, float presIn, float waterModIn ) 99 | { 100 | temperature_c = tempIn; 101 | pressure_mbar = presIn; 102 | depth_m = ( presIn - 1013.25f ) * waterModIn / 100.0f; 103 | isAvailable = true; 104 | } 105 | 106 | bool isAvailable = false; 107 | 108 | friend class MS5803_14BA; 109 | }; 110 | 111 | // Delay state info 112 | struct TTransitionDelay 113 | { 114 | orutil::CTimer timer; 115 | EState nextState; 116 | uint32_t delayTime_ms; 117 | }; 118 | 119 | // ---------- 120 | // OSR 121 | enum class EOversampleRate : uint8_t 122 | { 123 | OSR_256_SAMPLES = 0, 124 | OSR_512_SAMPLES, 125 | OSR_1024_SAMPLES, 126 | OSR_2048_SAMPLES, 127 | OSR_4096_SAMPLES, 128 | 129 | _OSR_COUNT 130 | }; 131 | 132 | struct TOversampleInfo 133 | { 134 | uint8_t commandMod; 135 | uint8_t conversionTime_ms; 136 | }; 137 | 138 | // Table of command modifiers and conversion times for each OSR 139 | constexpr TOversampleInfo kOSRInfo[ (uint8_t)EOversampleRate::_OSR_COUNT ] = 140 | { 141 | { 0x00, 1 }, // 256 142 | { 0x02, 2 }, // 512 143 | { 0x04, 3 }, // 1024 144 | { 0x06, 5 }, // 2048 145 | { 0x08, 10 } // 4096 146 | }; 147 | } -------------------------------------------------------------------------------- /libraries/MS5803_14BA/library.properties: -------------------------------------------------------------------------------- 1 | name=MS5803_14BA 2 | version=1.0 3 | author=OpenROV, Charles Cross 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/MS5837_30BA/MS5837_30BA.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Driver definitions and helpers 4 | #include "MS5837_30BA_Def.h" 5 | 6 | namespace ms5837_30ba 7 | { 8 | class MS5837_30BA 9 | { 10 | public: 11 | // Attributes 12 | TData m_data; 13 | 14 | // Methods 15 | MS5837_30BA( I2C *i2cInterfaceIn, ms5837_30ba::EAddress addressIn ); 16 | 17 | // State machine functions 18 | void Tick(); 19 | void HardReset(); 20 | void FullReset(); 21 | void Disable(); 22 | 23 | bool IsEnabled(); 24 | 25 | bool GetLock(); 26 | void FreeLock(); 27 | 28 | // Configuration 29 | EResult SetOversampleRate( EOversampleRate rateIn ); 30 | EResult SetWaterType( EWaterType typeIn ); 31 | 32 | uint32_t GetUpdatePeriod(); 33 | 34 | // Statistics 35 | uint32_t GetResultCount( EResult resultTypeIn ); 36 | void ClearResultCount( EResult resultTypeIn ); 37 | 38 | private: 39 | // I2C info 40 | I2C *m_pI2C; 41 | uint8_t m_address; 42 | 43 | // Configuration 44 | EOversampleRate m_osr = EOversampleRate::OSR_256_SAMPLES; 45 | EWaterType m_waterType = EWaterType::FRESH; 46 | float m_waterMod = kWaterModFresh; 47 | 48 | // State management 49 | bool m_enabled = true; 50 | EState m_state = EState::UNINITIALIZED; 51 | TTransitionDelay m_delay; 52 | //TStateCallback m_pStateMethods[ EState::_STATE_COUNT ] = { nullptr }; 53 | 54 | // Result statistics 55 | orutil::TResultCount m_results; 56 | 57 | // Computation parameters 58 | uint16_t m_coeffs[8] = {0}; // unsigned 16-bit integer (0-65535) 59 | 60 | uint32_t m_D1 = 0; // Digital pressure value 61 | uint32_t m_D2 = 0; // Digital temperature value 62 | 63 | int32_t m_dT = 0; // Difference between actual and reference temperature 64 | int32_t m_TEMP = 0; // Actual temperature 65 | 66 | int64_t m_OFF = 0; // Offset at actual temperature 67 | int64_t m_SENS = 0; // Sensitivity at actual temperature 68 | 69 | int64_t m_Ti = 0; // Second order temperature component 70 | int64_t m_OFFi = 0; // Second order offset component 71 | int64_t m_SENSi = 0; // Second order sens component 72 | 73 | int64_t m_OFF2 = 0; // Second order final off 74 | int64_t m_SENS2 = 0; // Second order final sens 75 | int64_t m_TEMP2 = 0; // Second order final temp 76 | 77 | int32_t m_P = 0; // Temperature compensated pressure 78 | 79 | // Bytes to hold the results from I2C communications with the sensor 80 | uint8_t m_highByte = 0; 81 | uint8_t m_midByte = 0; 82 | uint8_t m_lowByte = 0; 83 | 84 | // Methods 85 | void Transition( EState nextState ); 86 | void DelayedTransition( EState nextState, uint32_t millisIn ); 87 | 88 | EResult Cmd_Reset(); 89 | EResult Cmd_ReadCalibrationData(); 90 | EResult Cmd_StartTempConversion(); 91 | EResult Cmd_StartPresConversion(); 92 | EResult Cmd_ReadTemperature(); 93 | EResult Cmd_ReadPressure(); 94 | 95 | uint8_t CalculateCRC4(); 96 | void ProcessData(); 97 | 98 | // I2C Methods 99 | i2c::EI2CResult WriteByte( uint8_t registerIn ); 100 | i2c::EI2CResult ReadRegisterBytes( uint8_t registerIn, uint8_t *dataOut, uint8_t numBytesIn ); 101 | }; 102 | } -------------------------------------------------------------------------------- /libraries/MS5837_30BA/MS5837_30BA_Def.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace ms5837_30ba 8 | { 9 | // I2C Slave Addresses 10 | constexpr uint8_t I2C_ADDRESS_A = 0x76; 11 | constexpr uint8_t I2C_ADDRESS_B = 0x77; 12 | 13 | // Commands 14 | constexpr uint8_t CMD_ADC_READ = 0x00; 15 | constexpr uint8_t CMD_RESET = 0x1E; 16 | constexpr uint8_t CMD_PROM_READ_BASE = 0xA0; // Add coefficient index modifier 17 | constexpr uint8_t CMD_PRES_CONV_BASE = 0x40; // Add OSR modifier 18 | constexpr uint8_t CMD_TEMP_CONV_BASE = 0x40 + 0x10; // Add OSR modifier 19 | 20 | constexpr float kWaterModFresh = 1.019716f; 21 | constexpr float kWaterModSalt = 0.9945f; 22 | 23 | constexpr uint32_t kRetryDelay_ms = 1000; 24 | constexpr uint32_t kResetDelay_ms = 10; 25 | 26 | // Computation constants 27 | constexpr int32_t POW_2_7 = 128; // 2^7 28 | constexpr int32_t POW_2_8 = 256; // 2^8 29 | constexpr int32_t POW_2_13 = 8192; // 2^13 30 | constexpr int32_t POW_2_15 = 32768; // 2^15 31 | constexpr int32_t POW_2_16 = 65536; // 2^16 32 | constexpr int32_t POW_2_21 = 2097152; // 2^21 33 | constexpr int32_t POW_2_23 = 8388608; // 2^23 34 | constexpr int64_t POW_2_33 = 8589934592; // 2^33 35 | constexpr int64_t POW_2_37 = 137438953472; // 2^37 36 | 37 | //typedef void (*TStateCallback)(); 38 | 39 | enum class EAddress 40 | { 41 | ADDRESS_A, 42 | ADDRESS_B 43 | }; 44 | 45 | enum class EWaterType 46 | { 47 | FRESH = 0, 48 | SALT = 1 49 | }; 50 | 51 | enum EState : uint8_t 52 | { 53 | UNINITIALIZED, 54 | DISABLED, 55 | DELAY, 56 | READING_CALIB_DATA, 57 | CONVERTING_PRESSURE, 58 | CONVERTING_TEMPERATURE, 59 | PROCESSING_DATA, 60 | 61 | _STATE_COUNT 62 | }; 63 | 64 | enum EResult : uint32_t 65 | { 66 | RESULT_SUCCESS, 67 | 68 | RESULT_ERR_HARD_RESET, 69 | RESULT_ERR_FAILED_SEQUENCE, 70 | RESULT_ERR_I2C_TRANSACTION, 71 | RESULT_ERR_CRC_MISMATCH, 72 | 73 | _RESULT_COUNT 74 | }; 75 | 76 | // Data structure 77 | struct TData 78 | { 79 | public: 80 | float temperature_c = 0.0f; 81 | float pressure_mbar = 0.0f; 82 | float depth_m = 0.0f; 83 | 84 | bool SampleAvailable() 85 | { 86 | if( isAvailable ) 87 | { 88 | isAvailable = false; 89 | return true; 90 | } 91 | else 92 | { 93 | return false; 94 | } 95 | } 96 | 97 | private: 98 | void Update( float tempIn, float presIn, float waterModIn ) 99 | { 100 | temperature_c = tempIn; 101 | pressure_mbar = presIn; 102 | depth_m = ( presIn - 1013.25f ) * waterModIn / 100.0f; 103 | isAvailable = true; 104 | } 105 | 106 | bool isAvailable = false; 107 | 108 | friend class MS5837_30BA; 109 | }; 110 | 111 | // Delay state info 112 | struct TTransitionDelay 113 | { 114 | orutil::CTimer timer; 115 | EState nextState; 116 | uint32_t delayTime_ms; 117 | }; 118 | 119 | // ---------- 120 | // OSR 121 | enum class EOversampleRate : uint8_t 122 | { 123 | OSR_256_SAMPLES = 0, 124 | OSR_512_SAMPLES, 125 | OSR_1024_SAMPLES, 126 | OSR_2048_SAMPLES, 127 | OSR_4096_SAMPLES, 128 | OSR_8192_SAMPLES, 129 | 130 | _OSR_COUNT 131 | }; 132 | 133 | struct TOversampleInfo 134 | { 135 | uint8_t commandMod; 136 | uint8_t conversionTime_ms; 137 | }; 138 | 139 | // Table of command modifiers and conversion times for each OSR 140 | constexpr TOversampleInfo kOSRInfo[ (uint8_t)EOversampleRate::_OSR_COUNT ] = 141 | { 142 | { 0x00, 1 }, // 256 143 | { 0x02, 2 }, // 512 144 | { 0x04, 3 }, // 1024 145 | { 0x06, 5 }, // 2048 146 | { 0x08, 10 }, // 4096 147 | { 0x0A, 20 } // 8192 148 | }; 149 | } -------------------------------------------------------------------------------- /libraries/MS5837_30BA/library.properties: -------------------------------------------------------------------------------- 1 | name=MS5837_30BA 2 | version=1.0 3 | author=OpenROV, Charles Cross 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/ORUtil/library.properties: -------------------------------------------------------------------------------- 1 | name=ORUtil 2 | version=1.0 3 | author=OpenROV, Charles Cross 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/ORUtil/orutil.cpp: -------------------------------------------------------------------------------- 1 | #include "orutil.h" 2 | 3 | #if defined( ARDUINO_ARCH_AVR ) 4 | extern unsigned int __heap_start; 5 | extern void* __brkval; 6 | 7 | // The free list structure as maintained by the avr-libc memory allocation routines. 8 | struct __freelist 9 | { 10 | size_t sz; 11 | struct __freelist* nx; 12 | }; 13 | 14 | // The head of the free list structure 15 | extern struct __freelist* __flp; 16 | 17 | // Calculates the size of the free list 18 | int freeListSize() 19 | { 20 | struct __freelist* current; 21 | int total = 0; 22 | 23 | for( current = __flp; current; current = current->nx ) 24 | { 25 | // Add two bytes for the memory block's header 26 | total += 2; 27 | total += ( int ) current->sz; 28 | } 29 | 30 | return total; 31 | } 32 | #endif 33 | 34 | namespace orutil 35 | { 36 | // CTimer 37 | uint32_t CTimer::Now() 38 | { 39 | // Returns the current time in milliseconds 40 | return millis(); 41 | } 42 | 43 | bool CTimer::HasElapsed( uint32_t msIn ) 44 | { 45 | if( ( Now() - m_lastTime_ms ) > msIn ) 46 | { 47 | // Update the last time to the current time 48 | m_lastTime_ms = Now(); 49 | return true; 50 | } 51 | 52 | return false; 53 | } 54 | 55 | void CTimer::Reset() 56 | { 57 | m_lastTime_ms = Now(); 58 | } 59 | 60 | #if defined( ARDUINO_ARCH_AVR ) 61 | int FreeMemory() 62 | { 63 | int free_memory; 64 | 65 | if( ( int )__brkval == 0 ) 66 | { 67 | free_memory = ( ( int )&free_memory ) - ( ( int )&__heap_start ); 68 | } 69 | else 70 | { 71 | free_memory = ( ( int )&free_memory ) - ( ( int )__brkval ); 72 | free_memory += freeListSize(); 73 | } 74 | 75 | return free_memory; 76 | } 77 | 78 | #endif 79 | 80 | } -------------------------------------------------------------------------------- /libraries/ORUtil/orutil.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | 6 | // Macros 7 | #ifndef NORMALIZE_ANGLE 8 | #define NORMALIZE_ANGLE_360(a) ((a > 360.0f) ? (a - 360.0f) : ((a < 0.0f) ? (a + 360.0f) : a)) 9 | #define NORMALIZE_ANGLE_180(a) ((a > 180.0f) ? (a - 360.0f) : ((a < -180.0f) ? (a + 360.0f) : a)) 10 | #endif 11 | 12 | namespace orutil 13 | { 14 | template 15 | struct TResultCount 16 | { 17 | uint32_t AddResult( uint32_t resultIn ) 18 | { 19 | ++results[ resultIn ]; 20 | 21 | // Any results greater than the error threshold increment the error counter 22 | if( resultIn > errorThreshold ) 23 | { 24 | ++errors; 25 | } 26 | 27 | return resultIn; 28 | } 29 | 30 | uint32_t GetResultCount( uint32_t resultIn ) 31 | { 32 | return results[ resultIn ]; 33 | } 34 | 35 | uint32_t GetErrorCount() 36 | { 37 | return errors; 38 | } 39 | 40 | void Clear() 41 | { 42 | errors = 0; 43 | memset( results, 0, n * sizeof(uint32_t) ); 44 | } 45 | 46 | void ClearResult( uint32_t resultIn ) 47 | { 48 | results[ resultIn ] = 0; 49 | } 50 | 51 | private: 52 | uint32_t results[ n ] = { 0 }; 53 | uint32_t errors = 0; 54 | }; 55 | 56 | class CTimer 57 | { 58 | private: 59 | // Last time the timer was successfully polled for an elapsed amount of time 60 | uint32_t m_lastTime_ms; 61 | 62 | public: 63 | uint32_t Now(); 64 | bool HasElapsed( uint32_t msIn ); 65 | void Reset(); 66 | }; 67 | 68 | // Float<->Int conversion helpers 69 | constexpr int32_t Encode1K( float valueIn ) 70 | { 71 | return static_cast( valueIn * 1000.0f ); 72 | } 73 | 74 | constexpr float Decode1K( int32_t valueIn ) 75 | { 76 | return ( static_cast( valueIn ) * 0.001f ); 77 | } 78 | 79 | // Memory 80 | int FreeMemory(); 81 | } -------------------------------------------------------------------------------- /libraries/PCA9539/PCA9539.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | PCA9539 GPIO Expander Library 3 | By: OpenROV 4 | Date: September 14, 2016 5 | */ 6 | 7 | #include "PCA9539.h" 8 | 9 | using namespace pca9539; 10 | 11 | PCA9539::PCA9539( I2C* i2cInterfaceIn ) 12 | : m_i2cAddress( pca9539::PCA9539_ADDRESS ) 13 | , m_pI2C( i2cInterfaceIn ) 14 | , m_gpioDirection( 0xFF ) //All inputs 15 | , m_gpioState( 0xFF ) //All low 16 | { 17 | 18 | } 19 | 20 | ERetCode PCA9539::Initialize() 21 | { 22 | //Set all of the pins as inputs 23 | m_gpioDirection = 0xFF; 24 | 25 | //Set all of the pins to the LOW state 26 | m_gpioState = 0xFF; 27 | 28 | m_isInitialized = true; 29 | 30 | return ERetCode::SUCCESS; 31 | } 32 | 33 | //Overloaded function that sets all of the pins to a mode 34 | ERetCode PCA9539::PinMode( uint16_t mode ) 35 | { 36 | if( mode == INPUT ) 37 | { 38 | //Set all of the pins as inputs 39 | m_gpioDirection = 0xFF; 40 | } 41 | else if( mode == OUTPUT ) 42 | { 43 | //Set all of the pins as outputs 44 | m_gpioDirection = 0x00; 45 | } 46 | else 47 | { 48 | //User described mode. Set it to that 49 | //TODO: Should we allow this? 50 | m_gpioDirection = mode; 51 | } 52 | 53 | //Write it 54 | auto ret = WriteByte( PCA9539_REGISTER::CONFIG, m_gpioDirection); 55 | if( ret != i2c::EI2CResult::RESULT_SUCCESS ) 56 | { 57 | return ERetCode::FAILED_PIN_MODE; 58 | } 59 | return ERetCode::SUCCESS; 60 | } 61 | 62 | //Set a pin to a mode 63 | ERetCode PCA9539::PinMode( uint8_t pin, bool mode ) 64 | { 65 | //Pins 0..7 are r/w capable pins 66 | if( pin > 8 ) 67 | { 68 | return ERetCode::FAILED_PIN_MODE; 69 | } 70 | 71 | if( mode == INPUT ) 72 | { 73 | //Set that pin as an input pin (set the bit) 74 | m_gpioDirection |= ( 1 << pin ); 75 | 76 | } 77 | else if( mode == OUTPUT ) 78 | { 79 | //Set this pin as an output pin (clear the bit) 80 | m_gpioDirection &= ~( 1 << pin ); 81 | } 82 | else 83 | { 84 | //Fail out. Don't recognize that pin mode 85 | return ERetCode::FAILED_PIN_MODE; 86 | } 87 | 88 | //Write it 89 | auto ret = WriteByte( PCA9539_REGISTER::CONFIG, m_gpioDirection); 90 | if( ret != i2c::EI2CResult::RESULT_SUCCESS ) 91 | { 92 | return ERetCode::FAILED_PIN_MODE; 93 | } 94 | 95 | return ERetCode::SUCCESS; 96 | } 97 | 98 | 99 | ERetCode PCA9539::DigitalWrite( uint8_t pin, bool value ) 100 | { 101 | //Pins 0..7 are r/w capable pins 102 | if( pin > 8 ) 103 | { 104 | return ERetCode::FAILED_DIGITAL_WRITE; 105 | } 106 | 107 | if( value == HIGH ) 108 | { 109 | //Set the bit to high. (set the bit) 110 | m_gpioState |= ( 1 << pin ); 111 | } 112 | else if( value == LOW ) 113 | { 114 | //Set the bit to high. (clear the bit) 115 | m_gpioState &= ~( 1 << pin ); 116 | } 117 | else 118 | { 119 | //Don't know what this is 120 | return ERetCode::FAILED_DIGITAL_WRITE; 121 | } 122 | 123 | //Write it 124 | auto ret = WriteByte( PCA9539_REGISTER::OUTPUT_PORT, m_gpioState); 125 | if( ret != i2c::EI2CResult::RESULT_SUCCESS ) 126 | { 127 | return ERetCode::FAILED_DIGITAL_WRITE; 128 | } 129 | 130 | return ERetCode::SUCCESS; 131 | 132 | } 133 | 134 | ERetCode PCA9539::DigitalWriteHex( uint8_t value ) 135 | { 136 | //Pins 0..7 are r/w capable pins 137 | if( value > 0x1F ) 138 | { 139 | return ERetCode::FAILED_DIGITAL_WRITE; 140 | } 141 | 142 | m_gpioState = value; 143 | 144 | //Write it 145 | auto ret = WriteByte( PCA9539_REGISTER::OUTPUT_PORT, m_gpioState); 146 | if( ret != i2c::EI2CResult::RESULT_SUCCESS ) 147 | { 148 | return ERetCode::FAILED_DIGITAL_WRITE; 149 | } 150 | 151 | return ERetCode::SUCCESS; 152 | } 153 | 154 | ERetCode PCA9539::DigitalWriteDecimal( uint8_t value ) 155 | { 156 | //Pins 0..7 are r/w capable pins 157 | if( value > 31 ) 158 | { 159 | return ERetCode::FAILED_DIGITAL_WRITE; 160 | } 161 | 162 | m_gpioState = value; 163 | 164 | //Write it 165 | auto ret = WriteByte( PCA9539_REGISTER::OUTPUT_PORT, m_gpioState); 166 | if( ret != i2c::EI2CResult::RESULT_SUCCESS ) 167 | { 168 | return ERetCode::FAILED_DIGITAL_WRITE; 169 | } 170 | 171 | return ERetCode::SUCCESS; 172 | } 173 | 174 | 175 | 176 | 177 | /*************************************************************************** 178 | PRIVATE FUNCTIONS 179 | ***************************************************************************/ 180 | int32_t PCA9539::WriteByte( PCA9539_REGISTER addressIn, uint8_t dataIn ) 181 | { 182 | return (int32_t)m_pI2C->WriteRegisterByte( m_i2cAddress, (uint8_t)addressIn, dataIn ); 183 | } 184 | 185 | int32_t PCA9539::ReadByte( PCA9539_REGISTER addressIn, uint8_t &dataOut ) 186 | { 187 | return (int32_t)m_pI2C->ReadRegisterByte( m_i2cAddress, (uint8_t)addressIn, &dataOut ); 188 | } 189 | int32_t PCA9539::ReadNBytes( PCA9539_REGISTER addressIn, uint8_t* dataOut, uint8_t byteCountIn ) 190 | { 191 | return (int32_t)m_pI2C->ReadRegisterBytes( m_i2cAddress, (uint8_t)addressIn, dataOut, byteCountIn ); 192 | } 193 | 194 | -------------------------------------------------------------------------------- /libraries/PCA9539/PCA9539.h: -------------------------------------------------------------------------------- 1 | /* 2 | PCA9539 GPIO Expander Library 3 | By: OpenROV 4 | Date: September 14, 2016 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | namespace pca9539 13 | { 14 | //Unshifted 7-bit I2C address for device 15 | const uint8_t PCA9539_ADDRESS = 0x70; //b1110000 16 | 17 | enum ERetCode : int32_t 18 | { 19 | SUCCESS, 20 | FAILED, 21 | FAILED_PIN_MODE, 22 | FAILED_DIGITAL_WRITE, 23 | TIMED_OUT, 24 | UNKNOWN 25 | }; 26 | 27 | enum EPin : uint8_t 28 | { 29 | LED_0, 30 | LED_1, 31 | LED_2, 32 | LED_3, 33 | LED_4 34 | }; 35 | 36 | class PCA9539 37 | { 38 | public: 39 | PCA9539( I2C* i2cInterfaceIn ); 40 | 41 | ERetCode DigitalWrite( uint8_t pin, bool value ); 42 | ERetCode DigitalWriteHex( uint8_t value ); 43 | ERetCode DigitalWriteDecimal( uint8_t value ); 44 | 45 | ERetCode Initialize(); 46 | bool IsInitialized() const { return m_isInitialized; }; 47 | 48 | ERetCode PinMode( uint16_t mode ); 49 | ERetCode PinMode( uint8_t pin, bool mode ); 50 | 51 | private: 52 | 53 | enum class PCA9539_REGISTER : uint8_t 54 | { 55 | INPUT_PORT = 0x00, //Input port registers 56 | OUTPUT_PORT = 0x01, //Output port registers 57 | POLARITY_INVERSION = 0x02, //Polarity inversion registers 58 | CONFIG = 0x03 //Configuration registers 59 | }; 60 | 61 | 62 | //Private member functions 63 | int32_t ReadByte( PCA9539_REGISTER addressIn, uint8_t& dataOut ); 64 | int32_t ReadNBytes( PCA9539_REGISTER addressIn, uint8_t* dataOut, uint8_t byteCountIn ); 65 | int32_t WriteByte( PCA9539_REGISTER addressIn, uint8_t dataIn ); 66 | 67 | 68 | //Private member attributes 69 | uint8_t m_i2cAddress; 70 | I2C* m_pI2C; 71 | 72 | uint8_t m_gpioDirection; 73 | uint8_t m_gpioState; 74 | 75 | bool m_isInitialized = false; 76 | 77 | }; 78 | } -------------------------------------------------------------------------------- /libraries/PCA9539/library.properties: -------------------------------------------------------------------------------- 1 | name=PCA9539 2 | version=1.0 3 | author=OpenROV 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/Servo/library.properties: -------------------------------------------------------------------------------- 1 | name=Servo 2 | version=1.0 3 | author=Michael Margolis 4 | maintainer=OpenROV 5 | sentence= 6 | paragraph= 7 | category= 8 | url= 9 | architectures=* -------------------------------------------------------------------------------- /libraries/Servo/src/Servo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | A servo is activated by creating an instance of the Servo class passing 22 | the desired pin to the attach() method. 23 | The servos are pulsed in the background using the value most recently 24 | written using the write() method. 25 | 26 | Note that analogWrite of PWM on pins associated with the timer are 27 | disabled when the first servo is attached. 28 | Timers are seized as needed in groups of 12 servos - 24 servos use two 29 | timers, 48 servos will use four. 30 | The sequence used to sieze timers is defined in timers.h 31 | 32 | The methods are: 33 | 34 | Servo - Class for manipulating servo motors connected to Arduino pins. 35 | 36 | attach(pin ) - Attaches a servo motor to an i/o pin. 37 | attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds 38 | default min is 544, max is 2400 39 | 40 | write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) 41 | writeMicroseconds() - Sets the servo pulse width in microseconds 42 | read() - Gets the last written servo pulse width as an angle between 0 and 180. 43 | readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) 44 | attached() - Returns true if there is a servo attached. 45 | detach() - Stops an attached servos from pulsing its i/o pin. 46 | */ 47 | 48 | #ifndef Servo_h 49 | #define Servo_h 50 | 51 | #include 52 | 53 | /* 54 | * Defines for 16 bit timers used with Servo library 55 | * 56 | * If _useTimerX is defined then TimerX is a 16 bit timer on the current board 57 | * timer16_Sequence_t enumerates the sequence that the timers should be allocated 58 | * _Nbr_16timers indicates how many 16 bit timers are available. 59 | */ 60 | 61 | // Architecture specific include 62 | #if defined(ARDUINO_ARCH_AVR) 63 | #include "avr/ServoTimers.h" 64 | #elif defined(ARDUINO_ARCH_SAM) 65 | #include "sam/ServoTimers.h" 66 | #elif defined(ARDUINO_ARCH_SAMD) 67 | #include "samd/ServoTimers.h" 68 | #else 69 | #error "This library only supports boards with an AVR, SAM or SAMD processor." 70 | #endif 71 | 72 | #define Servo_VERSION 2 // software version of this library 73 | 74 | #define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo 75 | #define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo 76 | #define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached 77 | #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds 78 | 79 | #define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer 80 | #define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) 81 | 82 | #define INVALID_SERVO 255 // flag indicating an invalid servo index 83 | 84 | typedef struct { 85 | uint8_t nbr :6 ; // a pin number from 0 to 63 86 | uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false 87 | } ServoPin_t ; 88 | 89 | typedef struct { 90 | ServoPin_t Pin; 91 | volatile unsigned int ticks; 92 | } servo_t; 93 | 94 | class Servo 95 | { 96 | public: 97 | Servo(); 98 | uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure 99 | uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 100 | void detach(); 101 | void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 102 | void writeMicroseconds(int value); // Write pulse width in microseconds 103 | int read(); // returns current pulse width as an angle between 0 and 180 degrees 104 | int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) 105 | bool attached(); // return true if this servo is attached, otherwise false 106 | private: 107 | uint8_t servoIndex; // index into the channel data for this servo 108 | int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH 109 | int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH 110 | }; 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /libraries/Servo/src/avr/ServoTimers.h: -------------------------------------------------------------------------------- 1 | /* 2 | Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 3 | Copyright (c) 2009 Michael Margolis. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | * Defines for 16 bit timers used with Servo library 22 | * 23 | * If _useTimerX is defined then TimerX is a 16 bit timer on the current board 24 | * timer16_Sequence_t enumerates the sequence that the timers should be allocated 25 | * _Nbr_16timers indicates how many 16 bit timers are available. 26 | */ 27 | 28 | /** 29 | * AVR Only definitions 30 | * -------------------- 31 | */ 32 | 33 | // Say which 16 bit timers can be used and in what order 34 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 35 | // #define _useTimer5 - Not used in OpenROV board 36 | #define _useTimer1 37 | #define _useTimer3 38 | #define _useTimer4 39 | typedef enum { _timer4, _timer1, _timer3, _Nbr_16timers } timer16_Sequence_t; 40 | 41 | #elif defined(__AVR_ATmega32U4__) 42 | #define _useTimer1 43 | typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; 44 | 45 | #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) 46 | #define _useTimer3 47 | #define _useTimer1 48 | typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; 49 | 50 | #elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) 51 | #define _useTimer3 52 | #define _useTimer1 53 | typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; 54 | 55 | #else // everything else 56 | #define _useTimer1 57 | typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /libraries/Servo/src/samd/ServoTimers.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015 Arduino LLC. All right reserved. 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library; if not, write to the Free Software 16 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 17 | */ 18 | 19 | /* 20 | * Defines for 16 bit timers used with Servo library 21 | * 22 | * If _useTimerX is defined then TimerX is a 16 bit timer on the current board 23 | * timer16_Sequence_t enumerates the sequence that the timers should be allocated 24 | * _Nbr_16timers indicates how many 16 bit timers are available. 25 | */ 26 | 27 | #ifndef __SERVO_TIMERS_H__ 28 | #define __SERVO_TIMERS_H__ 29 | 30 | /** 31 | * SAMD Only definitions 32 | * --------------------- 33 | */ 34 | 35 | // For SAMD: 36 | #define _useTimer1 37 | //#define _useTimer2 // <- TODO do not activate until the code in Servo.cpp has been changed in order 38 | // to manage more than one channel per timer on the SAMD architecture 39 | 40 | #if defined (_useTimer1) 41 | #define TC_FOR_TIMER1 TC4 42 | #define CHANNEL_FOR_TIMER1 0 43 | #define INTENSET_BIT_FOR_TIMER_1 TC_INTENSET_MC0 44 | #define INTENCLR_BIT_FOR_TIMER_1 TC_INTENCLR_MC0 45 | #define INTFLAG_BIT_FOR_TIMER_1 TC_INTFLAG_MC0 46 | #define ID_TC_FOR_TIMER1 ID_TC4 47 | #define IRQn_FOR_TIMER1 TC4_IRQn 48 | #define HANDLER_FOR_TIMER1 TC4_Handler 49 | #define GCM_FOR_TIMER_1 GCM_TC4_TC5 50 | #endif 51 | #if defined (_useTimer2) 52 | #define TC_FOR_TIMER2 TC4 53 | #define CHANNEL_FOR_TIMER2 1 54 | #define INTENSET_BIT_FOR_TIMER_2 TC_INTENSET_MC1 55 | #define INTENCLR_BIT_FOR_TIMER_2 TC_INTENCLR_MC1 56 | #define ID_TC_FOR_TIMER2 ID_TC4 57 | #define IRQn_FOR_TIMER2 TC4_IRQn 58 | #define HANDLER_FOR_TIMER2 TC4_Handler 59 | #define GCM_FOR_TIMER_2 GCM_TC4_TC5 60 | #endif 61 | 62 | typedef enum { 63 | #if defined (_useTimer1) 64 | _timer1, 65 | #endif 66 | #if defined (_useTimer2) 67 | _timer2, 68 | #endif 69 | _Nbr_16timers } timer16_Sequence_t; 70 | 71 | #endif // __SERVO_TIMERS_H__ 72 | -------------------------------------------------------------------------------- /scripts/astyle-file.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | filename=$1 4 | 5 | if [[ -n "$filename" ]]; then 6 | astyle \ 7 | --style=allman \ 8 | --indent=tab \ 9 | --indent-switches \ 10 | --indent-cases \ 11 | --indent-namespaces \ 12 | --indent-labels \ 13 | --indent-preproc-block \ 14 | --indent-preproc-define \ 15 | --indent-col1-comments \ 16 | --min-conditional-indent=0 \ 17 | --max-instatement-indent=80 \ 18 | --break-blocks \ 19 | --pad-oper \ 20 | --pad-paren-in \ 21 | --unpad-paren \ 22 | --align-pointer=name \ 23 | --align-reference=name \ 24 | --break-closing-brackets \ 25 | --add-brackets \ 26 | --keep-one-line-statements \ 27 | --close-templates \ 28 | --remove-comment-prefix \ 29 | --max-code-length=200 \ 30 | --suffix=none \ 31 | --lineend=linux \ 32 | $filename 33 | else 34 | echo "argument error" 35 | fi -------------------------------------------------------------------------------- /scripts/astyle-source.sh: -------------------------------------------------------------------------------- 1 | astyle \ 2 | --style=allman \ 3 | --indent=tab \ 4 | --indent-switches \ 5 | --indent-cases \ 6 | --indent-namespaces \ 7 | --indent-labels \ 8 | --indent-preproc-block \ 9 | --indent-preproc-define \ 10 | --indent-col1-comments \ 11 | --min-conditional-indent=0 \ 12 | --max-instatement-indent=80 \ 13 | --break-blocks \ 14 | --pad-oper \ 15 | --pad-paren-in \ 16 | --unpad-paren \ 17 | --align-pointer=name \ 18 | --align-reference=name \ 19 | --break-closing-brackets \ 20 | --add-brackets \ 21 | --keep-one-line-statements \ 22 | --close-templates \ 23 | --remove-comment-prefix \ 24 | --max-code-length=200 \ 25 | --suffix=none \ 26 | --recursive \ 27 | --lineend=linux \ 28 | /opt/openrov/firmware/sketches/*.cpp \ 29 | /opt/openrov/firmware/sketches/*.h \ 30 | /opt/openrov/firmware/sketches/*.ino \ 31 | /opt/openrov/firmware/sketches/*.c \ 32 | /opt/openrov/firmware/sketches/*.hpp 33 | 34 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CAltServo.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_ALT_SERVO) 3 | 4 | #include "CPin.h" 5 | #include "CAltServo.h" 6 | #include 7 | #include "PinDefinitions.h" 8 | 9 | // AltServo 10 | #define ALTS_MIDPOINT 1500 11 | #define ALTS_MINPOINT 1000 12 | #define ALTS_MAXPOINT 2000 13 | 14 | namespace 15 | { 16 | Servo _altservo; 17 | 18 | int alts_val = ALTS_MIDPOINT; 19 | int new_alts = ALTS_MIDPOINT; 20 | int altsrate = 1; 21 | 22 | int smoothAdjustedServo( int target, int current ) 23 | { 24 | double x = target - current; 25 | int sign = ( x > 0 ) - ( x < 0 ); 26 | int adjustedVal = current + sign * ( min( abs( target - current ), altsrate ) ); 27 | return ( adjustedVal ); 28 | } 29 | } 30 | 31 | void CAltServo::Initialize() 32 | { 33 | _altservo.attach( PIN_ALTSERVO ); 34 | _altservo.writeMicroseconds( ALTS_MIDPOINT ); 35 | } 36 | 37 | void CAltServo::Update( CCommand& commandIn ) 38 | { 39 | if( commandIn.Equals( "asrt" ) ) 40 | { 41 | int ms = map( commandIn.m_arguments[1], -100, 100, ALTS_MINPOINT, ALTS_MAXPOINT ); 42 | 43 | if( ( ms >= ALTS_MINPOINT ) && ( ms <= ALTS_MAXPOINT ) ) 44 | { 45 | alts_val = ms; 46 | Serial.print( F( "asr.t:" ) ); 47 | Serial.print( alts_val ); 48 | Serial.println( ';' ); 49 | } 50 | } 51 | 52 | if( alts_val != new_alts ) 53 | { 54 | new_alts = smoothAdjustedServo( alts_val, new_alts ); 55 | _altservo.writeMicroseconds( new_alts ); 56 | Serial.print( F( "asr.v:" ) ); 57 | Serial.print( new_alts ); 58 | Serial.println( ';' ); 59 | } 60 | 61 | 62 | } 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CAltServo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | class CAltServo : public CModule 7 | { 8 | public: 9 | void Initialize(); 10 | void Update( CCommand& commandIn ); 11 | }; 12 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CAutopilot.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | class CAutopilot : public CModule 7 | { 8 | public: 9 | virtual void Initialize(); 10 | virtual void Update( CCommand& commandIn ); 11 | }; 12 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CAutopilot_STD.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_STD_AUTOPILOT) 3 | 4 | #include "CAutopilot.h" 5 | #include 6 | #include "NDataManager.h" 7 | #include "NCommManager.h" 8 | 9 | namespace 10 | { 11 | orutil::CTimer pilotTimer; 12 | 13 | bool _headingHoldEnabled = false; 14 | int _headingHoldTarget = 0; 15 | int hdg = 0; 16 | int hdg_Error; 17 | int raw_Left, raw_Right; 18 | float heading_loop_Gain = 1.0; 19 | float depth_hold_loop_gain = 0.6; 20 | long hdg_Error_Integral = 0; 21 | int tgt_Hdg = 0; 22 | bool _depthHoldEnabled = false; 23 | int _depthHoldTarget = 0; 24 | int depth = 0; 25 | int depth_Error = 0; 26 | int raw_lift = 0; 27 | int lift = 0; 28 | int target_depth; 29 | int raw_yaw, yaw; 30 | 31 | int depth_deadband = 4; // +/- cm 32 | int heading_deadband = 4; // +/i degrees 33 | 34 | } 35 | 36 | void CAutopilot::Initialize() 37 | { 38 | pilotTimer.Reset(); 39 | 40 | Serial.println( F( "log:pilot setup complete;" ) ); 41 | } 42 | 43 | 44 | 45 | void CAutopilot::Update( CCommand& command ) 46 | { 47 | //intended to respond to fly by wire commands: MaintainHeading(); TurnTo(compassheading); DiveTo(depth); 48 | 49 | if( NCommManager::m_isCommandAvailable ) 50 | { 51 | 52 | if( command.Equals( "headloff" ) || command.Equals( "holdHeading_off" ) ) 53 | { 54 | _headingHoldEnabled = false; 55 | raw_Left = 0; 56 | raw_Right = 0; 57 | hdg_Error_Integral = 0; // Reset error integrator 58 | tgt_Hdg = -500; // -500 = system not in hdg hold 59 | 60 | int32_t m_argumentsToSend[] = {1, 00}; //include number of parms as fist parm 61 | command.PushCommand( "yaw", m_argumentsToSend ); 62 | Serial.println( F( "log:heading_hold_disabled;" ) ); 63 | Serial.print( F( "targetHeading:" ) ); 64 | Serial.print( F("DISABLED") ); 65 | Serial.println( ';' ); 66 | 67 | } 68 | 69 | if( command.Equals( "headlon" ) || command.Equals( "holdHeading_on" ) ) 70 | { 71 | _headingHoldEnabled = true; 72 | 73 | if( command.m_arguments[0] == 0 ) 74 | { 75 | _headingHoldTarget = NDataManager::m_navData.YAW; 76 | } 77 | else 78 | { 79 | _headingHoldTarget = command.m_arguments[1]; 80 | } 81 | 82 | tgt_Hdg = _headingHoldTarget; 83 | Serial.print( F( "log:heading_hold_enabled on=" ) ); 84 | Serial.print( tgt_Hdg ); 85 | Serial.println( ';' ); 86 | Serial.print( F( "targetHeading:" ) ); 87 | Serial.print( tgt_Hdg ); 88 | Serial.println( ';' ); 89 | } 90 | 91 | //Backwards compatibility for a release or two (2.5.1 release) 92 | if( command.Equals( "holdHeading_toggle" ) ) 93 | { 94 | if( _headingHoldEnabled ) 95 | { 96 | int32_t m_argumentsToSend[] = {0}; //include number of parms as fist parm 97 | command.PushCommand( "headloff", m_argumentsToSend ); 98 | } 99 | else 100 | { 101 | if( command.m_arguments[0] == 0 ) 102 | { 103 | int32_t m_argumentsToSend[] = {0}; //include number of parms as fist parm 104 | command.PushCommand( "headlon", m_argumentsToSend ); 105 | } 106 | else 107 | { 108 | int32_t m_argumentsToSend[] = {1, command.m_arguments[1]}; //include number of parms as fist parm 109 | command.PushCommand( "headlon", m_argumentsToSend ); 110 | } 111 | 112 | } 113 | } 114 | 115 | if( command.Equals( "deptloff" ) || command.Equals( "holdDepth_off" ) ) 116 | { 117 | _depthHoldEnabled = false; 118 | raw_lift = 0; 119 | target_depth = 0; 120 | 121 | int32_t m_argumentsToSend[] = {1, 0}; //include number of parms as fist parm 122 | command.PushCommand( "lift", m_argumentsToSend ); 123 | Serial.println( F( "log:depth_hold_disabled;" ) ); 124 | Serial.print( F( "targetDepth:" ) ); 125 | Serial.print( F("DISABLED") ); 126 | Serial.println( ';' ); 127 | 128 | } 129 | 130 | if( command.Equals( "deptlon" ) || command.Equals( "holdDepth_on" ) ) 131 | { 132 | _depthHoldEnabled = true; 133 | 134 | if( command.m_arguments[0] == 0 ) 135 | { 136 | _depthHoldTarget = NDataManager::m_navData.DEEP * 100; //casting to cm 137 | } 138 | else 139 | { 140 | _depthHoldTarget = command.m_arguments[1]; 141 | } 142 | 143 | target_depth = _depthHoldTarget; 144 | Serial.print( F( "log:depth_hold_enabled on=" ) ); 145 | Serial.print( target_depth ); 146 | Serial.println( ';' ); 147 | Serial.print( F( "targetDepth:" ) ); 148 | Serial.print( target_depth ); 149 | Serial.println( ';' ); 150 | } 151 | 152 | 153 | if( command.Equals( "holdDepth_toggle" ) ) 154 | { 155 | if( _depthHoldEnabled ) 156 | { 157 | int32_t m_argumentsToSend[] = {0}; //include number of parms as fist parm 158 | command.PushCommand( "deptloff", m_argumentsToSend ); 159 | } 160 | else 161 | { 162 | if( command.m_arguments[0] == 0 ) 163 | { 164 | int32_t m_argumentsToSend[] = {0}; //include number of parms as fist parm 165 | command.PushCommand( "deptlon", m_argumentsToSend ); 166 | } 167 | else 168 | { 169 | int32_t m_argumentsToSend[] = {1, command.m_arguments[1]}; //include number of parms as fist parm 170 | command.PushCommand( "deptlon", m_argumentsToSend ); 171 | } 172 | } 173 | } 174 | } 175 | 176 | 177 | if( pilotTimer.HasElapsed( 50 ) ) 178 | { 179 | 180 | // Autopilot Test #3 6 Jan 2014 181 | // Hold vehicle at arbitrary heading 182 | // Integer math; proportional control plus basic integrator 183 | // No hysteresis around 180 degree error 184 | 185 | // Check whether hold mode is on 186 | 187 | if( _depthHoldEnabled ) 188 | { 189 | depth = NDataManager::m_navData.DEEP * 100; 190 | depth_Error = target_depth - depth; //positive error = positive lift = go deeper. 191 | 192 | raw_lift = ( float )depth_Error * depth_hold_loop_gain; 193 | lift = constrain( raw_lift, -100, 100 ); 194 | 195 | Serial.println( F( "log:dhold pushing command;" ) ); 196 | Serial.print( F( "dp_er:" ) ); 197 | Serial.print( depth_Error ); 198 | Serial.println( ';' ); 199 | 200 | if( abs( depth_Error ) > depth_deadband ) 201 | { 202 | int32_t m_argumentsToSend[] = {1, lift}; //include number of parms as fist parm 203 | command.PushCommand( "lift", m_argumentsToSend ); 204 | } 205 | else 206 | { 207 | int32_t m_argumentsToSend[] = {1, 0}; //include number of parms as fist parm 208 | command.PushCommand( "lift", m_argumentsToSend ); 209 | } 210 | 211 | } 212 | 213 | if( _headingHoldEnabled ) 214 | { 215 | 216 | // Code for hold mode here 217 | hdg = NDataManager::m_navData.YAW; 218 | 219 | // Calculate heading error 220 | 221 | hdg_Error = hdg - tgt_Hdg; 222 | 223 | if( hdg_Error > 180 ) 224 | { 225 | hdg_Error = hdg_Error - 360; 226 | } 227 | 228 | if( hdg_Error < -179 ) 229 | { 230 | hdg_Error = hdg_Error + 360; 231 | } 232 | 233 | // Run error accumulator (integrator) 234 | hdg_Error_Integral = hdg_Error_Integral + hdg_Error; 235 | 236 | // Calculator motor outputs 237 | raw_yaw = -1 * hdg_Error * heading_loop_Gain; 238 | 239 | // raw_Left = raw_Left - (hdg_Error_Integral / integral_Divisor); 240 | // raw_Right = raw_Right + (hdg_Error_Integral / integral_Divisor); 241 | 242 | // Constrain and output to motors 243 | 244 | yaw = constrain( raw_yaw, -50, 50 ); 245 | Serial.println( F( "log:hold pushing command;" ) ); 246 | Serial.print( F( "p_er:" ) ); 247 | Serial.print( hdg_Error ); 248 | Serial.println( ';' ); 249 | 250 | if( abs( hdg_Error ) > heading_deadband ) 251 | { 252 | //start the motor with least power 253 | if( hdg_Error > 0 ) 254 | { 255 | hdg_Error -= heading_deadband; 256 | } 257 | else 258 | { 259 | hdg_Error += heading_deadband; 260 | } 261 | 262 | int32_t m_argumentsToSend[] = {1, yaw}; //include number of parms as fist parm 263 | command.PushCommand( "yaw", m_argumentsToSend ); 264 | } 265 | else 266 | { 267 | int32_t m_argumentsToSend[] = {1, 0}; //include number of parms as fist parm 268 | command.PushCommand( "yaw", m_argumentsToSend ); 269 | } 270 | } 271 | 272 | 273 | } 274 | } 275 | #endif 276 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CBNO055.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_BNO055) 3 | 4 | #include "CBNO055.h" 5 | #include "NDataManager.h" 6 | #include "NCommManager.h" 7 | 8 | using namespace bno055; 9 | 10 | namespace 11 | { 12 | constexpr uint32_t kMaxHardResets = 5; 13 | 14 | constexpr uint32_t kStatusCheckDelay_ms = 1000; // 1hz 15 | constexpr uint32_t kTelemetryDelay_ms = 50; // 20hz 16 | constexpr uint32_t kCalibDelay_ms = bno055::kCalUpdateRate_ms; // 1hz 17 | } 18 | 19 | CBNO055::CBNO055( I2C *i2cInterfaceIn, bno055::EAddress addressIn ) 20 | : m_device( i2cInterfaceIn, addressIn ) 21 | { 22 | } 23 | 24 | void CBNO055::Initialize() 25 | { 26 | Serial.println( F( "bno055_init:1;" ) ); 27 | 28 | m_statusCheckTimer.Reset(); 29 | m_telemetryTimer.Reset(); 30 | m_calibTimer.Reset(); 31 | 32 | // 25% max failure rate before 33 | m_maxFailuresPerPeriod = ( kStatusCheckDelay_ms / 4 ) / m_device.GetUpdatePeriod(); 34 | } 35 | 36 | void CBNO055::Update( CCommand &commandIn ) 37 | { 38 | if( m_device.IsEnabled() == false ) 39 | { 40 | return; 41 | } 42 | 43 | // Handle commands 44 | if( NCommManager::m_isCommandAvailable ) 45 | { 46 | // Zero the yaw value 47 | if( commandIn.Equals( "imu_zyaw" ) ) 48 | { 49 | // Set offset based on current value 50 | m_yawOffset = m_device.m_data.yaw; 51 | 52 | // Send ack 53 | Serial.println( F( "imu_zyaw:ack;" ) ); 54 | } 55 | // Zero the roll and pich values 56 | else if( commandIn.Equals( "imu_level" ) ) 57 | { 58 | // Set offsets based on request from cockpit 59 | m_rollOffset = orutil::Decode1K( commandIn.m_arguments[1] ); 60 | m_pitchOffset = orutil::Decode1K( commandIn.m_arguments[2] ); 61 | 62 | // Report new settings 63 | Serial.print( F( "imu_roff:" ) ); Serial.print( commandIn.m_arguments[1] ); Serial.println( ';' ); 64 | Serial.print( F( "imu_poff:" ) ); Serial.print( commandIn.m_arguments[2] ); Serial.println( ';' ); 65 | } 66 | // Set the operating mode 67 | else if( commandIn.Equals( "imu_mode" ) ) 68 | { 69 | // For now: 0-GYRO, 1-COMPASS 70 | if( commandIn.m_arguments[1] == 0 ) 71 | { 72 | m_device.SetMode( EMode::GYRO ); 73 | 74 | Serial.print( F( "imu_mode:" ) ); 75 | Serial.print( commandIn.m_arguments[1] ); 76 | Serial.println( ';' ); 77 | } 78 | else if( commandIn.m_arguments[1] == 1 ) 79 | { 80 | m_device.SetMode( EMode::COMPASS ); 81 | 82 | Serial.print( F( "imu_mode:" ) ); 83 | Serial.print( commandIn.m_arguments[1] ); 84 | Serial.println( ';' ); 85 | } 86 | } 87 | } 88 | 89 | // Handle health checks 90 | if( m_statusCheckTimer.HasElapsed( kStatusCheckDelay_ms ) ) 91 | { 92 | // Check to see if the error threshold is above acceptable levels 93 | if( m_device.GetResultCount( EResult::RESULT_ERR_READING_EULER ) > m_maxFailuresPerPeriod ) 94 | { 95 | Serial.println( "bno055_HardReset:1" ); 96 | m_device.HardReset(); 97 | } 98 | else 99 | { 100 | // Clear the error counter 101 | m_device.ClearResultCount( EResult::RESULT_ERR_READING_EULER ); 102 | } 103 | 104 | // Check to see if we have surpassed our hard reset threshhold 105 | if( m_device.GetResultCount( EResult::RESULT_ERR_HARD_RESET ) > kMaxHardResets ) 106 | { 107 | // Permanently disable the device 108 | m_device.Disable(); 109 | Serial.println( F( "bno055_disabled:1;" ) ); 110 | return; 111 | } 112 | } 113 | 114 | // Run the state machine 115 | m_device.Tick(); 116 | 117 | // Update shared navdata 118 | if( m_device.m_data.SampleAvailable() ) 119 | { 120 | if( m_device.GetMode() == EMode::GYRO ) 121 | { 122 | NDataManager::m_navData.YAW = NORMALIZE_ANGLE_180( m_device.m_data.yaw - m_yawOffset ); 123 | } 124 | else 125 | { 126 | NDataManager::m_navData.YAW = NORMALIZE_ANGLE_360( m_device.m_data.yaw ); 127 | } 128 | 129 | NDataManager::m_navData.PITC = m_device.m_data.pitch - m_pitchOffset; 130 | NDataManager::m_navData.ROLL = m_device.m_data.roll - m_rollOffset; 131 | 132 | // Emit telemetry 133 | if( m_telemetryTimer.HasElapsed( kTelemetryDelay_ms ) ) 134 | { 135 | Serial.print( F( "imu_r:" ) ); Serial.print( orutil::Encode1K( NDataManager::m_navData.ROLL ) ); Serial.print( ';' ); 136 | Serial.print( F( "imu_p:" ) ); Serial.print( orutil::Encode1K( NDataManager::m_navData.PITC ) ); Serial.print( ';' ); 137 | Serial.print( F( "imu_y:" ) ); Serial.print( orutil::Encode1K( NDataManager::m_navData.YAW ) ); Serial.println( ';' ); 138 | } 139 | } 140 | 141 | // Report calibration levels of each subsystem 142 | if( m_device.m_calibration.SampleAvailable() ) 143 | { 144 | if( m_calibTimer.HasElapsed( kCalibDelay_ms ) ) 145 | { 146 | Serial.print( F( "bno055_cAcc:" ) ); Serial.print( m_device.m_calibration.accel ); Serial.print( ';' ); 147 | Serial.print( F( "bno055_cGyr:" ) ); Serial.print( m_device.m_calibration.gyro ); Serial.print( ';' ); 148 | Serial.print( F( "bno055_cMag:" ) ); Serial.print( m_device.m_calibration.mag ); Serial.print( ';' ); 149 | Serial.print( F( "bno055_cSys:" ) ); Serial.print( m_device.m_calibration.system ); Serial.println( ';' ); 150 | } 151 | } 152 | } 153 | 154 | #endif -------------------------------------------------------------------------------- /sketches/OpenROV2x/CBNO055.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include 6 | #include "CModule.h" 7 | 8 | class CBNO055 : public CModule 9 | { 10 | public: 11 | CBNO055( I2C *i2cInterfaceIn, bno055::EAddress addressIn ); 12 | void Initialize(); 13 | void Update( CCommand &commandIn ); 14 | 15 | private: 16 | // Device driver 17 | bno055::BNO055 m_device; 18 | 19 | // Timers 20 | orutil::CTimer m_statusCheckTimer; 21 | orutil::CTimer m_telemetryTimer; 22 | orutil::CTimer m_calibTimer; 23 | 24 | float m_yawOffset = 0.0f; 25 | float m_rollOffset = 0.0f; 26 | float m_pitchOffset = 0.0f; 27 | 28 | uint32_t m_maxFailuresPerPeriod = 0; 29 | }; 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CCalibrationLaser.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_STD_CALIBRATIONLASERS) 3 | 4 | // Includes 5 | #include "CCalibrationLaser.h" 6 | #include "NCommManager.h" 7 | 8 | CCalibrationLaser::CCalibrationLaser( uint8_t pinIn ) 9 | : m_pin( pinIn, CPin::kAnalog, CPin::kOutput ) 10 | { 11 | } 12 | 13 | void CCalibrationLaser::Initialize() 14 | { 15 | m_pin.Write( 0 ); 16 | } 17 | 18 | void CCalibrationLaser::Update( CCommand& command ) 19 | { 20 | // Check for messages 21 | if( !NCommManager::m_isCommandAvailable ) 22 | { 23 | return; 24 | } 25 | 26 | // Handle messages 27 | if( command.Equals( "claser" ) ) 28 | { 29 | // TODO: consistent api with lights 30 | 31 | // Set the laser pin value 32 | m_pin.Write( command.m_arguments[1] ); 33 | Serial.print( "claser:" ); Serial.print( command.m_arguments[1] ); Serial.println( ';' ); 34 | } 35 | } 36 | 37 | #endif 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CCalibrationLaser.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | #include "CPin.h" 6 | 7 | class CCalibrationLaser : public CModule 8 | { 9 | public: 10 | CCalibrationLaser( uint8_t pinIn ); 11 | 12 | void Initialize(); 13 | void Update( CCommand& commandIn ); 14 | 15 | private: 16 | CPin m_pin; 17 | }; 18 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CCameraServo.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_CAMERASERVO) 3 | 4 | // Includes 5 | #include 6 | #include 7 | #include 8 | 9 | #include "CCameraServo.h" 10 | #include 11 | #include "CPin.h" 12 | #include "NDataManager.h" 13 | #include "NModuleManager.h" 14 | #include "NCommManager.h" 15 | #include 16 | 17 | #include "CControllerBoard.h" 18 | 19 | #include 20 | 21 | // Defines 22 | #ifndef F_CPU 23 | #define F_CPU 16000000UL 24 | #endif 25 | 26 | // File local variables and methods 27 | namespace 28 | { 29 | constexpr float kZeroPosMicrosecs = 1487.0f; 30 | constexpr float kMicrosecPerDegree = 9.523809f; 31 | constexpr float kDegPerMicrosec = ( 1 / kMicrosecPerDegree ); 32 | 33 | // Helper functions specifically for the HITEC servo 34 | constexpr uint32_t DegreesToMicroseconds( float degreesIn, bool isInverted = false ) 35 | { 36 | return static_cast( ( kMicrosecPerDegree * ( isInverted ? -degreesIn : degreesIn ) ) + kZeroPosMicrosecs ); 37 | } 38 | 39 | constexpr float MicrosecondsToDegrees( uint32_t microsecondsIn, bool isInverted = false ) 40 | { 41 | return ( isInverted ? -( ( static_cast( microsecondsIn ) - kZeroPosMicrosecs ) * kDegPerMicrosec ) 42 | :( ( static_cast( microsecondsIn ) - kZeroPosMicrosecs ) * kDegPerMicrosec ) ); 43 | } 44 | 45 | constexpr float kNeutralPosition_deg = 0.0f; 46 | constexpr uint32_t kNeutralPosition_us = DegreesToMicroseconds( 0.0f ); 47 | 48 | constexpr float kDefaultSpeed = 50.0f; // Degrees per sec 49 | 50 | // Attributes 51 | orutil::CTimer m_controlTimer; 52 | orutil::CTimer m_telemetryTimer; 53 | 54 | float m_targetPos_deg = kNeutralPosition_deg; 55 | float m_currentPos_deg = kNeutralPosition_deg; 56 | uint32_t m_targetPos_us = kNeutralPosition_us; 57 | uint32_t m_currentPos_us = kNeutralPosition_us; 58 | float m_fCurrentPos_us = kZeroPosMicrosecs; 59 | 60 | uint32_t m_tDelta = 0; 61 | uint32_t m_tLast = 0; 62 | 63 | // Settings 64 | float m_speed_deg_per_s = kDefaultSpeed; 65 | int m_isInverted = 0; // 0 - Not inverted, 1 - Inverted 66 | 67 | // Derived from settings 68 | float m_speed_us_per_ms = ( kDefaultSpeed * 0.001f ) * kMicrosecPerDegree; 69 | 70 | // Float<->Int conversion helpers 71 | constexpr int32_t Encode( float valueIn ) 72 | { 73 | return static_cast( valueIn * 1000.0f ); 74 | } 75 | 76 | constexpr float Decode( int32_t valueIn ) 77 | { 78 | return ( static_cast( valueIn ) * 0.001f ); 79 | } 80 | 81 | void SetServoPosition( uint32_t microsecondsIn ) 82 | { 83 | // Set to 90° --> pulsewdith = 1.5ms 84 | OCR1A = microsecondsIn * 2; 85 | } 86 | } 87 | 88 | void CCameraServo::Initialize() 89 | { 90 | // Set up the pin for the camera servo 91 | pinMode( PIN_CAMERA_MOUNT, OUTPUT ); 92 | 93 | // Set up the timers driving the PWM for the servo (AVR specific) 94 | TCCR1A = 0; 95 | TCCR1B = 0; 96 | TCCR1A |= ( 1 << COM1A1 ) | ( 1 << WGM11 ); // non-inverting mode for OC1A 97 | TCCR1B |= ( 1 << WGM13 ) | ( 1 << WGM12 ) | ( 1 << CS11 ); // Mode 14, Prescaler 8 98 | 99 | ICR1 = 40000; // 320000 / 8 = 40000 100 | 101 | // Set initial position 102 | SetServoPosition( kNeutralPosition_us ); 103 | 104 | // Reset timers 105 | m_controlTimer.Reset(); 106 | m_telemetryTimer.Reset(); 107 | } 108 | 109 | void CCameraServo::Update( CCommand& command ) 110 | { 111 | // Check for messages 112 | if( NCommManager::m_isCommandAvailable ) 113 | { 114 | // Handle messages 115 | if( command.Equals( "camServ_tpos" ) ) 116 | { 117 | // TODO: Ideally this unit would have the ability to autonomously set its own target and ack receipt with a separate mechanism 118 | // Acknowledge target position 119 | Serial.print( F( "camServ_tpos:" ) ); 120 | Serial.print( static_cast( command.m_arguments[1] ) ); 121 | Serial.println( ';' ); 122 | 123 | // Update the target position 124 | m_targetPos_deg = Decode( static_cast( command.m_arguments[1] ) ); 125 | 126 | // Update the target microseconds 127 | m_targetPos_us = DegreesToMicroseconds( m_targetPos_deg, m_isInverted ); 128 | } 129 | else if( command.Equals( "camServ_spd" ) ) 130 | { 131 | // Acknowledge receipt of command 132 | Serial.print( F( "camServ_spd:" ) ); 133 | Serial.print( static_cast( command.m_arguments[1] ) ); 134 | Serial.println( ';' ); 135 | 136 | // Decode the requested speed and update the setting 137 | m_speed_deg_per_s = Decode( static_cast( command.m_arguments[1] ) ); 138 | m_speed_us_per_ms = ( m_speed_deg_per_s * 0.001f ) * kMicrosecPerDegree; 139 | } 140 | else if( command.Equals( "camServ_inv" ) ) 141 | { 142 | if( command.m_arguments[1] == 1 ) 143 | { 144 | // Set inverted 145 | m_isInverted = 1; 146 | 147 | // Report back 148 | Serial.print( F( "camServ_inv:1;" ) ); 149 | } 150 | else if( command.m_arguments[1] == 0 ) 151 | { 152 | // Set uninverted 153 | m_isInverted = 0; 154 | 155 | // Report back 156 | Serial.print( F( "camServ_inv:0;" ) ); 157 | } 158 | 159 | // Adjust target_us based on inversion 160 | m_targetPos_us = DegreesToMicroseconds( m_targetPos_deg, m_isInverted ); 161 | } 162 | } 163 | 164 | // Run servo adjustment at 200Hz 165 | if( m_controlTimer.HasElapsed( 5 ) ) 166 | { 167 | // Get time elapsed since last position update 168 | m_tDelta = millis() - m_tLast; 169 | 170 | // Update position if not at desired location 171 | if( m_currentPos_us != m_targetPos_us ) 172 | { 173 | float error = static_cast( m_targetPos_us ) - m_fCurrentPos_us; 174 | 175 | // Check to see if the error/dT is smaller than the speed limit 176 | if( abs( error / static_cast( m_tDelta ) ) < m_speed_us_per_ms ) 177 | { 178 | // Move directly to the target 179 | // NOTE: Cannot use the cast method like below, since the floating point 180 | // representation of the target pos might be comparatively less than the integer value. 181 | // I.e. target = 32, static_cast( 32 ) -> 31.99999, static_cast( 31.99999 ) -> 31 182 | // This could lead to the current position never reaching the target 183 | m_currentPos_us = m_targetPos_us; 184 | 185 | // Update the floating point representation as well, for use in future target updates 186 | m_fCurrentPos_us = static_cast( m_targetPos_us ); 187 | } 188 | else 189 | { 190 | // Move by the delta towards the target 191 | m_fCurrentPos_us += ( ( ( error < 0 ) ? -m_speed_us_per_ms : m_speed_us_per_ms ) * static_cast( m_tDelta ) ); 192 | 193 | // Cast the floating point servo command to an integer 194 | m_currentPos_us = static_cast( m_fCurrentPos_us ); 195 | } 196 | 197 | // Set the servo to this target 198 | SetServoPosition( m_currentPos_us ); 199 | 200 | // Update the position value in degrees 201 | m_currentPos_deg = MicrosecondsToDegrees( m_currentPos_us ); 202 | } 203 | 204 | m_tLast = millis(); 205 | } 206 | 207 | // Emit position telemetry at 3Hz 208 | if( m_telemetryTimer.HasElapsed( 333 ) ) 209 | { 210 | Serial.print( F( "camServ_pos:" ) ); 211 | Serial.print( Encode( m_currentPos_deg ) ); 212 | Serial.println( ';' ); 213 | } 214 | } 215 | 216 | #endif 217 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CCameraServo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | class CCameraServo : public CModule 7 | { 8 | public: 9 | void Initialize(); 10 | void Update( CCommand& commandIn ); 11 | }; 12 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CCommand.cpp: -------------------------------------------------------------------------------- 1 | #include "CCommand.h" 2 | 3 | // File local variables and methods 4 | namespace 5 | { 6 | char dataBuffer[COMMAND_DATA_BUFFER_SIZE + 1]; //Add 1 for NULL terminator 7 | uint8_t dataBufferIndex = 0; 8 | bool commandReady = false; 9 | const char endChar = ';'; // or '!', or whatever your end character is 10 | bool storeString = false; //This will be our flag to put the data in our buffer 11 | TInternalCommand internalCommandBuffer[ COMMAND_MAX_COUNT ]; 12 | int internalCommandBuffer_head = 0; 13 | int internalCommandBuffer_tail = 0; 14 | 15 | // CRC-8 - based on the CRC8 formulas by Dallas/Maxim 16 | // Code released under the therms of the GNU GPL 3.0 license 17 | uint8_t CRC8( uint8_t start, char* data, uint8_t len ) 18 | { 19 | uint8_t crc = 0x00; 20 | 21 | for( uint8_t j = 0; j < start; j++ ) 22 | { 23 | *data++; 24 | } 25 | 26 | while( len-- ) 27 | { 28 | uint8_t extract = *data++; 29 | 30 | for( uint8_t tempI = 8; tempI; tempI-- ) 31 | { 32 | uint8_t sum = ( crc ^ extract ) & 0x01; 33 | crc >>= 1; 34 | 35 | if( sum ) 36 | { 37 | crc ^= 0x8C; 38 | } 39 | 40 | extract >>= 1; 41 | } 42 | } 43 | 44 | return crc; 45 | } 46 | 47 | 48 | bool GetSerialString() 49 | { 50 | while( Serial.available() > 0 ) 51 | { 52 | char incomingbyte = Serial.read(); 53 | 54 | // Start accumulating new string 55 | if( storeString == false ) 56 | { 57 | storeString = true; 58 | dataBufferIndex = 0; 59 | } 60 | 61 | // If accumulating 62 | if( storeString ) 63 | { 64 | //Let's check our index here, and abort if we're outside our buffer size 65 | //We use our define here so our buffer size can be easily modified 66 | if( dataBufferIndex == COMMAND_DATA_BUFFER_SIZE ) 67 | { 68 | //Oops, our index is pointing to an array element outside our buffer. 69 | dataBufferIndex = 0; 70 | break; 71 | } 72 | 73 | if( incomingbyte == endChar ) 74 | { 75 | dataBuffer[dataBufferIndex++] = incomingbyte; 76 | dataBuffer[dataBufferIndex] = 0; //null terminate the C string 77 | storeString = false; 78 | 79 | //Our data string is complete. return true 80 | return true; 81 | } 82 | else 83 | { 84 | dataBuffer[dataBufferIndex++] = incomingbyte; 85 | dataBuffer[dataBufferIndex] = 0; //null terminate the C string 86 | } 87 | } 88 | else 89 | { 90 | } 91 | } 92 | 93 | //We've read in all the available Serial data, and don't have a valid string yet, so return false 94 | return false; 95 | } 96 | } 97 | 98 | 99 | // Static member initialization 100 | int32_t CCommand::m_arguments[COMMAND_MAX_ARGUMENTS]; 101 | char CCommand::m_text[COMMAND_DATA_BUFFER_SIZE + 1] ; 102 | 103 | bool CCommand::Equals( const char* targetcommand ) 104 | { 105 | if( !commandReady ) 106 | { 107 | return false; 108 | } 109 | 110 | char* pos = strstr( m_text, targetcommand ); 111 | 112 | if( pos == m_text ) //starts with 113 | { 114 | return true; 115 | } 116 | 117 | return false; 118 | } 119 | 120 | 121 | bool CCommand::GetCommandString() 122 | { 123 | // Get string from buffer 124 | 125 | commandReady = false; 126 | strcpy( m_text, "" ); 127 | 128 | for( int i = 0; i < COMMAND_MAX_ARGUMENTS; i++ ) 129 | { 130 | m_arguments[i] = 0; 131 | } 132 | 133 | if( GetSerialString() ) 134 | { 135 | //String available for parsing. Parse it here 136 | Parse(); 137 | commandReady = true; 138 | return true; 139 | } 140 | 141 | if( internalCommandBuffer_head != internalCommandBuffer_tail ) 142 | { 143 | // Advance tail index 144 | internalCommandBuffer_tail++; 145 | 146 | // Wrap around 147 | if( internalCommandBuffer_tail == COMMAND_MAX_COUNT ) 148 | { 149 | internalCommandBuffer_tail = 0; 150 | } 151 | 152 | // Get from the command buffer 153 | TInternalCommand c = internalCommandBuffer[ internalCommandBuffer_tail ]; 154 | 155 | // Copy command text 156 | strcpy( m_text, c.text ); 157 | 158 | // Check for invalid command 159 | if( strcmp( m_text, "" ) == 0 ) 160 | { 161 | Serial.print( F( "icmd: CMD MUNGED!;" ) ); 162 | return false; 163 | } 164 | 165 | // Print command 166 | Serial.print( F( "icmd:" ) ); 167 | Serial.print( m_text ); 168 | Serial.print( '(' ); 169 | 170 | for( int i = 1; i < c.arguments[0] + 1; i++ ) 171 | { 172 | m_arguments[i] = c.arguments[i]; 173 | 174 | if( i > 1 ) 175 | { 176 | Serial.print( ',' ); 177 | } 178 | 179 | Serial.print( m_arguments[i] ); 180 | } 181 | 182 | m_arguments[0] = c.arguments[0]; 183 | //need to add the trailing # of arguments to the count or else have people do it in the call which sucks. 184 | 185 | commandReady = true; 186 | 187 | Serial.println( ");" ); 188 | return true; 189 | } 190 | 191 | return false; 192 | } 193 | 194 | void CCommand::PushCommand( const char* cmdtext, int32_t cmdargs[COMMAND_MAX_ARGUMENTS] ) 195 | { 196 | // If commands are not being processed in time we overwrite the oldest ones. Technically we should probably 197 | // have a global array for all possible commands where only the most recent is ever processed to prevent 198 | // stale messages from floating around. 199 | TInternalCommand c; 200 | strcpy( c.text, cmdtext ); 201 | 202 | if( strlen( c.text ) < 1 ) 203 | { 204 | Serial.print( F( "pushcmd: cmdtext MUNGED!;" ) ); 205 | } 206 | 207 | for( int i = 0; i < cmdargs[0] + 1; i++ ) 208 | { 209 | c.arguments[i] = cmdargs[i]; 210 | } 211 | 212 | internalCommandBuffer_head++; 213 | 214 | if( internalCommandBuffer_head == COMMAND_MAX_COUNT ) 215 | { 216 | internalCommandBuffer_head = 0; 217 | } 218 | 219 | //go ahead and drop the command that has not yet been executed 220 | if( internalCommandBuffer_head == internalCommandBuffer_tail ) 221 | { 222 | Serial.println( F( "log: CommandBufferOverrun;" ) ); 223 | internalCommandBuffer_tail++; 224 | } 225 | 226 | if( internalCommandBuffer_tail == COMMAND_MAX_COUNT ) 227 | { 228 | internalCommandBuffer_tail = 0; 229 | } 230 | 231 | internalCommandBuffer[internalCommandBuffer_head] = c; 232 | } 233 | 234 | 235 | void CCommand::Reset() 236 | { 237 | // Removes any state 238 | commandReady = false; 239 | storeString = false; 240 | dataBufferIndex = 0; 241 | internalCommandBuffer_head = 0; 242 | internalCommandBuffer_tail = 0; 243 | } 244 | 245 | // get 'arguments' from command 246 | void CCommand::Parse() 247 | { 248 | char* pch; 249 | uint8_t crc = 0; 250 | uint8_t i = 0; 251 | crc = CRC8( 1, dataBuffer, dataBufferIndex - 1 ); 252 | 253 | Serial.print( F( "rawcmd:" ) ); 254 | uint8_t tt = 0; 255 | 256 | while( tt < dataBufferIndex ) 257 | { 258 | uint8_t zz = dataBuffer[tt]; 259 | Serial.print( zz, HEX ); 260 | Serial.print( ',' ); 261 | tt++; 262 | } 263 | 264 | Serial.println( ';' ); 265 | 266 | Serial.print( F( "crc:" ) ); 267 | uint8_t testcrc = dataBuffer[0]; 268 | 269 | if( crc == testcrc ) 270 | { 271 | Serial.print( F( "pass;" ) ); 272 | } 273 | else 274 | { 275 | Serial.print( F( "fail," ) ); 276 | Serial.print( crc, HEX ); 277 | Serial.print( "/" ); 278 | Serial.print( testcrc, HEX ); 279 | Serial.print( ';' ); 280 | return; 281 | } 282 | 283 | char* db2 = dataBuffer; 284 | db2++; 285 | pch = strtok( db2, " ,();" ); 286 | 287 | while( pch != NULL ) 288 | { 289 | if( i == 0 ) 290 | { 291 | //this is the command text 292 | Serial.print( F( "cmd:" ) ); 293 | Serial.print( pch ); 294 | Serial.print( '(' ); 295 | strcpy( m_text, pch ); 296 | } 297 | else 298 | { 299 | //this is a parameter 300 | m_arguments[i] = atol( pch ); 301 | 302 | if( i > 1 ) 303 | { 304 | Serial.print( ',' ); 305 | } 306 | 307 | Serial.print( pch ); 308 | } 309 | 310 | i++; 311 | pch = strtok( NULL, " ,();" ); 312 | } 313 | 314 | Serial.println( ");" ); 315 | m_arguments[0] = i - 1; 316 | } 317 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CCommand.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | 6 | // Commands 7 | #define COMMAND_MAX_ARGUMENTS 10 8 | #define COMMAND_MAX_STRING_LENGTH 40 9 | #define COMMAND_MAX_COUNT 5 // Use 3 for 328p 10 | #define COMMAND_DATA_BUFFER_SIZE 80 11 | 12 | struct TInternalCommand 13 | { 14 | char text[ COMMAND_DATA_BUFFER_SIZE + 1 ]; // Add 1 for NULL terminator 15 | int32_t arguments[ COMMAND_MAX_ARGUMENTS ]; 16 | }; 17 | 18 | class CCommand 19 | { 20 | // Attributes 21 | public: 22 | static int32_t m_arguments[ COMMAND_MAX_ARGUMENTS ]; 23 | 24 | private: 25 | static char m_text[ COMMAND_DATA_BUFFER_SIZE + 1 ]; 26 | 27 | // Methods 28 | public: 29 | bool GetCommandString(); 30 | bool Equals( const char* commandStringIn ); 31 | 32 | static void PushCommand( const char* textIn, int32_t argumentsIn[ COMMAND_MAX_ARGUMENTS ] ); 33 | static void Reset(); 34 | 35 | private: 36 | void Parse(); 37 | }; 38 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CControllerBoard.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if( HAS_OROV_CONTROLLERBOARD_25 ) 3 | 4 | // Includes 5 | #include 6 | #include "NDataManager.h" 7 | #include "CControllerBoard.h" 8 | #include 9 | #include "CPin.h" 10 | 11 | // File local variables and methods 12 | namespace 13 | { 14 | // Define the number of samples to keep track of. The higher the number, 15 | // the more the readings will be smoothed, but the slower the output will 16 | // respond to the input. Using a constant rather than a normal variable lets 17 | // use this value to determine the size of the readings array. 18 | const int numReadings = 1; 19 | int readings[numReadings]; // the readings from the analog input 20 | 21 | orutil::CTimer time; 22 | orutil::CTimer onesecondtimer; 23 | orutil::CTimer statustime2; 24 | 25 | int index = 0; // the index of the current reading 26 | int total = 0; // the running total 27 | int average = 0; // the average 28 | int temppin = A8; 29 | 30 | float celsiusTempRead; 31 | 32 | CPin i2cpower( PIN_ENABLE_I2C, CPin::kDigital, CPin::kOutput ); 33 | 34 | float mapf( long x, long in_min, long in_max, long out_min, long out_max ) 35 | { 36 | return ( float )( x - in_min ) * ( out_max - out_min ) / ( float )( in_max - in_min ) + out_min; 37 | } 38 | 39 | void readTemp() 40 | { 41 | float volt = mapf( (float)analogRead( temppin ), 0.0f, 1023.0f, 0.0f, 5.0f ); 42 | celsiusTempRead = ( volt - 0.4f ) * 51.28f; 43 | } 44 | 45 | float readCurrent( int pin ) 46 | { 47 | return mapf( (float)analogRead( pin ), 0.0f, 1023.0f, 0.0f, 10.0f ); 48 | } 49 | 50 | float read20Volts( int pin ) 51 | { 52 | return mapf( (float)analogRead( pin ), 0.0f, 1023.0f, 0.0f, 20.0f ); 53 | } 54 | 55 | float readBrdCurrent( int pin ) 56 | { 57 | return mapf( (float)analogRead( pin ), 0.0f, 1023.0f, 0.0f, 2.0f ); 58 | } 59 | 60 | long readVcc() 61 | { 62 | // Read 1.1V reference against AVcc 63 | // set the reference to Vcc and the measurement to the internal 1.1V reference 64 | #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 65 | ADMUX = _BV( REFS0 ) | _BV( MUX4 ) | _BV( MUX3 ) | _BV( MUX2 ) | _BV( MUX1 ); 66 | #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) 67 | ADMUX = _BV( MUX5 ) | _BV( MUX0 ); 68 | #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) 69 | ADMUX = _BV( MUX3 ) | _BV( MUX2 ); 70 | #else 71 | ADMUX = _BV( REFS0 ) | _BV( MUX3 ) | _BV( MUX2 ) | _BV( MUX1 ); 72 | #endif 73 | 74 | delay( 2 ); // Wait for Vref to settle 75 | ADCSRA |= _BV( ADSC ); // Start conversion 76 | 77 | while( bit_is_set( ADCSRA, ADSC ) ); // measuring 78 | 79 | uint8_t low = ADCL; // must read ADCL first - it then locks ADCH 80 | uint8_t high = ADCH; // unlocks both 81 | 82 | long result = ( high << 8 ) | low; 83 | 84 | result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000 85 | return result; // Vcc in millivolts 86 | } 87 | } 88 | 89 | void CControllerBoard::Initialize() 90 | { 91 | // Reset timers 92 | time.Reset(); 93 | statustime2.Reset(); 94 | onesecondtimer.Reset(); 95 | 96 | i2cpower.Reset(); 97 | i2cpower.Write( 0 ); 98 | delay( 10 ); 99 | i2cpower.Write( 1 ); 100 | 101 | // Initialize all the readings to 0: 102 | for( int thisReading = 0; thisReading < numReadings; ++thisReading ) 103 | { 104 | readings[ thisReading ] = 0; 105 | } 106 | } 107 | 108 | void CControllerBoard::Update( CCommand& commandIn ) 109 | { 110 | if( time.HasElapsed( 100 ) ) 111 | { 112 | // subtract the last reading: 113 | total = total - readings[index]; 114 | // read from the sensor: 115 | readings[index] = readBrdCurrent( A0 ); 116 | 117 | // add the reading to the total: 118 | total = total + readings[index]; 119 | // advance to the next position in the array: 120 | index = index + 1; 121 | 122 | // if we're at the end of the array... 123 | if( index >= numReadings ) 124 | // ...wrap around to the beginning: 125 | { 126 | index = 0; 127 | } 128 | 129 | // calculate the average: 130 | average = total / numReadings; 131 | } 132 | 133 | if( onesecondtimer.HasElapsed( 1000 ) ) 134 | { 135 | readTemp(); 136 | Serial.print( F( "BRDT:" ) ); 137 | Serial.print( celsiusTempRead ); 138 | Serial.print( ';' ); 139 | Serial.print( F( "SC1I:" ) ); 140 | Serial.print( readCurrent( A3 ) ); 141 | Serial.print( ';' ); 142 | Serial.print( F( "SC2I:" ) ); 143 | Serial.print( readCurrent( A2 ) ); 144 | Serial.print( ';' ); 145 | Serial.print( F( "SC3I:" ) ); 146 | Serial.print( readCurrent( A1 ) ); 147 | Serial.print( ';' ); 148 | Serial.print( F( "BRDI:" ) ); 149 | Serial.print( readBrdCurrent( A0 ) ); 150 | Serial.print( ';' ); 151 | Serial.print( F( "BT1I:" ) ); 152 | Serial.print( readCurrent( A6 ) ); 153 | Serial.print( ';' ); 154 | Serial.print( F( "BT2I:" ) ); 155 | Serial.print( readCurrent( A5 ) ); 156 | Serial.print( ';' ); 157 | Serial.print( F( "BRDV:" ) ); 158 | Serial.print( read20Volts( A4 ) ); 159 | Serial.print( ';' ); 160 | Serial.print( F( "AVCC:" ) ); 161 | Serial.print( readVcc() ); 162 | Serial.println( ';' ); 163 | 164 | } 165 | 166 | // Update Cape Data voltages and currents 167 | if( statustime2.HasElapsed( 100 ) ) 168 | { 169 | NDataManager::m_capeData.VOUT = read20Volts( A4 ); 170 | 171 | // #315: deprecated: this is the same thing as BRDI: 172 | NDataManager::m_capeData.IOUT = readBrdCurrent( A0 ); 173 | 174 | // Total current draw from batteries: 175 | NDataManager::m_capeData.BTTI = readCurrent( A5 ) + readCurrent( A6 ); 176 | NDataManager::m_capeData.FMEM = orutil::FreeMemory(); 177 | NDataManager::m_capeData.UTIM = millis(); 178 | } 179 | } 180 | 181 | #endif -------------------------------------------------------------------------------- /sketches/OpenROV2x/CControllerBoard.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | class CControllerBoard : public CModule 7 | { 8 | public: 9 | // Attributes 10 | 11 | // Methods 12 | virtual void Initialize(); 13 | virtual void Update( CCommand& commandIn ); 14 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CDeadManSwitch.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_STD_LIGHTS) 3 | 4 | // Includes 5 | #include 6 | #include "CDeadManSwitch.h" 7 | #include 8 | #include "NModuleManager.h" 9 | #include "NCommManager.h" 10 | 11 | // Deadman Switch 12 | #define DEADMAN_SWITCH_DELAY_TO_ARM_MS 180000 13 | 14 | // File local variables and methods 15 | namespace 16 | { 17 | bool _deadmanSwitchEnabled = false; 18 | bool blinkstate = false; 19 | bool _deadmanSwitchArmed = false; 20 | 21 | orutil::CTimer deadmanSwitchTimer; 22 | orutil::CTimer blinklightTimer; 23 | } 24 | 25 | void CDeadManSwitch::Initialize() 26 | { 27 | deadmanSwitchTimer.Reset(); 28 | blinklightTimer.Reset(); 29 | } 30 | 31 | void CDeadManSwitch::Update( CCommand& command ) 32 | { 33 | // Check for messages 34 | if( NCommManager::m_isCommandAvailable ) 35 | { 36 | if( command.Equals( "ping" ) ) 37 | { 38 | deadmanSwitchTimer.Reset(); 39 | 40 | if( _deadmanSwitchEnabled ) 41 | { 42 | int32_t argsToSend[] = {0}; 43 | command.PushCommand( "start", argsToSend ); 44 | _deadmanSwitchEnabled = false; 45 | } 46 | 47 | Serial.print( F( "pong:" ) ); 48 | Serial.print( command.m_arguments[0] ); 49 | Serial.print( "," ); 50 | Serial.print( millis() ); 51 | Serial.print( ";" ); 52 | } 53 | else if( command.Equals( "dms" ) ) 54 | { 55 | if( command.m_arguments[0] == 0 ) 56 | { 57 | _deadmanSwitchArmed = false; 58 | } 59 | else 60 | { 61 | _deadmanSwitchArmed = true; 62 | } 63 | } 64 | } 65 | 66 | //Auto arm the deadman switch after a reasonable boot time 67 | if( !_deadmanSwitchArmed && ( millis() > DEADMAN_SWITCH_DELAY_TO_ARM_MS ) ) 68 | { 69 | _deadmanSwitchArmed = true; 70 | } 71 | 72 | if( ( deadmanSwitchTimer.HasElapsed( 2000 ) ) && _deadmanSwitchArmed && ( _deadmanSwitchEnabled == false ) ) 73 | { 74 | int32_t argsToSend[] = {0}; //include number of parms as fist parm 75 | command.PushCommand( "deptloff", argsToSend ); 76 | command.PushCommand( "headloff", argsToSend ); 77 | command.PushCommand( "stop", argsToSend ); 78 | _deadmanSwitchEnabled = true; 79 | } 80 | 81 | if( _deadmanSwitchEnabled && blinklightTimer.HasElapsed( 500 ) ) 82 | { 83 | int32_t argsToSend[] = {1, 50}; 84 | 85 | if( blinkstate ) 86 | { 87 | argsToSend[1] = 0; 88 | } 89 | 90 | command.PushCommand( "ligt", argsToSend ); 91 | blinkstate = !blinkstate; 92 | } 93 | } 94 | #endif 95 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CDeadManSwitch.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | class CDeadManSwitch : public CModule 7 | { 8 | public: 9 | virtual void Initialize(); 10 | virtual void Update( CCommand& commandIn ); 11 | }; 12 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CExternalLights.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_EXT_LIGHTS ) 3 | 4 | // Includes 5 | #include "CExternalLights.h" 6 | #include "NCommManager.h" 7 | 8 | namespace 9 | { 10 | // 1% per 10ms 11 | const float kPowerDelta = 0.01f; 12 | 13 | inline uint32_t PercentToAnalog( float x ) 14 | { 15 | if( x < 0.0f ) 16 | { 17 | return 0; 18 | } 19 | else if( x < 0.33f ) 20 | { 21 | // Linear region, 0-80 22 | return static_cast( 242.424f * x ); 23 | } 24 | else 25 | { 26 | // Parabolic region 80-255 27 | return static_cast( ( 308.571f * x * x ) - ( 147.426f * x ) + 95.0f ); 28 | } 29 | } 30 | } 31 | 32 | CExternalLights::CExternalLights( uint32_t pinIn ) 33 | : m_pin( pinIn, CPin::kAnalog, CPin::kOutput ) 34 | { 35 | } 36 | 37 | void CExternalLights::Initialize() 38 | { 39 | // Reset pin 40 | m_pin.Reset(); 41 | m_pin.Write( 0 ); 42 | 43 | // Reset timers 44 | m_controlTimer.Reset(); 45 | m_telemetryTimer.Reset(); 46 | } 47 | 48 | void CExternalLights::Update( CCommand& commandIn ) 49 | { 50 | // Check for messages 51 | if( NCommManager::m_isCommandAvailable ) 52 | { 53 | // Handle messages 54 | if( commandIn.Equals( "elights_tpow" ) ) 55 | { 56 | // Update the target position 57 | m_targetPower = orutil::Decode1K( commandIn.m_arguments[1] ); 58 | 59 | // TODO: Ideally this unit would have the ability to autonomously set its own target and ack receipt with a separate mechanism 60 | // Acknowledge target position 61 | Serial.print( F( "elights_tpow:" ) ); 62 | Serial.print( commandIn.m_arguments[1] ); 63 | Serial.println( ';' ); 64 | 65 | // Pass through linearization function 66 | m_targetPower_an = PercentToAnalog( m_targetPower ); 67 | 68 | // Apply ceiling 69 | if( m_targetPower_an > 255 ) 70 | { 71 | m_targetPower_an = 255; 72 | } 73 | 74 | // Directly move to target power 75 | m_currentPower = m_targetPower; 76 | m_currentPower_an = m_targetPower_an; 77 | 78 | // Write the power value to the pin 79 | m_pin.Write( m_currentPower_an ); 80 | 81 | // Emit current power 82 | Serial.print( F( "elights_pow:" ) ); 83 | Serial.print( orutil::Encode1K( m_currentPower ) ); 84 | Serial.print( ';' ); 85 | } 86 | } 87 | } 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CExternalLights.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | #include "CPin.h" 6 | #include 7 | 8 | class CExternalLights : public CModule 9 | { 10 | public: 11 | CExternalLights( uint32_t pinIn ); 12 | void Initialize(); 13 | void Update( CCommand& commandIn ); 14 | 15 | private: 16 | orutil::CTimer m_controlTimer; 17 | orutil::CTimer m_telemetryTimer; 18 | CPin m_pin; 19 | 20 | float m_targetPower = 0.0f; 21 | float m_currentPower = 0.0f; 22 | 23 | uint32_t m_lastPower_an = 0; 24 | uint32_t m_targetPower_an = 0; 25 | uint32_t m_currentPower_an = 0; 26 | }; 27 | 28 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CLights.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_STD_LIGHTS) 3 | 4 | // Includes 5 | #include 6 | #include "CLights.h" 7 | #include "NCommManager.h" 8 | 9 | namespace 10 | { 11 | // 1% per 10ms 12 | const float kPowerDelta = 0.01f; 13 | 14 | bool isGreeting = false; 15 | bool greetState = false; 16 | int greetCycle = 0; 17 | const int cycleCount = 6; 18 | const int greetDelay_ms = 600; 19 | 20 | inline uint32_t PercentToAnalog( float x ) 21 | { 22 | if( x < 0.0f ) 23 | { 24 | return 0; 25 | } 26 | else if( x < 0.33f ) 27 | { 28 | // Linear region, 0-80 29 | return static_cast( 242.424f * x ); 30 | } 31 | else 32 | { 33 | // Parabolic region 80-255 34 | return static_cast( ( 308.571f * x * x ) - ( 147.426f * x ) + 95.0f ); 35 | } 36 | } 37 | } 38 | 39 | CLights::CLights( uint32_t pinIn ) 40 | : m_pin( pinIn, CPin::kAnalog, CPin::kOutput ) 41 | { 42 | } 43 | 44 | void CLights::Initialize() 45 | { 46 | // Reset pin 47 | m_pin.Reset(); 48 | m_pin.Write( 0 ); 49 | 50 | // Reset timers 51 | m_controlTimer.Reset(); 52 | m_telemetryTimer.Reset(); 53 | } 54 | 55 | void CLights::Update( CCommand& commandIn ) 56 | { 57 | // Check for messages 58 | if( !NCommManager::m_isCommandAvailable ) 59 | { 60 | return; 61 | } 62 | 63 | // Handle messages 64 | if( commandIn.Equals( "lights_tpow" ) ) 65 | { 66 | // Update the target position 67 | m_targetPower = orutil::Decode1K( commandIn.m_arguments[1] ); 68 | 69 | // TODO: Ideally this unit would have the ability to autonomously set its own target and ack receipt with a separate mechanism 70 | // Acknowledge target position 71 | Serial.print( F( "lights_tpow:" ) ); 72 | Serial.print( commandIn.m_arguments[1] ); 73 | Serial.println( ';' ); 74 | 75 | // Pass through linearization function 76 | m_targetPower_an = PercentToAnalog( m_targetPower ); 77 | 78 | // Apply ceiling 79 | if( m_targetPower_an > 255 ) 80 | { 81 | m_targetPower_an = 255; 82 | } 83 | 84 | // Directly move to target power 85 | m_currentPower = m_targetPower; 86 | m_currentPower_an = m_targetPower_an; 87 | 88 | // Write the power value to the pin 89 | m_pin.Write( m_currentPower_an ); 90 | 91 | // Emit current power 92 | Serial.print( F( "lights_pow:" ) ); 93 | Serial.print( orutil::Encode1K( m_currentPower ) ); 94 | Serial.print( ';' ); 95 | } 96 | else if( commandIn.Equals( "wake" ) ) 97 | { 98 | // Set greeting state to true and reset timer 99 | isGreeting = true; 100 | m_controlTimer.Reset(); 101 | 102 | // Set light state to off 103 | greetState = false; 104 | m_pin.Write( 0 ); 105 | } 106 | 107 | if( isGreeting ) 108 | { 109 | if( m_controlTimer.HasElapsed( greetDelay_ms ) ) 110 | { 111 | // Set to opposite state 112 | if( greetState ) 113 | { 114 | greetState = false; 115 | m_pin.Write( 0 ); 116 | } 117 | else 118 | { 119 | greetState = true; 120 | m_pin.Write( 150 ); 121 | } 122 | 123 | greetCycle++; 124 | 125 | if( greetCycle >= cycleCount ) 126 | { 127 | // Done blinking 128 | isGreeting = false; 129 | 130 | // Reset pin back to its original value 131 | m_pin.Write( m_currentPower_an ); 132 | } 133 | } 134 | } 135 | } 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CLights.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | #include "CPin.h" 6 | #include 7 | 8 | class CLights : public CModule 9 | { 10 | public: 11 | CLights( uint32_t pinIn ); 12 | void Initialize(); 13 | void Update( CCommand& commandIn ); 14 | 15 | private: 16 | orutil::CTimer m_controlTimer; 17 | orutil::CTimer m_telemetryTimer; 18 | CPin m_pin; 19 | 20 | float m_targetPower = 0.0f; 21 | float m_currentPower = 0.0f; 22 | 23 | uint32_t m_lastPower_an = 0; 24 | uint32_t m_targetPower_an = 0; 25 | uint32_t m_currentPower_an = 0; 26 | }; 27 | 28 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMPU9150.cpp: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include "SysConfig.h" 3 | 4 | #if( HAS_MPU9150 ) 5 | 6 | #include "CMPU9150.h" 7 | 8 | #include "NDataManager.h" 9 | #include "NCommManager.h" 10 | #include "CModule.h" 11 | #include 12 | 13 | #define MPU_MODULE_ID 0 14 | 15 | // MPU_UPDATE_RATE defines the rate (in Hz) at which the MPU updates the sensor data and DMP output 16 | #define MPU_UPDATE_RATE (20) 17 | 18 | // MAG_UPDATE_RATE defines the rate (in Hz) at which the MPU updates the magnetometer data 19 | // MAG_UPDATE_RATE should be less than or equal to the MPU_UPDATE_RATE 20 | #define MAG_UPDATE_RATE (10) 21 | 22 | // MPU_MAG_MIX defines the influence that the magnetometer has on the yaw output. 23 | // The magnetometer itself is quite noisy so some mixing with the gyro yaw can help 24 | // significantly. Some example values are defined below: 25 | #define MPU_MAG_MIX_GYRO_ONLY 0 // just use gyro yaw 26 | #define MPU_MAG_MIX_MAG_ONLY 1 // just use magnetometer and no gyro yaw 27 | #define MPU_MAG_MIX_GYRO_AND_MAG 10 // a good mix value 28 | #define MPU_MAG_MIX_GYRO_AND_SOME_MAG 50 // mainly gyros with a bit of mag correction 29 | 30 | // MPU_LPF_RATE is the low pass filter rate and can be between 5 and 188Hz 31 | #define MPU_LPF_RATE 40 32 | 33 | using namespace mpu9150; 34 | 35 | namespace 36 | { 37 | constexpr uint32_t kMaxHardResets = 5; 38 | 39 | constexpr uint32_t kStatusCheckDelay_ms = 1000; // 1hz 40 | constexpr uint32_t kTelemetryDelay_ms = 50; // 20hz 41 | constexpr uint32_t kResetDelay_ms = 1000; 42 | 43 | // Attempt to read at twice rate of updates to prevent MPU FIFO overflow 44 | // Normally handled by using a physical interrupt to service on demand 45 | constexpr uint32_t kDataUpdateRate_ms = ( ( 1000 / MPU_UPDATE_RATE ) / 2 ); 46 | } 47 | 48 | CMPU9150::CMPU9150( mpu9150::EAddress addressIn ) 49 | : m_device( addressIn ) 50 | { 51 | } 52 | 53 | void CMPU9150::Initialize() 54 | { 55 | Serial.println( F( "mpu9150_init:0;" ) ); 56 | 57 | m_statusCheckTimer.Reset(); 58 | m_telemetryTimer.Reset(); 59 | m_updateTimer.Reset(); 60 | m_resetTimer.Reset(); 61 | 62 | // 25% max failure rate before 63 | m_maxFailuresPerPeriod = 30; 64 | } 65 | 66 | void CMPU9150::Update( CCommand& commandIn ) 67 | { 68 | // If disabled, do nothing 69 | if( m_isDisabled ) 70 | { 71 | return; 72 | } 73 | 74 | // Handle initialization if necessary 75 | if( m_isInitialized == false ) 76 | { 77 | if( m_device.GetResultCount( EResult::RESULT_ERR_HARD_RESET ) > kMaxHardResets ) 78 | { 79 | m_isDisabled = true; 80 | Serial.println( F( "mpu9150_disabled:1;" ) ); 81 | return; 82 | } 83 | else 84 | { 85 | if( m_resetTimer.HasElapsed( kResetDelay_ms ) ) 86 | { 87 | // Attempt to initialize 88 | if( m_device.init( MPU_UPDATE_RATE, MPU_MAG_MIX_GYRO_ONLY, MAG_UPDATE_RATE, MPU_LPF_RATE ) == true ) 89 | { 90 | // Success 91 | m_isInitialized = true; 92 | Serial.println( F( "mpu9150_init:1;" ) ); 93 | } 94 | else 95 | { 96 | Serial.println( F( "mpu9150_init:0;" ) ); 97 | } 98 | 99 | // Increment hard reset counter, whether init succeeded or not 100 | m_device.AddResult( EResult::RESULT_ERR_HARD_RESET ); 101 | 102 | return; 103 | } 104 | else 105 | { 106 | return; 107 | } 108 | } 109 | } 110 | 111 | // Handle commands 112 | if( NCommManager::m_isCommandAvailable ) 113 | { 114 | // Zero the yaw value 115 | if( commandIn.Equals( "imu_zyaw" ) ) 116 | { 117 | // Set offset based on current value 118 | m_yawOffset = NDataManager::m_navData.YAW; 119 | 120 | // Send ack 121 | Serial.println( F( "imu_zyaw:ack;" ) ); 122 | } 123 | // Zero the roll and pich values 124 | else if( commandIn.Equals( "imu_level" ) ) 125 | { 126 | // Set offsets based on request from cockpit 127 | m_rollOffset = orutil::Decode1K( commandIn.m_arguments[1] ); 128 | m_pitchOffset = orutil::Decode1K( commandIn.m_arguments[2] ); 129 | 130 | // Report new settings 131 | Serial.print( F( "imu_roff:" ) ); Serial.print( commandIn.m_arguments[1] ); Serial.println( ';' ); 132 | Serial.print( F( "imu_poff:" ) ); Serial.print( commandIn.m_arguments[2] ); Serial.println( ';' ); 133 | } 134 | // Set the operating mode 135 | else if( commandIn.Equals( "imu_mode" ) ) 136 | { 137 | // Does not support mode changes right now, send nack 138 | Serial.println( F( "imu_level:nack;" ) ); 139 | } 140 | } 141 | 142 | // Handle health checks 143 | if( m_statusCheckTimer.HasElapsed( kStatusCheckDelay_ms ) ) 144 | { 145 | // Check to see if the error threshold is above acceptable levels 146 | if( m_device.GetResultCount( EResult::RESULT_ERR_READ_ERROR ) > m_maxFailuresPerPeriod ) 147 | { 148 | // Trigger a hard reset on the next loop 149 | m_isInitialized = false; 150 | 151 | // End this run of Update() 152 | return; 153 | } 154 | else 155 | { 156 | // Clear the error counter 157 | m_device.ClearResultCount( EResult::RESULT_ERR_READ_ERROR ); 158 | } 159 | } 160 | 161 | // Read data 162 | if( m_updateTimer.HasElapsed( kDataUpdateRate_ms ) ) 163 | { 164 | if( m_device.read() == true ) 165 | { 166 | // Successfully got data, update shared state 167 | NDataManager::m_navData.ROLL = m_device.m_fusedEulerPose[VEC3_X] * RAD_TO_DEGREE - m_rollOffset; 168 | NDataManager::m_navData.PITC = m_device.m_fusedEulerPose[VEC3_Y] * RAD_TO_DEGREE - m_pitchOffset; 169 | NDataManager::m_navData.YAW = NORMALIZE_ANGLE_180( ( m_device.m_fusedEulerPose[VEC3_Z] * RAD_TO_DEGREE ) - m_yawOffset ); 170 | 171 | // Handle telemetry updates 172 | if( m_telemetryTimer.HasElapsed( kTelemetryDelay_ms ) ) 173 | { 174 | Serial.print( F( "imu_r:" ) ); Serial.print( orutil::Encode1K( NDataManager::m_navData.ROLL ) ); Serial.print( ';' ); 175 | Serial.print( F( "imu_p:" ) ); Serial.print( orutil::Encode1K( NDataManager::m_navData.PITC ) ); Serial.print( ';' ); 176 | Serial.print( F( "imu_y:" ) ); Serial.print( orutil::Encode1K( NDataManager::m_navData.YAW ) ); Serial.println( ';' ); 177 | } 178 | } 179 | } 180 | } 181 | #endif 182 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMPU9150.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "CModule.h" 6 | 7 | class CMPU9150 : public CModule 8 | { 9 | public: 10 | CMPU9150( mpu9150::EAddress addressIn ); 11 | virtual void Initialize(); 12 | virtual void Update( CCommand& commandIn ); 13 | 14 | private: 15 | // Device driver 16 | mpu9150::MPU9150 m_device; 17 | 18 | // Timers 19 | orutil::CTimer m_statusCheckTimer; 20 | orutil::CTimer m_telemetryTimer; 21 | orutil::CTimer m_updateTimer; 22 | orutil::CTimer m_resetTimer; 23 | 24 | bool m_isInitialized = false; 25 | bool m_isDisabled = false; 26 | 27 | float m_yawOffset = 0.0f; 28 | float m_rollOffset = 0.0f; 29 | float m_pitchOffset = 0.0f; 30 | 31 | uint32_t m_maxFailuresPerPeriod = 0; 32 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMS5803_14BA.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_MS5803_14BA) 3 | 4 | #include "CMS5803_14BA.h" 5 | #include "NCommManager.h" 6 | 7 | using namespace ms5803_14ba; 8 | 9 | namespace 10 | { 11 | constexpr uint32_t kMaxHardResets = 5; 12 | 13 | constexpr uint32_t kStatusCheckDelay_ms = 1000; // 1hz 14 | constexpr uint32_t kTelemetryDelay_ms = 100; // 10hz 15 | } 16 | 17 | CMS5803_14BA::CMS5803_14BA( I2C *i2cInterfaceIn, ms5803_14ba::EAddress addressIn ) 18 | : m_device( i2cInterfaceIn, addressIn ) 19 | { 20 | } 21 | 22 | void CMS5803_14BA::Initialize() 23 | { 24 | Serial.println( F( "ms5803_init:1;" ) ); 25 | 26 | m_statusCheckTimer.Reset(); 27 | m_telemetryTimer.Reset(); 28 | 29 | // 25% max failure rate before 30 | m_maxFailuresPerPeriod = ( kStatusCheckDelay_ms / 4 ) / m_device.GetUpdatePeriod(); 31 | } 32 | 33 | void CMS5803_14BA::Update( CCommand& commandIn ) 34 | { 35 | if( m_device.IsEnabled() == false ) 36 | { 37 | return; 38 | } 39 | 40 | // Check for lock 41 | if( m_hasLock == false ) 42 | { 43 | // Attempt to get the lock 44 | m_hasLock = m_device.GetLock(); 45 | 46 | if( m_hasLock == false ) 47 | { 48 | // Don't allow the code to proceed 49 | return; 50 | } 51 | } 52 | 53 | // Handle health checks 54 | if( m_statusCheckTimer.HasElapsed( kStatusCheckDelay_ms ) ) 55 | { 56 | // Check to see if the error threshold is above acceptable levels 57 | if( m_device.GetResultCount( EResult::RESULT_ERR_FAILED_SEQUENCE ) > m_maxFailuresPerPeriod ) 58 | { 59 | Serial.println( "ms5803_HardReset:1" ); 60 | m_device.HardReset(); 61 | } 62 | else 63 | { 64 | // Clear the error counter 65 | m_device.ClearResultCount( EResult::RESULT_ERR_FAILED_SEQUENCE ); 66 | } 67 | 68 | // Check to see if we have surpassed our hard reset threshhold 69 | if( m_device.GetResultCount( EResult::RESULT_ERR_HARD_RESET ) > kMaxHardResets ) 70 | { 71 | // Free the lock 72 | if( m_hasLock ) 73 | { 74 | m_hasLock = false; 75 | m_device.FreeLock(); 76 | } 77 | 78 | // Permanently disable the device 79 | m_device.Disable(); 80 | Serial.println( F( "ms5803_disabled:1;" ) ); 81 | return; 82 | } 83 | } 84 | 85 | // Handle commands 86 | if( NCommManager::m_isCommandAvailable ) 87 | { 88 | // Zero the depth value 89 | if( commandIn.Equals( "depth_zero" ) ) 90 | { 91 | // Set offset based on current value 92 | m_depthOffset_m = m_device.m_data.depth_m; 93 | 94 | // Send ack 95 | Serial.println( F( "depth_zero:ack;" ) ); 96 | } 97 | // Clear the depth offset 98 | else if( commandIn.Equals( "depth_clroff" ) ) 99 | { 100 | // Set offset based on current value 101 | m_depthOffset_m = 0.0f; 102 | 103 | // Send ack 104 | Serial.println( F( "depth_clroff:ack;" ) ); 105 | } 106 | // Change water type 107 | else if( commandIn.Equals( "depth_water" ) ) 108 | { 109 | if( commandIn.m_arguments[1] == (uint32_t)EWaterType::FRESH ) 110 | { 111 | m_device.SetWaterType( EWaterType::FRESH ); 112 | 113 | // Ack 114 | Serial.print( F( "depth_water:" ) ); 115 | Serial.print( commandIn.m_arguments[1] ); 116 | Serial.println( ';' ); 117 | } 118 | else if( commandIn.m_arguments[1] == (uint32_t)EWaterType::SALT ) 119 | { 120 | m_device.SetWaterType( EWaterType::SALT ); 121 | 122 | // Ack 123 | Serial.print( F( "depth_water:" ) ); 124 | Serial.print( commandIn.m_arguments[1] ); 125 | Serial.println( ';' ); 126 | } 127 | } 128 | } 129 | 130 | // Tick device state machine 131 | m_device.Tick(); 132 | 133 | // Emit telemetry 134 | if( m_telemetryTimer.HasElapsed( kTelemetryDelay_ms ) ) 135 | { 136 | if( m_device.m_data.SampleAvailable() ) 137 | { 138 | // Report results 139 | Serial.print( F( "depth_t:" ) ); Serial.print( orutil::Encode1K( m_device.m_data.temperature_c ) ); Serial.print( ';' ); 140 | Serial.print( F( "depth_p:" ) ); Serial.print( orutil::Encode1K( m_device.m_data.pressure_mbar ) ); Serial.print( ';' ); 141 | Serial.print( F( "depth_d:" ) ); Serial.print( orutil::Encode1K( m_device.m_data.depth_m ) ); Serial.println( ';' ); 142 | } 143 | } 144 | } 145 | 146 | #endif -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMS5803_14BA.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include 6 | #include "CModule.h" 7 | 8 | class CMS5803_14BA : public CModule 9 | { 10 | public: 11 | CMS5803_14BA( I2C *i2cInterfaceIn, ms5803_14ba::EAddress addressIn ); 12 | void Initialize(); 13 | void Update( CCommand& commandIn ); 14 | 15 | private: 16 | // Device driver 17 | ms5803_14ba::MS5803_14BA m_device; 18 | 19 | // Timers 20 | orutil::CTimer m_statusCheckTimer; 21 | orutil::CTimer m_telemetryTimer; 22 | 23 | float m_depthOffset_m = 0.0f; 24 | uint32_t m_maxFailuresPerPeriod = 0; 25 | bool m_hasLock = false; 26 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMS5837_30BA.cpp: -------------------------------------------------------------------------------- 1 | #include "SysConfig.h" 2 | #if(HAS_MS5837_30BA) 3 | 4 | #include "CMS5837_30BA.h" 5 | #include "NCommManager.h" 6 | 7 | using namespace ms5837_30ba; 8 | 9 | namespace 10 | { 11 | constexpr uint32_t kMaxHardResets = 5; 12 | 13 | constexpr uint32_t kStatusCheckDelay_ms = 1000; // 1hz 14 | constexpr uint32_t kTelemetryDelay_ms = 100; // 10hz 15 | } 16 | 17 | CMS5837_30BA::CMS5837_30BA( I2C *i2cInterfaceIn, ms5837_30ba::EAddress addressIn ) 18 | : m_device( i2cInterfaceIn, addressIn ) 19 | { 20 | } 21 | 22 | void CMS5837_30BA::Initialize() 23 | { 24 | Serial.println( F( "ms5837_init:1;" ) ); 25 | 26 | m_statusCheckTimer.Reset(); 27 | m_telemetryTimer.Reset(); 28 | 29 | // 25% max failure rate before 30 | m_maxFailuresPerPeriod = ( kStatusCheckDelay_ms / 4 ) / m_device.GetUpdatePeriod(); 31 | } 32 | 33 | void CMS5837_30BA::Update( CCommand& commandIn ) 34 | { 35 | if( m_device.IsEnabled() == false ) 36 | { 37 | return; 38 | } 39 | 40 | // Check for lock 41 | if( m_hasLock == false ) 42 | { 43 | // Attempt to get the lock 44 | m_hasLock = m_device.GetLock(); 45 | 46 | if( m_hasLock == false ) 47 | { 48 | // Don't allow the code to proceed until device has achieved lock 49 | return; 50 | } 51 | } 52 | 53 | // Handle health checks 54 | if( m_statusCheckTimer.HasElapsed( kStatusCheckDelay_ms ) ) 55 | { 56 | // Check to see if the error threshold is above acceptable levels 57 | if( m_device.GetResultCount( EResult::RESULT_ERR_FAILED_SEQUENCE ) > m_maxFailuresPerPeriod ) 58 | { 59 | Serial.println( "ms5837_HardReset:1" ); 60 | m_device.HardReset(); 61 | } 62 | else 63 | { 64 | // Clear the error counter 65 | m_device.ClearResultCount( EResult::RESULT_ERR_FAILED_SEQUENCE ); 66 | } 67 | 68 | // Check to see if we have surpassed our hard reset threshhold 69 | if( m_device.GetResultCount( EResult::RESULT_ERR_HARD_RESET ) > kMaxHardResets ) 70 | { 71 | // Free the lock 72 | if( m_hasLock ) 73 | { 74 | m_hasLock = false; 75 | m_device.FreeLock(); 76 | } 77 | 78 | // Permanently disable the device 79 | m_device.Disable(); 80 | Serial.println( F( "ms5837_disabled:1;" ) ); 81 | return; 82 | } 83 | } 84 | 85 | // Handle commands 86 | if( NCommManager::m_isCommandAvailable ) 87 | { 88 | // Zero the depth value 89 | if( commandIn.Equals( "depth_zero" ) ) 90 | { 91 | // Set offset based on current value 92 | m_depthOffset_m = m_device.m_data.depth_m; 93 | 94 | // Send ack 95 | Serial.println( F( "depth_zero:ack;" ) ); 96 | } 97 | // Clear the depth offset 98 | else if( commandIn.Equals( "depth_clroff" ) ) 99 | { 100 | // Set offset based on current value 101 | m_depthOffset_m = 0.0f; 102 | 103 | // Send ack 104 | Serial.println( F( "depth_clroff:ack;" ) ); 105 | } 106 | // Change water type 107 | else if( commandIn.Equals( "depth_water" ) ) 108 | { 109 | if( commandIn.m_arguments[1] == (uint32_t)EWaterType::FRESH ) 110 | { 111 | m_device.SetWaterType( EWaterType::FRESH ); 112 | 113 | // Ack 114 | Serial.print( F( "depth_water:" ) ); 115 | Serial.print( commandIn.m_arguments[1] ); 116 | Serial.println( ';' ); 117 | } 118 | else if( commandIn.m_arguments[1] == (uint32_t)EWaterType::SALT ) 119 | { 120 | m_device.SetWaterType( EWaterType::SALT ); 121 | 122 | // Ack 123 | Serial.print( F( "depth_water:" ) ); 124 | Serial.print( commandIn.m_arguments[1] ); 125 | Serial.println( ';' ); 126 | } 127 | } 128 | } 129 | 130 | // Tick device state machine 131 | m_device.Tick(); 132 | 133 | // Emit telemetry 134 | if( m_telemetryTimer.HasElapsed( kTelemetryDelay_ms ) ) 135 | { 136 | if( m_device.m_data.SampleAvailable() ) 137 | { 138 | // Report results 139 | Serial.print( F( "depth_t:" ) ); Serial.print( orutil::Encode1K( m_device.m_data.temperature_c ) ); Serial.print( ';' ); 140 | Serial.print( F( "depth_p:" ) ); Serial.print( orutil::Encode1K( m_device.m_data.pressure_mbar ) ); Serial.print( ';' ); 141 | Serial.print( F( "depth_d:" ) ); Serial.print( orutil::Encode1K( m_device.m_data.depth_m ) ); Serial.println( ';' ); 142 | } 143 | } 144 | } 145 | 146 | #endif -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMS5837_30BA.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include 6 | #include "CModule.h" 7 | 8 | class CMS5837_30BA : public CModule 9 | { 10 | public: 11 | CMS5837_30BA( I2C *i2cInterfaceIn, ms5837_30ba::EAddress addressIn ); 12 | void Initialize(); 13 | void Update( CCommand& commandIn ); 14 | 15 | private: 16 | // Device driver 17 | ms5837_30ba::MS5837_30BA m_device; 18 | 19 | // Timers 20 | orutil::CTimer m_statusCheckTimer; 21 | orutil::CTimer m_telemetryTimer; 22 | 23 | float m_depthOffset_m = 0.0f; 24 | uint32_t m_maxFailuresPerPeriod = 0; 25 | bool m_hasLock = false; 26 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CModule.cpp: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include 3 | 4 | #include "CModule.h" 5 | #include "NModuleManager.h" 6 | 7 | CModule::CModule() 8 | { 9 | NModuleManager::RegisterModule( this ); 10 | } -------------------------------------------------------------------------------- /sketches/OpenROV2x/CModule.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CCommand.h" 4 | #include 5 | 6 | class CModule 7 | { 8 | public: 9 | // Methods 10 | CModule(); 11 | 12 | // Pure virtuals - These methods must be instantiated in your derived class! 13 | virtual void Initialize() = 0; 14 | virtual void Update( CCommand& commandIn ) = 0; 15 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMotor.cpp: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include "CMotor.h" 3 | #include "PinDefinitions.h" 4 | 5 | // Default constructor plus pin-setting 6 | CMotor::CMotor( int motorPinIn ) 7 | : m_motorPin( -1 ) 8 | , m_positiveModifier( MOTOR_DEFAULT_POSITIVE_MOD ) 9 | , m_negativeModifier( MOTOR_DEFAULT_NEGATIVE_MOD ) 10 | , m_positiveDeadzoneBuffer( MOTOR_DEFAULT_NEGATIVE_DEADZONE_BUFFER ) 11 | , m_negativeDeadzoneBuffer( MOTOR_DEFAULT_POSITIVE_DEADZONE_BUFFER ) 12 | { 13 | SetPin( motorPinIn ); 14 | } 15 | 16 | CMotor::CMotor() 17 | : m_motorPin( -1 ) 18 | , m_positiveModifier( MOTOR_DEFAULT_POSITIVE_MOD ) 19 | , m_negativeModifier( MOTOR_DEFAULT_NEGATIVE_MOD ) 20 | , m_positiveDeadzoneBuffer( MOTOR_DEFAULT_NEGATIVE_DEADZONE_BUFFER ) 21 | , m_negativeDeadzoneBuffer( MOTOR_DEFAULT_POSITIVE_DEADZONE_BUFFER ) 22 | { 23 | } 24 | 25 | void CMotor::SetPin( int motorPinIn ) 26 | { 27 | m_motorPin = motorPinIn; 28 | } 29 | 30 | void CMotor::Activate() 31 | { 32 | m_servo.attach( m_motorPin ); 33 | } 34 | 35 | void CMotor::Deactivate() 36 | { 37 | // Set the servo back to the neutral PWM target and deactivate the pin associated with it 38 | m_servo.writeMicroseconds( MOTOR_TARGET_NEUTRAL_US ); 39 | m_servo.detach(); 40 | } 41 | 42 | bool CMotor::IsActive() 43 | { 44 | // Find out if the motor is active in the list of servos 45 | return m_servo.attached(); 46 | } 47 | 48 | int CMotor::SetMotorTarget( int targetIn_us ) 49 | { 50 | float modifier( 1.0f ); 51 | 52 | // Get the appropriate modifier value 53 | if( targetIn_us > MOTOR_TARGET_NEUTRAL_US ) 54 | { 55 | modifier = m_positiveModifier; 56 | } 57 | else if( targetIn_us < MOTOR_TARGET_NEUTRAL_US ) 58 | { 59 | modifier = m_negativeModifier; 60 | } 61 | 62 | int midpointDelta_us = targetIn_us - MOTOR_TARGET_NEUTRAL_US; 63 | 64 | // First, apply a floor and ceiling on the target PWM value 65 | int constrainedTarget_us = constrain( ( MOTOR_TARGET_NEUTRAL_US + static_cast( midpointDelta_us * modifier ) ), MOTOR_TARGET_MIN_US, MOTOR_TARGET_MAX_US ); 66 | 67 | int finalTargetValue_us = MOTOR_TARGET_NEUTRAL_US; 68 | 69 | // Re-map the constrained target to a target value that falls outside of the ESC's deadband. 70 | // There are basically small regions around the neutral value where the motor will not actuate. 71 | // This makes sure that we are either exactly equal to the neutral value or fall on either side of the deadband. 72 | if( constrainedTarget_us < MOTOR_TARGET_NEUTRAL_US ) 73 | { 74 | finalTargetValue_us = map( constrainedTarget_us, MOTOR_TARGET_MIN_US, MOTOR_TARGET_NEUTRAL_US, MOTOR_TARGET_MIN_US, MOTOR_TARGET_NEUTRAL_US - m_negativeDeadzoneBuffer ); 75 | } 76 | else if( constrainedTarget_us > MOTOR_TARGET_NEUTRAL_US ) 77 | { 78 | finalTargetValue_us = map( constrainedTarget_us, MOTOR_TARGET_NEUTRAL_US, MOTOR_TARGET_MAX_US, MOTOR_TARGET_NEUTRAL_US + m_positiveDeadzoneBuffer, MOTOR_TARGET_MAX_US ); 79 | } 80 | 81 | // Set the final target value in the servo interface and return the value that was set 82 | m_servo.writeMicroseconds( constrain( finalTargetValue_us, MOTOR_TARGET_MIN_US, MOTOR_TARGET_MAX_US ) ); 83 | return m_servo.readMicroseconds(); 84 | } 85 | 86 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/CMotor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include 6 | 7 | // Motors 8 | #define MOTOR_TARGET_MIN_US 1000 9 | #define MOTOR_TARGET_NEUTRAL_US 1500 10 | #define MOTOR_TARGET_MAX_US 2000 11 | #define MOTOR_DEFAULT_POSITIVE_MOD 1.0f 12 | #define MOTOR_DEFAULT_NEGATIVE_MOD 1.0f 13 | #define MOTOR_DEFAULT_POSITIVE_DEADZONE_BUFFER 50 14 | #define MOTOR_DEFAULT_NEGATIVE_DEADZONE_BUFFER 50 15 | 16 | class CMotor 17 | { 18 | private: 19 | Servo m_servo; 20 | int m_motorPin; 21 | 22 | public: 23 | float m_positiveModifier; 24 | float m_negativeModifier; 25 | int m_positiveDeadzoneBuffer; 26 | int m_negativeDeadzoneBuffer; 27 | 28 | 29 | CMotor( int motorPinIn ); 30 | CMotor(); 31 | 32 | void SetPin( int motorPinIn ); 33 | void Activate(); 34 | void Deactivate(); 35 | 36 | int SetMotorTarget( int ms ); 37 | bool IsActive(); 38 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CPin.cpp: -------------------------------------------------------------------------------- 1 | #include "CPin.h" 2 | 3 | CPin::CPin( int pin_number, bool digital_truth, bool in_out ) 4 | { 5 | m_pinNumber = pin_number; 6 | m_isDigital = digital_truth; 7 | m_isInput = in_out; 8 | } 9 | 10 | CPin::CPin( int pin_number, bool digital_truth ) 11 | { 12 | m_pinNumber = pin_number; 13 | m_isDigital = digital_truth; 14 | m_isInput = kInput; // default to input 15 | } 16 | 17 | void CPin::Reset() 18 | { 19 | if( m_isDigital ) 20 | { 21 | if( m_isInput ) 22 | { 23 | pinMode( m_pinNumber, INPUT ); 24 | } 25 | else 26 | { 27 | pinMode( m_pinNumber, OUTPUT ); 28 | } 29 | } 30 | } 31 | 32 | int CPin::Read() 33 | { 34 | if( m_isInput ) 35 | { 36 | if( m_isDigital ) 37 | { 38 | m_value = digitalRead( m_pinNumber ); 39 | } 40 | else 41 | { 42 | m_value = analogRead( m_pinNumber ); 43 | } 44 | 45 | return m_value; 46 | } 47 | 48 | return -1; 49 | } 50 | 51 | void CPin::Write( int val ) 52 | { 53 | if( !m_isInput ) 54 | { 55 | if( m_isDigital ) 56 | { 57 | if( val == 0 ) 58 | { 59 | digitalWrite( m_pinNumber, LOW ); 60 | } 61 | else 62 | { 63 | digitalWrite( m_pinNumber, HIGH ); 64 | } 65 | } 66 | else 67 | { 68 | analogWrite( m_pinNumber, val ); 69 | } 70 | } 71 | 72 | m_value = val; 73 | } -------------------------------------------------------------------------------- /sketches/OpenROV2x/CPin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include "PinDefinitions.h" 6 | 7 | class CPin 8 | { 9 | private: 10 | static const char kSeparator = ':'; 11 | static const char kDelimeter = ';'; 12 | 13 | int m_pinNumber; 14 | int m_value; 15 | 16 | bool m_isDigital; 17 | bool m_isInput; 18 | 19 | public: 20 | static const bool kAnalog = false; 21 | static const bool kDigital = true; 22 | static const bool kOutput = false; 23 | static const bool kInput = true; 24 | 25 | CPin( int pin_number, bool digital_truth, bool in_out ); 26 | CPin( int pin_number, bool digital_truth ); 27 | 28 | int Read(); 29 | void Write( int val ); 30 | 31 | void Reset(); 32 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CThrusters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | class CThrusters : public CModule 7 | { 8 | public: 9 | static const int kMotorCount; 10 | 11 | void Initialize(); 12 | void Update( CCommand& commandIn ); 13 | }; -------------------------------------------------------------------------------- /sketches/OpenROV2x/CompileOptions.h: -------------------------------------------------------------------------------- 1 | // This file is automatically generated in the build process. Don't put anything here! 2 | #define VERSION_HASH "ver:<<{{0000000000000000000000000000000000000000}}>>;" -------------------------------------------------------------------------------- /sketches/OpenROV2x/NArduinoManager.cpp: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include "NArduinoManager.h" 3 | 4 | #include 5 | #include 6 | 7 | namespace NArduinoManager 8 | { 9 | // Variable initialization 10 | volatile uint8_t m_wdtResetInfo = 0; 11 | 12 | void Initialize() 13 | { 14 | // Hardware intialization 15 | DisableWatchdogTimer(); 16 | EnableWatchdogTimer(); 17 | 18 | // Initialize the serial port 19 | Serial.begin( 115200 ); 20 | 21 | // Start I2C Device 22 | I2C0.Enable(); 23 | 24 | // // Read first byte in EEPROM. If the watchdog triggered and the ISR completed, the first byte will be a "1" 25 | if( EEPROM.read( 0 ) == 1 ) 26 | { 27 | // Read the second byte in the EEPROM 28 | m_wdtResetInfo = EEPROM.read( 1 ); 29 | 30 | // Reset byte so the EEPROM is not read on next startup 31 | EEPROM.write( 0, 0 ); 32 | 33 | // Print debug information about the watchdog triggering 34 | Serial.println( F( "log:Watchdog was triggered and the following was read from EEPROM;" ) ); 35 | Serial.print( F( "log:" ) ); 36 | Serial.println( m_wdtResetInfo ); 37 | Serial.print( ';' ); 38 | } 39 | 40 | // Set pin 13 as an output 41 | pinMode( 13, OUTPUT ); 42 | } 43 | 44 | void EnableWatchdogTimer() 45 | { 46 | // Sets the watchdog timer both interrupt and reset mode with an 8 second timeout 47 | 48 | cli(); 49 | MCUSR &= ~( 1 << WDRF ); 50 | wdt_reset(); 51 | WDTCSR |= ( 1 << WDCE ) | ( 1 << WDE ); 52 | WDTCSR = ( ~( 1 << WDP1 ) & ~( 1 << WDP2 ) ) | ( ( 1 << WDE ) | ( 1 << WDIE ) | ( 1 << WDP3 ) | ( 1 << WDP0 ) ); 53 | sei(); 54 | } 55 | 56 | void DisableWatchdogTimer() 57 | { 58 | // Disables the watchdog timer 59 | 60 | cli(); 61 | wdt_reset(); 62 | MCUSR &= ~( 1 << WDRF ); 63 | WDTCSR |= ( 1 << WDCE ) | ( 1 << WDE ); 64 | WDTCSR = 0x00; 65 | sei(); 66 | } 67 | 68 | ISR( WDT_vect ) 69 | { 70 | // Write the random number to the second byte 71 | EEPROM.write( 1, m_wdtResetInfo + 1 ); 72 | 73 | // Write a "1" to the first byte to indicate the data in second byte is valid and the ISR triggered properly 74 | EEPROM.write( 0, 1 ); 75 | 76 | // Triggers the second watchdog timeout for a reset 77 | while( true ) {}; 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/NArduinoManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include 6 | 7 | namespace NArduinoManager 8 | { 9 | // Variables 10 | extern volatile uint8_t m_wdtResetInfo; 11 | 12 | // Methods 13 | extern void Initialize(); 14 | 15 | extern void EnableWatchdogTimer(); 16 | extern void DisableWatchdogTimer(); 17 | 18 | ISR( WDT_vect ); 19 | } 20 | 21 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/NCommManager.cpp: -------------------------------------------------------------------------------- 1 | #include "NCommManager.h" 2 | 3 | namespace NCommManager 4 | { 5 | // Initialize variables 6 | CCommand m_currentCommand; 7 | bool m_isCommandAvailable = false; 8 | 9 | void Initialize() 10 | { 11 | } 12 | 13 | void GetCurrentCommand() 14 | { 15 | // Get the current command string from the serial port 16 | m_isCommandAvailable = m_currentCommand.GetCommandString(); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/NCommManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CCommand.h" 5 | 6 | namespace NCommManager 7 | { 8 | // Variables 9 | extern CCommand m_currentCommand; 10 | 11 | extern bool m_isCommandAvailable; 12 | 13 | // Methods 14 | extern void Initialize(); 15 | extern void GetCurrentCommand(); 16 | } 17 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/NDataManager.cpp: -------------------------------------------------------------------------------- 1 | #include "NDataManager.h" 2 | 3 | #include "NModuleManager.h" 4 | #include "PinDefinitions.h" 5 | 6 | #include 7 | 8 | namespace NDataManager 9 | { 10 | // Default initialize all data 11 | TNavData m_navData; 12 | TEnvironmentData m_environmentData; 13 | TCapeData m_capeData; 14 | TThrusterData m_thrusterData; 15 | 16 | orutil::CTimer m_timer_1hz; 17 | orutil::CTimer m_timer_10hz; 18 | 19 | uint32_t m_loopsPerSec = 0; 20 | 21 | // Called during Setup() to initialize any DataManager members to specific values 22 | void Initialize() 23 | { 24 | m_thrusterData.MATC = true; 25 | } 26 | 27 | void OutputNavData() 28 | { 29 | // Print Nav Data on the serial line 30 | // Serial.print( F( "hdgd:" ) ); 31 | // Serial.print( m_navData.HDGD ); 32 | // Serial.print( ';' ); 33 | // Serial.print( F( "deap:" ) ); 34 | // Serial.print( m_navData.DEEP ); 35 | // Serial.print( ';' ); 36 | // Serial.print( F( "deep:" ) ); // Compatibility for 30.1.x cockpit updates 37 | // Serial.print( m_navData.DEEP ); 38 | // Serial.print( ';' ); 39 | // Serial.print( F( "pitc:" ) ); 40 | // Serial.print( m_navData.PITC ); 41 | // Serial.print( ';' ); 42 | // Serial.print( F( "roll:" ) ); 43 | // Serial.print( m_navData.ROLL ); 44 | // Serial.print( ';' ); 45 | // Serial.print( F( "yaw:" ) ); 46 | // Serial.print( m_navData.YAW ); 47 | // Serial.print( ';' ); 48 | Serial.print( F( "fthr:" ) ); 49 | Serial.print( m_navData.FTHR ); 50 | Serial.println( ';' ); 51 | } 52 | 53 | void OutputSharedData() 54 | { 55 | // Print all other shared data on the serial line 56 | 57 | // Thruster Data 58 | Serial.print( F( "motorAttached:" ) ); 59 | Serial.print( m_thrusterData.MATC ); 60 | Serial.println( ';' ); 61 | 62 | // Cape Data 63 | Serial.print( F( "fmem:" ) ); 64 | Serial.print( m_capeData.FMEM ); 65 | Serial.print( ';' ); 66 | Serial.print( F( "vout:" ) ); 67 | Serial.print( m_capeData.VOUT ); 68 | Serial.print( ';' ); 69 | Serial.print( F( "iout:" ) ); 70 | Serial.print( m_capeData.IOUT ); 71 | Serial.print( ';' ); 72 | Serial.print( F( "btti:" ) ); 73 | Serial.print( m_capeData.BTTI ); 74 | Serial.print( ';' ); 75 | Serial.print( F( "time:" ) ); 76 | Serial.print( m_capeData.UTIM ); 77 | Serial.println( ';' ); 78 | 79 | // Environment Data 80 | Serial.print( F( "pres:" ) ); 81 | Serial.print( m_environmentData.PRES ); 82 | Serial.print( ';' ); 83 | Serial.print( F( "temp:" ) ); 84 | Serial.print( m_environmentData.TEMP ); 85 | Serial.println( ';' ); 86 | 87 | // I2C Debugging 88 | // ----------------------------------------------------------------- 89 | // RESULT_SUCCESS = 0, // Operation successful 90 | // RESULT_NACK, // Transaction was denied or there was no response 91 | 92 | // // Errors 93 | // RESULT_ERR_TIMEOUT, // Operation timed out 94 | // RESULT_ERR_FAILED, // Operation failed 95 | // RESULT_ERR_ALREADY_INITIALIZED, // Interface already initialized 96 | // RESULT_ERR_INVALID_BAUD, // Invalid baud rate specified 97 | // RESULT_ERR_LOST_ARBITRATION, // Lost arbitration during transaction 98 | // RESULT_ERR_BAD_ADDRESS, // Invalid I2C slave address specified 99 | 100 | Serial.print( F( "i2c.OK:" ) ); 101 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_SUCCESS ) ); 102 | Serial.println( ';' ); 103 | Serial.print( F( "i2c.NACK:" ) ); 104 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_NACK ) ); 105 | Serial.println( ';' ); 106 | Serial.print( F( "i2c.TIMEOUT:" ) ); 107 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_ERR_TIMEOUT ) ); 108 | Serial.println( ';' ); 109 | Serial.print( F( "i2c.FAILED:" ) ); 110 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_ERR_FAILED ) ); 111 | Serial.println( ';' ); 112 | Serial.print( F( "i2c.AL_INIT:" ) ); 113 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_ERR_ALREADY_INITIALIZED ) ); 114 | Serial.println( ';' ); 115 | Serial.print( F( "i2c.BAD_BAUD:" ) ); 116 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_ERR_INVALID_BAUD ) ); 117 | Serial.println( ';' ); 118 | Serial.print( F( "i2c.LOST_ARB:" ) ); 119 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_ERR_LOST_ARBITRATION ) ); 120 | Serial.println( ';' ); 121 | Serial.print( F( "i2c.BAD_ADDR:" ) ); 122 | Serial.print( I2C0.GetResultCount( i2c::EI2CResult::RESULT_ERR_BAD_ADDRESS ) ); 123 | Serial.println( ';' ); 124 | } 125 | 126 | void HandleOutputLoops() 127 | { 128 | ++m_loopsPerSec; 129 | 130 | // 1Hz update loop 131 | if( m_timer_1hz.HasElapsed( 1000 ) ) 132 | { 133 | // Send shared data to beaglebone 134 | OutputSharedData(); 135 | 136 | // Loops per sec 137 | Serial.print( F( "alps:" ) ); 138 | Serial.print( m_loopsPerSec ); 139 | Serial.println( ';' ); 140 | 141 | // Reset loop counter 142 | m_loopsPerSec = 0; 143 | } 144 | 145 | // 10Hz Update loop 146 | if( m_timer_10hz.HasElapsed( 100 ) ) 147 | { 148 | // Send nav data to beaglebone 149 | OutputNavData(); 150 | } 151 | } 152 | } -------------------------------------------------------------------------------- /sketches/OpenROV2x/NDataManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | #include 6 | 7 | // Data structures 8 | struct TNavData 9 | { 10 | float DEEP = 0.0f; // Depth in meters 11 | float ROLL = 0.0f; // Roll in degrees 12 | float PITC = 0.0f; // Pitch in degrees 13 | float YAW = 0.0f; // Yaw in degrees 14 | float FTHR = 0.0f; // % of power in forward thrust 15 | }; 16 | 17 | struct TEnvironmentData 18 | { 19 | float PRES = 0.0f; // Pressure in millibars 20 | float TEMP = 0.0f; // Temperature in C 21 | }; 22 | 23 | struct TCapeData 24 | { 25 | uint32_t FMEM = 0; // Free memory on the Arduino in bytes 26 | float VOUT = 0.0f; // Voltage as meassed at the cape in milli-volts 27 | float IOUT = 0.0f; // Current measured in to the cape in milli-amps. 28 | float BTTI = 0.0f; // Current draw from both battery banks 29 | uint32_t UTIM = 0; // Up-time since Arduino was started in milliseconds 30 | }; 31 | 32 | struct TThrusterData 33 | { 34 | bool MATC = false; // Motors on-line indicator 35 | bool MotorsActive = false; // Whether or not there is currently a motor running 36 | }; 37 | 38 | // Shared data namespace 39 | namespace NDataManager 40 | { 41 | extern TNavData m_navData; 42 | extern TEnvironmentData m_environmentData; 43 | extern TCapeData m_capeData; 44 | extern TThrusterData m_thrusterData; 45 | 46 | extern orutil::CTimer m_timer_1hz; 47 | extern orutil::CTimer m_timer_10hz; 48 | 49 | extern uint32_t m_loopsPerSec; 50 | 51 | extern void Initialize(); 52 | extern void OutputSharedData(); 53 | extern void OutputNavData(); 54 | extern void HandleOutputLoops(); 55 | } -------------------------------------------------------------------------------- /sketches/OpenROV2x/NModuleManager.cpp: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include 3 | 4 | #include "NModuleManager.h" 5 | #include "SysModules.h" 6 | 7 | namespace NModuleManager 8 | { 9 | // Initialize variables 10 | int m_moduleCount = 0 ; 11 | CModule* m_pModules[ MAX_MODULES ]; 12 | 13 | // Method definitions 14 | void Initialize() 15 | { 16 | for( int i = 0; i < m_moduleCount; ++i ) 17 | { 18 | // Each module setup can take up to a full second or so to init. 19 | // Reset the WDT after each init to keep it from triggering during this step. 20 | wdt_reset(); 21 | 22 | m_pModules[ i ]->Initialize(); 23 | } 24 | } 25 | 26 | void RegisterModule( CModule* moduleIn ) 27 | { 28 | // Check to see if we have room left to register a module 29 | if( m_moduleCount >= MAX_MODULES ) 30 | { 31 | Serial.println( F( "ERROR: TOO MANY MODULES;" ) ); 32 | return; 33 | } 34 | 35 | // Set module pointer 36 | m_pModules[ m_moduleCount ] = moduleIn; 37 | 38 | // Increment module count 39 | ++m_moduleCount; 40 | } 41 | 42 | void HandleModuleUpdates( CCommand& commandIn ) 43 | { 44 | //unsigned long start = 0; 45 | //unsigned long stop = 0; 46 | 47 | // Loop through each module and call its update functon 48 | for( int i = 0; i < m_moduleCount; ++i ) 49 | { 50 | // Time and execute the update functions for this module 51 | //start = millis(); 52 | m_pModules[ i ]->Update( commandIn ); 53 | //stop = millis(); 54 | 55 | // Set the delta for the module's execution time so we can send this out as debug info 56 | //m_pModules[i]->m_executionTime = stop - start; 57 | } 58 | } 59 | } -------------------------------------------------------------------------------- /sketches/OpenROV2x/NModuleManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include "CModule.h" 5 | 6 | // Defines 7 | #define MAX_MODULES 15 8 | 9 | // TODO: CCommand sucks, going to be replaced by a packet format (Mavlink) with intentional message registration and distribution 10 | 11 | namespace NModuleManager 12 | { 13 | // Attributes 14 | extern int m_moduleCount; 15 | extern CModule* m_pModules[ MAX_MODULES ]; 16 | 17 | // Methods 18 | extern void Initialize(); 19 | extern void RegisterModule( CModule* moduleIn ); 20 | extern void HandleModuleUpdates( CCommand& commandIn ); 21 | } -------------------------------------------------------------------------------- /sketches/OpenROV2x/NVehicleManager.cpp: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include "NVehicleManager.h" 3 | #include "NCommManager.h" 4 | #include "CCommand.h" 5 | #include "CompileOptions.h" 6 | 7 | namespace NVehicleManager 8 | { 9 | // --------------------------------------------------------- 10 | // Variable initialization 11 | // --------------------------------------------------------- 12 | 13 | // TODO: Move 14 | uint32_t m_throttleSmoothingIncrement = 40; 15 | uint32_t m_deadZoneMin = 50; 16 | uint32_t m_deadZoneMax = 50; 17 | 18 | // --------------------------------------------------------- 19 | // Method Definitions 20 | // --------------------------------------------------------- 21 | 22 | void Initialize() 23 | { 24 | } 25 | 26 | void HandleMessages( CCommand &commandIn ) 27 | { 28 | if( NCommManager::m_isCommandAvailable ) 29 | { 30 | if( commandIn.Equals( "version" ) ) 31 | { 32 | // SHA1 hash of all of the source used to build the firmware 33 | // This gets automatically generated at compile time by the ArduinoBuilder library and appended to CompileOptions.h 34 | Serial.println( VERSION_HASH ); 35 | } 36 | else if( commandIn.Equals( "wake" ) ) 37 | { 38 | // Acknowledge greeting 39 | Serial.println( "awake:;" ); 40 | } 41 | // TODO: These should be handled by the control module 42 | else if( commandIn.Equals( "reportSetting" ) ) 43 | { 44 | Serial.print( F( "*settings:" ) ); 45 | Serial.print( F( "smoothingIncrement|" ) ); Serial.print( m_throttleSmoothingIncrement ); Serial.print( ',' ); 46 | Serial.print( F( "deadZone_min|" ) ); Serial.print( m_deadZoneMin ); Serial.print( ',' ); 47 | Serial.print( F( "deadZone_max|" ) ); Serial.print( m_deadZoneMax ); Serial.println( ';' ); 48 | } 49 | else if( commandIn.Equals( "updateSetting" ) ) 50 | { 51 | // TODO: Need to update the motors with new deadZone setting. Probably move 52 | // deadzone to the thruster resposibilitiy 53 | m_throttleSmoothingIncrement = commandIn.m_arguments[1]; 54 | m_deadZoneMin = commandIn.m_arguments[2]; 55 | m_deadZoneMax = commandIn.m_arguments[3]; 56 | } 57 | } 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/NVehicleManager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes 4 | #include 5 | 6 | // Forward declarations 7 | class CCommand; 8 | 9 | namespace NVehicleManager 10 | { 11 | // TODO: Move 12 | extern uint32_t m_throttleSmoothingIncrement; 13 | extern uint32_t m_deadZoneMin; 14 | extern uint32_t m_deadZoneMax; 15 | 16 | // Methods 17 | extern void Initialize(); 18 | extern void HandleMessages( CCommand &commandIn ); 19 | } 20 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/OpenROV2x.ino: -------------------------------------------------------------------------------- 1 | // Includes 2 | #include 3 | 4 | #include "NArduinoManager.h" 5 | #include "NVehicleManager.h" 6 | #include "NDataManager.h" 7 | #include "NCommManager.h" 8 | #include "NModuleManager.h" 9 | 10 | 11 | void setup() 12 | { 13 | // Initialize main subsystems 14 | NArduinoManager::Initialize(); 15 | NCommManager::Initialize(); 16 | NVehicleManager::Initialize(); 17 | NModuleManager::Initialize(); 18 | NDataManager::Initialize(); 19 | 20 | // Set timer 5 divisor to 8 for PWM frequency of 3921.16Hz (D44, D45, D46) 21 | TCCR5B = ( TCCR5B & B11111000 ) | B00000010; 22 | 23 | // Boot complete 24 | Serial.println( F( "boot:1;" ) ); 25 | } 26 | 27 | void loop() 28 | { 29 | // Reset the watchdog timer 30 | wdt_reset(); 31 | 32 | // Attempt to read a current command off of the command line 33 | NCommManager::GetCurrentCommand(); 34 | 35 | // Handle any config change requests 36 | NVehicleManager::HandleMessages( NCommManager::m_currentCommand ); 37 | 38 | // Handle update loops for each module 39 | NModuleManager::HandleModuleUpdates( NCommManager::m_currentCommand ); 40 | 41 | // Handle update loops that send data back to the beaglebone 42 | NDataManager::HandleOutputLoops(); 43 | } 44 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/PinDefinitions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "SysConfig.h" 5 | 6 | // Pin definitions 7 | // ------------------------------ 8 | // See your core's variant.cpp for full pinmuxing configuration for this board 9 | // ------------------------------ 10 | 11 | // ------------------------------ 12 | // Pin assignments 13 | // ------------------------------ 14 | 15 | // Board LEDs 16 | #define PIN_LED_0 13 17 | #define PIN_LED_1 49 18 | 19 | // Servo Pins (also digital outputs) 20 | #define PIN_SERVO_1 6 21 | #define PIN_SERVO_2 7 22 | #define PIN_SERVO_3 8 23 | #define PIN_SERVO_4 9 24 | #define PIN_SERVO_5 10 25 | #define PIN_SERVO_6 11 26 | 27 | // PWM Pins 28 | #define PIN_PWM_1 44 29 | #define PIN_PWM_2 45 30 | #define PIN_PWM_3 46 31 | #define PIN_PWM_4 12 32 | 33 | // GPIO 34 | #define PIN_ENABLE_ESC 16 35 | #define PIN_ENABLE_I2C 48 36 | 37 | // Standard Motor pin setup 38 | #define PIN_PORT_MOTOR PIN_SERVO_1 39 | #define PIN_STARBOARD_MOTOR PIN_SERVO_3 40 | #define PIN_VERTICAL_MOTOR PIN_SERVO_2 41 | // Not used PIN_SERVO_4 42 | // Not used PIN_SERVO_5 43 | // Not used PIN_SERVO_6 44 | 45 | // 2xV2 alternative pin setup 46 | // PIN_PORT_MOTOR PIN_SERVO_1 47 | // PIN_STARBOARD_MOTOR PIN_SERVO_3 48 | #define PIN_PORT_VERTICAL_MOTOR PIN_SERVO_2 49 | #define PIN_STAR_VERTICAL_MOTOR PIN_SERVO_4 50 | // Not used PIN_SERVO_5 51 | // Not used PIN_SERVO_6 52 | 53 | // V2x1xV2 alternative pin setup 54 | #define PIN_PORT_FORWARD_MOTOR PIN_SERVO_4 55 | #define PIN_STAR_FORWARD_MOTOR PIN_SERVO_5 56 | // PIN_VERTICAL_MOTOR PIN_SERVO_2 57 | #define PIN_PORT_AFT_MOTOR PIN_SERVO_1 58 | #define PIN_STAR_AFT_MOTOR PIN_SERVO_3 59 | // Not used PIN_SERVO_6 60 | 61 | // Other Servos 62 | #define PIN_CAMERA_MOUNT PIN_SERVO_6 63 | #define PIN_ALTSERVO PIN_SERVO_4 64 | 65 | // ----------- 66 | // PWM pins 67 | 68 | #define PIN_STANDARD_LIGHTS PIN_PWM_1 69 | #define PIN_LASERS PIN_PWM_2 70 | // Not yet used PIN_PWM_3 71 | // Not yet used PIN_PWM_4 72 | 73 | // ----------- 74 | // Analog pins 75 | 76 | #define PIN_BOARD_TEMP A8 // 77 | #define PIN_BOARD_I A0 // 78 | #define PIN_ESC1_I A3 // 79 | #define PIN_ESC2_I A2 // 80 | #define PIN_ESC3_I A1 // 81 | #define PIN_BATT_TUBE1_I A6 // 82 | #define PIN_BATT_TUBE2_I A5 // 83 | #define PIN_BOARD_V A4 // 84 | 85 | // Other Defines 86 | #define HAS_ESC_POWER_SWITCH 1 87 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/Plugins.h: -------------------------------------------------------------------------------- 1 | // This file is automatically generated during the build process. Don't put anything here! -------------------------------------------------------------------------------- /sketches/OpenROV2x/SysConfig.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "PinDefinitions.h" 4 | #include "CompileOptions.h" 5 | 6 | // TODO: The MCU ARCH and Board Type should eventually be passed in solely from the build script. 7 | 8 | // --------------------------------------------------------- 9 | // MCU Architecture Selection 10 | // --------------------------------------------------------- 11 | 12 | #ifndef ARDUINO_ARCH_AVR 13 | #error "This sketch is only supported on for ARDUINO_ARCH_AVR!" 14 | #endif 15 | 16 | // Configuration 17 | #define HAS_OROV_CONTROLLERBOARD_25 (1) 18 | #define HAS_STD_LIGHTS (1) 19 | #define HAS_STD_CALIBRATIONLASERS (1) 20 | #define HAS_CAMERASERVO (1) 21 | #define HAS_STD_AUTOPILOT (1) 22 | #define DEADMANSWITCH_ON (0) 23 | 24 | // Thrusters configurations (These appear depricated and replaced by the THRUSTER_CONFIGURATION below) 25 | #define THRUSTER_CONFIG_NONE (0) 26 | #define THRUSTER_CONFIG_2X1 (1) 27 | #define THRUSTER_CONFIG_2Xv2 (2) 28 | #define THRUSTER_CONFIG_v2X1Xv2 (3) 29 | 30 | // Selected Thruster Configuration 31 | #define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2X1 32 | //#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_2Xv2 33 | //#define THRUSTER_CONFIGURATION THRUSTER_CONFIG_v2X1Xv2 34 | 35 | // --------------------------------------------------------- 36 | // After Market Modules 37 | // --------------------------------------------------------- 38 | 39 | // External Lights 40 | #define HAS_EXT_LIGHTS (1) 41 | 42 | // AltServo 43 | #define HAS_ALT_SERVO (0) 44 | 45 | // MS5803_14BA Depth Sensor 46 | #define HAS_MS5803_14BA (1) 47 | 48 | // MS5837_30BA Depth Sensor 49 | #define HAS_MS5837_30BA (1) 50 | 51 | // MPU9150 52 | #define HAS_MPU9150 (1) 53 | 54 | // BNO055 IMU 55 | #define HAS_BNO055 (1) -------------------------------------------------------------------------------- /sketches/OpenROV2x/SysModules.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "SysConfig.h" 3 | 4 | #include "Plugins.h" 5 | #include "PinDefinitions.h" 6 | 7 | 8 | 9 | // --------------------------------------- 10 | // Conditional module definitions 11 | // ------------------------------- 12 | // Once these objects are instantiated, they will register themselves as modules in the module manager. 13 | // They can also be accessed individually as namespace members. 14 | // --------------------------------------- 15 | 16 | #if( HAS_OROV_CONTROLLERBOARD_25 ) 17 | #include "CControllerBoard.h" 18 | CControllerBoard m_controllerBoard; 19 | #endif 20 | 21 | #if(HAS_STD_LIGHTS) 22 | #include "CLights.h" 23 | CLights m_lights( PIN_STANDARD_LIGHTS ); 24 | #endif 25 | 26 | #if(HAS_EXT_LIGHTS ) 27 | #include "CExternalLights.h" 28 | CExternalLights m_elights( PIN_PWM_3 ); 29 | #endif 30 | 31 | #if(HAS_STD_CALIBRATIONLASERS) 32 | #include "CCalibrationLaser.h" 33 | CCalibrationLaser m_calibrationLaser( PIN_LASERS ); 34 | #endif 35 | 36 | #if(THRUSTER_CONFIGURATION != THRUSTER_CONFIG_NONE ) 37 | #include "CThrusters.h" 38 | CThrusters m_thrusters; 39 | #endif 40 | 41 | #if(HAS_STD_AUTOPILOT) 42 | #include "CAutopilot.h" 43 | CAutopilot m_autopilot; 44 | #endif 45 | 46 | #if(HAS_CAMERASERVO) 47 | #include "CCameraServo.h" 48 | CCameraServo m_cameraServo; 49 | #endif 50 | 51 | #if(HAS_ALT_SERVO) 52 | #include "CAltServo.h" 53 | CAltServo altservo1; 54 | #endif 55 | 56 | #if(DEADMANSWITCH_ON) 57 | #include "CDeadManSwitch.h" 58 | CDeadManSwitch m_deadManSwitch; 59 | #endif 60 | 61 | // IMU1 62 | #if( HAS_MPU9150 ) 63 | #include "CMPU9150.h" 64 | CMPU9150 m_mpu9150( mpu9150::EAddress::ADDRESS_B ); 65 | #endif 66 | 67 | #if(HAS_MS5803_14BA) 68 | #include "CMS5803_14BA.h" 69 | CMS5803_14BA m_ms5803( &I2C0, ms5803_14ba::EAddress::ADDRESS_A ); 70 | #endif 71 | 72 | // IMU2 73 | #if(HAS_BNO055) 74 | #include "CBNO055.h" 75 | CBNO055 m_bno055( &I2C0, bno055::EAddress::ADDRESS_A ); 76 | #endif 77 | 78 | #if(HAS_MS5837_30BA) 79 | #include "CMS5837_30BA.h" 80 | CMS5837_30BA m_ms5837( &I2C0, ms5837_30ba::EAddress::ADDRESS_A ); 81 | #endif 82 | 83 | 84 | -------------------------------------------------------------------------------- /sketches/OpenROV2x/open_source_attribution.md: -------------------------------------------------------------------------------- 1 | The great things about being an open source project is being able to use other open source projects. We ask all contributors to list all open source projects that are used in the OpenROV Arduino firmware. 2 | 3 | FreeMem | None | http://playground.arduino.cc/Code/AvailableMemory 4 | I2Cdev device library | MIT | https://github.com/jrowberg/i2cdevlib 5 | MPU9150Lib | https://github.com/Pansenti/MPU9150Lib/blob/master/LICENSE | https://github.com/Pansenti/MPU9150Lib 6 | Embedded Motion Driver | missing from driver download from vendor | http://www.invensense.com/developers/index.php?_r=downloads&ajax=dlfile&file=Embedded_MotionDriver_v5.1.zip 7 | --------------------------------------------------------------------------------