diff --git a/ASV-2/POKB/main code/POKB_Main/XBee.h b/ASV-2/POKB/main code/POKB_Main/XBee.h new file mode 100644 index 00000000..c37fd0c4 --- /dev/null +++ b/ASV-2/POKB/main code/POKB_Main/XBee.h @@ -0,0 +1,1345 @@ +/** + * Copyright (c) 2009 Andrew Rapp. All rights reserved. + * + * This file is part of XBee-Arduino. + * + * XBee-Arduino is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * XBee-Arduino is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with XBee-Arduino. If not, see . + */ + +#ifndef XBee_h +#define XBee_h + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include + +#define SERIES_1 +#define SERIES_2 + +// set to ATAP value of XBee. AP=2 is recommended +#define ATAP 2 + +#define START_BYTE 0x7e +#define ESCAPE 0x7d +#define XON 0x11 +#define XOFF 0x13 + +// This value determines the size of the byte array for receiving RX packets +// Most users won't be dealing with packets this large so you can adjust this +// value to reduce memory consumption. But, remember that +// if a RX packet exceeds this size, it cannot be parsed! + +// This value is determined by the largest packet size (100 byte payload + 64-bit address + option byte and rssi byte) of a series 1 radio +#define MAX_FRAME_DATA_SIZE 110 + +#define BROADCAST_ADDRESS 0xffff +#define ZB_BROADCAST_ADDRESS 0xfffe + +// the non-variable length of the frame data (not including frame id or api id or variable data size (e.g. payload, at command set value) +#define ZB_TX_API_LENGTH 12 +#define ZB_EXPLICIT_TX_API_LENGTH 18 +#define TX_16_API_LENGTH 3 +#define TX_64_API_LENGTH 9 +#define AT_COMMAND_API_LENGTH 2 +#define REMOTE_AT_COMMAND_API_LENGTH 13 +// start/length(2)/api/frameid/checksum bytes +#define PACKET_OVERHEAD_LENGTH 6 +// api is always the third byte in packet +#define API_ID_INDEX 3 + +// frame position of rssi byte +#define RX_16_RSSI_OFFSET 2 +#define RX_64_RSSI_OFFSET 8 + +#define DEFAULT_FRAME_ID 1 +#define NO_RESPONSE_FRAME_ID 0 + +// These are the parameters used by the XBee ZB modules when you do a +// regular "ZB TX request". +#define DEFAULT_ENDPOINT 232 +#define DEFAULT_CLUSTER_ID 0x0011 +#define DEFAULT_PROFILE_ID 0xc105 + +// TODO put in tx16 class +#define ACK_OPTION 0 +#define DISABLE_ACK_OPTION 1 +#define BROADCAST_OPTION 4 + +// RX options +#define ZB_PACKET_ACKNOWLEDGED 0x01 +#define ZB_BROADCAST_PACKET 0x02 + +// not everything is implemented! +/** + * Api Id constants + */ +#define TX_64_REQUEST 0x0 +#define TX_16_REQUEST 0x1 +#define AT_COMMAND_REQUEST 0x08 +#define AT_COMMAND_QUEUE_REQUEST 0x09 +#define REMOTE_AT_REQUEST 0x17 +#define ZB_TX_REQUEST 0x10 +#define ZB_EXPLICIT_TX_REQUEST 0x11 +#define RX_64_RESPONSE 0x80 +#define RX_16_RESPONSE 0x81 +#define RX_64_IO_RESPONSE 0x82 +#define RX_16_IO_RESPONSE 0x83 +#define AT_RESPONSE 0x88 +#define TX_STATUS_RESPONSE 0x89 +#define MODEM_STATUS_RESPONSE 0x8a +#define ZB_RX_RESPONSE 0x90 +#define ZB_EXPLICIT_RX_RESPONSE 0x91 +#define ZB_TX_STATUS_RESPONSE 0x8b +#define ZB_IO_SAMPLE_RESPONSE 0x92 +#define ZB_IO_NODE_IDENTIFIER_RESPONSE 0x95 +#define AT_COMMAND_RESPONSE 0x88 +#define REMOTE_AT_COMMAND_RESPONSE 0x97 + + +/** + * TX STATUS constants + */ +#define SUCCESS 0x0 +#define CCA_FAILURE 0x2 +#define INVALID_DESTINATION_ENDPOINT_SUCCESS 0x15 +#define NETWORK_ACK_FAILURE 0x21 +#define NOT_JOINED_TO_NETWORK 0x22 +#define SELF_ADDRESSED 0x23 +#define ADDRESS_NOT_FOUND 0x24 +#define ROUTE_NOT_FOUND 0x25 +#define PAYLOAD_TOO_LARGE 0x74 +// Returned by XBeeWithCallbacks::waitForStatus on timeout +#define XBEE_WAIT_TIMEOUT 0xff + +// modem status +#define HARDWARE_RESET 0 +#define WATCHDOG_TIMER_RESET 1 +#define ASSOCIATED 2 +#define DISASSOCIATED 3 +#define SYNCHRONIZATION_LOST 4 +#define COORDINATOR_REALIGNMENT 5 +#define COORDINATOR_STARTED 6 + +#define ZB_BROADCAST_RADIUS_MAX_HOPS 0 + +#define ZB_TX_UNICAST 0 +#define ZB_TX_BROADCAST 8 + +#define AT_OK 0 +#define AT_ERROR 1 +#define AT_INVALID_COMMAND 2 +#define AT_INVALID_PARAMETER 3 +#define AT_NO_RESPONSE 4 + +#define NO_ERROR 0 +#define CHECKSUM_FAILURE 1 +#define PACKET_EXCEEDS_BYTE_ARRAY_LENGTH 2 +#define UNEXPECTED_START_BYTE 3 + +/** + * C++11 introduced the constexpr as a hint to the compiler that things + * can be evaluated at compiletime. This can help to remove + * startup code for global objects, or otherwise help the compiler to + * optimize. Since the keyword is introduced in C++11, but supporting + * older compilers is a matter of removing the keyword, we use a macro + * for this. + */ +#if __cplusplus >= 201103L +#define CONSTEXPR constexpr +#else +#define CONSTEXPR +#endif + +/** + * The super class of all XBee responses (RX packets) + * Users should never attempt to create an instance of this class; instead + * create an instance of a subclass + * It is recommend to reuse subclasses to conserve memory + */ +class XBeeResponse { +public: + //static const int MODEM_STATUS = 0x8a; + /** + * Default constructor + */ + XBeeResponse(); + /** + * Returns Api Id of the response + */ + uint8_t getApiId(); + void setApiId(uint8_t apiId); + /** + * Returns the MSB length of the packet + */ + uint8_t getMsbLength(); + void setMsbLength(uint8_t msbLength); + /** + * Returns the LSB length of the packet + */ + uint8_t getLsbLength(); + void setLsbLength(uint8_t lsbLength); + /** + * Returns the packet checksum + */ + uint8_t getChecksum(); + void setChecksum(uint8_t checksum); + /** + * Returns the length of the frame data: all bytes after the api id, and prior to the checksum + * Note up to release 0.1.2, this was incorrectly including the checksum in the length. + */ + uint8_t getFrameDataLength(); + void setFrameData(uint8_t* frameDataPtr); + /** + * Returns the buffer that contains the response. + * Starts with byte that follows API ID and includes all bytes prior to the checksum + * Length is specified by getFrameDataLength() + * Note: Unlike Digi's definition of the frame data, this does not start with the API ID.. + * The reason for this is all responses include an API ID, whereas my frame data + * includes only the API specific data. + */ + uint8_t* getFrameData(); + + void setFrameLength(uint8_t frameLength); + // to support future 65535 byte packets I guess + /** + * Returns the length of the packet + */ + uint16_t getPacketLength(); + /** + * Resets the response to default values + */ + void reset(); + /** + * Initializes the response + */ + void init(); +#ifdef SERIES_2 + /** + * Call with instance of ZBTxStatusResponse class only if getApiId() == ZB_TX_STATUS_RESPONSE + * to populate response + */ + void getZBTxStatusResponse(XBeeResponse &response); + /** + * Call with instance of ZBRxResponse class only if getApiId() == ZB_RX_RESPONSE + * to populate response + */ + void getZBRxResponse(XBeeResponse &response); + /** + * Call with instance of ZBExplicitRxResponse class only if getApiId() == ZB_EXPLICIT_RX_RESPONSE + * to populate response + */ + void getZBExplicitRxResponse(XBeeResponse &response); + /** + * Call with instance of ZBRxIoSampleResponse class only if getApiId() == ZB_IO_SAMPLE_RESPONSE + * to populate response + */ + void getZBRxIoSampleResponse(XBeeResponse &response); +#endif +#ifdef SERIES_1 + /** + * Call with instance of TxStatusResponse only if getApiId() == TX_STATUS_RESPONSE + */ + void getTxStatusResponse(XBeeResponse &response); + /** + * Call with instance of Rx16Response only if getApiId() == RX_16_RESPONSE + */ + void getRx16Response(XBeeResponse &response); + /** + * Call with instance of Rx64Response only if getApiId() == RX_64_RESPONSE + */ + void getRx64Response(XBeeResponse &response); + /** + * Call with instance of Rx16IoSampleResponse only if getApiId() == RX_16_IO_RESPONSE + */ + void getRx16IoSampleResponse(XBeeResponse &response); + /** + * Call with instance of Rx64IoSampleResponse only if getApiId() == RX_64_IO_RESPONSE + */ + void getRx64IoSampleResponse(XBeeResponse &response); +#endif + /** + * Call with instance of AtCommandResponse only if getApiId() == AT_COMMAND_RESPONSE + */ + void getAtCommandResponse(XBeeResponse &responses); + /** + * Call with instance of RemoteAtCommandResponse only if getApiId() == REMOTE_AT_COMMAND_RESPONSE + */ + void getRemoteAtCommandResponse(XBeeResponse &response); + /** + * Call with instance of ModemStatusResponse only if getApiId() == MODEM_STATUS_RESPONSE + */ + void getModemStatusResponse(XBeeResponse &response); + /** + * Returns true if the response has been successfully parsed and is complete and ready for use + */ + bool isAvailable(); + void setAvailable(bool complete); + /** + * Returns true if the response contains errors + */ + bool isError(); + /** + * Returns an error code, or zero, if successful. + * Error codes include: CHECKSUM_FAILURE, PACKET_EXCEEDS_BYTE_ARRAY_LENGTH, UNEXPECTED_START_BYTE + */ + uint8_t getErrorCode(); + void setErrorCode(uint8_t errorCode); +protected: + // pointer to frameData + uint8_t* _frameDataPtr; +private: + void setCommon(XBeeResponse &target); + uint8_t _apiId; + uint8_t _msbLength; + uint8_t _lsbLength; + uint8_t _checksum; + uint8_t _frameLength; + bool _complete; + uint8_t _errorCode; +}; + +class XBeeAddress { +public: + CONSTEXPR XBeeAddress() {}; +}; + +/** + * Represents a 64-bit XBee Address + * + * Note that avr-gcc as of 4.9 doesn't optimize uint64_t very well, so + * for the smallest and fastest code, use msb and lsb separately. See + * https://gcc.gnu.org/bugzilla/show_bug.cgi?id=66511 + */ +class XBeeAddress64 : public XBeeAddress { +public: + CONSTEXPR XBeeAddress64(uint64_t addr) : _msb(addr >> 32), _lsb(addr) {} + CONSTEXPR XBeeAddress64(uint32_t msb, uint32_t lsb) : _msb(msb), _lsb(lsb) {} + CONSTEXPR XBeeAddress64() : _msb(0), _lsb(0) {} + uint32_t getMsb() {return _msb;} + uint32_t getLsb() {return _lsb;} + uint64_t get() {return (static_cast(_msb) << 32) | _lsb;} + operator uint64_t() {return get();} + void setMsb(uint32_t msb) {_msb = msb;} + void setLsb(uint32_t lsb) {_lsb = lsb;} + void set(uint64_t addr) { + _msb = addr >> 32; + _lsb = addr; + } +private: + // Once https://gcc.gnu.org/bugzilla/show_bug.cgi?id=66511 is + // fixed, it might make sense to merge these into a uint64_t. + uint32_t _msb; + uint32_t _lsb; +}; + +//class XBeeAddress16 : public XBeeAddress { +//public: +// XBeeAddress16(uint16_t addr); +// XBeeAddress16(); +// uint16_t getAddress(); +// void setAddress(uint16_t addr); +//private: +// uint16_t _addr; +//}; + +/** + * This class is extended by all Responses that include a frame id + */ +class FrameIdResponse : public XBeeResponse { +public: + FrameIdResponse(); + uint8_t getFrameId(); +private: + uint8_t _frameId; +}; + +/** + * Common functionality for both Series 1 and 2 data RX data packets + */ +class RxDataResponse : public XBeeResponse { +public: + RxDataResponse(); + /** + * Returns the specified index of the payload. The index may be 0 to getDataLength() - 1 + * This method is deprecated; use uint8_t* getData() + */ + uint8_t getData(int index); + /** + * Returns the payload array. This may be accessed from index 0 to getDataLength() - 1 + */ + uint8_t* getData(); + /** + * Returns the length of the payload + */ + virtual uint8_t getDataLength() = 0; + /** + * Returns the position in the frame data where the data begins + */ + virtual uint8_t getDataOffset() = 0; +}; + +// getResponse to return the proper subclass: +// we maintain a pointer to each type of response, when a response is parsed, it is allocated only if NULL +// can we allocate an object in a function? + +#ifdef SERIES_2 +/** + * Represents a Series 2 TX status packet + */ +class ZBTxStatusResponse : public FrameIdResponse { + public: + ZBTxStatusResponse(); + uint16_t getRemoteAddress(); + uint8_t getTxRetryCount(); + uint8_t getDeliveryStatus(); + uint8_t getDiscoveryStatus(); + bool isSuccess(); + + static const uint8_t API_ID = ZB_TX_STATUS_RESPONSE; +}; + +/** + * Represents a Series 2 RX packet + */ +class ZBRxResponse : public RxDataResponse { +public: + ZBRxResponse(); + XBeeAddress64& getRemoteAddress64(); + uint16_t getRemoteAddress16(); + uint8_t getOption(); + uint8_t getDataLength(); + // frame position where data starts + uint8_t getDataOffset(); + + static const uint8_t API_ID = ZB_RX_RESPONSE; +private: + XBeeAddress64 _remoteAddress64; +}; + +/** + * Represents a Series 2 Explicit RX packet + * + * Note: The receive these responses, set AO=1. With the default AO=0, + * you will receive ZBRxResponses, not knowing exact details. + */ +class ZBExplicitRxResponse : public ZBRxResponse { +public: + ZBExplicitRxResponse(); + uint8_t getSrcEndpoint(); + uint8_t getDstEndpoint(); + uint16_t getClusterId(); + uint16_t getProfileId(); + uint8_t getOption(); + uint8_t getDataLength(); + // frame position where data starts + uint8_t getDataOffset(); + + static const uint8_t API_ID = ZB_EXPLICIT_RX_RESPONSE; +}; + +/** + * Represents a Series 2 RX I/O Sample packet + */ +class ZBRxIoSampleResponse : public ZBRxResponse { +public: + ZBRxIoSampleResponse(); + bool containsAnalog(); + bool containsDigital(); + /** + * Returns true if the pin is enabled + */ + bool isAnalogEnabled(uint8_t pin); + /** + * Returns true if the pin is enabled + */ + bool isDigitalEnabled(uint8_t pin); + /** + * Returns the 10-bit analog reading of the specified pin. + * Valid pins include ADC:xxx. + */ + uint16_t getAnalog(uint8_t pin); + /** + * Returns true if the specified pin is high/on. + * Valid pins include DIO:xxx. + */ + bool isDigitalOn(uint8_t pin); + uint8_t getDigitalMaskMsb(); + uint8_t getDigitalMaskLsb(); + uint8_t getAnalogMask(); + + static const uint8_t API_ID = ZB_IO_SAMPLE_RESPONSE; +}; + +#endif + +#ifdef SERIES_1 +/** + * Represents a Series 1 TX Status packet + */ +class TxStatusResponse : public FrameIdResponse { + public: + TxStatusResponse(); + uint8_t getStatus(); + bool isSuccess(); + + static const uint8_t API_ID = TX_STATUS_RESPONSE; +}; + +/** + * Represents a Series 1 RX packet + */ +class RxResponse : public RxDataResponse { +public: + RxResponse(); + // remember rssi is negative but this is unsigned byte so it's up to you to convert + uint8_t getRssi(); + uint8_t getOption(); + bool isAddressBroadcast(); + bool isPanBroadcast(); + uint8_t getDataLength(); + uint8_t getDataOffset(); + virtual uint8_t getRssiOffset() = 0; +}; + +/** + * Represents a Series 1 16-bit address RX packet + */ +class Rx16Response : public RxResponse { +public: + Rx16Response(); + uint8_t getRssiOffset(); + uint16_t getRemoteAddress16(); + + static const uint8_t API_ID = RX_16_RESPONSE; +protected: + uint16_t _remoteAddress; +}; + +/** + * Represents a Series 1 64-bit address RX packet + */ +class Rx64Response : public RxResponse { +public: + Rx64Response(); + uint8_t getRssiOffset(); + XBeeAddress64& getRemoteAddress64(); + + static const uint8_t API_ID = RX_64_RESPONSE; +private: + XBeeAddress64 _remoteAddress; +}; + +/** + * Represents a Series 1 RX I/O Sample packet + */ +class RxIoSampleBaseResponse : public RxResponse { + public: + RxIoSampleBaseResponse(); + /** + * Returns the number of samples in this packet + */ + uint8_t getSampleSize(); + bool containsAnalog(); + bool containsDigital(); + /** + * Returns true if the specified analog pin is enabled + */ + bool isAnalogEnabled(uint8_t pin); + /** + * Returns true if the specified digital pin is enabled + */ + bool isDigitalEnabled(uint8_t pin); + /** + * Returns the 10-bit analog reading of the specified pin. + * Valid pins include ADC:0-5. Sample index starts at 0 + */ + uint16_t getAnalog(uint8_t pin, uint8_t sample); + /** + * Returns true if the specified pin is high/on. + * Valid pins include DIO:0-8. Sample index starts at 0 + */ + bool isDigitalOn(uint8_t pin, uint8_t sample); + uint8_t getSampleOffset(); + + /** + * Gets the offset of the start of the given sample. + */ + uint8_t getSampleStart(uint8_t sample); + private: +}; + +class Rx16IoSampleResponse : public RxIoSampleBaseResponse { +public: + Rx16IoSampleResponse(); + uint16_t getRemoteAddress16(); + uint8_t getRssiOffset(); + + static const uint8_t API_ID = RX_16_IO_RESPONSE; +}; + +class Rx64IoSampleResponse : public RxIoSampleBaseResponse { +public: + Rx64IoSampleResponse(); + XBeeAddress64& getRemoteAddress64(); + uint8_t getRssiOffset(); + + static const uint8_t API_ID = RX_64_IO_RESPONSE; +private: + XBeeAddress64 _remoteAddress; +}; + +#endif + +/** + * Represents a Modem Status RX packet + */ +class ModemStatusResponse : public XBeeResponse { +public: + ModemStatusResponse(); + uint8_t getStatus(); + + static const uint8_t API_ID = MODEM_STATUS_RESPONSE; +}; + +/** + * Represents an AT Command RX packet + */ +class AtCommandResponse : public FrameIdResponse { + public: + AtCommandResponse(); + /** + * Returns an array containing the two character command + */ + uint8_t* getCommand(); + /** + * Returns the command status code. + * Zero represents a successful command + */ + uint8_t getStatus(); + /** + * Returns an array containing the command value. + * This is only applicable to query commands. + */ + uint8_t* getValue(); + /** + * Returns the length of the command value array. + */ + uint8_t getValueLength(); + /** + * Returns true if status equals AT_OK + */ + bool isOk(); + + static const uint8_t API_ID = AT_COMMAND_RESPONSE; +}; + +/** + * Represents a Remote AT Command RX packet + */ +class RemoteAtCommandResponse : public AtCommandResponse { + public: + RemoteAtCommandResponse(); + /** + * Returns an array containing the two character command + */ + uint8_t* getCommand(); + /** + * Returns the command status code. + * Zero represents a successful command + */ + uint8_t getStatus(); + /** + * Returns an array containing the command value. + * This is only applicable to query commands. + */ + uint8_t* getValue(); + /** + * Returns the length of the command value array. + */ + uint8_t getValueLength(); + /** + * Returns the 16-bit address of the remote radio + */ + uint16_t getRemoteAddress16(); + /** + * Returns the 64-bit address of the remote radio + */ + XBeeAddress64& getRemoteAddress64(); + /** + * Returns true if command was successful + */ + bool isOk(); + + static const uint8_t API_ID = REMOTE_AT_COMMAND_RESPONSE; + private: + XBeeAddress64 _remoteAddress64; +}; + + +/** + * Super class of all XBee requests (TX packets) + * Users should never create an instance of this class; instead use an subclass of this class + * It is recommended to reuse Subclasses of the class to conserve memory + *

