diff --git a/.gitignore b/.gitignore index a5ad35f..ea56863 100644 --- a/.gitignore +++ b/.gitignore @@ -189,3 +189,4 @@ venv.bak/ # mypy .mypy_cache/ +.vscode/ diff --git a/debug.sh b/debug.sh new file mode 100755 index 0000000..86ff32d --- /dev/null +++ b/debug.sh @@ -0,0 +1,22 @@ +#!/bin/sh + +if [ $# -eq 0 ] +then + echo "Usage: sudo $0 " + exit 1 +fi + +if [ ! `ls elfs | grep "^$1\.elf$"` ] +then + echo "No module called \"$1\"" + exit 1 +fi + +openocd="sudo openocd -f board/ek-tm4c123gxl.cfg" +serial="sleep 1; picocom -b 115200 /dev/ttyACM0" +debug="sleep 2; ./flash.sh elfs/$1.elf" + +gnome-terminal --tab --command="bash -c '$openocd; $SHELL'" \ + --tab --command="bash -c '$serial; $SHELL'" \ + --tab --command="bash -c '$debug; $SHELL'" \ + --disable-factory diff --git a/src/libs/adc/CMakeLists.txt b/src/libs/adc/CMakeLists.txt new file mode 100644 index 0000000..bd2dd2d --- /dev/null +++ b/src/libs/adc/CMakeLists.txt @@ -0,0 +1,21 @@ +project(adc) +cmake_minimum_required(VERSION 3.5) + +find_package(catkin REQUIRED + COMPONENTS ti) + +catkin_package( + INCLUDE_DIRS ./ + LIBRARIES adc + CATKIN_DEPENDS ti +) + +set(BUILD_TOOLS_DIR ../../../build-tools) +include(../../../build-tools/build_env.cmake) + +add_library(adc STATIC adc.c) +target_include_directories(adc PUBLIC + ${CMAKE_INCLUDE_PATH} + ./ + ${catkin_INCLUDE_DIRS} +) diff --git a/src/libs/adc/DESIGNS b/src/libs/adc/DESIGNS new file mode 100644 index 0000000..d41c132 --- /dev/null +++ b/src/libs/adc/DESIGNS @@ -0,0 +1,12 @@ +Initialisation: + 1. Enable ADC clock if not enabled + 2. Enable GPIO clock if not enabled + 3. Set GPIO AFSEL bit ? + 4. Clear GPIO DEN bit in GPIODEN + 5. Disable analogue isolation via GPIOAMSEL + 6. Disable sample sequencer via ASENn in ADCACTSS + 7. Append new source to sequencer via ADCSSCTLn + 8. Enable sample sequencer via ASENn in ADCACTSS + +Notes + - ADC should use PIOSC for clock at 1 divider for clock diff --git a/src/libs/adc/DIFFERENTIAL_PAIR b/src/libs/adc/DIFFERENTIAL_PAIR new file mode 100644 index 0000000..efe477b --- /dev/null +++ b/src/libs/adc/DIFFERENTIAL_PAIR @@ -0,0 +1,11 @@ +Differential pair count = 4 + +When VIN+ == VIN- the output will be 0x800 + +Pair map + VIND0 <= AIN0 - AIN1 + VIND1 <= AIN2 - AIN3 + VIND2 <= AIN4 - AIN5 + VIND3 <= AIN6 - AIN7 + + diff --git a/src/libs/adc/INTERNAL_TEMP_SENS b/src/libs/adc/INTERNAL_TEMP_SENS new file mode 100644 index 0000000..ec23540 --- /dev/null +++ b/src/libs/adc/INTERNAL_TEMP_SENS @@ -0,0 +1,6 @@ +The internal temperature sensor can be accessed as part of a sample +sequence by setting the relevant TSn bit in ADCSSCTLn register. + +The sample can be converted into a temperature with: + TEMP = 147.5 - ((75 * (VREFP - VREFN) × ADC CODE ) / 4096) +Accuracy is +- 5 degrees celsius. diff --git a/src/libs/adc/PIN_DESCRIPTION b/src/libs/adc/PIN_DESCRIPTION new file mode 100644 index 0000000..de8d474 --- /dev/null +++ b/src/libs/adc/PIN_DESCRIPTION @@ -0,0 +1,33 @@ +Pin count = 9 + +Pin map: + AIN0 => PE3 + AIN1 => PE2 + AIN2 => PE1 + AIN3 => PE0 + AIN4 => PD7 + AIN5 => PD6 + AIN6 => PD5 + AIN7 => PD4 + AIN20 => PE7 + +Pin inputs: + GPIO + AIN0 + AIN1 + AIN2 + AIN3 + AIN4 + AIN5 + AIN6 + AIN7 + AIN20 + Filtered input? + AIN0 + AIN1 + AIN2 + AIN3 + Current sensor + AIN7 + Temperature sensor + AIN20 diff --git a/src/libs/adc/VREF b/src/libs/adc/VREF new file mode 100644 index 0000000..4faff56 --- /dev/null +++ b/src/libs/adc/VREF @@ -0,0 +1,2 @@ +VREFA+ => 3V from a precision voltage reference IC +VREFA- => 0V from ground diff --git a/src/libs/adc/adc.c b/src/libs/adc/adc.c new file mode 100644 index 0000000..d03403f --- /dev/null +++ b/src/libs/adc/adc.c @@ -0,0 +1,180 @@ +//------------------------------INCLUDES------------------------------// +#include "adc.h" + +//---------------------------ERROR_HANDLING---------------------------// + +//---------------------------SIZE_CONSTANTS---------------------------// +#define PC 8 /* ADC pin count */ +#define GPIO_PORT_COUNT 2 /* number of GPIO ports used by ADC */ + + +//-------------------------------ENUMS--------------------------------// +enum { + PRE_INIT, + POST_INIT +}; + +//------------------------------STRUCTS-------------------------------// +struct gpio_port { + const uint32_t sysctl; + const uint32_t base; +}; /* struct for data required for manipulating GPIO ports */ + +struct gpio_pin { + const struct gpio_port port; + const uint8_t pin; +}; + + +//---------------------SHARED_HARDWARE_CONSTANTS----------------------// +/* ADC configuration arguments */ +const uint32_t adc_clock_div = 1; +const uint32_t adc_clock_config = + ADC_CLOCK_SRC_PIOSC | ADC_CLOCK_RATE_FULL; +const uint32_t sequence_num = 0; +const uint32_t adc_sc_module = SYSCTL_PERIPH_ADC0; + + +//----------------------BOARD_SPECIFIC_CONSTANTS----------------------// +#ifdef PART_TM4C123GH6PM +const uint32_t adc_reference = ADC_REF_INT; + +const struct gpio_pin gpio_lut[PC] = { + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_3}, + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_2}, + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_1}, + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_0}, + {{SYSCTL_PERIPH_GPIOB, GPIO_PORTB_BASE}, GPIO_PIN_7}, + {{SYSCTL_PERIPH_GPIOB, GPIO_PORTB_BASE}, GPIO_PIN_6}, + {{SYSCTL_PERIPH_GPIOB, GPIO_PORTB_BASE}, GPIO_PIN_5}, + {{SYSCTL_PERIPH_GPIOB, GPIO_PORTB_BASE}, GPIO_PIN_4}}; +#endif +#ifdef PART_TM4C123GH6PGE +const uint32_t adc_reference = ADC_REF_EXT_3V; + +const struct gpio_pin gpio_lut[PC] = { + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_3}, + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_2}, + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_1}, + {{SYSCTL_PERIPH_GPIOE, GPIO_PORTE_BASE}, GPIO_PIN_0}, + {{SYSCTL_PERIPH_GPIOD, GPIO_PORTD_BASE}, GPIO_PIN_7}, + {{SYSCTL_PERIPH_GPIOD, GPIO_PORTD_BASE}, GPIO_PIN_6}, + {{SYSCTL_PERIPH_GPIOD, GPIO_PORTD_BASE}, GPIO_PIN_5}, + {{SYSCTL_PERIPH_GPIOD, GPIO_PORTD_BASE}, GPIO_PIN_4}}; +#endif + + +//------------------------------GLOBALS-------------------------------// +int status = PRE_INIT; +uint8_t active_pins = 0; +uint32_t *adc_buffer = (void *)0; +void (*adc_callback)(void) = (void *)0; + + +//--------------------------LOCAL_FUNCTIONS---------------------------// +/* If a module is not already enabled: enables it, then waits until it + * is responding as ready. */ +void init_module(uint32_t sysctl_module) { + if (SysCtlPeripheralReady(sysctl_module) == false) { + SysCtlPeripheralEnable(sysctl_module); + while (SysCtlPeripheralReady(sysctl_module) == false) + ; + } +} + + +//-------------------------EXTERNAL_FUNCTIONS-------------------------// +/* Interrupt handler that needs to be configured in a module's prx file + * if the interrupt mode is used. + */ +void adc_irq_handler(void) { + /* Clear interrupt flag so we don't instantly return to the handler */ + ADCIntClear(ADC0_BASE, sequence_num); + ADCSequenceDataGet(ADC0_BASE, sequence_num, adc_buffer); + /* call the callback */ + adc_callback(); +} + +enum adc_return adc_init_pins(enum adc_pin *pins, uint8_t num_pins, bool interrupt_mode) { +// adc_assert(status == PRE_INIT); +// adc_assert(num_pins < 9); +// adc_assert(pins != (void *)0); + + active_pins = num_pins; + + /* initialise ADC module */ + init_module(adc_sc_module); + ADCIntDisable(ADC0_BASE, sequence_num); + + /* configure ADC module */ + ADCClockConfigSet(ADC0_BASE, adc_clock_config, adc_clock_div); + ADCReferenceSet(ADC0_BASE, adc_reference); + + /* pin initialisation and configuration */ + for (int i = 0; i < num_pins; i++) { + /* initialise GPIO module */ + init_module(gpio_lut[pins[i]].port.sysctl); + /* configure ADC/GPIO pins */ + GPIOPinTypeADC(gpio_lut[pins[i]].port.base, gpio_lut[pins[i]].pin); + } + + /* disable before configuring sequencer */ + ADCSequenceDisable(ADC0_BASE, sequence_num); + + /* setup ADC sample sequencer, trigger manually in software */ + ADCSequenceConfigure(ADC0_BASE, sequence_num, + ADC_TRIGGER_PROCESSOR, 0); + + /* configure each step of sequence to be an analog input source */ + int i = 0; + for (i = 0; i < num_pins-1; i++) { + ADCSequenceStepConfigure(ADC0_BASE, sequence_num, i, pins[i]); + } + /* last step is end of sequence, generate an interrupt here */ + ADCSequenceStepConfigure(ADC0_BASE, sequence_num, i, + pins[i] | ADC_CTL_IE | ADC_CTL_END); + /* never use this function, echronos handles irq registration */ + // ADCIntRegister(ADC0_BASE, sequence_num, adc_irq_handler); + ADCSequenceEnable(ADC0_BASE, sequence_num); + /* enable interrupts if in interrupt mode */ + if (interrupt_mode) { + ADCIntEnable(ADC0_BASE, sequence_num); + IntEnable(INT_ADC0SS0); + } + /* ready for capture */ + status = POST_INIT; + return ADC_SUCCESS; +} + +enum adc_return adc_capture_interrupt(uint32_t *buffer, void (*callback)(void)) { +// adc_assert(status == POST_INIT); +// adc_assert(buffer != (void *)0); +// adc_assert(callback !=(void *)0); + adc_buffer = buffer; + adc_callback = callback; + ADCProcessorTrigger(ADC0_BASE, sequence_num); + return ADC_SUCCESS; +} + + + +uint32_t adc_capture_polling(uint32_t *buffer) { +// adc_assert(status == POST_INIT); + ADCProcessorTrigger(ADC0_BASE, sequence_num); + while (!ADCIntStatus(ADC0_BASE, sequence_num, false)) {}; + uint32_t num_samples = ADCSequenceDataGet(ADC0_BASE, sequence_num, buffer); + return num_samples; +} + + +enum adc_return adc_interrupt_disable() { + ADCIntDisable(ADC0_BASE, sequence_num); + IntDisable(INT_ADC0SS0); + return ADC_SUCCESS; +} + +enum adc_return adc_interrupt_enable() { + ADCIntEnable(ADC0_BASE, sequence_num); + IntEnable(INT_ADC0SS0); + return ADC_SUCCESS; +} diff --git a/src/libs/adc/adc.h b/src/libs/adc/adc.h new file mode 100644 index 0000000..6c04c8b --- /dev/null +++ b/src/libs/adc/adc.h @@ -0,0 +1,105 @@ +/* Author: William Miles + * Date: 2018-03-03 + * + * NB: This library provides a vastly simplified interface to the ADC + * module. If more features or greater performance is required then + * direct usage of the module through the Tiva libraries is + * recommended. + */ + +#ifndef ADC_H +#define ADC_H + +#include +#include + +#include "driverlib/adc.h" +#include "driverlib/sysctl.h" +#include "driverlib/gpio.h" +#include "inc/hw_memmap.h" +#include "inc/hw_ints.h" +#include "driverlib/pin_map.h" +#include "driverlib/debug.h" + +//------------------------------ENUMS---------------------------------// +/* ADC Pins, using ADC Pin names. See ./PIN_MAPPING for the other names + * for these pins. */ +enum adc_pin { + AIN0 = 0x0, + AIN1 = 0x1, + AIN2 = 0x2, + AIN3 = 0x3, + AIN4 = 0x4, + AIN5 = 0x5, + AIN6 = 0x6, + AIN7 = 0x7, +}; + +/* Possible return values for the ADC functions. */ +enum adc_return { + ADC_SUCCESS, + ADC_FAILURE +}; + +/* Used as a return status indicating whether an ADC capture has + * completed. */ +enum adc_status { + ADC_COMPLETE, + ADC_BUSY +}; + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------FUNCTIONS-------------------------------// +/* Must be configured in a module's PRX file if the interrupt mode is + * used. + */ +void adc_irq_handler(void); + +/* INITIALISATION */ +/* Initialises the ADC module, configures the needed GPIO pins, and + * configures,enables the needed ADC pins, and configures the ADC for interrupt + * or polling mode. + * + * This function should ONLY BE RUN ONCE during I/O setup. + * + * The maximum number of pins currently supported is 8. This function + * should fail if num_pins > 8. */ +enum adc_return adc_init_pins(enum adc_pin *pins, uint8_t num_pins, bool interrupt_mode); + +/* INTERRUPT CAPTURE INTERFACE */ +/* Sets up an ADC capture using its hardware capture interrupt. That is, + * it registers the buffer and callback such that when the capture + * completes, an interrupt occurs, the capture data is copied to the + * buffer, and the callback is called. + * + * The ADC module must be initialised in interrupt mode, or interrupts can be + * enabled manually via adc_interrupt_enable() for this to function properly. + */ +enum adc_return adc_capture_interrupt(uint32_t *buffer, + void (*callback)(void)); + +/* POLLING CAPTURE INTERFACE */ +/* Initiates an ADC capture sequence in polling mode. The status register is + * polled until a capture sequence is complete and the result is stored in the + * specified buffer. + * + * The ADC module must be intiialised in polling mode, or interrupts can be + * disbled manually via adc_interrupt_disable() for this to function properly. + * + * Returns the number of samples captured in the sequence. + */ +uint32_t adc_capture_polling(uint32_t *buffer); + +enum adc_return adc_interrupt_disable(); + +enum adc_return adc_interrupt_enable(); + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/src/libs/adc/package.xml b/src/libs/adc/package.xml new file mode 100644 index 0000000..887a787 --- /dev/null +++ b/src/libs/adc/package.xml @@ -0,0 +1,55 @@ + + + adc + 0.0.0 + ADC Library + + + + + BLUEsat UNSW + William Milles + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + ti + ti + + + + + + + + + + diff --git a/src/libs/i2c/CMakeLists.txt b/src/libs/i2c/CMakeLists.txt new file mode 100644 index 0000000..56a7470 --- /dev/null +++ b/src/libs/i2c/CMakeLists.txt @@ -0,0 +1,30 @@ +project(i2c) +cmake_minimum_required(VERSION 3.5) + +find_package(catkin REQUIRED + COMPONENTS ti) + +catkin_package( + INCLUDE_DIRS ./ + LIBRARIES i2c #science-mod + CATKIN_DEPENDS ti +) + +set(BUILD_TOOLS_DIR ../../../build-tools) +include(../../../build-tools/build_env.cmake) + +add_library(i2c STATIC i2c.c) +target_include_directories(i2c PUBLIC + ${CMAKE_INCLUDE_PATH} + ./ + ${catkin_INCLUDE_DIRS} +) + +#FILE(GLOB science-mod_files science-mod/*.cpp) +#add_library(science-mod STATIC ${science-mod_files}) +#target_include_directories(science-mod PUBLIC +# ${CMAKE_INCLUDE_PATH} +# ./ +# ${catkin_INCLUDE_DIRS} +#) +#target_link_libraries(science-mod i2c) diff --git a/src/libs/i2c/PIN_DESCRIPTION b/src/libs/i2c/PIN_DESCRIPTION new file mode 100644 index 0000000..5c55d7b --- /dev/null +++ b/src/libs/i2c/PIN_DESCRIPTION @@ -0,0 +1,9 @@ +Pin map: + I2C0SCL => PB2 + I2C0SDA => PB3 + I2C1SCL => PA6 + I2C1SDA => PA7 + I2C2SCL => PF6 + I2C2SDA => PF7 + I2C3SCL => PD0 + I2C3SDA => PD1 diff --git a/src/libs/i2c/i2c.c b/src/libs/i2c/i2c.c new file mode 100644 index 0000000..70df3fd --- /dev/null +++ b/src/libs/i2c/i2c.c @@ -0,0 +1,144 @@ +#include "i2c.h" +#include +#include +#include "inc/hw_i2c.h" +#include "inc/hw_ints.h" +#include "inc/hw_memmap.h" +#include "inc/hw_sysctl.h" +#include "inc/hw_types.h" +#include "driverlib/debug.h" +#include "driverlib/i2c.h" +#include "driverlib/interrupt.h" +#include "driverlib/sysctl.h" +#include "driverlib/gpio.h" +#include "driverlib/pin_map.h" + +struct gpio_config_struct { + uint32_t scl; + uint32_t sda; +}; + +struct gpio_pin_i2c_struct { + uint32_t base; + uint32_t scl; + uint32_t sda; +}; + +const uint32_t sysctl_module_i2c[] = {SYSCTL_PERIPH_I2C0, SYSCTL_PERIPH_I2C1, + SYSCTL_PERIPH_I2C2, SYSCTL_PERIPH_I2C3}; + +const uint32_t i2c_module[] = {I2C0_BASE, I2C1_BASE, I2C2_BASE, I2C3_BASE}; + +const struct gpio_pin_i2c_struct gpio_pin_i2c[] = { + {GPIO_PORTB_BASE, GPIO_PIN_2, GPIO_PIN_3}, {GPIO_PORTA_BASE, GPIO_PIN_6, GPIO_PIN_7}, + {GPIO_PORTE_BASE, GPIO_PIN_4, GPIO_PIN_5}, {GPIO_PORTD_BASE, GPIO_PIN_0, GPIO_PIN_1}}; + + + +const struct gpio_config_struct gpio_config[] = { + {GPIO_PB2_I2C0SCL, GPIO_PB3_I2C0SDA}, {GPIO_PA6_I2C1SCL, GPIO_PA7_I2C1SDA}, + {GPIO_PE4_I2C2SCL, GPIO_PE5_I2C2SDA}, {GPIO_PD0_I2C3SCL, GPIO_PD1_I2C3SDA}}; + +static bool initialised[4] = {false}; + +// 0 - 100 Kbps, 1 - 400 Kbps, 2 - 1 Mbps (fast mode plus) +void i2c_init(i2cModule_t module, i2cMode_t mode) { + // if this module has already been intialised, do nothing + if (initialised[module]) { + return; + } + if (SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOB) == false) { + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); + while (!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOB)); + } + + // if i2c module not enabled + if (SysCtlPeripheralReady(sysctl_module_i2c[module]) == false) { + SysCtlPeripheralEnable(sysctl_module_i2c[module]); + while (!SysCtlPeripheralReady(sysctl_module_i2c[module])); + SysCtlPeripheralReset(sysctl_module_i2c[module]); + } + + //GPIOIntRegister(i2c_module[module], i2cIntHandler) // might do interrupts later + // only module 0 defaults to the i2c function, need to configure for the other gpios + // set SCL push pull + GPIOPinTypeI2CSCL(gpio_pin_i2c[module].base, (uint8_t) gpio_pin_i2c[module].scl); + // set SDA open drain, weak pullup + GPIOPinTypeI2C(gpio_pin_i2c[module].base, (uint8_t) gpio_pin_i2c[module].sda); + // enable i2c functionality on the respective pins + GPIOPinConfigure(gpio_config[module].scl); + GPIOPinConfigure(gpio_config[module].sda); + + uint32_t i2c_clk = SysCtlClockGet(); + // initialise master + I2CMasterInitExpClk(i2c_module[module], i2c_clk, mode > 0); + if (mode == FAST_PLUS) { + uint32_t scl_freq = 1000000; + uint32_t tpr = ((i2c_clk + (2 * 10 * scl_freq) - 1) / + (2 * 10 * scl_freq)) - 1; + HWREG(i2c_module[module] + I2C_O_MTPR) = tpr; + } + initialised[module] = true; +} + +void i2c_set_slave_addr(i2cModule_t module, uint8_t slave_addr, bool read) { + // set slave device to communicate with + I2CMasterSlaveAddrSet(i2c_module[module], slave_addr, read); +} + + +int i2c_stop(i2cModule_t module) { + I2CMasterControl(i2c_module[module], I2C_CMD_STOP); + int error = I2CMasterErr(i2c_module[module]); + return error; +} + + +void i2c_write(i2cModule_t module, uint8_t data, uint32_t command) { + // put byte to transmit on bus + I2CMasterDataPut(i2c_module[module], data); + I2CMasterControl(i2c_module[module], command); + while(I2CMasterBusy(i2c_module[module])); + uint32_t err = I2CMasterErr(i2c_module[module]); + if (err & I2C_MASTER_ERR_ADDR_ACK) { + UARTprintf("ADDR ACK ERROR = %d\n", err); + } + if (err & I2C_MASTER_ERR_DATA_ACK) { + UARTprintf("DATA ACK ERROR = %d\n", err); + } +} + +int i2c_read(i2cModule_t module, uint8_t *data, uint32_t command) { + I2CMasterControl(i2c_module[module], command); + while(I2CMasterBusy(i2c_module[module])); + int error = I2CMasterErr(i2c_module[module]); + if (error) { // got an error, stop transmission + if (error & I2C_MASTER_ERR_ADDR_ACK) { + UARTprintf("READ ADDR ACK ERROR = %d\n", error); + } + *data = I2CMasterDataGet(i2c_module[module]); + i2c_stop(i2c_module[module]); + UARTprintf("read error = %d data= %d\n", error, *data); + } else { // no error, read data + *data = I2CMasterDataGet(i2c_module[module]); + } + return error; +} + +/* +void i2c_write(i2cModule_t module, uint8_t msg[], size_t length) { + if (length > 1) { // more than one byte to send + // repeated start + i2c_write_byte(i2c_module[module], msg[0], I2C_MASTER_CMD_BURST_SEND_START); + for (size_t i = 1; i < (length - 1); i++) { + // send byte, stay in transmit state + i2c_write_byte(i2c_module[module], msg[i], I2C_MASTER_CMD_BURST_SEND_CONT); + } + // send last byte then signal stop condition + i2c_write_byte(i2c_module[module], msg[length - 1], I2C_MASTER_CMD_BURST_SEND_FINISH); + } else { + // start, transmit one byte, stop + i2c_write_byte(i2c_module[module], msg[0], I2C_MASTER_CMD_SINGLE_SEND); + } + +}*/ diff --git a/src/libs/i2c/i2c.h b/src/libs/i2c/i2c.h new file mode 100644 index 0000000..8d452e6 --- /dev/null +++ b/src/libs/i2c/i2c.h @@ -0,0 +1,136 @@ +/* + * Date Started: 3/7/18 + * Original Author: [Original Author's Name] + * Editors: [Editor 1], [Editor 2] + * Purpose: I2C library + * This code is released under the MIT [GPL for embeded] License. Copyright BLUEsat UNSW, 2017 + */ + +#ifndef I2C_H +#define I2C_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +/* I2C master commands */ +#define I2C_CMD_SINGLE_SEND 0x00000007 // start -> transmit -> stop +#define I2C_CMD_SINGLE_RECEIVE 0x00000007 // start -> receive -> stop +#define I2C_CMD_STOP 0x00000004 // stop +#define I2C_CMD_SEND_START 0x00000003 // (rep) start -> transmit +#define I2C_CMD_RECEIVE_START 0x0000000b // (rep) start -> receive (ack) +#define I2C_CMD_RECEIVE_NACK_START 0x00000003 // (rep) start -> receive (nack) +/* Must currently be in transmit state for these commands to function properly */ +#define I2C_CMD_SEND_CONT 0x00000001 // transmit +#define I2C_CMD_SEND_FINISH 0x00000005 // transmit -> stop +/* Must currently be in receive state for these commands to function properly */ +#define I2C_CMD_RECEIVE_CONT 0x00000009 // receive (ack) +#define I2C_CMD_RECEIVE_NACK_CONT 0x00000001 // receive (nack) +#define I2C_CMD_RECEIVE_FINISH 0x00000005 // receive -> stop + +typedef enum { + NORMAL, + FAST, + FAST_PLUS +} i2cMode_t; + +typedef enum { + I2C0, + I2C1, + I2C2, + I2C3 +} i2cModule_t; + + +/** Initialises an I2C module with a bus speed mode. + * This must be called if the module has not been initialised yet + * + * @param module one of four possible modules: I2C0, I2C1, I2C2, I2C3 + * @param mode one of three modes defining the bus speed: + * NORMAL - 100 Kbps + * FAST - 400 Kbps + * FAST_PLUS - 1 Mbps + */ +extern void i2c_init(i2cModule_t module, i2cMode_t mode); + +/* Sets the address of the slave to communicate with, and signals + * whether the master will write or read from the slave. + * This must be called once before a new transmission sequence. + * A transmission is not started until i2c_read() or i2c_write() is run. + * + * A repeated call to this before stopping a transmission allows + * the master to communicate with a different slave and/or change the + * read/write mode while maintaining control of the bus. + * + * @param read if true, data is to be read from the slave + */ +extern void i2c_set_slave_addr(i2cModule_t module, uint8_t slave_addr, bool read); + +/* Stops a transmission initiated by i2c_write() or i2c_read(), freeing + * the bus. This must be called after communication is complete, unless + * an appropriate command sequence is used in i2c_cmd_write_byte() or + * i2c_cmd_read_byte(). + */ +extern int i2c_stop(i2cModule_t module); + +/** + * Writes a byte to the current slave with a specified send command. + * The command specifies a sequence of commands to execute automatically + * in hardware. + * + * The first call to i2c_write() must have a command that generates a start + * condition. + * + * @param command an appropriate send command sequence defined in the header + */ +extern void i2c_write(i2cModule_t module, uint8_t data, uint32_t command); + +/** + * Reads a byte from the current slave with a specified receive command. + * + * The first call to i2c_read() must have a command that generates a start + * condition. + * + * @param command an appropriate receive command sequence defined in the header + * @return int 0 if no error occurs, otherwise an error code + */ +extern int i2c_read(i2cModule_t module, uint8_t *data, uint32_t command); + + +/** + * Starts a transmission and writes an array of bytes to the slave address + * set by i2c_set_slave_addr(). + * + * @param msg array of bytes to send + * @param length number of bytes to send + */ +//void i2c_write(i2cModule_t, uint8_t msg[], size_t length); + + + +/** + * Writes a byte to the current slave with a specified send command. + * The command specifies a sequence of commands to execute automatically + * in hardware. + * + * @param command an appropriate send command sequence defined in the header + */ +//void i2c_cmd_write_byte(i2cModule_t module, uint8_t data, uint32_t command); + +/** + * Reads a byte from the current slave with a specified receive command. + * + * @param command an appropriate receive command sequence defined in the header + * @return byte read from slave + */ +//uint8_t i2c_cmd_read_byte(i2cModule_t module, uint32_t command); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/libs/i2c/package.xml b/src/libs/i2c/package.xml new file mode 100644 index 0000000..9b42f49 --- /dev/null +++ b/src/libs/i2c/package.xml @@ -0,0 +1,55 @@ + + + i2c + 0.0.0 + I2C Library + + + + + BLUEsat UNSW + Alan Nguyen, Edward Dai + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + ti + ti + + + + + + + + + + diff --git a/src/libs/i2c/science-mod/HX711.cpp b/src/libs/i2c/science-mod/HX711.cpp new file mode 100644 index 0000000..502ba52 --- /dev/null +++ b/src/libs/i2c/science-mod/HX711.cpp @@ -0,0 +1,189 @@ +#include "HX711.h" +#include "driverlib/gpio.h" +#include "driverlib/sysctl.h" + +#define SIZE_OF_BYTE 8 +#define NUM_DATA_BITS 24 + +HX711::HX711(port_t dout_port, pinNum_t dout_pin, port_t sck_port, pinNum_t sck_pin) { + DOUT_PORT = dout_port; + DOUT_PIN = dout_pin; + SCK_PORT = sck_port; + SCK_PIN = sck_pin; + scale = 1; + tare_offset = 0; +} + +void HX711::init(gain_t gain) { + enable_gpio(DOUT_PORT); + enable_gpio(SCK_PORT); + GPIOPinTypeGPIOInput(DOUT_PORT, DOUT_PIN); + GPIOPinTypeGPIOOutput(SCK_PORT, SCK_PIN); + set_gain(gain); +} + +void HX711::set_gain(gain_t gain) { + switch(gain) { + case CHANNEL_A_128: + GAIN_CYCLES = 1; + break; + case CHANNEL_A_64: + GAIN_CYCLES = 2; + break; + case CHANNEL_B_32: + GAIN_CYCLES = 3; + break; + } + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); + read(); // update gain conversion +} + + +bool HX711::is_ready(void) { + // if high, data is not ready for retrieval + return (GPIOPinRead(DOUT_PORT, DOUT_PIN) == 0); +} + +int32_t HX711::read(void) { + while(!is_ready()); // poll until the device is ready + /* + uint8_t data[3] = {0}; + uint8_t filler = 0x00; + + // pulse the clock pin 24 times to read the data + data[2] = read_byte(); + data[1] = read_byte();; + data[0] = read_byte();; + + for (unsigned int i = 0; i < GAIN_CYCLES; i++) { + GPIOPinWrite(SCK_PORT, SCK_PIN, SCK_PIN); + SysCtlDelay(25); // approximately 1us delay + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); + SysCtlDelay(25); + }*/ + + int32_t value = 0; + // read each bit by toggling the clock for a total of 24 bits + for (uint8_t i = 0; i < NUM_DATA_BITS; i++) { + GPIOPinWrite(SCK_PORT, SCK_PIN, SCK_PIN); + SysCtlDelay(25); // approximately 1us delay + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); + value <<= 1; + if (GPIOPinRead(DOUT_PORT, DOUT_PIN)) { + value++; // increment if pin high + } + SysCtlDelay(25); + } + + // set the channel and the gain factor for the next conversion + for (uint8_t i = 0; i < GAIN_CYCLES; i++) { + GPIOPinWrite(SCK_PORT, SCK_PIN, SCK_PIN); + SysCtlDelay(25); //( approximately 1us delay + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); + SysCtlDelay(25); + } + + if (value & 0x800000) { // if msb is set, then it is negative so extend sign + value |= (0xFF << NUM_DATA_BITS); + } + + return value; +} + +int32_t HX711::read_avg(uint8_t num_samples) { + int32_t sum = 0; + for (uint8_t i = 0; i < num_samples; i++) { + sum += read(); + } + return sum / num_samples; +} + +double HX711::read_scaled(void) { + return (double) (read() - tare_offset) / scale; +} + +double HX711::read_scaled_avg(uint8_t num_samples) { + return (double) (read_avg(num_samples) - tare_offset) / scale; +} + +void HX711::tare(uint8_t num_samples) { + tare_offset = read_avg(num_samples); +} + + +int32_t HX711::get_tare_offset(void) { + return tare_offset; +} + +void HX711::set_tare_offset(int32_t offset) { + tare_offset = offset; +} + +float HX711::get_scale(void) { + return scale; +} + +void HX711::set_scale(float scaling_factor) { + scale = scaling_factor; +} + +/* PD_SCK high time min 0.2 typ 1 max 50 µs + PD_SCK low time min 0.2 typ 1 µs */ +/* Clock speed = 80 MHz */ +/* +uint8_t HX711::read_byte(void) { + uint8_t result = 0; + uint8_t value; + // read byte one bit at a time, msb first + for (int i = 0; i < SIZE_OF_BYTE; i++) { + // toggle next bit + GPIOPinWrite(SCK_PORT, SCK_PIN, SCK_PIN); + SysCtlDelay(25); // approximately 1us delay + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); + value = GPIOPinRead(DOUT_PORT, DOUT_PIN); + result |= (value << (SIZE_OF_BYTE-1-i)); + SysCtlDelay(25); + } + return result; +}*/ + +void HX711::power_down(void) { + // transition SCK from low to high to power down + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); + GPIOPinWrite(SCK_PORT, SCK_PIN, SCK_PIN); + // gain defaults to ch A 128 after power down + set_gain(CHANNEL_A_128); +} + +void HX711::power_up(void) { + GPIOPinWrite(SCK_PORT, SCK_PIN, 0); +} + +void HX711::enable_gpio(port_t port) { + switch(port) { + case PORTA: + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); + while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOA)) {}; + break; + case PORTB: + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); + while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOB)) {}; + break; + case PORTC: + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); + while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOC)) {}; + break; + case PORTD: + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); + while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOD)) {}; + break; + case PORTE: + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); + while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOE)) {}; + break; + case PORTF: + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); + while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOF)) {}; + break; + } +} diff --git a/src/libs/i2c/science-mod/HX711.h b/src/libs/i2c/science-mod/HX711.h new file mode 100644 index 0000000..311a1f8 --- /dev/null +++ b/src/libs/i2c/science-mod/HX711.h @@ -0,0 +1,90 @@ +#ifndef HX711_H +#define HX711_H + +/** + * This device is not i2c compliant so it should not share a bus network + * with other devices + */ + +#include +#include + +enum port_t { + /* TM4C123GH6PM only supports up to PORTF */ + PORTA = 0x40004000, // GPIO Port A + PORTB = 0x40005000, // GPIO Port B + PORTC = 0x40006000, // GPIO Port C + PORTD = 0x40007000, // GPIO Port D + PORTE = 0x40024000, // GPIO Port E + PORTF = 0x40025000, // GPIO PORT F + /* + #ifdef PART_TM4C123GH6PGE + PORTG = 0x40026000, // GPIO Port G + PORTH = 0x40027000, // GPIO Port H + PORTJ = 0x4003D000, // GPIO Port J + PORTK = 0x40061000, // GPIO Port K + PORTL = 0x40062000, // GPIO Port L + PORTM = 0x40063000, // GPIO Port M + PORTN = 0x40064000, // GPIO Port N + PORTP = 0x40065000 // GPIO Port P + #endif*/ +}; + +enum pinNum_t { + PIN_0 = 0x00000001, // GPIO pin 0 + PIN_1 = 0x00000002, // GPIO pin 1 + PIN_2 = 0x00000004, // GPIO pin 2 + PIN_3 = 0x00000008, // GPIO pin 3 + PIN_4 = 0x00000010, // GPIO pin 4 + PIN_5 = 0x00000020, // GPIO pin 5 + PIN_6 = 0x00000040, // GPIO pin 6 + PIN_7 = 0x00000080 // GPIO pin 7 +}; + +class HX711 { + public: + enum gain_t { + CHANNEL_A_128, // channel A with a gain of 128 + CHANNEL_A_64, // channel A with a gain of 64 + CHANNEL_B_32 // channel B has fixed gain of 32 + }; + + /** + * Constructor takes in the ports and pins connected to DOUT and PD_SCK + * of the device. + */ + HX711(port_t dout_port, pinNum_t dout_pin, port_t sck_port, pinNum_t sck_pin); + /* Initialises the SCK and DOUT pins, gain defaults to the maximum 128 */ + void init(gain_t gain = CHANNEL_A_128); + void set_gain(gain_t gain); + /* Calibrate tare weight based on number of samples */ + void tare(uint8_t num_samples = 30); + /* Set tare value manually */ + void set_tare_offset(int32_t offset); + int32_t get_tare_offset(void); + + /* Read raw A/D value */ + int32_t read(void); + int32_t read_avg(uint8_t num_samples); + /* Read tare calibrated and scaled A/D value */ + double read_scaled(void); + double read_scaled_avg(uint8_t num_samples); + /* Set the scaling factor for the read_scaled() function */ + void set_scale(float scaling_factor); + float get_scale(void); + + void power_up(void); + void power_down(void); + private: + bool is_ready(void); + void enable_gpio(port_t port); + port_t DOUT_PORT; + uint32_t DOUT_PIN; + port_t SCK_PORT; + uint32_t SCK_PIN; + uint8_t GAIN_CYCLES; + int32_t tare_offset; + float scale; +}; + +#endif diff --git a/src/libs/i2c/science-mod/LIS3MDL.cpp b/src/libs/i2c/science-mod/LIS3MDL.cpp new file mode 100644 index 0000000..f3fd80b --- /dev/null +++ b/src/libs/i2c/science-mod/LIS3MDL.cpp @@ -0,0 +1,138 @@ +#include "LIS3MDL.h" + +#define LIS3MDL_HIGH_ADDR 0x1E +#define LIS3MDL_LOW_ADDR 0x1C + +#define TEST_ADDR_ERROR -1 +#define LIS3MDL_WHO_ID 0x3D + + +#define LIS3MDL_SENSITIVITY_SCALE_4G 6842 /* Sensitivity for 4 gauss scale [LSB/gauss] */ +#define LIS3MDL_SENSITIVITY_SCALE_8G 3421 /* Sensitivity for 8 gauss scale [LSB/gauss] */ +#define LIS3MDL_SENSITIVITY_SCALE_12G 2281 /* Sensitivity for 12 gauss scale [LSB/gauss] */ +#define LIS3MDL_SENSITIVITY_SCALE_16G 1711 /* Sensitivity for 16 gauss scale [LSB/gauss] */ + +LIS3MDL::LIS3MDL(i2cModule_t i2c_module, slaveAddrState_t slave_state) { + module = i2c_module; + switch (slave_state) { + case SLAVE_LOW: + lis3mdl_addr = LIS3MDL_LOW_ADDR; + case SLAVE_HIGH: + lis3mdl_addr = LIS3MDL_HIGH_ADDR; + default: + lis3mdl_addr = SLAVE_AUTO; + } + sensitivity = 0; +} + +bool LIS3MDL::init(void) { + i2c_init(module, FAST); + if (lis3mdl_addr == SLAVE_AUTO) { + if (test_device_addr(LIS3MDL_HIGH_ADDR) == LIS3MDL_WHO_ID) { + lis3mdl_addr = LIS3MDL_HIGH_ADDR; + } else if (test_device_addr(LIS3MDL_LOW_ADDR) == LIS3MDL_WHO_ID) { + lis3mdl_addr = LIS3MDL_LOW_ADDR; + } + } + // couldn't determine slave address, failed to initialise + if (lis3mdl_addr == SLAVE_AUTO) { + return false; + } + enable_default(); + return true; +} + +void LIS3MDL::enable_default(void) { + // 0x70 = 0b01110000 + // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR) + write_register(CTRL_REG1, 0x70); + + // 0x00 = 0b00000000 + // FS = 00 (+/- 4 gauss full scale) + write_register(CTRL_REG2, 0x00); + sensitivity = (float) LIS3MDL_SENSITIVITY_SCALE_4G; + + // 0x00 = 0b00000000 + // MD = 00 (continuous-conversion mode) + write_register(CTRL_REG3, 0x00); + + // 0x0C = 0b00001100 + // OMZ = 11 (ultra-high-performance mode for Z) + write_register(CTRL_REG4, 0x0C); +} + +void LIS3MDL::read_raw_magnetism(int16_t *x, int16_t *y, int16_t *z) { + i2c_set_slave_addr(module, lis3mdl_addr, false); + i2c_write(module, OUT_X_L | 0x80, I2C_CMD_SEND_START); + uint8_t xlm, xhm, ylm, yhm, zlm, zhm; + i2c_set_slave_addr(module, lis3mdl_addr, true); + i2c_read(module, &xlm, I2C_CMD_RECEIVE_START); + i2c_read(module, &xhm, I2C_CMD_RECEIVE_CONT); + i2c_read(module, &ylm, I2C_CMD_RECEIVE_CONT); + i2c_read(module, &yhm, I2C_CMD_RECEIVE_CONT); + i2c_read(module, &zlm, I2C_CMD_RECEIVE_CONT); + i2c_read(module, &zhm, I2C_CMD_RECEIVE_FINISH); + *x = (int16_t) (xhm << 8 | xlm); + *y = (int16_t) (yhm << 8 | ylm); + *z = (int16_t) (zhm << 8 | zlm); +} + +void LIS3MDL::read_magnetism(float *x, float *y, float *z) { + int16_t xr, yr, zr; + read_raw_magnetism(&xr, &yr, &zr); + *x = xr / sensitivity; + *y = yr / sensitivity; + *z = zr / sensitivity; +} + +void LIS3MDL::set_scale(lis3mdlScale_t scale) { + write_register(CTRL_REG2, scale); + switch (scale) { + case SCALE_4G: + sensitivity = LIS3MDL_SENSITIVITY_SCALE_4G; + break; + case SCALE_8G: + sensitivity = LIS3MDL_SENSITIVITY_SCALE_8G; + break; + case SCALE_12G: + sensitivity = LIS3MDL_SENSITIVITY_SCALE_12G; + break; + case SCALE_16G: + sensitivity = LIS3MDL_SENSITIVITY_SCALE_16G; + break; + } +} + + +void LIS3MDL::write_register(lis3mdlReg_t reg, uint8_t data) { + i2c_set_slave_addr(module, lis3mdl_addr, false); + i2c_write(module, reg, I2C_CMD_SEND_START); + i2c_write(module, data, I2C_CMD_SEND_FINISH); +} + +uint8_t LIS3MDL::read_register(lis3mdlReg_t reg) { + i2c_set_slave_addr(module, lis3mdl_addr, false); + i2c_write(module, reg, I2C_CMD_SEND_START); + i2c_set_slave_addr(module, lis3mdl_addr, true); + uint8_t data = 0; + // read then nack + i2c_read(module, &data, I2C_CMD_RECEIVE_NACK_START); + i2c_stop(module); + return data; +} + + +uint8_t LIS3MDL::test_device_addr(uint8_t addr) { + i2c_set_slave_addr(module, addr, false); + i2c_write(module, WHO_AM_I, I2C_CMD_SEND_START); + if (i2c_stop(module) != 0) { + return TEST_ADDR_ERROR; + } + i2c_set_slave_addr(module, addr, true); + uint8_t data = 0; + if (i2c_read(module, &data, I2C_CMD_SINGLE_RECEIVE)) { + return TEST_ADDR_ERROR; + } else { + return data; + } +} diff --git a/src/libs/i2c/science-mod/LIS3MDL.h b/src/libs/i2c/science-mod/LIS3MDL.h new file mode 100644 index 0000000..42f4cf7 --- /dev/null +++ b/src/libs/i2c/science-mod/LIS3MDL.h @@ -0,0 +1,88 @@ +/* + * Date Started: 6/7/18 + * Original Author: [Original Author's Name] + * Editors: [Editor 1], [Editor 2] + * Purpose: Library for LIS3MDL magnetometer + * Ported from Polulu's implementation with some slight modifications + * https://github.com/pololu/lis3mdl-arduino + * This code is released under the MIT [GPL for embeded] License. Copyright BLUEsat UNSW, 2017 + */ + +#ifndef LIS3MDL_H +#define LIS3MDL_H + +#include "i2c.h" + +class LIS3MDL { + public: + /* slave address is modified by the SDO/SA1 pin */ + enum slaveAddrState_t { + SLAVE_LOW = 0, // SDO/SA1 connected to ground + SLAVE_HIGH = 1, // SDO/SA1 connected to supply + SLAVE_AUTO = 2 // automatically detect the state of SDO/SA1 + }; + + enum lis3mdlReg_t { + WHO_AM_I = 0x0F, + CTRL_REG1 = 0x20, + CTRL_REG2 = 0x21, + CTRL_REG3 = 0x22, + CTRL_REG4 = 0x23, + CTRL_REG5 = 0x24, + STATUS_REG = 0x27, + OUT_X_L = 0x28, + OUT_X_H = 0x29, + OUT_Y_L = 0x2A, + OUT_Y_H = 0x2B, + OUT_Z_L = 0x2C, + OUT_Z_H = 0x2D, + TEMP_OUT_L = 0x2E, + TEMP_OUT_H = 0x2F, + INT_CFG = 0x30, + INT_SRC = 0x31, + INT_THS_L = 0x32, + INT_THS_H = 0x33 + }; + + enum lis3mdlScale_t { + SCALE_4G = 0x00, /* +-4 gauss full scale */ + SCALE_8G = 0x20, /* +-8 gauss full scale */ + SCALE_12G = 0x40, /* +-12 gauss full scale */ + SCALE_16G = 0x60 /* +-16 gauss full scale */ + }; + + /** + * Constructor that takes in the I2C module connected to the device and + * optionally the state of the SD0/SA1 pin. + * + * The state determines the I2C slave address. If not specified, the + * address will be detected automatically. + */ + LIS3MDL(i2cModule_t i2c_module, slaveAddrState_t = SLAVE_AUTO); + + /* Initialises the I2C module and finds the device address if necessary.*/ + bool init(void); + + /* Powers up the magnetometer with default settings */ + void enable_default(void); + + /* Reads the magnetism scaled by the sensitivity in the x,y,z directions */ + void read_magnetism(float *x, float *y, float *z); + + /* Reads the raw magnetism in the x,y,z directions */ + void read_raw_magnetism(int16_t *x, int16_t *y, int16_t *z); + + /* Sets the full range scale */ + void set_scale(lis3mdlScale_t scale); + + void write_register(lis3mdlReg_t reg, uint8_t data); + uint8_t read_register(lis3mdlReg_t reg); + + private: + uint8_t test_device_addr(uint8_t addr); + float sensitivity; + i2cModule_t module; + uint8_t lis3mdl_addr; +}; + +#endif diff --git a/src/libs/i2c/science-mod/SI7021.cpp b/src/libs/i2c/science-mod/SI7021.cpp new file mode 100644 index 0000000..e04b1e2 --- /dev/null +++ b/src/libs/i2c/science-mod/SI7021.cpp @@ -0,0 +1,136 @@ +#include "SI7021.h" + +#define SI7021_ADDR 0x40 +#define HUMID_MEAS_HOLD_CMD 0xE5 +#define HUMID_MEAS_NOHOLD_CMD 0xF5 +#define TEMP_MEAS_HOLD_CMD 0xE3 +#define TEMP_MEAS_NOHOLD_CMD 0xF3 +#define SI7021_RESET_CMD 0xFE +#define SI7021_ID1_CMD 0xFA0F +#define SI7021_ID2_CMD 0xFCC9 +#define NUM_READS_ID 4 +#define USER_REG_1_READ_CMD 0xE7 +#define USER_REG_1_WRITE_CMD 0xE6 +#define HEATER_CTRL_REG_READ_CMD 0x51 +#define HEATER_CTRL_REG_WRITE_CMD 0x11 +#define FIRMWARE_REV_CMD 0x84B8 + +SI7021::SI7021(i2cModule_t i2c_module) { + module = i2c_module; +} + +bool SI7021::init(void) { + i2c_init(module, FAST); + uint32_t ser_lo = read_serial32(SI7021_ID2_CMD); + if ((ser_lo >> 24) == 0x15) { // SI7021 msb is 0x15 + return true; + } else { + return false; + } +} + +uint32_t SI7021::read_temperature(void) { + i2c_set_slave_addr(module, SI7021_ADDR, false); + // perform temperature measurement + i2c_write(module, TEMP_MEAS_HOLD_CMD, I2C_CMD_SEND_START); + i2c_set_slave_addr(module, SI7021_ADDR, true); + uint8_t msb; + uint8_t lsb; + //uint8_t checksum; + // request temperature measurement + i2c_read(module, &msb, I2C_CMD_RECEIVE_START); // read msb + // read lsb, nack and stop transmission + i2c_read(module, &lsb, I2C_CMD_RECEIVE_FINISH); + //i2c_read(module, &checksum, I2C_CMD_RECEIVE_FINISH); // optional checksum + uint16_t temp16 = (msb << 8) | lsb; + float temp = 175.72 * (float) temp16 / 65536 - 46.85; + return (uint32_t) temp; +} + +uint32_t SI7021::read_humidity(void) { + i2c_set_slave_addr(module, SI7021_ADDR, false); + // perform temperature measurement + i2c_write(module, HUMID_MEAS_HOLD_CMD, I2C_CMD_SEND_START); + i2c_set_slave_addr(module, SI7021_ADDR, true); + uint8_t msb; + uint8_t lsb; + //uint8_t checksum; + // request temperature measurement + i2c_read(module, &msb, I2C_CMD_RECEIVE_START); + i2c_read(module, &lsb, I2C_CMD_RECEIVE_FINISH); + uint16_t hum16 = (msb << 8) | lsb; + float humidity = 125 * (float) hum16 / 65536 - 6; + return (uint32_t) humidity; +} + +void SI7021::reset(void) { + i2c_set_slave_addr(module, SI7021_ADDR, false); + i2c_write(module, SI7021_RESET_CMD, I2C_CMD_SINGLE_SEND); +} + +void SI7021::read_serial_number(uint32_t *ser_hi, uint32_t *ser_lo) { + *ser_hi = read_serial32(SI7021_ID1_CMD); + *ser_lo = read_serial32(SI7021_ID2_CMD); +} + +uint32_t SI7021::read_serial32(uint32_t cmd) { + i2c_set_slave_addr(module, SI7021_ADDR, false); + i2c_write(module, cmd >> 8, I2C_CMD_SEND_START); + i2c_write(module, cmd & 0xFF, I2C_CMD_SEND_CONT); + i2c_set_slave_addr(module, SI7021_ADDR, true); + uint32_t ser; + uint8_t tmp; + uint8_t checksum; + // start reading first upper byte + i2c_read(module, &tmp, I2C_CMD_RECEIVE_START); + ser = tmp; + // loop and read the remaining upper bytes + for (int i = 1; i < NUM_READS_ID; i++) { + i2c_read(module, &checksum, I2C_CMD_RECEIVE_CONT); + i2c_read(module, &tmp, I2C_CMD_RECEIVE_CONT); + ser = (ser << 8 | tmp); + } + // nack last byte + i2c_read(module, &checksum, I2C_CMD_RECEIVE_FINISH); + return ser; +} + +uint8_t SI7021::read_firmware_revision(void) { + i2c_set_slave_addr(module, SI7021_ADDR, false); + i2c_write(module, FIRMWARE_REV_CMD >> 8, I2C_CMD_SEND_START); + i2c_write(module, FIRMWARE_REV_CMD & 0xFF, I2C_CMD_SEND_CONT); + i2c_set_slave_addr(module, SI7021_ADDR, true); + uint8_t firmware; + i2c_read(module, &firmware, I2C_CMD_RECEIVE_NACK_START); + i2c_stop(module); + return firmware; +} + +void SI7021::write_register(controlReg_t reg, uint8_t data) { + uint8_t cmd; + if (reg == USER_REG_1) { + cmd = USER_REG_1_WRITE_CMD; + } else if (reg == HEATER_CTRL_REG) { + cmd = HEATER_CTRL_REG_WRITE_CMD; + } + i2c_set_slave_addr(module, SI7021_ADDR, false); + i2c_write(module, cmd, I2C_CMD_SEND_START); // write cmd + i2c_write(module, data, I2C_CMD_SEND_FINISH); // write to reg +} + +uint8_t SI7021::read_register(controlReg_t reg) { + uint8_t cmd; + if (reg == USER_REG_1) { + cmd = USER_REG_1_READ_CMD; + } else if (reg == HEATER_CTRL_REG) { + cmd = HEATER_CTRL_REG_READ_CMD; + } + i2c_set_slave_addr(module, SI7021_ADDR, false); + i2c_write(module, cmd, I2C_CMD_SEND_START); // read cmd + i2c_set_slave_addr(module, SI7021_ADDR, true); // switch to read + uint8_t data; + // read then nack + i2c_read(module, &data, I2C_CMD_RECEIVE_NACK_START); + i2c_stop(module); + return data; +} diff --git a/src/libs/i2c/science-mod/SI7021.h b/src/libs/i2c/science-mod/SI7021.h new file mode 100644 index 0000000..41b48e0 --- /dev/null +++ b/src/libs/i2c/science-mod/SI7021.h @@ -0,0 +1,40 @@ +/* + * Date Started: 6/7/18 + * Original Author: [Original Author's Name] + * Editors: [Editor 1], [Editor 2] + * Purpose: Library for SI7021 temperature/humidity sensor + * Ported from Adafruit's implementation with some slight modifications + * https://github.com/adafruit/Adafruit_Si7021 + * This code is released under the MIT [GPL for embeded] License. Copyright BLUEsat UNSW, 2017 + */ + +#ifndef SI7021_H +#define SI7021_H + +#include "i2c.h" + +class SI7021 { + public: + /* Control registers to set the configuration of SI7021 */ + enum controlReg_t { + USER_REG_1 = 0, + HEATER_CTRL_REG = 1 // used to modify the heater current + }; + + SI7021(i2cModule_t); + bool init(void); + uint32_t read_temperature(void); + uint32_t read_humidity(void); + void write_register(controlReg_t reg, uint8_t data); + uint8_t read_register(controlReg_t reg); + void reset(void); + void read_serial_number(uint32_t *ser_hi, uint32_t *ser_lo); + uint8_t read_firmware_revision(void); + + private: + uint32_t read_serial32(uint32_t cmd); + + i2cModule_t module; +}; + +#endif diff --git a/src/libs/i2c/science-mod/TCA9548A.cpp b/src/libs/i2c/science-mod/TCA9548A.cpp new file mode 100644 index 0000000..9219299 --- /dev/null +++ b/src/libs/i2c/science-mod/TCA9548A.cpp @@ -0,0 +1,9 @@ +#include "TCA9548A.h" + +#define TCA_ADDRESS 0x70 + +void i2c_select(i2cModule_t module, uint8_t num) { + i2c_set_slave_addr(module, TCA_ADDRESS, false); + i2c_write(module, 1 << num, I2C_CMD_SINGLE_SEND); +} + diff --git a/src/libs/i2c/science-mod/TCA9548A.h b/src/libs/i2c/science-mod/TCA9548A.h new file mode 100644 index 0000000..f9e9981 --- /dev/null +++ b/src/libs/i2c/science-mod/TCA9548A.h @@ -0,0 +1,9 @@ +#ifndef TCA9548A_H +#define TCA9548A_H + +#include "i2c.h" + +void i2c_select(i2cModule_t module, uint8_t num); + + +#endif diff --git a/src/libs/i2c/science-mod/TCS34725.cpp b/src/libs/i2c/science-mod/TCS34725.cpp new file mode 100644 index 0000000..52d8fa0 --- /dev/null +++ b/src/libs/i2c/science-mod/TCS34725.cpp @@ -0,0 +1,229 @@ +//#include +#include "TCS34725.h" +#include "driverlib/sysctl.h" + +#define TCS34725_ADDRESS (0x29) +#define TCS34725_COMMAND_BIT (0x80) + +#define TCS34725_ENABLE (0x00) +#define TCS34725_ENABLE_AIEN (0x10) /* RGBC Interrupt Enable */ +#define TCS34725_ENABLE_WEN (0x08) /* Wait enable */ +#define TCS34725_ENABLE_AEN (0x02) /* RGBC Enable */ +#define TCS34725_ENABLE_PON (0x01) /* Power on */ + +#define TCS34725_WTIME (0x03) /* Wait time (if WEN is set) */ +#define TCS34725_CONFIG (0x0D) +#define TCS34725_CONFIG_WLONG (0x02) /* Multiply wait times by 12x */ + +#define TCS34725_CDATA (0x14) /* Clear channel data */ +#define TCS34725_CDATAH (0x15) +#define TCS34725_RDATA (0x16) /* Red channel data */ +#define TCS34725_RDATAH (0x17) +#define TCS34725_GDATA (0x18) /* Green channel data */ +#define TCS34725_GDATAH (0x19) +#define TCS34725_BDATA (0x1A) /* Blue channel data */ +#define TCS34725_BDATAH (0x1B) +#define TCS34725_AILTL (0x04) /* Clear channel lower threshold */ +#define TCS34725_AILTH (0x05) +#define TCS34725_AIHTL (0x06) /* Clear channel upper threshold */ +#define TCS34725_AIHTH (0x07) + +#define TCS34725_PERS (0x0C) /* Persistence register */ + +#define TCS34725_STATUS (0x13) +#define TCS34725_STATUS_AINT (0x10) /* Clear channel interrupt state */ +#define TCS34725_STATUS_AVALID (0x01) /* Integration cycle complete */ + +#define TCS34725_ID 0x12 + +namespace { + static void delay(uint32_t ms) { + for (uint32_t i=0; i < ms; i++) { + SysCtlDelay(25000); // 1 ms delay + } + } +} + +/* +float powf(const float x, const float y) { + return (float)(pow((double)x, (double)y)); +}*/ + +/* Constructor */ +TCS34725::TCS34725(i2cModule_t i2c_module, integrationTime_t it, gain_t gain) { + module = i2c_module; + tcs34725_integration_time = it; + tcs34725_gain = gain; + interrupt_enabled = false; +} + + +bool TCS34725::init(void) { + i2c_init(module, FAST); + uint8_t id = read8(TCS34725_ID); + if (id != 0x44) { + return false; + } + set_integration_time(tcs34725_integration_time); + set_gain(tcs34725_gain); + power_on(); + return true; +} + +void TCS34725::set_integration_time(integrationTime_t it) { + write8(TCS34725_ATIME, it); // update timing register + tcs34725_integration_time = it; // update value placeholders +} + + +void TCS34725::set_gain(gain_t gain) { + write8(TCS34725_CONTROL, gain); + tcs34725_gain = gain; +} + +void TCS34725::read_raw_data(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c) { + // if interrupts are not enabled, we have to poll the status register + // until the integration cycle is complete + if (!interrupt_enabled) { + uint8_t status = 0; + while (!(status & TCS34725_STATUS_AVALID)) { + status = read8(TCS34725_STATUS); + } + + } + *c = read16(TCS34725_CDATA); + *r = read16(TCS34725_RDATA); + *g = read16(TCS34725_GDATA); + *b = read16(TCS34725_BDATA); +} + +uint16_t TCS34725::calculate_colour_temperature(uint16_t r, uint16_t g, uint16_t b) { + float X, Y, Z; /* RGB to XYZ correlation */ + float xc, yc; /* Chromaticity co-ordinates */ + double n; /* McCamy's formula */ + float cct; + + /* 1. Map RGB values to their XYZ counterparts. */ + /* Based on 6500K fluorescent, 3000K fluorescent */ + /* and 60W incandescent values for a wide range. */ + /* Note: Y = Illuminance or lux */ + X = (-0.14282F * r) + (1.54924F * g) + (-0.95641F * b); + Y = (-0.32466F * r) + (1.57837F * g) + (-0.73191F * b); + Z = (-0.68202F * r) + (0.77073F * g) + ( 0.56332F * b); + + /* 2. Calculate the chromaticity co-ordinates */ + xc = (X) / (X + Y + Z); + yc = (Y) / (X + Y + Z); + + /* 3. Use McCamy's formula to determine the CCT */ + n = (xc - 0.3320F) / (0.1858F - yc); + + /* Calculate the final CCT */ + //cct = (449.0F * powf(n, 3)) + (3525.0F * powf(n, 2)) + (6823.3F * n) + 5520.33F; + cct = (449.0F * n*n*n) + (3525.0F * n*n) + (6823.3F * n) + 5520.33F; + /* Return the results in degrees Kelvin */ + return (uint16_t) cct; +} + +uint16_t TCS34725::calculate_lux(uint16_t r, uint16_t g, uint16_t b){ + float illuminance; + + /* This only uses RGB ... how can we integrate clear or calculate lux */ + /* based exclusively on clear since this might be more reliable? */ + illuminance = (-0.32466F * r) + (1.57837F * g) + (-0.73191F * b); + return (uint16_t) illuminance; +} + +void TCS34725::set_wait_time(waitTime_t wt, bool wlong) { + if (wlong) { + write8(TCS34725_CONFIG, TCS34725_CONFIG_WLONG); + } else { + write8(TCS34725_CONFIG, 0x00); + } + write8(TCS34725_WTIME, wt); +} + +void TCS34725::enable_wait(void) { + // read current enable register settings and set the wait enable bit + uint8_t state = read8(TCS34725_ENABLE); + write8(TCS34725_ENABLE, state | TCS34725_ENABLE_WEN); +} + +void TCS34725::disable_wait(void) { + uint8_t state = read8(TCS34725_ENABLE); + // unset the wait enable bit + write8(TCS34725_ENABLE, state & ~TCS34725_ENABLE_WEN); +} + +void TCS34725::power_on(void) { + write8(TCS34725_ENABLE, TCS34725_ENABLE_PON); + // delay 3 ms + delay(3); + write8(TCS34725_ENABLE, TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN); +} + +void TCS34725::power_down(void) { + // power down device + uint8_t reg = 0; + reg = read8(TCS34725_ENABLE); + write8(TCS34725_ENABLE, reg & ~(TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN)); +} + +void TCS34725::write8(uint8_t reg, uint8_t data) { + i2c_set_slave_addr(module, TCS34725_ADDRESS, false); + i2c_write(module, TCS34725_COMMAND_BIT | reg, I2C_CMD_SEND_START); + i2c_write(module, data, I2C_CMD_SEND_FINISH); +} + +uint8_t TCS34725::read8(uint8_t reg) { + i2c_set_slave_addr(module, TCS34725_ADDRESS, false); + i2c_write(module, TCS34725_COMMAND_BIT | reg, I2C_CMD_SEND_START); + i2c_set_slave_addr(module, TCS34725_ADDRESS, true); + uint8_t data = 0; + i2c_read(module, &data, I2C_CMD_RECEIVE_NACK_START); + i2c_stop(module); + return data; +} + +uint16_t TCS34725::read16(uint8_t reg) { + i2c_set_slave_addr(module, TCS34725_ADDRESS, false); + i2c_write(module, TCS34725_COMMAND_BIT | reg, I2C_CMD_SEND_START); + i2c_set_slave_addr(module, TCS34725_ADDRESS, true); + uint16_t data = 0; + uint8_t lsb = 0; + uint8_t msb = 0; + // lsb is read first + i2c_read(module, &lsb, I2C_CMD_RECEIVE_START); + i2c_read(module, &msb, I2C_CMD_RECEIVE_FINISH); + data = (msb << 8) | lsb; + return data; +} + + +void TCS34725::set_interrupt(bool enable) { + uint8_t r = read8(TCS34725_ENABLE); + if (enable) { + r |= TCS34725_ENABLE_AIEN; + } else { + r &= ~TCS34725_ENABLE_AIEN; + } + write8(TCS34725_ENABLE, r); + interrupt_enabled = enable; +} + + +void TCS34725::clear_interrupt(void) { + i2c_set_slave_addr(module, TCS34725_ADDRESS, false); + i2c_write(module, TCS34725_COMMAND_BIT | 0x66, I2C_CMD_SINGLE_SEND); +} + +void TCS34725::set_interrupt_limits(uint16_t low, uint16_t high) { + write8(TCS34725_AILTL, low & 0xFF); + write8(TCS34725_AILTH, low >> 8); + write8(TCS34725_AIHTL, high & 0xFF); + write8(TCS34725_AIHTH, high >> 8); +} + +void TCS34725::set_interrupt_persistence(uint8_t value) { + write8(TCS34725_PERS, value); +} diff --git a/src/libs/i2c/science-mod/TCS34725.h b/src/libs/i2c/science-mod/TCS34725.h new file mode 100644 index 0000000..9dee09c --- /dev/null +++ b/src/libs/i2c/science-mod/TCS34725.h @@ -0,0 +1,123 @@ +/* + * Date Started: 6/7/18 + * Original Author: [Original Author's Name] + * Editors: [Editor 1], [Editor 2] + * Purpose: Library for TCS34725 RGB colour sensor + * Ported from Adafruit's implementation with some slight modifications + * https://github.com/adafruit/Adafruit_TCS34725 + * This code is released under the MIT [GPL for embeded] License. Copyright BLUEsat UNSW, 2017 + */ + +#ifndef TCS34725_H +#define TCS34725_H + +#include "i2c.h" + +#define TCS34725_CONTROL (0x0F) /* Gain control register */ +#define TCS34725_ATIME (0x01) /* Integration time control register */ +#define TCS34725_PERS_NONE (0b0000) /* Every RGBC cycle generates an interrupt*/ +#define TCS34725_PERS_1 (0b0001) /* 1 clear channel value outisde limits */ +#define TCS34725_PERS_2 (0b0010) /* 2 clean channel values outside limits */ +#define TCS34725_PERS_3 (0b0011) /* 3 clean channel values outside limits */ +#define TCS34725_PERS_5 (0b0100) /* 5 clean channel values outside limits */ +#define TCS34725_PERS_10 (0b0101) /* 10 clean channel values outside limits */ +#define TCS34725_PERS_15 (0b0110) /* 15 clean channel values outside limits */ +#define TCS34725_PERS_20 (0b0111) /* 20 clean channel values outside limits */ +#define TCS34725_PERS_25 (0b1000) /* 25 clean channel values outside limits */ +#define TCS34725_PERS_30 (0b1001) /* 30 clean channel values outside limits */ +#define TCS34725_PERS_35 (0b1010) /* 35 clean channel values outside limits */ +#define TCS34725_PERS_40 (0b1011) /* 40 clean channel values outside limits */ +#define TCS34725_PERS_45 (0b1100) /* 45 clean channel values outside limits */ +#define TCS34725_PERS_50 (0b1101) /* 50 clean channel values outside limits */ +#define TCS34725_PERS_55 (0b1110) /* 55 clean channel values outside limits */ +#define TCS34725_PERS_60 (0b1111) /* 60 clean channel values outside limits */ + + +class TCS34725 { + public: + /* The integration time is not limited to these values. It can be + * controlled in increments of 2.4ms by manually setting the TCS34725_ATIME + * register. + */ + enum integrationTime_t { + INTEGRATIONTIME_2_4MS = 0xFF, /* 2.4m */ + INTEGRATIONTIME_24MS = 0xF6, /* 24ms */ + INTEGRATIONTIME_50MS = 0xEB, /* 50ms */ + INTEGRATIONTIME_101MS = 0xD5, /* 101ms */ + INTEGRATIONTIME_154MS = 0xC0, /* 154ms */ + INTEGRATIONTIME_700MS = 0x00 /* 700ms */ + }; + + /* Gain scaling for the RGBC values */ + enum gain_t { + GAIN_1X = 0x00, /* No gain */ + GAIN_4X = 0x01, /* 4x gain */ + GAIN_16X = 0x02, /* 16x gain */ + GAIN_60X = 0x03 /* 60x gain */ + }; + + /* Wait times are 12x longer if the WLONG bit is asserted */ + enum waitTime_t { + WTIME_2_4MS = 0xFF, + WTIME_204MS = 0xAB, + WTIME_614MS = 0x00 + }; + + TCS34725(i2cModule_t, integrationTime_t = INTEGRATIONTIME_50MS, gain_t = GAIN_1X); + /* + * Initialises the I2C module and powers on the device. The wait cycle + * (power saving mode) and interrupt generation are disabled by default. + */ + bool init(void); + void set_integration_time(integrationTime_t it); + void set_gain(gain_t gain); + void read_raw_data(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c); + + /* + * Calculates the temperature in degrees Kelvin from the given R/G/B values + * @return colour temperature in degrees Kelvin + */ + uint16_t calculate_colour_temperature(uint16_t r, uint16_t g, uint16_t b); + + /* Calculates the illuminance from the R/G/B values */ + uint16_t calculate_lux(uint16_t r, uint16_t g, uint16_t b); + + /* Enables the low power wait state */ + void enable_wait(void); + + /* Sets the wait time between each RGBC integration cycle. Wait state + * must be enabled for this to take into effect. + * @param wlong if set, wait time is multipled by 12 + */ + void set_wait_time(waitTime_t wt, bool wlong); + void disable_wait(void); + + void power_on(void); + /* Transitions the state into low power sleep mode */ + void power_down(void); + + void write8(uint8_t reg, uint8_t data); + uint8_t read8(uint8_t reg); + uint16_t read16(uint8_t reg); + + /* Enables or disables interrupts depending on the specified boolean value */ + void set_interrupt(bool enable); + /* Clears pending interrupts */ + void clear_interrupt(void); + /* Sets the high and low thresholds for interrupt generation. An interrupt + * is generated if the clear channel value is outisde the specified limits. + */ + void set_interrupt_limits(uint16_t low, uint16_t high); + /* Sets the number of consecutive values the clear channel must have + * outside the limits before an interrupt is generated. + */ + void set_interrupt_persistence(uint8_t value); + + private: + i2cModule_t module; + gain_t tcs34725_gain; + integrationTime_t tcs34725_integration_time; + bool interrupt_enabled; +}; + +#endif diff --git a/src/libs/ros_echronos/build_tools/msg_gen.py b/src/libs/ros_echronos/build_tools/msg_gen.py deleted file mode 100755 index 5436af7..0000000 --- a/src/libs/ros_echronos/build_tools/msg_gen.py +++ /dev/null @@ -1,797 +0,0 @@ -#!/usr/bin/env python -""" -Modified version of ros's message generation tool to be used for generating messages compatible with our -bin system. - -The code is dual licensed under the BSD License and AGPLv3 license. -Copyright of code written after 13/05/2017 is BLUEsat UNSW, 2017. - - -This program was based on msg_gen.py from the ros project U{https://github.com/ros/ros_comm/} -The original source code was released under the BSD License below. This license is compatible with -the AGPLv3 License used in this project. See: https://www.gnu.org/licenses/license-compatibility.en.html - - Software License Agreement (BSD License) - - Copyright (c) 2009, Willow Garage, Inc. - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions - are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above - copyright notice, this list of conditions and the following - disclaimer in the documentation and/or other materials provided - with the distribution. - * Neither the name of Willow Garage, Inc. nor the names of its - contributors may be used to endorse or promote products derived - from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. -""" - -## ROS message source code generation for C++ -## -## Converts ROS .msg files in a package into C++ source code implementations. - -import sys -import os -import traceback - -import roslib.msgs -import roslib.packages -import roslib.gentools -from rospkg import RosPack - -try: - from cStringIO import StringIO #Python 2.x -except ImportError: - from io import StringIO #Python 3.x - -MSG_TYPE_TO_CPP = {'byte': 'int8_t', 'char': 'uint8_t', - 'bool': 'uint8_t', - 'uint8': 'uint8_t', 'int8': 'int8_t', - 'uint16': 'uint16_t', 'int16': 'int16_t', - 'uint32': 'uint32_t', 'int32': 'int32_t', - 'uint64': 'uint64_t', 'int64': 'int64_t', - 'float32': 'float', - 'float64': 'double', - 'string': 'ros_echronos::String', - 'time': 'ros::Time', - 'duration': 'ros::Duration'} - -def msg_type_to_cpp(type): - """ - Converts a message type (e.g. uint32, std_msgs/String, etc.) into the C++ declaration - for that type (e.g. uint32_t, std_msgs::String_) - - @param type: The message type - @type type: str - @return: The C++ declaration - @rtype: str - """ - (base_type, is_array, array_len) = roslib.msgs.parse_type(type) - cpp_type = None - if (roslib.msgs.is_builtin(base_type)): - cpp_type = MSG_TYPE_TO_CPP[base_type] - elif (len(base_type.split('/')) == 1): - if (roslib.msgs.is_header_type(base_type)): - cpp_type = ' ::std_msgs::Header_' - else: - cpp_type = '%s_'%(base_type) - else: - pkg = base_type.split('/')[0] - msg = base_type.split('/')[1] - cpp_type = ' ::%s::%s_'%(pkg, msg) - - if (is_array): - if (array_len is None): - return 'ros_echronos::Array<%s> '%(cpp_type) - else: - return 'ros_echronos::Array<%s> '%(cpp_type) - else: - return cpp_type - -def cpp_message_declarations(name_prefix, msg): - """ - Returns the different possible C++ declarations for a message given the message itself. - - @param name_prefix: The C++ prefix to be prepended to the name, e.g. "std_msgs::" - @type name_prefix: str - @param msg: The message type - @type msg: str - @return: A tuple of 3 different names. cpp_message_decelarations("std_msgs::", "String") returns the tuple - ("std_msgs::String_", "std_msgs::String_", "std_msgs::String") - @rtype: str - """ - pkg, basetype = roslib.names.package_resource_name(msg) - cpp_name = ' ::%s%s'%(name_prefix, msg) - if (pkg): - cpp_name = ' ::%s::%s'%(pkg, basetype) - return ('%s_'%(cpp_name), '%s_ '%(cpp_name), '%s'%(cpp_name)) - -def write_begin(s, spec, file): - """ - Writes the beginning of the header file: a comment saying it's auto-generated and the include guards - - @param s: The stream to write to - @type s: stream - @param spec: The spec - @type spec: roslib.msgs.MsgSpec - @param file: The file this message is being generated for - @type file: str - """ - s.write("/* Auto-generated by genmsg_cpp for file %s */\n"%(file)) - s.write('#ifndef %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper())) - s.write('#define %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper())) - -def write_end(s, spec): - """ - Writes the end of the header file: the ending of the include guards - - @param s: The stream to write to - @type s: stream - @param spec: The spec - @type spec: roslib.msgs.MsgSpec - """ - s.write('#endif // %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper())) - -def write_generic_includes(s): - """ - Writes the includes that all messages need - - @param s: The stream to write to - @type s: stream - """ - s.write('#include "ros.hpp"\n') - s.write('#include "Message.hpp"\n') - s.write('#include \n') - #s.write('#include "ros/serialization.h"\n') - #s.write('#include "ros/builtin_message_traits.h"\n') - #s.write('#include "ros/message_operations.h"\n') - #s.write('#include "ros/time.h"\n\n') - #s.write('#include "ros/macros.h"\n\n') - #s.write('#include "ros/assert.h"\n\n') - -def write_includes(s, spec): - """ - Writes the message-specific includes - - @param s: The stream to write to - @type s: stream - @param spec: The message spec to iterate over - @type spec: roslib.msgs.MsgSpec - """ - for field in spec.parsed_fields(): - if (not field.is_builtin): - if (field.is_header): - s.write('#include "std_msgs/Header.h"\n') - else: - (pkg, name) = roslib.names.package_resource_name(field.base_type) - pkg = pkg or spec.package # convert '' to package - s.write('#include "%s/%s.h"\n'%(pkg, name)) - - s.write('\n') - -def write_struct(s, spec, cpp_name_prefix, extra_deprecated_traits = {}): - """ - Writes the entire message struct: declaration, constructors, members, constants and (deprecated) member functions - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - - msg = spec.short_name - s.write('class %s_ : public ros_echronos::Message {\n'%(msg)) - s.write(' typedef %s_ Type;\n\n'%(msg)) - s.write(' public:\n') - - declare_constructors(s, spec, cpp_name_prefix) - declare_deconstructor(s, spec, cpp_name_prefix) - declare_virtual_functions(s, spec, cpp_name_prefix) - write_members(s, spec) - - #rospack = RosPack() - #gendeps_dict = roslib.gentools.get_dependencies(spec, spec.package, compute_files=False, rospack=rospack) - #md5sum = roslib.gentools.compute_md5(gendeps_dict, rospack=rospack) - #full_text = compute_full_text_escaped(gendeps_dict) - - # write_deprecated_member_functions(s, spec, dict(list({'MD5Sum': md5sum, 'DataType': '%s/%s'%(spec.package, spec.short_name), 'MessageDefinition': full_text}.items()) + list(extra_deprecated_traits.items()))) - - (cpp_msg_unqualified, cpp_msg_with_alloc, cpp_msg_base) = cpp_message_declarations(cpp_name_prefix, msg) - s.write(' typedef %s * Ptr;\n'%(cpp_msg_with_alloc)) - s.write(' typedef %s * const ConstPtr;\n'%(cpp_msg_with_alloc)) - - s.write('}; // class %s\n'%(msg)) - - s.write('typedef %s_ %s;\n\n'%(cpp_msg_base, msg)) - s.write('typedef %s %sPtr;\n'%(cpp_msg_base, msg)) - s.write('typedef %s const %sConstPtr;\n\n'%(cpp_msg_base, msg)) - - -def default_value(type): - """ - Returns the value to initialize a message member with. 0 for integer types, 0.0 for floating point, false for bool, - empty string for everything else - - @param type: The type - @type type: str - """ - if type in ['byte', 'int8', 'int16', 'int32', 'int64', - 'char', 'uint8', 'uint16', 'uint32', 'uint64']: - return '0' - elif type in ['float32', 'float64']: - return '0.0' - elif type == 'bool': - return 'false' - - return "" - -def takes_allocator(type): - """ - Returns whether or not a type can take an allocator in its constructor. False for all builtin types except string. - True for all others. - - @param type: The type - @type: str - """ - return not type in ['byte', 'int8', 'int16', 'int32', 'int64', - 'char', 'uint8', 'uint16', 'uint32', 'uint64', - 'float32', 'float64', 'bool', 'time', 'duration'] - -def write_initializer_list(s, spec, container_gets_allocator): - """ - Writes the initializer list for a constructor - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string) - should have the allocator passed to its constructor. Assumes the allocator is named _alloc. - @type container_gets_allocator: bool - """ - - i = 0 - for field in spec.parsed_fields(): - if (i == 0): - s.write(' : ') - else: - s.write(' , ') - - val = default_value(field.base_type) - use_alloc = takes_allocator(field.base_type) - if (field.is_array): - if (field.array_len is None and container_gets_allocator): - s.write('%s(_alloc)\n'%(field.name)) - else: - s.write('%s()\n'%(field.name)) - else: - if (container_gets_allocator and use_alloc): - s.write('%s(_alloc)\n'%(field.name)) - else: - s.write('%s(%s)\n'%(field.name, val)) - i = i + 1 - -def write_fixed_length_assigns(s, spec, container_gets_allocator, cpp_name_prefix): - """ - Initialize any fixed-length arrays - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string) - should have the allocator passed to its constructor. Assumes the allocator is named _alloc. - @type container_gets_allocator: bool - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - # Assign all fixed-length arrays their default values - for field in spec.parsed_fields(): - if (not field.is_array or field.array_len is None): - continue - - val = default_value(field.base_type) - if (container_gets_allocator and takes_allocator(field.base_type)): - # String is a special case, as it is the only builtin type that takes an allocator - if (field.base_type == "string"): - string_cpp = msg_type_to_cpp("string") - s.write(' %s.assign(%s(_alloc));\n'%(field.name, string_cpp)) - else: - (cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, field.base_type) - s.write(' %s.assign(%s(_alloc));\n'%(field.name, cpp_msg_with_alloc)) - elif (len(val) > 0): - s.write(' %s.assign(%s);\n'%(field.name, val)) - -def write_constructors(s, spec, cpp_name_prefix): - """ - Writes any necessary constructors for the message - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - - msg = spec.short_name - - # Default constructor - s.write(' %s%s_::%s_()\n'%(cpp_name_prefix,msg, msg)) - write_initializer_list(s, spec, False) - s.write(' {\n') - write_fixed_length_assigns(s, spec, False, cpp_name_prefix) - s.write(' }\n\n') - - -def write_deconstructor(s, spec, cpp_name_prefix): - """ - Writes any necessary deconstructors for the message - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - - msg = spec.short_name - - # Default deconstructor - s.write(' %s%s_::~%s_() {\n'%(cpp_name_prefix, msg, msg)) - s.write(" } //deconstructor\n\n") - # TODO(hjed): work out if we need to kill the Message Descriptor here - -def declare_constructors(s, spec, cpp_name_prefix): - """ - Declares any necessary constructors for the message - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - - msg = spec.short_name - - # Default constructor - s.write(' %s_();\n'%(msg)) - - -def declare_deconstructor(s, spec, cpp_name_prefix): - """ - Declares any necessary deconstructors for the message - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - - msg = spec.short_name - - # Default deconstructor - s.write(' ~%s_();\n'%(msg)) - -def declare_virtual_functions(s, spec, cpp_name_prefix): - """ - Declares any necessary virtual functions for the message - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::" - @type cpp_name_prefix: str - """ - - msg = spec.short_name - - # Default functions - s.write(' virtual void generate_block_impl();\n') - s.write(' virtual ros_echronos::Message_Descriptor * generate_descriptor();\n') - -def write_member(s, field): - """ - Writes a single member's declaration and type typedef - - @param s: The stream to write to - @type s: stream - @param type: The member type - @type type: str - @param name: The name of the member - @type name: str - """ - cpp_type = msg_type_to_cpp(field.type) - s.write(' typedef %s _%s_type;\n'%(cpp_type, field.name)) - s.write(' %s %s;\n\n'%(cpp_type, field.name)) - -def write_members(s, spec): - """ - Write all the member declarations - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - """ - [write_member(s, field) for field in spec.parsed_fields()] - - # write descriptor pointer - s.write(' ros_echronos::Message_Descriptor * desc = NULL;\n') - - -def write_deserialiser(s, spec, cpp_name_prefix): - """ - Writes the deserialisation function for the message class. - - Approach - - # declare new message class - # Store a list of structs storing pointer to a message variable and the size of the variable. Include in the list - items for array/string size values that point to the size variable in the next struct. Have a flag for variable length - variables. - # Loop through each can message packet, storing an index of where we are in the array of varriables. - # Write to the variables byte by byte, using the counter in the struct. - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: the prefix in cpp - @type cpp_name_prefix: str - """ - - num_messages = 0 # TODO: calculate how many can messages this ros message should take up - - msg = spec.short_name - s.write(' static %s%s %s%s_::msg_from_CAN(can::CAN_ROS_MSG * msg, size_t msgs) {\n' % (cpp_name_prefix, msg, cpp_name_prefix, msg)) - - - s.write(' %s%s msg;' % (cpp_name_prefix, msg)) - - num_var_fields = 0 # TODO: count these - - s.write(' _Field_Store fields[%d];' % len(spec.parsed_fields()) + num_var_fields) - index = 0 - for field in spec.parsed_fields(): - - (base_type, is_array, array_len) = roslib.msgs.parse_type(field.type) - - if is_array: - - s.write(' fields[%d].start_of_field = &(msg.%s.size);' % (index, field.name)) # stores the size field as a pointer - s.write(' fields[%d].size = sizeof(msg.%s.size);' % (index, field.name)) - s.write(' fields[%d].is_var_length = false;' % (index)) # store that it is an array - index+=1 - s.write(' fields[%d].start_of_field = &(msg.%s.value);' % (index, field.name)) # store the address of the field - s.write(' fields[%d].size = 0;' % (index)) # store the size (0 for an array) - s.write(' fields[%d].is_var_length = true;' % (index)) # store that it is an array - else: - s.write(' fields[%d].start_of_field = &(msg.%s);' % (index, field.name)) # stores the address of the field - s.write(' fields[%d].size = sizeof(msg.%s);' % (index, field.name)) - s.write(' fields[%d].is_var_length = false;' % (index)) # store that it is an array - index+=1 - - - s.write(' size_t msg_index = 0;') - s.write(' size_t msg_offset = 0;') - s.write(' for(int i = 0; i < %d; ++i) { ' % num_var_fields) - # copy the length from the size field into the read arrays length - s.write(' if(fields[i].is_var_length) { ' - ' fields[i].size = *((uint16_t)fields[i-1].start_of_field);' - ' }') # TODO: the variable length array's size field should NOT be in bytes - s.write(' for(int field_index = 0; field_index < fields[i].size; ++field_index, ++message_index) {' - ' if(message_index >= CAN_MESSAGE_MAX_LEN) { ++msg_index; msg_offset=0; }' - ' fields[i].start_of_field[field_index] = msg[msg_index].body[msg_offset];' - ' }') - - s.write(' } // for loop') - s.write(' return msg;') - - s.write(' } // msg_from_CAN') - - -def write_virtual_functions(s, spec, cpp_name_prefix): - """ - Writes the virtual functions of the message class - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param cpp_name_prefix: the prefix in cpp - @type cpp_name_prefix: str - """ - - msg = spec.short_name - s.write(' void %s%s_::generate_block_impl() {\n' % (cpp_name_prefix, msg)) - output = "" - sizes=[] - for field in spec.parsed_fields(): - - (base_type, is_array, array_len) = roslib.msgs.parse_type(field.type) - if is_array: - sizes.append("%s.bytes+2" % field.name) - output+=' memcpy(block+offset, sizeof(short), %s.size);\n' % (field.name) - output+=' memcpy(block+offset+sizeof(short), %s.values, %s.bytes);\n' % (field.name, field.name) - output+=' offset+=%s.bytes+sizeof(short);\n' % (field.name) - else: - sizes.append("sizeof(%s)" % field.name) - output+=' memcpy(block+offset, &%s, sizeof(%s));\n' % (field.name, field.name) - output+=' offset+=sizeof(%s);\n' % (field.name) - s.write(' size_t offset = 0;\n') - s.write(' size = %s;\n' %("+".join(sizes))) - s.write(' block = (uint8_t *) alloc::malloc(size);\n') - s.write(output); - s.write(' } // generate_block\n') - - num_fields = len(spec.parsed_fields()) - s.write(' ros_echronos::Message_Descriptor * %s%s_::generate_descriptor() {\n' % (cpp_name_prefix, msg)) - s.write(' void * desc = alloc::malloc(sizeof(ros_echronos::Message_Descriptor_Fixed<%d>));\n' % num_fields) - s.write(' ros_echronos::Message_Descriptor_Fixed<%d> * descriptor = new (desc) ros_echronos::Message_Descriptor_Fixed<%d>();\n' % (num_fields, num_fields)) - i = 0 - for field in spec.parsed_fields(): - (base_type, is_array, array_len) = roslib.msgs.parse_type(field.type) - s.write(' descriptor->fixed_field_ptrs[%d] = &%s;\n' % (i, field.name)) - if is_array: - s.write(' descriptor->fixed_field_sizes[%d] = 0;\n' % (i)) - else: - s.write(' descriptor->fixed_field_sizes[%d] = sizeof(%s);\n' % (i, field.name)) - i+=1 - s.write(' return descriptor;\n') - s.write(' }\n') - -def escape_string(str): - str = str.replace('\\', '\\\\') - str = str.replace('"', '\\"') - return str - -def write_constant_declaration(s, constant): - """ - Write a constant value as a static member - - @param s: The stream to write to - @type s: stream - @param constant: The constant - @type constant: roslib.msgs.Constant - """ - - # integral types get their declarations as enums to allow use at compile time - if (constant.type in ['byte', 'int8', 'int16', 'int32', 'int64', 'char', 'uint8', 'uint16', 'uint32', 'uint64']): - s.write(' enum { %s = %s };\n'%(constant.name, constant.val)) - else: - s.write(' static const %s %s;\n'%(msg_type_to_cpp(constant.type), constant.name)) - -def write_constant_definitions(s, spec): - """ - Write all the constants from a spec as static members - - @param s: The stream to write to - @type s: stream - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - """ - [write_constant_definition(s, spec, constant) for constant in spec.constants] - s.write('\n') - -def is_fixed_length(spec): - """ - Returns whether or not the message is fixed-length - - @param spec: The message spec - @type spec: roslib.msgs.MsgSpec - @param package: The package of the - @type package: str - """ - types = [] - for field in spec.parsed_fields(): - if (field.is_array and field.array_len is None): - return False - - if (field.base_type == 'string'): - return False - - if (not field.is_builtin): - types.append(field.base_type) - - types = set(types) - for type in types: - type = roslib.msgs.resolve_type(type, spec.package) - (_, new_spec) = roslib.msgs.load_by_type(type, spec.package) - if (not is_fixed_length(new_spec)): - return False - - return True - -def write_deprecated_member_functions(s, spec, traits): - """ - Writes the deprecated member functions for backwards compatibility - """ - for field in spec.parsed_fields(): - if (field.is_array): - s.write(' ROS_DEPRECATED uint32_t get_%s_size() const { return (uint32_t)%s.size(); }\n'%(field.name, field.name)) - - if (field.array_len is None): - s.write(' ROS_DEPRECATED void set_%s_size(uint32_t size) { %s.resize((size_t)size); }\n'%(field.name, field.name)) - s.write(' ROS_DEPRECATED void get_%s_vec(%s& vec) const { vec = this->%s; }\n'%(field.name, msg_type_to_cpp(field.type), field.name)) - s.write(' ROS_DEPRECATED void set_%s_vec(const %s& vec) { this->%s = vec; }\n'%(field.name, msg_type_to_cpp(field.type), field.name)) - - for k, v in traits.items(): - s.write('private:\n') - s.write(' static const char* __s_get%s_() { return "%s"; }\n'%(k, v)) - s.write('public:\n') - s.write(' ROS_DEPRECATED static const std::string __s_get%s() { return __s_get%s_(); }\n\n'%(k, k)) - s.write(' ROS_DEPRECATED const std::string __get%s() const { return __s_get%s_(); }\n\n'%(k, k)) - - s.write(' ROS_DEPRECATED virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const\n {\n') - s.write(' ros::serialization::OStream stream(write_ptr, 1000000000);\n') - for field in spec.parsed_fields(): - s.write(' ros::serialization::serialize(stream, %s);\n'%(field.name)) - s.write(' return stream.getData();\n }\n\n') - - s.write(' ROS_DEPRECATED virtual uint8_t *deserialize(uint8_t *read_ptr)\n {\n') - s.write(' ros::serialization::IStream stream(read_ptr, 1000000000);\n'); - for field in spec.parsed_fields(): - s.write(' ros::serialization::deserialize(stream, %s);\n'%(field.name)) - s.write(' return stream.getData();\n }\n\n') - - s.write(' ROS_DEPRECATED virtual uint32_t serializationLength() const\n {\n') - s.write(' uint32_t size = 0;\n'); - for field in spec.parsed_fields(): - s.write(' size += ros::serialization::serializationLength(%s);\n'%(field.name)) - s.write(' return size;\n }\n\n') - -def compute_full_text_escaped(gen_deps_dict): - """ - Same as roslib.gentools.compute_full_text, except that the - resulting text is escaped to be safe for C++ double quotes - - @param get_deps_dict: dictionary returned by get_dependencies call - @type get_deps_dict: dict - @return: concatenated text for msg/srv file and embedded msg/srv types. Text will be escaped for double quotes - @rtype: str - """ - definition = roslib.gentools.compute_full_text(gen_deps_dict) - lines = definition.split('\n') - s = StringIO() - for line in lines: - line = escape_string(line) - s.write('%s\\n\\\n'%(line)) - - val = s.getvalue() - s.close() - return val - -def is_hex_string(str): - for c in str: - if c not in '0123456789abcdefABCDEF': - return False - - return True - - -def write_template_includes(s, spec, cpp_prefix): - """ - Writes the includes and template definitions for the message - @param s: the output stream - @param spec: the message spec - @param cpp_prefix: the package cpp prefix - """ - s.write('#include "templates/Publisher.cpp"\n') - s.write('#include "templates/Subscriber.cpp"\n') - - s.write('template class ros_echronos::Publisher<%s%s>;\n' % (cpp_prefix, spec.short_name)) - s.write('template class ros_echronos::Subscriber<%s%s>;\n' % (cpp_prefix, spec.short_name)) - -def write_cpp_body(s, spec, cpp_prefix): - """ - Writes the cpp body file we need to generate all our templates - - @param s (StringIO): the feed to write to - @param spec: the message spec - @param cpp_prefix: the cpp prefix - """ - - # headers - s.write('#include "%s/%s.hpp"\n' % (spec.package, spec.short_name)) - - # due to compiler problems these need to be decleared externally - write_constructors(s, spec, cpp_prefix) - write_deconstructor(s, spec, cpp_prefix) - write_virtual_functions(s, spec, cpp_prefix) - write_template_includes(s, spec, cpp_prefix) - - -def generate(package, msg_name): - """ - Generate a message - - @param package: the package name - - @param msg_path: The path to the .msg file - @type msg_path: str - """ - package_dir = roslib.packages.get_pkg_dir(package) - #(package_dir, package) = roslib.packages.get_dir_pkg(msg_path) - #TODO: remove this - package_dir = "./" - - (_, spec) = roslib.msgs.load_by_type(msg_name, package) - # (_, spec) = roslib.msgs.load_from_file(msg_path, package) - - header = StringIO() - write_begin(header, spec, msg_name) - write_generic_includes(header) - write_includes(header, spec) - - cpp_prefix = '%s::'%(package) - - header.write('namespace %s\n{\n'%(package)) - write_struct(header, spec, cpp_prefix) - write_constant_definitions(header, spec) - header.write('} // namespace %s\n\n'%(package)) - write_end(header, spec) - - cpp = StringIO() - write_cpp_body(cpp, spec, cpp_prefix) - - output_dir = '%s/msg_gen/cpp/include/%s'%(package_dir, package) - if (not os.path.exists(output_dir)): - # if we're being run concurrently, the above test can report false but os.makedirs can still fail if - # another copy just created the directory - try: - os.makedirs(output_dir) - except OSError as e: - pass - - f = open('%s/%s.hpp'%(output_dir, spec.short_name), 'w') - f.write(header.getvalue() + "\n") - - cpp_output_dir = "%s/msg_gen/cpp/%s" % (package_dir, package) - if (not os.path.exists(cpp_output_dir)): - # if we're being run concurrently, the above test can report false but os.makedirs can still fail if - # another copy just created the directory - try: - os.makedirs(cpp_output_dir) - except OSError as e: - pass - - with open("%s/%s.cpp" % (cpp_output_dir, spec.short_name), 'w') as f: - f.write(cpp.getvalue() + "\n") - - header.close() - -def generate_messages(argv): - assert(len(argv) >= 3) - for arg in argv[2:]: - generate(argv[1], arg) - -if __name__ == "__main__": - roslib.msgs.set_verbose(False) - generate_messages(sys.argv) - diff --git a/src/libs/ros_echronos/build_tools/msg_gen.pyc b/src/libs/ros_echronos/build_tools/msg_gen.pyc new file mode 100644 index 0000000..4fd045c Binary files /dev/null and b/src/libs/ros_echronos/build_tools/msg_gen.pyc differ diff --git a/src/libs/ros_echronos/ros_echronos.cmake b/src/libs/ros_echronos/ros_echronos.cmake index 351e2d6..fb7477b 100644 --- a/src/libs/ros_echronos/ros_echronos.cmake +++ b/src/libs/ros_echronos/ros_echronos.cmake @@ -16,25 +16,25 @@ FILE(GLOB ros_files ${ROS_ECHRONOS_DIR}/arch/tm4c/*.cpp ${ROS_ECHRONOS_DIR}/*.cp FILE(GLOB ros_include_files ${ROS_INCLUDE_DIR}/*.hpp ${ROS_INCLUDE_DIR}/templates/*.hpp ) # define the doxygen target if there isn't one already -if(NOT TARGET ros-echronos-docs) +#if(NOT TARGET ros-echronos-docs) - find_package(Doxygen REQUIRED dot OPTIONAL_COMPONENTS mscgen dia) - if( COMMAND doxygen_add_docs ) - set(DOXYGEN_OUTPUT_DIRECTORY ../../../docs/doxygen) - doxygen_add_docs( - ros-echronos-docs - ${ros_files} - ${ros_include_files} + # find_package(Doxygen REQUIRED dot OPTIONAL_COMPONENTS mscgen dia) + # if( COMMAND doxygen_add_docs ) + # set(DOXYGEN_OUTPUT_DIRECTORY ../../../docs/doxygen) + # doxygen_add_docs( + # ros-echronos-docs + # ${ros_files} + # ${ros_include_files} - ) - add_custom_target( - ros-echronos-doc-gen ALL - COMMAND doxygen Doxyfile.ros-echronos-docs - COMMAND cp -r html ../../../docs/doxygen - DEPENDS ros-echronos-docs - ) - endif() -endif() + #) + # add_custom_target( + # ros-echronos-doc-gen ALL + # COMMAND doxygen Doxyfile.ros-echronos-docs + # COMMAND cp -r html ../../../docs/doxygen + # DEPENDS ros-echronos-docs + # ) + # endif() +#endif() function(build_ros_echronos echronos_build_dir module_name echronos_target node_id serial_on) set(ROS_BUILD_DIR ${echronos_build_dir}/lib/ros-echronos) @@ -57,10 +57,22 @@ function(build_ros_echronos echronos_build_dir module_name echronos_target node_ ) set_property(TARGET ${module_name} APPEND_STRING PROPERTY COMPILE_FLAGS "-DROS_NODE_ID=${node_id} -DROS_INFO_SERIAL=${serial_on} ") add_dependencies(${module_name} ${echronos_target}) - + # add_dependencies(ros_echronos_${module_name} ${ROS_BUILD_DIR}) endfunction() +function(link_science_library module_name) + set(SCIENCE_BUILD_DIR ../../libs/i2c/science-mod) + FILE(GLOB science-mod_files ${SCIENCE_BUILD_DIR}/*.cpp) + FILE(GLOB science-mod_headers ${SCIENCE_BUILD_DIR}/*.hpp) + target_sources(${module_name} + PRIVATE + ${science-mod_files} + PUBLIC + ${science-mod_headers} + ) +endfunction() + set(MSG_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/msg_gen/cpp/") function(generate_msgs ros_pkg msgs) foreach(msg ${msgs}) diff --git a/src/libs/servo/servo.c b/src/libs/servo/servo.c index 39b531d..67be667 100644 --- a/src/libs/servo/servo.c +++ b/src/libs/servo/servo.c @@ -1,8 +1,8 @@ #include "servo.h" -static const period_ms period[SERVO_COUNT] = {10.0}; -static const duty_pct neutral[SERVO_COUNT] = {15.0}; -static const double conversion[SERVO_COUNT] = {155.0}; +static const period_ms period[SERVO_COUNT] = {10.0, 10}; +static const duty_pct neutral[SERVO_COUNT] = {15.0, 15.78}; +static const double conversion[SERVO_COUNT] = {155.0, 154.1}; // initialise servo signal and set to neutral void servo_init(enum servos servo, enum pwm_pin pin) { diff --git a/src/libs/servo/servo.h b/src/libs/servo/servo.h index 90b9c2b..823645a 100644 --- a/src/libs/servo/servo.h +++ b/src/libs/servo/servo.h @@ -8,10 +8,11 @@ typedef double servo_deg; typedef double servo_rad; #define PI ((double)3.14159265359) -#define SERVO_COUNT 1 +#define SERVO_COUNT 2 enum servos { - HS_785HB + HS_785HB, + SCIENCE_SERVO }; // initialise servo signal and set to neutral diff --git a/src/modules/adc_test/CMakeLists.txt b/src/modules/adc_test/CMakeLists.txt new file mode 100644 index 0000000..848c596 --- /dev/null +++ b/src/modules/adc_test/CMakeLists.txt @@ -0,0 +1,27 @@ +project(adc_test) +cmake_minimum_required(VERSION 3.5) + +find_package(catkin REQUIRED + COMPONENTS ti adc tlsf) + +catkin_package( + INCLUDE_DIRS ./ + LIBRARIES adc_test + CATKIN_DEPENDS ti adc tlsf +) + +include_directories( + ${CMAKE_INCLUDE_PATH} + ./ + ${catkin_INCLUDE_DIRS} +) + +set(BUILD_TOOLS_DIR ../../../build-tools) +include(${BUILD_TOOLS_DIR}/module_template.cmake) + +#set(CPP_FILES adc_test.cpp can_wait_task.cpp) +set(CPP_FILES adc_test.cpp) +add_module(adc_test 4 1 0 "${CPP_FILES}") +target_link_libraries(adc_test adc m) + + diff --git a/src/modules/adc_test/adc_test.cpp b/src/modules/adc_test/adc_test.cpp new file mode 100644 index 0000000..99885d9 --- /dev/null +++ b/src/modules/adc_test/adc_test.cpp @@ -0,0 +1,58 @@ +#include "boilerplate.h" +#include "rtos-kochab.h" +#include "ros.hpp" +#include "adc.h" + +#define NUM_PINS 4 + + +uint32_t out[NUM_PINS] = {0}; +bool capture = true; + + +extern "C" bool tick_irq(void) { + rtos_timer_tick(); + return true; +} + +extern "C" void adc_callback_fn(void) { + capture = true; + UARTprintf("out = %d %d %d %d\n", out[0], out[1], out[2], out[3]); +} + +extern "C" void task_adc_test_fn(void) { + UARTprintf("Entered ADC Test task. Initializing...\n"); + + //enum adc_pin pins[NUM_PINS] = {AIN0, AIN1, AIN2, AIN3, AIN4, AIN5, AIN6, AIN7}; + enum adc_pin pins[NUM_PINS] = {AIN0,AIN1,AIN2,AIN3}; + adc_init_pins(pins, NUM_PINS, true); + while(1) { + if (capture) { + capture = false; + adc_capture_interrupt(out, adc_callback_fn); + } + //adc_capture_polling(out); + //UARTprintf("out = %d %d %d %d\n", out[0], out[1], out[2], out[3]); + } +} + +int main(void) { + + // Initialize the floating-point unit. + InitializeFPU(); + + // Set the clocking to run from the PLL at 50 MHz. + ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | + SYSCTL_XTAL_16MHZ); + + // Initialize the UART for stdio so we can use UARTPrintf + InitializeUARTStdio(); + + // Actually start the RTOS + UARTprintf("Starting RTOS...\n"); + rtos_start(); + + /* Should never reach here, but if we do, an infinite loop is + easier to debug than returning somewhere random. */ + for (;;) ; +} diff --git a/src/modules/adc_test/adc_test.prx b/src/modules/adc_test/adc_test.prx new file mode 100644 index 0000000..77023f0 --- /dev/null +++ b/src/modules/adc_test/adc_test.prx @@ -0,0 +1,91 @@ + + + + + + + + + systick + tick_irq + + + + + 0x0000000 + nmi + hardfault + memmanage + usagefault + busfault + true + exception_preempt_trampoline_systick + + + 39 + ros_can_interupt_handler + + + 14 + adc_irq_handler + + + + + + + true + true + fatal + rtos + 8 + 8 + 192 + 224 + + + task_adc_test + task_adc_test_fn + 30 + 1024 + + + + + can_delay + + + can_receive_signal + + + + + + can_delay_timer + true + 100 + task_adc_test + can_delay + + + + + alloc + + + print + + + + false + + + + can_receive_event + can_receive_signal + task_adc_test + + + + + diff --git a/src/modules/adc_test/package.xml b/src/modules/adc_test/package.xml new file mode 100644 index 0000000..331e17b --- /dev/null +++ b/src/modules/adc_test/package.xml @@ -0,0 +1,59 @@ + + + adc_test + 0.0.0 + ADC library test module + + + + + BLUEsat UNSW + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + ti + ti + tlsf + tlsf + adc + adc + + + + + + + + + + + diff --git a/src/modules/science/CMakeLists.txt b/src/modules/science/CMakeLists.txt new file mode 100644 index 0000000..5c265b6 --- /dev/null +++ b/src/modules/science/CMakeLists.txt @@ -0,0 +1,31 @@ +project(science) +cmake_minimum_required(VERSION 3.5) + +find_package(catkin REQUIRED + COMPONENTS ti tlsf i2c adc servo pwm) + +catkin_package( + INCLUDE_DIRS ./ + LIBRARIES science + CATKIN_DEPENDS ti tlsf i2c adc servo pwm +) + +include_directories( + ${CMAKE_INCLUDE_PATH} + ./ + ${catkin_INCLUDE_DIRS} +) + +set(BUILD_TOOLS_DIR ../../../build-tools) +include(${BUILD_TOOLS_DIR}/module_template.cmake) + +set(CPP_FILES science.cpp can_wait_task.cpp) +add_module(science 7 1 0 "${CPP_FILES}") +link_science_library(science) +target_link_libraries(science i2c adc servo pwm) +generate_msgs(std_msgs Int16) +generate_msgs(std_msgs Float64) +generate_msgs(owr_messages science) +depend_msgs(science std_msgs Float64) +depend_msgs(science std_msgs Int16) +depend_msgs(science owr_messages science) diff --git a/src/modules/science/README b/src/modules/science/README new file mode 100644 index 0000000..1180f93 --- /dev/null +++ b/src/modules/science/README @@ -0,0 +1,53 @@ +---DEPENDENCIES--- +Custom message "science.msg" defined in owr_software/rover/src/owr_messages with the fields: + uint32 temperature + uint32 humidity + float32 mag_x + float32 mag_y + float32 mag_z + uint32 illuminance + uint32 colour_temperature + uint32 moisture + float64 weight + +---Subscribers--- +TOPIC Type Function +science/request Int16 Take measurements from a module (1-4) and publish to science/data +science/servo Int16 Rotate the servo to open a desired module (1-4) or close all (0) +science/tare Int16 Resets the zero weight reference (message value is irrelevant) + + +---PIN CONNECTIONS--- +I2C multiplexer: +A0, A1, A2 - GND +RST - Vcc via pullup resistor +SCL - PB2 +SDA - PB3 +SCL and SDA both must be connected to Vcc via pullup resistors (2-10K) + + +I2C sensors (colour, temperature, magnetism): +Connect each set of sensors to the multiplexer outputs: +Module 1: SC0 and SD0 +Module 2: SC1 and SD1 +Module 3: SC2 and SD2 +Module 4: SC3 and SD3 +Pullup resistors to Vcc for all SC* and SD* + +Magnetism sensors: +CS - Vcc (enable i2c mode) + +Moisture sensors: +Module 1: AIN0 (PE3) +Module 2: AIN1 (PE2) +Module 3: AIN2 (PE1) +Module 4: AIN3 (PE0) + +Weight amplifier: +CLK - PA6 +DATA - PA7 +Make sure to calibrate the scaling factor to scale to the appropriate units. + - Set scale to 1 (defaults to 1 automatically) + - Call tare() with zero load on sensor + - Add known weight + - read_scaled_avg() and divide result by the known weight to obtain the scaling factor diff --git a/src/modules/science/can_wait_task.cpp b/src/modules/science/can_wait_task.cpp new file mode 100644 index 0000000..17f13c3 --- /dev/null +++ b/src/modules/science/can_wait_task.cpp @@ -0,0 +1,26 @@ +/* + * @date: 26/12/17 + * @author: (original author) Harry J.E Day + * @authors: (editors) + * @details: Defines a task to run the ros subscriber task in + * @copydetails: This code is released under the LGPLv3 and newer License and the BSD License + * @copyright: Copyright BLUEsat UNSW 2017 + */ +#include "boilerplate.h" +#include "ros.hpp" +#include "NodeHandle.hpp" +extern ros_echronos::NodeHandle * volatile nh_ptr; + +extern "C" void task_can_wait_fn(void) { + + ros_echronos::ROS_INFO("Entered wait task. Waiting for node handle to be initalised\n"); + while (!nh_ptr) { + rtos_sleep(2); + } + + ros_echronos::ROS_INFO("Node Handle Initalised\n"); + + nh_ptr->run_handle_message_loop(); + ros_echronos::ROS_INFO("Message Loop Exited Unexpectedly!\n"); + +} diff --git a/src/modules/science/package.xml b/src/modules/science/package.xml new file mode 100644 index 0000000..9cf8121 --- /dev/null +++ b/src/modules/science/package.xml @@ -0,0 +1,65 @@ + + + science + 0.0.0 + Science Library + + + + + BLUEsat UNSW + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + ti + ti + tlsf + tlsf + i2c + i2c + adc + adc + pwm + pwm + servo + servo> + + + + + + + + + + + diff --git a/src/modules/science/science.cpp b/src/modules/science/science.cpp new file mode 100644 index 0000000..e289b22 --- /dev/null +++ b/src/modules/science/science.cpp @@ -0,0 +1,283 @@ +/** + * Date Started: 8/9/18 + * @author: (Original Author) Alan Nguyen, Edward Dai + * @description: Science module, see readme for usage + * @copyright: This code is released under the MIT [GPL for embeded] License. Copyright BLUEsat UNSW, 2018 + */ + +#include +#include "boilerplate.h" +#include "ros.hpp" +#include "Subscriber.hpp" +#include "Publisher.hpp" +#include "NodeHandle.hpp" +#include "owr_messages/science.hpp" +#include "std_msgs/Int16.hpp" +#include "std_msgs/Float64.hpp" +#include "science-mod/LIS3MDL.h" +#include "science-mod/SI7021.h" +#include "science-mod/TCS34725.h" +#include "science-mod/HX711.h" +#include "science-mod/TCA9548A.h" +#include "adc.h" +#include "servo.h" + +#define NUM_MODULES 3 +#define SCIENCE_SERVO_PIN PWM0 + +#define NEUTRAL_POS 0 +#define MODULE_1_POS 52.5 +#define MODULE_2_POS 126.4 // (should be 135) - will need to recalibrate if mech issues are fixed +#define MODULE_3_POS -132.5 // (should be -135) +// #define MODULE_4_POS -52.5 + +ros_echronos::NodeHandle * volatile nh_ptr = NULL; + +#define SYSTICKS_PER_SECOND 100 +#define CAN_BITRATE 500000 +#define CAN_MSG_LEN 8 + + +static tCANMsgObject rx_object; +static uint8_t can_input_buffer[CAN_MSG_LEN]; +static void init_can(void); +static void write_can(uint32_t message_id, uint8_t *buffer, uint32_t buffer_size); +void data_request_callback(const std_msgs::Float64 &msg); +void servo_position_callback(const std_msgs::Int16 &msg); +void tare_callback(const std_msgs::Int16 &msg); + +extern "C" bool tick_irq(void) { + rtos_timer_tick(); + return true; +} + +bool sent_message; + +static uint32_t error_flag; + +static ros_echronos::Publisher *science_pub_ptr; +static TCS34725 *tcs34725_ptr; +static LIS3MDL *lis3mdl_ptr; +static SI7021 *si7021_ptr; +static HX711 *hx711_ptr; +uint32_t moisture_buffer[NUM_MODULES] = {0}; + +std_msgs::Float64 my_data; + +extern "C" void task_science_fn(void) { + // this creates a node handle + ros_echronos::ROS_INFO("Entered CAN task. Initializing...\n"); + ros_echronos::NodeHandle nh; + nh.init("science_fn", "science_fn", RTOS_INTERRUPT_EVENT_ID_CAN_RECEIVE_EVENT, RTOS_SIGNAL_ID_CAN_RECEIVE_SIGNAL); + nh_ptr = &nh; + + // Create the subscriber + ros_echronos::ROS_INFO("Data request sub init\n"); + std_msgs::Float64 request_buffer_in[5]; + ros_echronos::Subscriber request_sub("/science/request", request_buffer_in, 5, data_request_callback); + request_sub.set_topic_id(13); + request_sub.init(nh); + + ros_echronos::ROS_INFO("Servo position sub init\n"); + std_msgs::Int16 servo_buffer_in[5]; + ros_echronos::Subscriber servo_pos_sub("/science/servo", servo_buffer_in, 5, servo_position_callback); + servo_pos_sub.set_topic_id(14); + servo_pos_sub.init(nh); + + std_msgs::Int16 tare_buffer_in[5]; + ros_echronos::Subscriber weight_tare_sub("/science/tare", tare_buffer_in, 5, tare_callback); + weight_tare_sub.set_topic_id(15); + weight_tare_sub.init(nh); + + // Create the publisher + ros_echronos::ROS_INFO("Data pub init\n"); + owr_messages::science science_buffer_out[5]; + ros_echronos::Publisher science_pub("/science/data", science_buffer_out, 5, false); + // science_pub.set_topic_id(16); + science_pub_ptr = &science_pub; + science_pub_ptr->init(nh); + + ros_echronos::ROS_INFO("Science servo init\n"); + servo_init(SCIENCE_SERVO, SCIENCE_SERVO_PIN); + servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, NEUTRAL_POS); + + ros_echronos::ROS_INFO("ADC init\n"); + enum adc_pin moisture_pins[NUM_MODULES] = {AIN0, AIN1, AIN2}; + adc_init_pins(moisture_pins, NUM_MODULES, false); + + TCS34725 tcs34725(I2C0); + tcs34725_ptr = &tcs34725; + LIS3MDL lis3mdl(I2C0); + lis3mdl_ptr = &lis3mdl; + SI7021 si7021(I2C0); + si7021_ptr = &si7021; + ros_echronos::ROS_INFO("Init sensors of each module\n"); + bool success = false; + i2c_init(I2C0, FAST); + // initialise sensors of each module + for (uint8_t i = 0; i < NUM_MODULES; i++) { + ros_echronos::ROS_INFO("Module %d\n", i); + i2c_select(I2C0, i); // select multiplexer output + success = tcs34725_ptr->init(); + if (success) { + ros_echronos::ROS_INFO("TCS34725 successfully initialised\n"); + } else { + ros_echronos::ROS_INFO("TCS34725 failed to initialise\n"); + } + + success = lis3mdl_ptr->init(); + if (success) { + ros_echronos::ROS_INFO("LIS3MDL successfully initialised\n"); + } else { + ros_echronos::ROS_INFO("LIS3MDL failed to initialise\n"); + } + + success = si7021_ptr->init(); + if (success) { + ros_echronos::ROS_INFO("SI7021 successfully initialised\n"); + } else { + ros_echronos::ROS_INFO("SI7021 failed to initialise\n"); + } + } + + my_data.data = 1.0; + HX711 hx711(PORTA, PIN_7, PORTA, PIN_6); + hx711_ptr = &hx711; + hx711_ptr->init(); + float scale = -67.29; // CALIBRATE THIS + hx711_ptr->set_scale(scale); + ros_echronos::ROS_INFO("HX711 weight sensor initialised\n"); + + while(true) { + // this causes the callbacks to be called + nh.spin(); + } +} + + +int main(void) { + + // Initialize the floating-point unit. + InitializeFPU(); + + // Set the clocking to run from the PLL at 50 MHz. + ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | + SYSCTL_XTAL_16MHZ); + + // Set up the systick interrupt used by the RTOS + ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / SYSTICKS_PER_SECOND); + ROM_SysTickIntEnable(); + ROM_SysTickEnable(); + + // Initialize the UART for stdio so we can use UARTPrintf + InitializeUARTStdio(); + + init_can(); + + alloc::init_mm(RTOS_MUTEX_ID_ALLOC); + + ros_echronos::write_mutex = RTOS_MUTEX_ID_PRINT; + ros_echronos::write_mutex_set = true; + + // Actually start the RTOS + UARTprintf("Starting RTOS...\n"); + rtos_start(); + + /* Should never reach here, but if we do, an infinite loop is + easier to debug than returning somewhere random. */ + for (;;) ; +} + +void init_can(void) { + // We enable GPIO E - E4 for RX and E5 for TX + SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); + GPIOPinConfigure(GPIO_PE4_CAN0RX); + GPIOPinConfigure(GPIO_PE5_CAN0TX); + + // enables the can function we have just configured on those pins + GPIOPinTypeCAN(GPIO_PORTE_BASE, GPIO_PIN_4 | GPIO_PIN_5); + + //enable and initalise CAN0 + SysCtlPeripheralEnable(SYSCTL_PERIPH_CAN0); + CANInit(CAN0_BASE); + + //TODO: change this to use the eChronos clock + // Set the bitrate for the CAN BUS. It uses the system clock + CANBitRateSet(CAN0_BASE, ROM_SysCtlClockGet(), CAN_BITRATE); + + // enable can interupts + CANIntEnable(CAN0_BASE, CAN_INT_MASTER | CAN_INT_ERROR); //| CAN_INT_STATUS); + IntEnable(INT_CAN0); + + //start CAN + CANEnable(CAN0_BASE); + +} + +/* + * Reads measurements specified module and publishes the data + * + * Module number should be an integer between 1-4 (zero not included to be consistent + * with the servo position module number) + */ +void data_request_callback(const std_msgs::Float64 &msg) { + // if (msg.data <= 0 || msg.data > NUM_MODULES) { + // UARTprintf("Message value should be between 1-3\n"); + // return; + // } + // switch multiplexer output to the desired module + // subtract 1 since multiplexer output starts from 0 + int board_num = (int) msg.data; + i2c_select(I2C0, board_num); + // create a message containing sensor data + owr_messages::science data_msg; + // read temperature and humidity + data_msg.temperature = si7021_ptr->read_temperature(); + data_msg.humidity = si7021_ptr->read_humidity(); + // magnetism + lis3mdl_ptr->read_magnetism(&data_msg.mag_x, &data_msg.mag_y, &data_msg.mag_z); + // colour sensor + uint16_t r, g, b, c; + tcs34725_ptr->read_raw_data(&r, &g, &b, &c); + data_msg.colour_temperature = (uint32_t) tcs34725_ptr->calculate_colour_temperature(r, g, b); + data_msg.illuminance = (uint32_t) tcs34725_ptr->calculate_lux(r, g, b); + // moisture + adc_capture_polling(moisture_buffer); + // get the moisture value corresponding to the desired module + data_msg.moisture = moisture_buffer[board_num]; // index starts from 0 + // weight (averaging 30 samples) + data_msg.weight = hx711_ptr->read_scaled_avg(30); + + // publish data message + science_pub_ptr->publish(data_msg, 0); +} + +/* + * See hash defines for exact angle positions of each module + */ +void servo_position_callback(const std_msgs::Int16 &msg) { + UARTprintf("Change servo position to %d\n", msg.data); + switch (msg.data) { + case 0: + servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, NEUTRAL_POS); + break; + case 1: + servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, MODULE_1_POS); + break; + case 2: + servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, MODULE_2_POS); + break; + case 3: + servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, MODULE_3_POS); + break; + // case 4: + // servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, MODULE_4_POS); + // break; + } +} + +/* Recalibrate zero reference (tare) for weight sensor */ +void tare_callback(const std_msgs::Int16 &msg) { + UARTprintf("Received command to recalibrate zero weight reference\n"); + hx711_ptr->tare(50); +} diff --git a/src/modules/science/science.prx b/src/modules/science/science.prx new file mode 100644 index 0000000..14f6bd7 --- /dev/null +++ b/src/modules/science/science.prx @@ -0,0 +1,100 @@ + + + + + + + + + systick + tick_irq + + + + + 0x0000000 + nmi + hardfault + memmanage + usagefault + busfault + true + exception_preempt_trampoline_systick + + + 39 + ros_can_interupt_handler + + + 5 + uart0_int_handler + + + + + + + + true + true + fatal + rtos + 8 + 8 + 192 + 224 + + + task_science + task_science_fn + 30 + 4096 + + + task_can_wait + task_can_wait_fn + 31 + 512 + + + + + + can_delay + + + can_receive_signal + + + + + + can_delay_timer + true + 100 + task_science + can_delay + + + + + alloc + + + print + + + + + false + + + + can_receive_event + can_receive_signal + task_can_wait + + + + + diff --git a/src/modules/science_test/CMakeLists.txt b/src/modules/science_test/CMakeLists.txt new file mode 100644 index 0000000..8972726 --- /dev/null +++ b/src/modules/science_test/CMakeLists.txt @@ -0,0 +1,25 @@ +project(science_test) +cmake_minimum_required(VERSION 3.5) + +find_package(catkin REQUIRED + COMPONENTS ti tlsf i2c adc servo pwm) + +catkin_package( + INCLUDE_DIRS ./ + LIBRARIES science_test + CATKIN_DEPENDS ti tlsf i2c adc servo pwm +) + +include_directories( + ${CMAKE_INCLUDE_PATH} + ./ + ${catkin_INCLUDE_DIRS} +) + +set(BUILD_TOOLS_DIR ../../../build-tools) +include(${BUILD_TOOLS_DIR}/module_template.cmake) + +add_module(science_test 6 1 0 science_test.cpp) +link_science_library(science_test) +target_link_libraries(science_test i2c adc servo pwm) + diff --git a/src/modules/science_test/README b/src/modules/science_test/README new file mode 100644 index 0000000..ceb308a --- /dev/null +++ b/src/modules/science_test/README @@ -0,0 +1,33 @@ +I2C multiplexer: +A0, A1, A2 - GND +RST - Vcc via pullup resistor +SCL - PB2 +SDA - PB3 +SCL and SDA both must be connected to Vcc via pullup resistors (2-10K) + + +I2C sensors (colour, temperature, magnetism): +Connect each set of sensors to the multiplexer outputs: +Module 1: SC0 and SD0 +Module 2: SC1 and SD1 +Module 3: SC2 and SD2 +Module 4: SC3 and SD3 +Pullup resistors to Vcc for all SC* and SD* + +Magnetism sensors: +CS - Vcc (enable i2c mode) + +Moisture sensors: +Module 1: AIN0 (PE3) +Module 2: AIN1 (PE2) +Module 3: AIN2 (PE1) +Module 4: AIN3 (PE0) + +Weight amplifier: +CLK - PA6 +DATA - PA7 +Make sure to calibrate the scaling factor to scale to the appropriate units. + - Set scale to 1 (defaults to 1 automatically) + - Call tare() with zero load on sensor + - Add known weight + - read_scaled_avg() and divide result by the known weight to obtain the scaling factor diff --git a/src/modules/science_test/package.xml b/src/modules/science_test/package.xml new file mode 100644 index 0000000..2058de1 --- /dev/null +++ b/src/modules/science_test/package.xml @@ -0,0 +1,64 @@ + + + science_test + 0.0.0 + Science Library + + + + + BLUEsat UNSW + + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + ti + ti + tlsf + tlsf + i2c + i2c + adc + adc + pwm + pwm + servo + servo + + + + + + + + + diff --git a/src/modules/science_test/science_test.cpp b/src/modules/science_test/science_test.cpp new file mode 100644 index 0000000..c2edba0 --- /dev/null +++ b/src/modules/science_test/science_test.cpp @@ -0,0 +1,173 @@ +#include "boilerplate.h" +#include "rtos-kochab.h" +#include "science-mod/LIS3MDL.h" +#include "science-mod/SI7021.h" +#include "science-mod/TCS34725.h" +#include "science-mod/HX711.h" +#include "science-mod/TCA9548A.h" +#include "adc.h" +#include "servo.h" + +#define NUM_MODULES 3 +#define SCIENCE_SERVO_PIN PWM0 + +#define NEUTRAL_POS 0 +#define MODULE_1_POS 52.5 +#define MODULE_2_POS 126.4 // (should be 135) - will need to recalibrate if mech issues are fixed +#define MODULE_3_POS -132.5 // (should be -135) + +void ftoa(float f,char *buf) { + int pos=0,ix,dp,num; + if (f<0) { + buf[pos++]='-'; + f = -f; + } + dp=0; + while (f>=10.0) { + f=f/10.0; + dp++; + } + for (ix=1;ix<8;ix++) { + num = (int)f; + f=f-num; + if (num>9) + buf[pos++]='#'; + else + buf[pos++]='0'+num; + if (dp==0) buf[pos++]='.'; + f=f*10.0; + dp--; + } +} + +extern "C" void task_science_test_fn(void) { + UARTprintf("Entered science test task\n"); + i2c_init(I2C0, FAST); + int failure_counter = 0; + LIS3MDL lis3mdl(I2C0); + TCS34725 tcs34725(I2C0); + SI7021 si7021(I2C0); + UARTprintf("Initialising adc in polling mode\n"); + uint32_t adc_buffer[NUM_MODULES] = {0}; + enum adc_pin pins[NUM_MODULES] = {AIN0, AIN3, AIN2}; + adc_init_pins(pins, NUM_MODULES, false); + UARTprintf("ADC initialised\n"); + HX711 hx711(PORTA, PIN_7, PORTA, PIN_6); + for (int module_num = 0; module_num < NUM_MODULES; module_num += 1) { + i2c_select(I2C0, module_num); // select multiplexer output + // UARTprintf("Initialising science servo to neutral position\n"); + // servo_init(SCIENCE_SERVO, SCIENCE_SERVO_PIN); + // servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, NEUTRAL_POS); + + bool success = false; + success = lis3mdl.init(); + if (success) { + UARTprintf("LIS3MDL successfully initialised\n"); + } else { + failure_counter += 1; + UARTprintf("LIS3MDL failed to initialise\n"); + } + success = tcs34725.init(); + if (success) { + UARTprintf("TCS34725 successfully initialised\n"); + } else { + failure_counter += 1; + UARTprintf("TCS34725 failed to initialise\n"); + } + uint8_t it = tcs34725.read8(TCS34725_ATIME); + UARTprintf("Check integration time set %d\n", it); + uint8_t gain = tcs34725.read8(TCS34725_CONTROL); + UARTprintf("Check gain set %d\n", gain); + + success = si7021.init(); + if (success) { + UARTprintf("SI7021 successfully initialised\n"); + } else { + failure_counter += 1; + UARTprintf("SI7021 failed to initialise\n"); + } + UARTprintf("Checking SI7021 serial number\n"); + uint32_t ser_hi = 0; + uint32_t ser_lo = 0; + si7021.read_serial_number(&ser_hi, &ser_lo); + UARTprintf("ser = %x %x\n", ser_hi, ser_lo); + hx711.init(); // init to default gain = 128 + UARTprintf("HX711 initialised\n"); + hx711.tare(50); + UARTprintf("Zero out HX711 with 50 samples\n"); + + //change this to scale raw value to appropriate units + float scale = -67.29; + hx711.set_scale(scale); + UARTprintf("HX711 scaling factor set to %d\n", (int) scale); + } + i2c_select(I2C0, 0); // select multiplexer output + uint16_t r, g, b, c; + uint16_t colour_temp, illuminance; + uint32_t temp, humidity; + float mx, my, mz; + char mx_buf[10]; + char my_buf[10]; + char mz_buf[10]; + int32_t weight_raw; + float weight; + char weight_buf[10]; + while (1) { + lis3mdl.read_magnetism(&mx, &my, &mz); + ftoa(mx, mx_buf); + ftoa(my, my_buf); + ftoa(mz, mz_buf); + UARTprintf("Scaled magnetism: x = %s y = %s z = %s gauss\n", mx_buf, my_buf, mz_buf); + + temp = si7021.read_temperature(); + UARTprintf("Temperature = %d degrees Celsius\n", temp); + humidity = si7021.read_humidity(); + UARTprintf("Humidity = %d percent\n", humidity); + + tcs34725.read_raw_data(&r, &g, &b, &c); + UARTprintf("Raw light sensor values: r=%d, g=%d, b=%d, c=%d\n", r, g, b, c); + colour_temp = tcs34725.calculate_colour_temperature(r, g, b); + UARTprintf("Colour temperature = %d Kelvin\n", colour_temp); + illuminance = tcs34725.calculate_lux(r, g, b); + UARTprintf("Illuminance = %d lux\n", illuminance); + + adc_capture_polling(adc_buffer); + UARTprintf("Moisture readings = %d %d %d, poll res: %d\n", + adc_buffer[0], adc_buffer[1], adc_buffer[2]); + + weight_raw = hx711.read(); + UARTprintf("Raw weight value = %d\n", weight_raw); + weight_raw = hx711.read_avg(30); + UARTprintf("Raw weight value averaged (30 samples) = %d\n", weight_raw); + weight = (float) hx711.read_scaled_avg(30); + ftoa(weight, weight_buf); + UARTprintf("Tare calibrated and scaled weight = %s\n", weight_buf); + + // delay 0.1s + for (uint32_t i=0; i < 100; i++) { + SysCtlDelay(25000); // 1 ms delay + } + // servo_write(SCIENCE_SERVO, SCIENCE_SERVO_PIN, MODULE_1_POS); + } +} + +int main(void) { + + // Initialize the floating-point unit. + InitializeFPU(); + + // Set the clocking to run from the PLL at 50 MHz. + ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | + SYSCTL_XTAL_16MHZ); + + // Initialize the UART for stdio so we can use UARTPrintf + InitializeUARTStdio(); + + // Actually start the RTOS + UARTprintf("Starting RTOS...\n"); + rtos_start(); + + /* Should never reach here, but if we do, an infinite loop is + easier to debug than returning somewhere random. */ + for (;;) ; +} diff --git a/src/modules/science_test/science_test.prx b/src/modules/science_test/science_test.prx new file mode 100644 index 0000000..453a51f --- /dev/null +++ b/src/modules/science_test/science_test.prx @@ -0,0 +1,61 @@ + + + + + + + + 0x0000000 + nmi + hardfault + memmanage + usagefault + busfault + true + + + + + + true + true + fatal + rtos + 8 + 8 + 192 + 224 + + + task_science_test + task_science_test_fn + 30 + 1024 + + + + + false + + + + + + alloc + + + + + blink_delay + + + + + can_receive_event + task_science_test + blink_delay + + + + +