+ * This class allocates a buffer to + */ +class XBeeRequest { +public: + /** + * Constructor + * TODO make protected + */ + XBeeRequest(uint8_t apiId, uint8_t frameId); + /** + * Sets the frame id. Must be between 1 and 255 inclusive to get a TX status response. + */ + void setFrameId(uint8_t frameId); + /** + * Returns the frame id + */ + uint8_t getFrameId(); + /** + * Returns the API id + */ + uint8_t getApiId(); + // setting = 0 makes this a pure virtual function, meaning the subclass must implement, like abstract in java + /** + * Starting after the frame id (pos = 0) and up to but not including the checksum + * Note: Unlike Digi's definition of the frame data, this does not start with the API ID. + * The reason for this is the API ID and Frame ID are common to all requests, whereas my definition of + * frame data is only the API specific data. + */ + virtual uint8_t getFrameData(uint8_t pos) = 0; + /** + * Returns the size of the api frame (not including frame id or api id or checksum). + */ + virtual uint8_t getFrameDataLength() = 0; + //void reset(); +protected: + void setApiId(uint8_t apiId); +private: + uint8_t _apiId; + uint8_t _frameId; +}; + +// TODO add reset/clear method since responses are often reused +/** + * Primary interface for communicating with an XBee Radio. + * This class provides methods for sending and receiving packets with an XBee radio via the serial port. + * The XBee radio must be configured in API (packet) mode (AP=2) + * in order to use this software. + *

+ * Since this code is designed to run on a microcontroller, with only one thread, you are responsible for reading the + * data off the serial buffer in a timely manner. This involves a call to a variant of readPacket(...). + * If your serial port is receiving data faster than you are reading, you can expect to lose packets. + * Arduino only has a 128 byte serial buffer so it can easily overflow if two or more packets arrive + * without a call to readPacket(...) + *

+ * In order to conserve resources, this class only supports storing one response packet in memory at a time. + * This means that you must fully consume the packet prior to calling readPacket(...), because calling + * readPacket(...) overwrites the previous response. + *

+ * This class creates an array of size MAX_FRAME_DATA_SIZE for storing the response packet. You may want + * to adjust this value to conserve memory. + * + * \author Andrew Rapp + */ +class XBee { +public: + XBee(); + /** + * Reads all available serial bytes until a packet is parsed, an error occurs, or the buffer is empty. + * You may call xbee.getResponse().isAvailable() after calling this method to determine if + * a packet is ready, or xbee.getResponse().isError() to determine if + * a error occurred. + *

+ * This method should always return quickly since it does not wait for serial data to arrive. + * You will want to use this method if you are doing other timely stuff in your loop, where + * a delay would cause problems. + * NOTE: calling this method resets the current response, so make sure you first consume the + * current response + */ + void readPacket(); + /** + * Waits a maximum of timeout milliseconds for a response packet before timing out; returns true if packet is read. + * Returns false if timeout or error occurs. + */ + bool readPacket(int timeout); + /** + * Reads until a packet is received or an error occurs. + * Caution: use this carefully since if you don't get a response, your Arduino code will hang on this + * call forever!! often it's better to use a timeout: readPacket(int) + */ + void readPacketUntilAvailable(); + /** + * Starts the serial connection on the specified serial port + */ + void begin(Stream &serial); + void getResponse(XBeeResponse &response); + /** + * Returns a reference to the current response + * Note: once readPacket is called again this response will be overwritten! + */ + XBeeResponse& getResponse(); + /** + * Sends a XBeeRequest (TX packet) out the serial port + */ + void send(XBeeRequest &request); + //uint8_t sendAndWaitForResponse(XBeeRequest &request, int timeout); + /** + * Returns a sequential frame id between 1 and 255 + */ + uint8_t getNextFrameId(); + /** + * Specify the serial port. Only relevant for Arduinos that support multiple serial ports (e.g. Mega) + */ + void setSerial(Stream &serial); +private: + bool available(); + uint8_t read(); + void flush(); + void write(uint8_t val); + void sendByte(uint8_t b, bool escape); + void resetResponse(); + XBeeResponse _response; + bool _escape; + // current packet position for response. just a state variable for packet parsing and has no relevance for the response otherwise + uint8_t _pos; + // last byte read + uint8_t b; + uint8_t _checksumTotal; + uint8_t _nextFrameId; + // buffer for incoming RX packets. holds only the api specific frame data, starting after the api id byte and prior to checksum + uint8_t _responseFrameData[MAX_FRAME_DATA_SIZE]; + Stream* _serial; +}; + + +/** + * This class can be used instead of the XBee class and allows + * user-specified callback functions to be called when responses are + * received, simplifying the processing code and reducing boilerplate. + * + * To use it, first register your callback functions using the onXxx + * methods. Each method has a uintptr_t data argument, that can be used to + * pass arbitrary data to the callback (useful when using the same + * function for multiple callbacks, or have a generic function that can + * behave differently in different circumstances). Supplying the data + * parameter is optional, but the callback must always accept it (just + * ignore it if it's unused). The uintptr_t type is an integer type + * guaranteed to be big enough to fit a pointer (it is 16-bit on AVR, + * 32-bit on ARM), so it can also be used to store a pointer to access + * more data if required (using proper casts). + * + * There can be only one callback of each type registered at one time, + * so registering callback overwrites any previously registered one. To + * unregister a callback, pass NULL as the function. + * + * To ensure that the callbacks are actually called, call the loop() + * method regularly (in your loop() function, for example). This takes + * care of calling readPacket() and getResponse() other methods on the + * XBee class, so there is no need to do so directly (though it should + * not mess with this class if you do, it would only mean some callbacks + * aren't called). + * + * Inside callbacks, you should generally not be blocking / waiting. + * Since callbacks can be called from inside waitFor() and friends, a + * callback that doesn't return quickly can mess up the waitFor() + * timeout. + * + * Sending packets is not a problem inside a callback, but avoid + * receiving a packet (e.g. calling readPacket(), loop() or waitFor() + * and friends) inside a callback (since that would overwrite the + * current response, messing up any pending callbacks and waitFor() etc. + * methods already running). + */ +class XBeeWithCallbacks : public XBee { +public: + + /** + * Register a packet error callback. It is called whenever an + * error occurs in the packet reading process. Arguments to the + * callback will be the error code (as returned by + * XBeeResponse::getErrorCode()) and the data parameter. while + * registering the callback. + */ + void onPacketError(void (*func)(uint8_t, uintptr_t), uintptr_t data = 0) { _onPacketError.set(func, data); } + + /** + * Register a response received callback. It is called whenever + * a response was succesfully received, before a response + * specific callback (or onOtherResponse) below is called. + * + * Arguments to the callback will be the received response and + * the data parameter passed while registering the callback. + */ + void onResponse(void (*func)(XBeeResponse&, uintptr_t), uintptr_t data = 0) { _onResponse.set(func, data); } + + /** + * Register an other response received callback. It is called + * whenever a response was succesfully received, but no response + * specific callback was registered using the functions below + * (after the onResponse callback is called). + * + * Arguments to the callback will be the received response and + * the data parameter passed while registering the callback. + */ + void onOtherResponse(void (*func)(XBeeResponse&, uintptr_t), uintptr_t data = 0) { _onOtherResponse.set(func, data); } + + // These functions register a response specific callback. They + // are called whenever a response of the appropriate type was + // succesfully received (after the onResponse callback is + // called). + // + // Arguments to the callback will be the received response + // (already converted to the appropriate type) and the data + // parameter passed while registering the callback. + void onZBTxStatusResponse(void (*func)(ZBTxStatusResponse&, uintptr_t), uintptr_t data = 0) { _onZBTxStatusResponse.set(func, data); } + void onZBRxResponse(void (*func)(ZBRxResponse&, uintptr_t), uintptr_t data = 0) { _onZBRxResponse.set(func, data); } + void onZBExplicitRxResponse(void (*func)(ZBExplicitRxResponse&, uintptr_t), uintptr_t data = 0) { _onZBExplicitRxResponse.set(func, data); } + void onZBRxIoSampleResponse(void (*func)(ZBRxIoSampleResponse&, uintptr_t), uintptr_t data = 0) { _onZBRxIoSampleResponse.set(func, data); } + void onTxStatusResponse(void (*func)(TxStatusResponse&, uintptr_t), uintptr_t data = 0) { _onTxStatusResponse.set(func, data); } + void onRx16Response(void (*func)(Rx16Response&, uintptr_t), uintptr_t data = 0) { _onRx16Response.set(func, data); } + void onRx64Response(void (*func)(Rx64Response&, uintptr_t), uintptr_t data = 0) { _onRx64Response.set(func, data); } + void onRx16IoSampleResponse(void (*func)(Rx16IoSampleResponse&, uintptr_t), uintptr_t data = 0) { _onRx16IoSampleResponse.set(func, data); } + void onRx64IoSampleResponse(void (*func)(Rx64IoSampleResponse&, uintptr_t), uintptr_t data = 0) { _onRx64IoSampleResponse.set(func, data); } + void onModemStatusResponse(void (*func)(ModemStatusResponse&, uintptr_t), uintptr_t data = 0) { _onModemStatusResponse.set(func, data); } + void onAtCommandResponse(void (*func)(AtCommandResponse&, uintptr_t), uintptr_t data = 0) { _onAtCommandResponse.set(func, data); } + void onRemoteAtCommandResponse(void (*func)(RemoteAtCommandResponse&, uintptr_t), uintptr_t data = 0) { _onRemoteAtCommandResponse.set(func, data); } + + /** + * Regularly call this method, which ensures that the serial + * buffer is processed and the appropriate callbacks are called. + */ + void loop(); + + /** + * Wait for a API response of the given type, optionally + * filtered by the given match function. + * + * If a match function is given it is called for every response + * of the right type received, passing the response and the data + * parameter passed to this method. If the function returns true + * (or if no function was passed), waiting stops and this method + * returns 0. If the function returns false, waiting + * continues. After the given timeout passes, this method + * returns XBEE_WAIT_TIMEOUT. + * + * If a valid frameId is passed (e.g. 0-255 inclusive) and a + * status API response frame is received while waiting, that has + * a *non-zero* status, waiting stops and that status is + * received. This is intended for when a TX packet was sent and + * you are waiting for an RX reply, which will most likely never + * arrive when TX failed. However, since the status reply is not + * guaranteed to arrive before the RX reply (a remote module can + * send a reply before the ACK), first calling waitForStatus() + * and then waitFor() can sometimes miss the reply RX packet. + * + * Note that when the intended response is received *before* the + * status reply, the latter will not be processed by this + * method and will be subsequently processed by e.g. loop() + * normally. + * + * While waiting, any other responses received are passed to the + * relevant callbacks, just as if calling loop() continuously + * (except for the response sought, that one is only passed to + * the OnResponse handler and no others). + * + * After this method returns, the response itself can still be + * retrieved using getResponse() as normal. + */ + template + uint8_t waitFor(Response& response, uint16_t timeout, bool (*func)(Response&, uintptr_t) = NULL, uintptr_t data = 0, int16_t frameId = -1) { + return waitForInternal(Response::API_ID, &response, timeout, (void*)func, data, frameId); + } + + /** + * Sends a XBeeRequest (TX packet) out the serial port, and wait + * for a status response API frame (up until the given timeout). + * Essentially this just calls send() and waitForStatus(). + * See waitForStatus for the meaning of the return value and + * more details. + */ + uint8_t sendAndWait(XBeeRequest &request, uint16_t timeout) { + send(request); + return waitForStatus(request.getFrameId(), timeout); + } + + /** + * Wait for a status API response with the given frameId and + * return the status from the packet (for ZB_TX_STATUS_RESPONSE, + * this returns just the delivery status, not the routing + * status). If the timeout is reached before reading the + * response, XBEE_WAIT_TIMEOUT is returned instead. + * + * While waiting, any other responses received are passed to the + * relevant callbacks, just as if calling loop() continuously + * (except for the status response sought, that one is only + * passed to the OnResponse handler and no others). + * + * After this method returns, the response itself can still be + * retrieved using getResponse() as normal. + */ + uint8_t waitForStatus(uint8_t frameId, uint16_t timeout); +private: + /** + * Internal version of waitFor that does not need to be + * templated (to prevent duplication the implementation for + * every response type you might want to wait for). Instead of + * using templates, this accepts the apiId to wait for and will + * cast the given response object and the argument to the given + * function to the corresponding type. This means that the + * void* given must match the api id! + */ + uint8_t waitForInternal(uint8_t apiId, void *response, uint16_t timeout, void *func, uintptr_t data, int16_t frameId); + + /** + * Helper that checks if the current response is a status + * response with the given frame id. If so, returns the status + * byte from the response, otherwise returns 0xff. + */ + uint8_t matchStatus(uint8_t frameId); + + /** + * Top half of a typical loop(). Calls readPacket(), calls + * onPacketError on error, calls onResponse when a response is + * available. Returns in the true in the latter case, after + * which a caller should typically call loopBottom(). + */ + bool loopTop(); + + /** + * Bottom half of a typical loop. Call only when a valid + * response was read, will call all response-specific callbacks. + */ + void loopBottom(); + + template struct Callback { + void (*func)(Arg, uintptr_t); + uintptr_t data; + void set(void (*func)(Arg, uintptr_t), uintptr_t data) { + this->func = func; + this->data = data; + } + bool call(Arg arg) { + if (this->func) { + this->func(arg, this->data); + return true; + } + return false; + } + }; + + Callback _onPacketError; + Callback _onResponse; + Callback _onOtherResponse; + Callback _onZBTxStatusResponse; + Callback _onZBRxResponse; + Callback _onZBExplicitRxResponse; + Callback _onZBRxIoSampleResponse; + Callback _onTxStatusResponse; + Callback _onRx16Response; + Callback _onRx64Response; + Callback _onRx16IoSampleResponse; + Callback _onRx64IoSampleResponse; + Callback _onModemStatusResponse; + Callback _onAtCommandResponse; + Callback _onRemoteAtCommandResponse; +}; + +/** + * All TX packets that support payloads extend this class + */ +class PayloadRequest : public XBeeRequest { +public: + PayloadRequest(uint8_t apiId, uint8_t frameId, uint8_t *payload, uint8_t payloadLength); + /** + * Returns the payload of the packet, if not null + */ + uint8_t* getPayload(); + /** + * Sets the payload array + */ + void setPayload(uint8_t* payloadPtr); + + /* + * Set the payload and its length in one call. + */ + void setPayload(uint8_t* payloadPtr, uint8_t payloadLength) { + setPayload(payloadPtr); + setPayloadLength(payloadLength); + } + + /** + * Returns the length of the payload array, as specified by the user. + */ + uint8_t getPayloadLength(); + /** + * Sets the length of the payload to include in the request. For example if the payload array + * is 50 bytes and you only want the first 10 to be included in the packet, set the length to 10. + * Length must be <= to the array length. + */ + void setPayloadLength(uint8_t payloadLength); +private: + uint8_t* _payloadPtr; + uint8_t _payloadLength; +}; + +#ifdef SERIES_1 + +/** + * Represents a Series 1 TX packet that corresponds to Api Id: TX_16_REQUEST + *

+ * Be careful not to send a data array larger than the max packet size of your radio. + * This class does not perform any validation of packet size and there will be no indication + * if the packet is too large, other than you will not get a TX Status response. + * The datasheet says 100 bytes is the maximum, although that could change in future firmware. + */ +class Tx16Request : public PayloadRequest { +public: + Tx16Request(uint16_t addr16, uint8_t option, uint8_t *payload, uint8_t payloadLength, uint8_t frameId); + /** + * Creates a Unicast Tx16Request with the ACK option and DEFAULT_FRAME_ID + */ + Tx16Request(uint16_t addr16, uint8_t *payload, uint8_t payloadLength); + /** + * Creates a default instance of this class. At a minimum you must specify + * a payload, payload length and a destination address before sending this request. + */ + Tx16Request(); + uint16_t getAddress16(); + void setAddress16(uint16_t addr16); + uint8_t getOption(); + void setOption(uint8_t option); + uint8_t getFrameData(uint8_t pos); + uint8_t getFrameDataLength(); +protected: +private: + uint16_t _addr16; + uint8_t _option; +}; + +/** + * Represents a Series 1 TX packet that corresponds to Api Id: TX_64_REQUEST + * + * Be careful not to send a data array larger than the max packet size of your radio. + * This class does not perform any validation of packet size and there will be no indication + * if the packet is too large, other than you will not get a TX Status response. + * The datasheet says 100 bytes is the maximum, although that could change in future firmware. + */ +class Tx64Request : public PayloadRequest { +public: + Tx64Request(XBeeAddress64 &addr64, uint8_t option, uint8_t *payload, uint8_t payloadLength, uint8_t frameId); + /** + * Creates a unicast Tx64Request with the ACK option and DEFAULT_FRAME_ID + */ + Tx64Request(XBeeAddress64 &addr64, uint8_t *payload, uint8_t payloadLength); + /** + * Creates a default instance of this class. At a minimum you must specify + * a payload, payload length and a destination address before sending this request. + */ + Tx64Request(); + XBeeAddress64& getAddress64(); + void setAddress64(XBeeAddress64& addr64); + // TODO move option to superclass + uint8_t getOption(); + void setOption(uint8_t option); + uint8_t getFrameData(uint8_t pos); + uint8_t getFrameDataLength(); +private: + XBeeAddress64 _addr64; + uint8_t _option; +}; + +#endif + + +#ifdef SERIES_2 + +/** + * Represents a Series 2 TX packet that corresponds to Api Id: ZB_TX_REQUEST + * + * Be careful not to send a data array larger than the max packet size of your radio. + * This class does not perform any validation of packet size and there will be no indication + * if the packet is too large, other than you will not get a TX Status response. + * The datasheet says 72 bytes is the maximum for ZNet firmware and ZB Pro firmware provides + * the ATNP command to get the max supported payload size. This command is useful since the + * maximum payload size varies according to certain settings, such as encryption. + * ZB Pro firmware provides a PAYLOAD_TOO_LARGE that is returned if payload size + * exceeds the maximum. + */ +class ZBTxRequest : public PayloadRequest { +public: + /** + * Creates a unicast ZBTxRequest with the ACK option and DEFAULT_FRAME_ID + */ + ZBTxRequest(const XBeeAddress64 &addr64, uint8_t *payload, uint8_t payloadLength); + ZBTxRequest(const XBeeAddress64 &addr64, uint16_t addr16, uint8_t broadcastRadius, uint8_t option, uint8_t *payload, uint8_t payloadLength, uint8_t frameId); + /** + * Creates a default instance of this class. At a minimum you must specify + * a payload, payload length and a 64-bit destination address before sending + * this request. + */ + ZBTxRequest(); + XBeeAddress64& getAddress64(); + uint16_t getAddress16(); + uint8_t getBroadcastRadius(); + uint8_t getOption(); + void setAddress64(const XBeeAddress64& addr64); + void setAddress16(uint16_t addr16); + void setBroadcastRadius(uint8_t broadcastRadius); + void setOption(uint8_t option); +protected: + // declare virtual functions + uint8_t getFrameData(uint8_t pos); + uint8_t getFrameDataLength(); + XBeeAddress64 _addr64; + uint16_t _addr16; + uint8_t _broadcastRadius; + uint8_t _option; +}; + +/** + * Represents a Series 2 TX packet that corresponds to Api Id: ZB_EXPLICIT_TX_REQUEST + * + * See the warning about maximum packet size for ZBTxRequest above, + * which probably also applies here as well. + * + * Note that to distinguish reply packets from non-XBee devices, set + * AO=1 to enable reception of ZBExplicitRxResponse packets. + */ +class ZBExplicitTxRequest : public ZBTxRequest { +public: + /** + * Creates a unicast ZBExplicitTxRequest with the ACK option and + * DEFAULT_FRAME_ID. + * + * It uses the Maxstream profile (0xc105), both endpoints 232 + * and cluster 0x0011, resulting in the same packet as sent by a + * normal ZBTxRequest. + */ + ZBExplicitTxRequest(XBeeAddress64 &addr64, uint8_t *payload, uint8_t payloadLength); + /** + * Create a ZBExplicitTxRequest, specifying all fields. + */ + ZBExplicitTxRequest(XBeeAddress64 &addr64, uint16_t addr16, uint8_t broadcastRadius, uint8_t option, uint8_t *payload, uint8_t payloadLength, uint8_t frameId, uint8_t srcEndpoint, uint8_t dstEndpoint, uint16_t clusterId, uint16_t profileId); + /** + * Creates a default instance of this class. At a minimum you + * must specify a payload, payload length and a destination + * address before sending this request. + * + * Furthermore, it uses the Maxstream profile (0xc105), both + * endpoints 232 and cluster 0x0011, resulting in the same + * packet as sent by a normal ZBExplicitTxRequest. + */ + ZBExplicitTxRequest(); + uint8_t getSrcEndpoint(); + uint8_t getDstEndpoint(); + uint16_t getClusterId(); + uint16_t getProfileId(); + void setSrcEndpoint(uint8_t endpoint); + void setDstEndpoint(uint8_t endpoint); + void setClusterId(uint16_t clusterId); + void setProfileId(uint16_t profileId); +protected: + // declare virtual functions + uint8_t getFrameData(uint8_t pos); + uint8_t getFrameDataLength(); +private: + uint8_t _srcEndpoint; + uint8_t _dstEndpoint; + uint16_t _profileId; + uint16_t _clusterId; +}; + +#endif + +/** + * Represents an AT Command TX packet + * The command is used to configure the serially connected XBee radio + */ +class AtCommandRequest : public XBeeRequest { +public: + AtCommandRequest(); + AtCommandRequest(uint8_t *command); + AtCommandRequest(uint8_t *command, uint8_t *commandValue, uint8_t commandValueLength); + uint8_t getFrameData(uint8_t pos); + uint8_t getFrameDataLength(); + uint8_t* getCommand(); + void setCommand(uint8_t* command); + uint8_t* getCommandValue(); + void setCommandValue(uint8_t* command); + uint8_t getCommandValueLength(); + void setCommandValueLength(uint8_t length); + /** + * Clears the optional commandValue and commandValueLength so that a query may be sent + */ + void clearCommandValue(); + //void reset(); +private: + uint8_t *_command; + uint8_t *_commandValue; + uint8_t _commandValueLength; +}; + +/** + * Represents an Remote AT Command TX packet + * The command is used to configure a remote XBee radio + */ +class RemoteAtCommandRequest : public AtCommandRequest { +public: + RemoteAtCommandRequest(); + /** + * Creates a RemoteAtCommandRequest with 16-bit address to set a command. + * 64-bit address defaults to broadcast and applyChanges is true. + */ + RemoteAtCommandRequest(uint16_t remoteAddress16, uint8_t *command, uint8_t *commandValue, uint8_t commandValueLength); + /** + * Creates a RemoteAtCommandRequest with 16-bit address to query a command. + * 64-bit address defaults to broadcast and applyChanges is true. + */ + RemoteAtCommandRequest(uint16_t remoteAddress16, uint8_t *command); + /** + * Creates a RemoteAtCommandRequest with 64-bit address to set a command. + * 16-bit address defaults to broadcast and applyChanges is true. + */ + RemoteAtCommandRequest(XBeeAddress64 &remoteAddress64, uint8_t *command, uint8_t *commandValue, uint8_t commandValueLength); + /** + * Creates a RemoteAtCommandRequest with 16-bit address to query a command. + * 16-bit address defaults to broadcast and applyChanges is true. + */ + RemoteAtCommandRequest(XBeeAddress64 &remoteAddress64, uint8_t *command); + uint16_t getRemoteAddress16(); + void setRemoteAddress16(uint16_t remoteAddress16); + XBeeAddress64& getRemoteAddress64(); + void setRemoteAddress64(XBeeAddress64 &remoteAddress64); + bool getApplyChanges(); + void setApplyChanges(bool applyChanges); + uint8_t getFrameData(uint8_t pos); + uint8_t getFrameDataLength(); + static XBeeAddress64 broadcastAddress64; +// static uint16_t broadcast16Address; +private: + XBeeAddress64 _remoteAddress64; + uint16_t _remoteAddress16; + bool _applyChanges; +}; + + + +#endif //XBee_h diff --git a/ASV-2/POKB/main code/POKB_Main/can.h b/ASV-2/POKB/main code/POKB_Main/can.h new file mode 100644 index 00000000..6943bfd7 --- /dev/null +++ b/ASV-2/POKB/main code/POKB_Main/can.h @@ -0,0 +1,119 @@ +/* + can.h + 2012 Copyright (c) Seeed Technology Inc. All right reserved. + + Author:Loovee + Contributor: Cory J. Fowler + 2014-1-16 + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110- + 1301 USA +*/ +#ifndef _MCP2515_H_ +#define _MCP2515_H_ + +#include "can_defines.h" + +#define MAX_CHAR_IN_MESSAGE 8 + +class MCP_CAN +{ + private: + + INT8U m_nExtFlg; /* identifier xxxID */ + /* either extended (the 29 LSB) */ + /* or standard (the 11 LSB) */ + INT32U m_nID; /* can id */ + INT8U m_nDlc; /* data length: */ + INT8U m_nDta[MAX_CHAR_IN_MESSAGE]; /* data */ + INT8U m_nRtr; /* rtr */ + INT8U m_nfilhit; + INT8U SPICS; + +/* +* mcp2515 driver function +*/ + // private: +private: + + void mcp2515_reset(void); /* reset mcp2515 */ + + INT8U mcp2515_readRegister(const INT8U address); /* read mcp2515's register */ + + void mcp2515_readRegisterS(const INT8U address, + INT8U values[], + const INT8U n); + void mcp2515_setRegister(const INT8U address, /* set mcp2515's register */ + const INT8U value); + + void mcp2515_setRegisterS(const INT8U address, /* set mcp2515's registers */ + const INT8U values[], + const INT8U n); + + void mcp2515_initCANBuffers(void); + + void mcp2515_modifyRegister(const INT8U address, /* set bit of one register */ + const INT8U mask, + const INT8U data); + + INT8U mcp2515_readStatus(void); /* read mcp2515's Status */ + INT8U mcp2515_setCANCTRL_Mode(const INT8U newmode); /* set mode */ + INT8U mcp2515_configRate(const INT8U canSpeed); /* set boadrate */ + INT8U mcp2515_init(const INT8U canSpeed); /* mcp2515init */ + + void mcp2515_write_id( const INT8U mcp_addr, /* write can id */ + const INT8U ext, + const INT32U id ); + + void mcp2515_read_id( const INT8U mcp_addr, /* read can id */ + INT8U* ext, + INT32U* id ); + + void mcp2515_write_canMsg( const INT8U buffer_sidh_addr ); /* write can msg */ + void mcp2515_read_canMsg( const INT8U buffer_sidh_addr); /* read can msg */ + void mcp2515_start_transmit(const INT8U mcp_addr); /* start transmit */ + INT8U mcp2515_getNextFreeTXBuf(INT8U *txbuf_n); /* get Next free txbuf */ + +/* +* can operator function +*/ + + INT8U setMsg(INT32U id, INT8U ext, INT8U len, INT8U rtr, INT8U *pData); /* set message */ + INT8U setMsg(INT32U id, INT8U ext, INT8U len, INT8U *pData); /* set message */ + INT8U readMsg(); /* read message */ + INT8U sendMsg(); /* send message */ + +public: + MCP_CAN(INT8U _CS); + INT8U begin(INT8U speedset); /* init can */ + INT8U init_Mask(INT8U num, INT8U ext, INT32U ulData); /* init Masks */ + INT8U init_Filt(INT8U num, INT8U ext, INT32U ulData); /* init filters */ + INT8U sendMsgBuf(INT32U id, INT8U ext, INT8U rtr, INT8U len, INT8U *buf); /* send buf */ + INT8U sendMsgBuf(INT32U id, INT8U ext, INT8U len, INT8U *buf); /* send buf */ + INT8U readMsgBuf(INT8U *len, INT8U *buf); /* read buf */ + INT8U readMsgBufID(INT32U *ID, INT8U *len, INT8U *buf); /* read buf with object ID */ + INT8U checkTXStatus(INT8U buf); + INT8U clearMsg(); /* clear all message to zero */ + INT8U checkReceive(void); /* if something received */ + INT8U checkError(void); /* if something error */ + INT32U getCanId(void); /* get can id when receive */ + INT8U isRemoteRequest(void); /* get RR flag when receive */ + INT8U setupCANFrame(INT8U *buf, INT8U pos, INT8U size, INT32U val); + INT32U parseCANFrame(INT8U *buf, INT8U pos, INT8U size); +}; + +#endif +/********************************************************************************************************* + END FILE +*********************************************************************************************************/ diff --git a/ASV-2/POKB/main code/POKB_Main/can_asv_defines.h b/ASV-2/POKB/main code/POKB_Main/can_asv_defines.h new file mode 100644 index 00000000..d4bcf679 --- /dev/null +++ b/ASV-2/POKB/main code/POKB_Main/can_asv_defines.h @@ -0,0 +1,64 @@ +//################################################### +//################################################### + +//___. ___. +//\_ |__\_ |__ _____ _________ __ +// | __ \| __ \\__ \ / ___/\ \/ / +// | \_\ \ \_\ \/ __ \_\___ \ \ / +// |___ /___ (____ /____ > \_/ +// \/ \/ \/ \/ + +// Written by Ng Ren Zhi + +// Change Log for v1.0: +// - Initial commit + +//################################################### +//################################################### + +#ifndef _DEFINE_H_ +#define _DEFINE_H_ + +#define CAN_thruster 101 +#define CAN_manual_thruster 102 +#define CAN_control_link 103 +#define CAN_heartbeat 104 +#define CAN_soft_e_stop 105 +#define CAN_e_stop 106 +#define CAN_LARS 107 +#define CAN_POPB_control 108 +#define CAN_Shooter 109 +#define CAN_LARS_stats 110 +#define CAN_battery1_stats 111 +#define CAN_battery2_stats 112 +#define CAN_esc1_motor_stats 113 +#define CAN_esc2_motor_stats 114 +#define CAN_remote_kill_stats 115 +#define CAN_INS_stats 116 +#define CAN_GPS_stats 117 +#define CAN_cpu_temp 118 +#define CAN_POSB_stats 119 +#define CAN_POPB_stats 120 +#define CAN_POSB_BUS_stats 121 +#define CAN_POKB_BUS_stats 122 +#define CAN_POPB_BUS_stats 123 +#define CAN_Tele_BUS_stats 124 +#define CAN_LARS_BUS_stats 125 +#define CAN_MANI_stats 126 + +//CAN Heartbeat +#define HEARTBEAT_POSB 1 +#define HEARTBEAT_POPB 2 +#define HEARTBEAT_POKB 3 +#define HEARTBEAT_Tele 4 +#define HEARTBEAT_LARS 5 +#define HEARTBEAT_Cogswell 6 +#define HEARTBEAT_OCS 7 +#define HEARTBEAT_RC 8 +//Bit position +#define HEARTBEAT_BATT1 0 +#define HEARTBEAT_BATT2 1 +#define HEARTBEAT_ESC1 2 +#define HEARTBEAT_ESC2 3 + +#endif \ No newline at end of file diff --git a/ASV-3/POKB/POKB.ino b/ASV-3/POKB/POKB.ino new file mode 100644 index 00000000..f9ec2297 --- /dev/null +++ b/ASV-3/POKB/POKB.ino @@ -0,0 +1,236 @@ +#include +#include +#include +#include "defines.h" + +#define CAN_CHIP_SELECT 8 +#define SERIAL_BAUD_RATE 115200 +#define N2420_BAUD_RATE 115200 +#define REMOTE_KILL_ADDRESS 4 + +#define CONTACTOR_CONTROL 11 //NMOS, Active High +#define ONBOARD_SWITCH 21 +#define KILL 31 + +#define RECEIVE_REMOTE_KILL_TIMEOUT 500 +#define RECEIVE_POSB_HEARTBEAT_TIMEOUT 3000 +#define UPDATE_CONTACTOR_TIMEOUT 100 +#define SEND_POKB_HEARTBEAT_TIMEOUT 500 +#define SEND_POKB_STATUS_TIMEOUT 1000 + +// CAN Setup +uint32_t id = 0; +uint8_t len = 0; +uint8_t buf[8]; + +MCP_CAN CAN(CAN_CHIP_SELECT); + +// N2420 Setup +N2420 n2420 (POKB); +uint8_t inByte = 0; +uint8_t* inBuf; + +// Time Counter Variables +uint32_t receiveRemoteKillTime = 0; +uint32_t receivePOSBHeartbeatTime = 0; +uint32_t updateContactorTime = 0; +uint32_t sendPOKBHeartbeatTime = 0; +uint32_t sendPOKBStatusTime = 0; + +int noData = 0; + +// Control Variables +bool onboardKill = false; +bool remoteKill = false; +bool softwareKill = false; + +void setup() { + // put your setup code here, to run once: + Serial.begin(SERIAL_BAUD_RATE); + Serial1.begin(N2420_BAUD_RATE); + + n2420.setSerial(&Serial1); + + pinMode(CONTACTOR_CONTROL, OUTPUT); + digitalWrite(CONTACTOR_CONTROL, LOW); + + pinMode(ONBOARD_SWITCH, INPUT); + + Serial.println("Plenty of Kill Board."); + + canInitialise(); + + receiveRemoteKillTime = millis(); + receivePOSBHeartbeatTime = millis(); + updateContactorTime = millis(); + sendPOKBHeartbeatTime = millis(); + sendPOKBStatusTime = millis(); +} + +void loop() { + // Receive Onboard Kill via ATmega 2560 Input Pin + onboardKill = !digitalRead(ONBOARD_SWITCH); + + // Receive Remote Kill via N2420 + if ((millis() - receiveRemoteKillTime) > RECEIVE_REMOTE_KILL_TIMEOUT) { + receiveRemoteKill(); + receiveRemoteKillTime = millis(); + } + + // Receive Software Kill and POSB Heartbeat via CAN + receiveCanMessage(); + + // Update contactor + updateContactor(); + + // Send POKB Heartbeat via CAN + if ((millis() - sendPOKBHeartbeatTime) > SEND_POKB_HEARTBEAT_TIMEOUT) { + sendPOKBHeartbeat(); + sendPOKBHeartbeatTime = millis(); + } + + // Send POKB Status via CAN + if ((millis() - sendPOKBStatusTime) > SEND_POKB_STATUS_TIMEOUT) { + if (onboardKill || remoteKill || softwareKill) { + sendPOKBStatus(true); + } + + else { + sendPOKBStatus(false); + } + + sendPOKBStatusTime = millis(); + } +} + +void canInitialise() { +START_INIT: + if (CAN.begin(CAN_1000KBPS) == CAN_OK) { + Serial.println("CAN Bus: Initialisation successful."); + } + + else { + Serial.println("CAN Bus: Initialisation failed."); + Serial.println("CAN Bus: Re-initialising."); + delay(1000); + goto START_INIT; + } + + Serial.println("Starting transmission..."); +} + +void receiveRemoteKill() { + // Continuously read packets + n2420.readPacket(); + + if (n2420.isAvailable()) { + // Serial.println("Response available."); + + if (n2420.getReceivingAddress() == REMOTE_KILL_ADDRESS){ //REMOTE_KILL_ADDRESS is defined as 4 from library + // Get data + inBuf = n2420.showReceived(); + inByte = *(inBuf+2); + Serial.print("inByte: "); + Serial.println(inByte, HEX); + //Need to modify this if not 15, go to noData++ not false directly + //remoteKill = (inByte == 0x15) ? false : true; + //noData = 0; + if (inByte == 0x15) { + remoteKill = false; + noData = 0; + } + else if (inByte == 0x44) { + remoteKill = true; + noData = 0; + } + else { + noData++; + if (noData >= 20) { + remoteKill = true; + noData = 0; + Serial.println("Connection timeout kill"); + } + } + } + } + + else { + noData++; + Serial.print("noData: "); + Serial.println(noData); + + if (noData >= 20) { + remoteKill = true; + noData = 0; + Serial.println("Connection timeout kill."); + } + } +} + +void receiveCanMessage() { + if (CAN.checkReceive() == CAN_MSGAVAIL) { + CAN.readMsgBufID(&id, &len, buf); + + switch (id) { + case CAN_HEARTBEAT: + + if (buf[0] == HEARTBEAT_POSB) + receivePOSBHeartbeatTime = millis(); + break; + case CAN_SOFT_E_STOP: + softwareKill = !buf[0]; + break; + default: + break; + } + + CAN.clearMsg(); + } +} + +void updateContactor() { + // Failsafe + // Check POSB Heartbeat + + /* + if ((millis() - receivePOSBHeartbeatTime) > RECEIVE_POSB_HEARTBEAT_TIMEOUT) { + // digitalWrite(CONTACTOR_CONTROL, LOW); + } + */ + + // else if ((millis() - updateContactorTime) > UPDATE_CONTACTOR_TIMEOUT) { + if ((millis() - updateContactorTime) > UPDATE_CONTACTOR_TIMEOUT) { + // Serial.print("Onboard Kill: "); + // Serial.print(onboardKill); + // Serial.print(" | "); + // Serial.print("Remote Kill: "); + // Serial.print(remoteKill); + // Serial.print(" | "); + // Serial.print("Software Kill: "); + // Serial.println(softwareKill); + + if (onboardKill || remoteKill || softwareKill) { + digitalWrite(CONTACTOR_CONTROL, LOW); + digitalWrite(KILL, HIGH); + //Serial.println("Contactor switched off."); + } + + else { + digitalWrite(CONTACTOR_CONTROL, HIGH); + digitalWrite(KILL, LOW); + //Serial.println("Contactor switched on."); + } + + updateContactorTime = millis(); + } +} + +void sendPOKBHeartbeat() { + CAN.setupCANFrame(buf, 0, 1, HEARTBEAT_POKB); + CAN.sendMsgBuf(CAN_HEARTBEAT, 0, 1, buf); +} + +void sendPOKBStatus(bool pokbStatus) { + CAN.setupCANFrame(buf, 0, 1, pokbStatus); + CAN.sendMsgBuf(CAN_E_STOP, 0, 1, buf); +} diff --git a/ASV-3/POKB/defines.h b/ASV-3/POKB/defines.h new file mode 100644 index 00000000..509e7af1 --- /dev/null +++ b/ASV-3/POKB/defines.h @@ -0,0 +1,33 @@ +#ifndef _DEFINES_H_ +#define _DEFINES_H_ + +// CAN +#define CAN_Chip_Select 8 + +// KILL +#define NMOS_CONTACTOR 11 +#define HARDKILL_IN 5 + +// XBEE +#define XBEE_BAUDRATE 115200 +#define SERIAL_BAUDRATE 115200 + +// TIMEOUT +#define XBEE_TIMEOUT 500 +#define HEARTBEAT_TIMEOUT 500 +#define CAN_TIMEOUT 1000 +#define ESTOP_TIMEOUT 100 +#define FAILSAFE_TIMEOUT 3000 + +// HEARTBEAT + +#define HEARTBEAT_POSB 0x01 +#define HEARTBEAT_POPB 0x02 +#define HEARTBEAT_POKB 0x03 +#define HEARTBEAT_Tele 0x04 +#define HEARTBEAT_LARS 0x05 +#define HEARTBEAT_Cogswell 0x06 +#define HEARTBEAT_OCS 0x07 + + +#endif diff --git a/ASV-3/Telemetry/main_code/Telemetry.ino b/ASV-3/Telemetry/main_code/Telemetry/Telemetry.ino similarity index 100% rename from ASV-3/Telemetry/main_code/Telemetry.ino rename to ASV-3/Telemetry/main_code/Telemetry/Telemetry.ino diff --git a/AUV-3.99/Sensor and Telemetry/main code/Sensor and Telemetry/stb/stb.ino b/AUV-3.99/Sensor and Telemetry/main code/Sensor and Telemetry/stb/stb.ino new file mode 100644 index 00000000..1586a4ce --- /dev/null +++ b/AUV-3.99/Sensor and Telemetry/main code/Sensor and Telemetry/stb/stb.ino @@ -0,0 +1,658 @@ +//################################################### +//################################################### +// +//#### #### +//# # # # ###### ######## ######## +//# #### # #### # ## # ## # # ## # +//# ## # ## #### # # ## # # ## # +//# ## # # ## # ## # # ## # # ## # +//# ## # # ## # # ## # # ## # ## ## +//# ## # ## ## # ## # ## ## +// # #### # #### ####### ####### #### +// +// +// Sensor and Telemetry for BBAUV 3.99 +// Firmware Version : v1.2 +// +// Written by Chia Che edited by titus +// Change log v1.2: +// Changed SoftPMW library to PalatisSoftPWM by per1234 due to compatibility issues +// Update depth calculation for US381-000005-050PG 50psi gauge depth sensor +// Correct SBC temperature code +// +//################################################### +//################################################### + +#include +#include +#include +#include +#include +#include "LCD_Driver.h" +#include "define.h" +#include +#include +#include //for CAN controller +#include +#include "can_auv_define.h" +#include + + +// CAN variable +MCP_CAN CAN(CAN_Chip_Select); +uint32_t id = 0; +uint8_t len = 0; //length of CAN message, taken care by library +uint8_t buf[8]; //Buffer for CAN message + +//Screen variables +LCD screen = LCD(SCREEN_CS, SCREEN_RESET); +static uint16_t internalStats[INT_STAT_COUNT] = { 0 }; +static uint16_t powerStats[POWER_STAT_COUNT] = { 0 }; +static uint32_t heartbeat_timeout[HB_COUNT] = { 0 }; +static uint32_t loopTime = 0; + +//Sensor variables +Adafruit_ADS1115 ads(ADS_ADDR); +HIH613x humid(HUMIDITY_ADDR); +uint8_t humidity = 0; +uint8_t IntPressure = 0; +uint8_t temperature = 0; +uint16_t ExtPressure = 0; +uint8_t InitialP = 0; +uint16_t rawExtPressure = 0; +static uint32_t humidloop = 0; +bool readHumid = false; +static uint32_t pressure_loop = 0; +static uint32_t filter_loop = 0; + +//LED +//========== Read this before use ========== +// https://github.com/per1234/PalatisSoftPWM + +SOFTPWM_DEFINE_PIN22_CHANNEL(0); //Arduino pin 22 as channel 0 +SOFTPWM_DEFINE_PIN23_CHANNEL(1); //Arduino pin 23 as channel 1 +SOFTPWM_DEFINE_PIN24_CHANNEL(2); //Arduino pin 24 as channel 2 +SOFTPWM_DEFINE_OBJECT(3); // Initialize 3 channels +bool blink_on = false; +uint32_t time = 0; +uint8_t lightColour = 0; // 0 is off +uint8_t selfSetColour[3] = { 0 }; + + +//Others +uint8_t CPU_CoreTemp = 0; +bool sonar = false; +static uint32_t pmb1_timeout = 0; +static uint32_t pmb2_timeout = 0; +static uint32_t sbc_timeout = 0; +static uint32_t dna_timeout = 0; +static uint32_t heartbeat_loop = 0; +static uint32_t stats_loop = 0; +static uint16_t dna_pressure = 0; + +static uint32_t testing_time = 0; + +void setup() +{ + pinMode(SCREEN_CS, OUTPUT); //CS screen + digitalWrite(SCREEN_CS, HIGH); + pinMode(CAN_Chip_Select, OUTPUT); //CS CAN + digitalWrite(CAN_Chip_Select, HIGH); + + Serial.begin(115200); + Serial.println("Hi, I'm STB!"); + + //CAN init + CAN_init(); + Serial.println("CAN OK"); + CANSetMask(); + + //Screen init + screen.screen_init(); + Serial.println("Screen Ok"); + screen_prepare(); + + //Sensor init + Wire.begin(); + Serial.println("Sensors OK"); + InitialP = readInternalPressure(); + + //led init + led_init(); + Serial.println("LED OK"); + + for (int i = 0; i < HB_COUNT; i++) { + heartbeat_timeout[i] = millis(); + } +} + +void loop() +{ + reset_stats(); + update_ST_stats(); + + if ((millis() - loopTime) > SCREEN_LOOP) { // 1000ms + screen_update(); + update_heartbeat(); + loopTime = millis(); + } + + if (millis() - filter_loop > LPF_LOOP) { // 25ms + //externalPressureLPF(); + filter_loop = millis(); + } + + checkCANmsg(); + + publishCAN(); +} + +//=========================================== +// +// CAN FUNCTIONS +// +//=========================================== + +void CAN_init() { +START_INIT: + if (CAN_OK == CAN.begin(CAN_1000KBPS)) { // init can bus : baudrate = 1000k +#if DEBUG_MODE == NORMAL + Serial.println("CAN init ok!"); +#endif + } + else { +#if DEBUG_MODE == NORMAL + Serial.println("CAN init fail"); + Serial.println("Init CAN again"); + delay(1000); +#endif + goto START_INIT; + } +} + +void CANSetMask() { + /* + Truth table + mask filter id bit reject + 0 X X no + 1 0 0 no + 1 0 1 yes + 1 1 0 yes + 1 1 1 no + + Mask 0 connects to filt 0,1 + Mask 1 connects to filt 2,3,4,5 + + Mask decide which bit to check + Filt decide which bit to accept + */ + + CAN.init_Mask(0, 0, 0xA); // check 11XX + CAN.init_Mask(1, 0, 0xF); // check all bit + + CAN.init_Filt(0, 0, 0x8); // let 10XX pass (8, 9) + CAN.init_Filt(1, 0, 0xA); // let 11XX pass (10 to 15) + + CAN.init_Filt(2, 0, 0x3); // let 0011 pass (3) + CAN.init_Filt(3, 0, 0x4); // let 0100 pass (4) + CAN.init_Filt(4, 0, 0xF); // let 1111 pass (15) + CAN.init_Filt(5, 0, 0xF); // let 1111 pass (15) + +} + +/* Receive these CAN ID +3: Heartbeat: SBC, SBC_CAN, PCB, TB, ST, MANI, PMB1, PMB2 +4: Sonar trigger +9: LED +10: CAN_DNA_Stats +11: PMB1 stat1 +12: PMB1 stat2 +13: PMB2 stat1 +14: PMB2 stat2 +15: CPU Temp +*/ + +void checkCANmsg() { + if (CAN_MSGAVAIL == CAN.checkReceive()) { + CAN.readMsgBufID(&id, &len, buf); // read data, len: data length, buf: data buf + switch (CAN.getCanId()) { + case CAN_heartbeat: + { + uint32_t device = CAN.parseCANFrame(buf, 0, 1); + heartbeat_timeout[device] = millis(); + break; + } + case CAN_SONAR: + { + uint8_t temp = CAN.parseCANFrame(buf, 0, 1); + (temp == 1) ? sonar = true : sonar = false; + break; + } + case CAN_LED: + { + selfSetColour[0] = CAN.parseCANFrame(buf, 0, 1); + selfSetColour[1] = CAN.parseCANFrame(buf, 1, 1); + selfSetColour[2] = CAN.parseCANFrame(buf, 2, 1); +#ifdef SOFTPWM + setcolour(selfSetColour[0], selfSetColour[1], selfSetColour[2]); +#else + //led.colour(lightColour); +#endif + break; + } + case CAN_DNA_Stats: + internalStats[DNA_PRESS] = CAN.parseCANFrame(buf, 0, 1); + sbc_timeout = millis(); + break; + case CAN_PMB1_stats: + powerStats[BATT1_CURRENT] = CAN.parseCANFrame(buf, 0, 2); + powerStats[BATT1_VOLTAGE] = CAN.parseCANFrame(buf, 2, 2); + pmb1_timeout = millis(); + break; + case CAN_PMB1_stats2: + internalStats[PMB1_PRESS] = CAN.parseCANFrame(buf, 4, 1); + internalStats[PMB1_TEMP] = CAN.parseCANFrame(buf, 3, 1); + powerStats[BATT1_CAPACITY] = CAN.parseCANFrame(buf, 2, 1); + pmb1_timeout = millis(); + break; + case CAN_PMB2_stats: + powerStats[BATT2_CURRENT] = CAN.parseCANFrame(buf, 0, 2); + powerStats[BATT2_VOLTAGE] = CAN.parseCANFrame(buf, 2, 2); + pmb2_timeout = millis(); + break; + case CAN_PMB2_stats2: + internalStats[PMB2_PRESS] = CAN.parseCANFrame(buf, 4, 1); + internalStats[PMB2_TEMP] = CAN.parseCANFrame(buf, 3, 1); + powerStats[BATT2_CAPACITY] = CAN.parseCANFrame(buf, 2, 1); + pmb2_timeout = millis(); + break; + case CAN_CPU: + { + uint8_t temp[3] = { 0 }; + for (int i = 0; i < 3; i++) { + temp[i] = CAN.parseCANFrame(buf, i, 1); + } + uint8_t max = temp[0]; + for (int i = 1; i < 3; i++) { + if (temp[i] >= max) { + max = temp[i]; + } + } + CPU_CoreTemp = max; + internalStats[CPU_TEMP] = CPU_CoreTemp; + sbc_timeout = millis(); + break; + } + default: + break; + } + CAN.clearMsg(); + } +} + +//publish raw pressure, heartbeat and stats to CAN bus +void publishCAN() +{ + //publish raw external pressure reading every 50ms + if (millis() - pressure_loop > 50) { + publishCAN_pressure(); + pressure_loop = millis(); + } + + //publish heartbeat every 500ms + if (millis() - heartbeat_loop > 500) { + publishCAN_heartbeat(HEARTBEAT_ST); + heartbeat_loop = millis(); + } + + //publish ST stats every 1000ms + if (millis() - stats_loop > 1000) { + publishST_stats(); + stats_loop = millis(); + } +} + +void publishCAN_heartbeat(int device_id) +{ + id = CAN_heartbeat; + len = 1; + buf[0] = device_id; + CAN.sendMsgBuf(CAN_heartbeat, 0, 1, buf); +} + +void publishCAN_pressure() { + CAN.setupCANFrame(buf, 0, 2, rawExtPressure); + CAN.sendMsgBuf(CAN_pressure, 0, 2, buf); +} + +void publishST_stats() { + CAN.setupCANFrame(buf, 0, 1, temperature); + CAN.setupCANFrame(buf, 1, 1, humidity); + CAN.setupCANFrame(buf, 2, 1, IntPressure); + leak() ? CAN.setupCANFrame(buf, 3, 1, 1) : CAN.setupCANFrame(buf, 3, 1, 0); + CAN.sendMsgBuf(CAN_ST_stats, 0, 4, buf); +} + + +//========================================== +// +// LCD FUNCTIONS +// +//========================================== + +void screen_prepare() { + screen.set_cursor(0 + OFFSET, 0); + screen.write_string("Ext press:"); + screen.write_string("Int press:"); + screen.write_string("PMB1 press:"); + screen.write_string("PMB2 press:"); + screen.write_string("PMB1 temp:"); + screen.write_string("PMB2 temp:"); + screen.write_string("CPU temp:"); + screen.write_string("Humidity:"); + screen.write_string("ST temp:"); + screen.write_string("DNA press: "); + screen.write_string("SBC OK:"); + screen.write_string("SBC-CAN OK:"); + screen.write_string("PCB OK:"); + + screen.set_cursor(400 + OFFSET, 0); + screen.write_string("Batt1 capacity:"); + screen.write_string("Batt2 capacity:"); + screen.write_string("Batt1 current:"); + screen.write_string("Batt2 current:"); + screen.write_string("Batt1 voltage:"); + screen.write_string("Batt2 voltage:"); + screen.write_string("Thruster OK:"); + screen.write_string("Manipulator OK:"); + screen.write_string("PMB1 OK:"); + screen.write_string("PMB2 OK:"); +} + +void screen_update() { + // row height 35, increment_row() + screen.set_cursor(200 + OFFSET, 0); + for (int i = 0; i < INT_STAT_COUNT; i++) + { + screen.write_value_int(internalStats[i]); + } + + screen.set_cursor(645 + OFFSET, 0); + for (int i = 0; i < POWER_STAT_COUNT; i++) + { + if (i > BATT2_CAPACITY) { + screen.write_value_with_dp(powerStats[i], 1); + } + else { + screen.write_value_int(powerStats[i]); + } + } +} + +void update_heartbeat() +{ + // row height 35, increment_row() + int i; + screen.set_cursor(200 + OFFSET, 350); + for (i = 1; i < 4; i++) { + if (i != HEARTBEAT_ST) // Skip ST HB + { + if ((millis() - heartbeat_timeout[i]) > HB_TIMEOUT) { + screen.write_value_string("NO"); + } + else + screen.write_value_string("YES"); + } + } + + screen.set_cursor(645 + OFFSET, 210); + for (; i < 9; i++) { + if (i != HEARTBEAT_ST) { // Skip ST HB + if ((millis() - heartbeat_timeout[i]) > HB_TIMEOUT) { + screen.write_value_string("NO"); + } + else + screen.write_value_string("YES"); + } + } +} + +//reset pmb1 pmb2 and sbc stats +void reset_stats() +{ + reset_pmb1_stat(); + reset_pmb2_stat(); + reset_sbc_stat(); +} + +void reset_pmb1_stat() { + if ((millis() - pmb1_timeout) > STAT_TIMEOUT) { + internalStats[PMB1_PRESS] = 0xFFFF; + internalStats[PMB1_TEMP] = 0xFFFF; + powerStats[BATT1_CAPACITY] = 0xFFFF; + powerStats[BATT1_CURRENT] = 0xFFFF; + powerStats[BATT1_VOLTAGE] = 0xFFFF; + pmb1_timeout = millis(); + } +} + +void reset_pmb2_stat() { + if ((millis() - pmb2_timeout) > STAT_TIMEOUT) { + internalStats[PMB2_PRESS] = 0xFFFF; + internalStats[PMB2_TEMP] = 0xFFFF; + powerStats[BATT2_CAPACITY] = 0xFFFF; + powerStats[BATT2_CURRENT] = 0xFFFF; + powerStats[BATT2_VOLTAGE] = 0xFFFF; + pmb2_timeout = millis(); + } +} + +void reset_sbc_stat() { + if ((millis() - sbc_timeout) > STAT_TIMEOUT) { + internalStats[CPU_TEMP] = 0xFFFF; + internalStats[DNA_PRESS] = 0xFFFF; + sbc_timeout = millis(); + } +} + +//read Temperature, Humidity, External and Internal pressure +// and assign them to array for update +void update_ST_stats() { + readTempHumididty(); + rawExtPressure = readExternalPressure(); + internalStats[EXT_PRESS] = ExtPressure; + IntPressure = readInternalPressure(); + internalStats[INT_PRESS] = readInternalPressure(); + internalStats[HUMIDITY] = humidity; + internalStats[ST_TEMP] = temperature; +} + +//========================================== +// +// Sensor Functions +// +//========================================== + +//Return Internal Pressure +byte readInternalPressure() { + /* + VOUT = VS x (0.004 x P - 0.040)�� (Pressure Error x Temp Factor x 0.004 x VS) + VS = 5.1 �� 0.36 Vdc + */ + // internal raw value 9690 = 1010mb = 101kPa + ads.set_continuous_conv(1); + delay(ADS_DELAY); + uint16_t adc1 = ads.readADC_Continuous(); + + return (((double)adc1*0.0001875) / (Vref*0.0040) + 10); +} + +//Updates ExtPressure and return raw 16bit External Pressure reading +uint16_t readExternalPressure() { + // output 4-20mA range upto 50psi gauge pressure sensor -> 0psi @ sea level + // US381-000005-050PG + // ==> 1-5V as shunt resistor is 250.0ohm + ads.set_continuous_conv(0); + delay(ADS_DELAY); + uint16_t adc0 = ads.readADC_Continuous(); + // ADC Range: +/- 6.144V + // 1 psi = 6895 pascal + double voltage = ((double)adc0/32767) * 6.144; // 0 = 0V, 2^15 = 6.144V + double gauge_psi = ((voltage - 1) / 4) * 50; // 1-5V, 50psi range + double absolute_psi = gauge_psi + 14.6; // psi at sea level = 14.6psi + ExtPressure = (uint16_t)(absolute_psi * 6.895); // pressure in kpa + if (ExtPressure < 80 || ExtPressure > 350) { + ExtPressure = 0xFFFF; + } + return adc0; // for legacy reasons, return raw adc value +} + +//Filter External Pressure with LPF +void externalPressureLPF() { + //LPF filter for ext pressure sensor + uint16_t temp = readExternalPressure(); + if (temp != 0) + { + rawExtPressure = rawExtPressure + LPF_CONSTANT * (float)(temp - rawExtPressure); + } + ExtPressure = rawExtPressure; +} + +//Updates Temperature and Humidity +void readTempHumididty() { + // reading temp or humid takes 36.65ms, 2 takes 74ms + if (millis() - humidloop > 100) { + if (!readHumid) { + humid.measurementRequest(); + readHumid = true; + } + else { + humid.dataFetch(); + humidity = humid.getHumidity() + 0.5; + temperature = humid.getTemperature() + 0.5; + readHumid = false; + } + humidloop = millis(); + } +} + + +//Return bool to indicate whether is it leaking +//Blinks led if it is leaking +bool leak() { + bool leaking = false; + if ((InitialP - IntPressure > 10) || humidity > 85) { + leaking = true; + } + if (leaking) { + //colour(8); // 8 for white + blink(1, 8, 300); + } + return leaking; +} + +void sonar_init() { + pinMode(SONAR_IN, OUTPUT); + pinMode(SONAR_OUT, OUTPUT); + digitalWrite(SONAR_OUT, LOW); + + sonar ? digitalWrite(SONAR_IN, HIGH) : digitalWrite(SONAR_IN, LOW); +} + +//Updates hardware trigger of Sonar +void sonar_update() { + digitalWrite(SONAR_OUT, LOW); + sonar ? digitalWrite(SONAR_IN, HIGH) : digitalWrite(SONAR_IN, LOW); +} + +//========================================== +// +// LED Functions +// +//========================================== + +//Blinks through all colour +void led_init() { + PalatisSoftPWM.begin(200); //200Hz + for (int i = 0; i < 10; i++) { + colour(i); + delay(200); + } + colour(lightColour); +} + +void setcolour(int red, int green, int blue) { + PalatisSoftPWM.set(RED, red); + PalatisSoftPWM.set(GREEN, green); + PalatisSoftPWM.set(BLUE, blue); +} + +// 9 for off +void colour(int colour) +{ +/* 0 - Off #000000 + 1 - Red #FF0000 + 2 - Violet #EE82EE + 3 - Pink #FFCCFF + 4 - Blue #0000FF + 5 - Green #00FF00 + 6 - Cyan #00FFFF + 7 - Maroon #80000 + 8 - white #FFFFFF + 9 - Yellow #FFFF00 +*/ + switch (colour) + { + case 0://Off + setcolour(0, 0, 0); + break; + case 1://Red + setcolour(255, 0, 0); + break; + case 2://Violet + setcolour(238, 130, 238); + break; + case 3://Pink + setcolour(255, 204, 255); + break; + case 4://Blue + setcolour(0, 0, 255); + break; + case 5://Green + setcolour(0, 255, 0); + break; + case 6://Cyan + setcolour(0, 255, 255); + break; + case 7://Maroon + setcolour(128, 0, 0); + break; + case 8://White + setcolour(255, 255, 255); + break; + case 9://Yellow + setcolour(255, 255, 0); + break; + default: + setcolour(0, 0, 0); + break; + } +} + +// a for first colour, b for second colour, +// period in ms +void blink(uint8_t a, uint8_t b, uint32_t period) { + if (a > 10 || b > 10) { + return; + } + if (millis() - time > period) { + if (blink_on) { + colour(a); + } + else { + colour(b); + } + blink_on = !blink_on; + time = millis(); + } +} diff --git a/AUV-4/STB/main code/STB_4.0/MS5837.cpp b/AUV-4/STB/main code/STB_4.0/MS5837.cpp new file mode 100644 index 00000000..142f971d --- /dev/null +++ b/AUV-4/STB/main code/STB_4.0/MS5837.cpp @@ -0,0 +1,258 @@ +#include "MS5837.h" +#include + +const uint8_t MS5837_ADDR = 0x76; +const uint8_t MS5837_RESET = 0x1E; +const uint8_t MS5837_ADC_READ = 0x00; +const uint8_t MS5837_PROM_READ = 0xA0; +const uint8_t MS5837_CONVERT_D1_8192 = 0x4A; +const uint8_t MS5837_CONVERT_D2_8192 = 0x5A; + +const float MS5837::Pa = 100.0f; +const float MS5837::bar = 0.001f; +const float MS5837::mbar = 1.0f; + +const uint8_t MS5837::MS5837_30BA = 0; +const uint8_t MS5837::MS5837_02BA = 1; +const uint8_t MS5837::MS5837_UNRECOGNISED = 255; + +const uint8_t MS5837_02BA01 = 0x00; // Sensor version: From MS5837_02BA datasheet Version PROM Word 0 +const uint8_t MS5837_02BA21 = 0x15; // Sensor version: From MS5837_02BA datasheet Version PROM Word 0 +const uint8_t MS5837_30BA26 = 0x1A; // Sensor version: From MS5837_30BA datasheet Version PROM Word 0 + +MS5837::MS5837() { + fluidDensity = 1029; +} + +bool MS5837::begin(TwoWire &wirePort) { + return (init(wirePort)); +} + +bool MS5837::init(TwoWire &wirePort) { + _i2cPort = &wirePort; //Grab which port the user wants us to use + + // Reset the MS5837, per datasheet + _i2cPort->beginTransmission(MS5837_ADDR); + _i2cPort->write(MS5837_RESET); + _i2cPort->endTransmission(); + + // Wait for reset to complete + delay(10); + + // Read calibration values and CRC + for ( uint8_t i = 0 ; i < 7 ; i++ ) { + _i2cPort->beginTransmission(MS5837_ADDR); + _i2cPort->write(MS5837_PROM_READ+i*2); + _i2cPort->endTransmission(); + + _i2cPort->requestFrom(MS5837_ADDR,2); + C[i] = (_i2cPort->read() << 8) | _i2cPort->read(); + } + + // Verify that data is correct with CRC + uint8_t crcRead = C[0] >> 12; + uint8_t crcCalculated = crc4(C); + + if ( crcCalculated != crcRead ) { + return false; // CRC fail + } + + uint8_t version = (C[0] >> 5) & 0x7F; // Extract the sensor version from PROM Word 0 + + // Set _model according to the sensor version + if (version == MS5837_02BA01) + { + _model = MS5837_02BA; + } + else if (version == MS5837_02BA21) + { + _model = MS5837_02BA; + } + else if (version == MS5837_30BA26) + { + _model = MS5837_30BA; + } + else + { + _model = MS5837_UNRECOGNISED; + } + // The sensor has passed the CRC check, so we should return true even if + // the sensor version is unrecognised. + // (The MS5637 has the same address as the MS5837 and will also pass the CRC check) + // (but will hopefully be unrecognised.) + return true; +} + +void MS5837::setModel(uint8_t model) { + _model = model; +} + +uint8_t MS5837::getModel() { + return (_model); +} + +void MS5837::setFluidDensity(float density) { + fluidDensity = density; +} + +void MS5837::read() { + //Check that _i2cPort is not NULL (i.e. has the user forgoten to call .init or .begin?) + if (_i2cPort == NULL) + { + return; + } + + // Request D1 conversion + _i2cPort->beginTransmission(MS5837_ADDR); + _i2cPort->write(MS5837_CONVERT_D1_8192); + _i2cPort->endTransmission(); + + delay(20); // Max conversion time per datasheet + + _i2cPort->beginTransmission(MS5837_ADDR); + _i2cPort->write(MS5837_ADC_READ); + _i2cPort->endTransmission(); + + _i2cPort->requestFrom(MS5837_ADDR,3); + D1_pres = 0; + D1_pres = _i2cPort->read(); + D1_pres = (D1_pres << 8) | _i2cPort->read(); + D1_pres = (D1_pres << 8) | _i2cPort->read(); + + // Request D2 conversion + _i2cPort->beginTransmission(MS5837_ADDR); + _i2cPort->write(MS5837_CONVERT_D2_8192); + _i2cPort->endTransmission(); + + delay(20); // Max conversion time per datasheet + + _i2cPort->beginTransmission(MS5837_ADDR); + _i2cPort->write(MS5837_ADC_READ); + _i2cPort->endTransmission(); + + _i2cPort->requestFrom(MS5837_ADDR,3); + D2_temp = 0; + D2_temp = _i2cPort->read(); + D2_temp = (D2_temp << 8) | _i2cPort->read(); + D2_temp = (D2_temp << 8) | _i2cPort->read(); + + calculate(); +} + +void MS5837::calculate() { + // Given C1-C6 and D1, D2, calculated TEMP and P + // Do conversion first and then second order temp compensation + + int32_t dT = 0; + int64_t SENS = 0; + int64_t OFF = 0; + int32_t SENSi = 0; + int32_t OFFi = 0; + int32_t Ti = 0; + int64_t OFF2 = 0; + int64_t SENS2 = 0; + + // Terms called + dT = D2_temp-uint32_t(C[5])*256l; + if ( _model == MS5837_02BA ) { + SENS = int64_t(C[1])*65536l+(int64_t(C[3])*dT)/128l; + OFF = int64_t(C[2])*131072l+(int64_t(C[4])*dT)/64l; + P = (D1_pres*SENS/(2097152l)-OFF)/(32768l); + } else { + SENS = int64_t(C[1])*32768l+(int64_t(C[3])*dT)/256l; + OFF = int64_t(C[2])*65536l+(int64_t(C[4])*dT)/128l; + P = (D1_pres*SENS/(2097152l)-OFF)/(8192l); + } + + // Temp conversion + TEMP = 2000l+int64_t(dT)*C[6]/8388608LL; + + //Second order compensation + if ( _model == MS5837_02BA ) { + if((TEMP/100)<20){ //Low temp + Ti = (11*int64_t(dT)*int64_t(dT))/(34359738368LL); + OFFi = (31*(TEMP-2000)*(TEMP-2000))/8; + SENSi = (63*(TEMP-2000)*(TEMP-2000))/32; + } + } else { + if((TEMP/100)<20){ //Low temp + Ti = (3*int64_t(dT)*int64_t(dT))/(8589934592LL); + OFFi = (3*(TEMP-2000)*(TEMP-2000))/2; + SENSi = (5*(TEMP-2000)*(TEMP-2000))/8; + if((TEMP/100)<-15){ //Very low temp + OFFi = OFFi+7*(TEMP+1500l)*(TEMP+1500l); + SENSi = SENSi+4*(TEMP+1500l)*(TEMP+1500l); + } + } + else if((TEMP/100)>=20){ //High temp + Ti = 2*(dT*dT)/(137438953472LL); + OFFi = (1*(TEMP-2000)*(TEMP-2000))/16; + SENSi = 0; + } + } + + OFF2 = OFF-OFFi; //Calculate pressure and temp second order + SENS2 = SENS-SENSi; + + TEMP = (TEMP-Ti); + + if ( _model == MS5837_02BA ) { + P = (((D1_pres*SENS2)/2097152l-OFF2)/32768l); + } else { + P = (((D1_pres*SENS2)/2097152l-OFF2)/8192l); + } +} + +float MS5837::pressure(float conversion) { + if ( _model == MS5837_02BA ) { + return P*conversion/100.0f; + } + else { + return P*conversion/10.0f; + } +} + +float MS5837::temperature() { + return TEMP/100.0f; +} + +// The pressure sensor measures absolute pressure, so it will measure the atmospheric pressure + water pressure +// We subtract the atmospheric pressure to calculate the depth with only the water pressure +// The average atmospheric pressure of 101300 pascal is used for the calcuation, but atmospheric pressure varies +// If the atmospheric pressure is not 101300 at the time of reading, the depth reported will be offset +// In order to calculate the correct depth, the actual atmospheric pressure should be measured once in air, and +// that value should subtracted for subsequent depth calculations. +float MS5837::depth() { + return (pressure(MS5837::Pa)-101300)/(fluidDensity*9.80665); +} + +float MS5837::altitude() { + return (1-pow((pressure()/1013.25),.190284))*145366.45*.3048; +} + + +uint8_t MS5837::crc4(uint16_t n_prom[]) { + uint16_t n_rem = 0; + + n_prom[0] = ((n_prom[0]) & 0x0FFF); + n_prom[7] = 0; + + for ( uint8_t i = 0 ; i < 16; i++ ) { + if ( i%2 == 1 ) { + n_rem ^= (uint16_t)((n_prom[i>>1]) & 0x00FF); + } else { + n_rem ^= (uint16_t)(n_prom[i>>1] >> 8); + } + for ( uint8_t n_bit = 8 ; n_bit > 0 ; n_bit-- ) { + if ( n_rem & 0x8000 ) { + n_rem = (n_rem << 1) ^ 0x3000; + } else { + n_rem = (n_rem << 1); + } + } + } + + n_rem = ((n_rem >> 12) & 0x000F); + + return n_rem ^ 0x00; +} \ No newline at end of file diff --git a/AUV-4/STB/main code/STB_4.0/MS5837.h b/AUV-4/STB/main code/STB_4.0/MS5837.h new file mode 100644 index 00000000..0a910dff --- /dev/null +++ b/AUV-4/STB/main code/STB_4.0/MS5837.h @@ -0,0 +1,103 @@ +/* Blue Robotics Arduino MS5837-30BA Pressure/Temperature Sensor Library +------------------------------------------------------------ +Title: Blue Robotics Arduino MS5837-30BA Pressure/Temperature Sensor Library +Description: This library provides utilities to communicate with and to +read data from the Measurement Specialties MS5837-30BA pressure/temperature +sensor. +Authors: Rustom Jehangir, Blue Robotics Inc. + Adam Šimko, Blue Robotics Inc. +------------------------------- +The MIT License (MIT) +Copyright (c) 2015 Blue Robotics Inc. +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +-------------------------------*/ + +#ifndef MS5837_H_BLUEROBOTICS +#define MS5837_H_BLUEROBOTICS + +#include "Arduino.h" +#include + +class MS5837 { +public: + static const float Pa; + static const float bar; + static const float mbar; + + static const uint8_t MS5837_30BA; + static const uint8_t MS5837_02BA; + static const uint8_t MS5837_UNRECOGNISED; + + MS5837(); + + bool init(TwoWire &wirePort = Wire); + bool begin(TwoWire &wirePort = Wire); // Calls init() + + /** Set model of MS5837 sensor. Valid options are MS5837::MS5837_30BA (default) + * and MS5837::MS5837_02BA. + */ + void setModel(uint8_t model); + uint8_t getModel(); + + /** Provide the density of the working fluid in kg/m^3. Default is for + * seawater. Should be 997 for freshwater. + */ + void setFluidDensity(float density); + + /** The read from I2C takes up to 40 ms, so use sparingly is possible. + */ + void read(); + + /** Pressure returned in mbar or mbar*conversion rate. + */ + float pressure(float conversion = 1.0f); + + /** Temperature returned in deg C. + */ + float temperature(); + + /** Depth returned in meters (valid for operation in incompressible + * liquids only. Uses density that is set for fresh or seawater. + */ + float depth(); + + /** Altitude returned in meters (valid for operation in air only). + */ + float altitude(); + +private: + + //This stores the requested i2c port + TwoWire * _i2cPort; + + uint16_t C[8]; + uint32_t D1_pres, D2_temp; + int32_t TEMP; + int32_t P; + uint8_t _model; + + float fluidDensity; + + /** Performs calculations per the sensor data sheet for conversion and + * second order compensation. + */ + void calculate(); + + uint8_t crc4(uint16_t n_prom[]); +}; + +#endif \ No newline at end of file