diff --git a/cosmicpi-arduino.cc b/cosmicpi-arduino.cc deleted file mode 120000 index d71f80f..0000000 --- a/cosmicpi-arduino.cc +++ /dev/null @@ -1 +0,0 @@ -cosmicpi-arduino.ino \ No newline at end of file diff --git a/cosmicpi-arduino.ino b/cosmicpi-arduino.ino index 0e2bcb8..a7439b4 100644 --- a/cosmicpi-arduino.ino +++ b/cosmicpi-arduino.ino @@ -1,307 +1,565 @@ -//initialisation code +// This program collects up to PPS_EVENTS events each second into a double buffer. +// While the ISR fills one event buffer, the user space loop function reads from the other. +// Each PPS interrupt the read/write buffers are swapped over by the event ISR +// If any events are available in the read buffer the loop function puts them onto a queue +// along with the UTC time string where they get stored. Once there are at +// least DUMP_THRESHOLD entries on the queue, the loop function outputs them over the serial +// line for processing. + +// In this version interrupts come from the accelerometer chip, if the acceleration exceeds +// the threshold (in meters/sec/sec) in any direction the chip interrupts and its logged to +// the serial output stream. + +// Julian Lewis lewis.julian@gmail.com + +#define VERS "2016/Mar/29" + +// In this sketch I am using an Adafruite ultimate GPS breakout which exposes the PPS output +// The Addafruite Rx is connected to the DUE TX1 (Pin 18) and its Tx to DUE RX1 (Pin 19) +// The Adafruite 3.3V power is provided from the DUE 3.3V and ground pins +// N.B. Go to the Adafruit downloads page and copy Adafruit_GPS.h and Adafruit_GPS.cc to +// your sketch directory. + +// The output from this program is processed by a Python monitor on the other end of the +// serial line. There has to be mutual aggreement between this program and the monitor. + +// Output strings +// All fields in all output strings conform to the json standard + +// Here is the list of all records where 'f' denotes float and 'i' denotes integer ... +// {'HTU':{'Tmh':f,'Hum':f}} +// HTU21DF record containing Tmh:temperature in C Hum:humidity percent +// +// {'BMP':{'Tmb':f,'Prs':f,'Alb':f}} +// BMP085 record containing Tmb:temperature Prs:pressure Alb:Barrometric altitude +// +// {'VIB':{'Vax':i,'Vcn':i}} +// Vibration record containing Vax:3 bit xyz direction mask Vcn:vibration count +// This record is always immediatly followed by 3 more records, TIM, ACL, and MAG +// +// {'MAG':{'Mgx':f,'Mgy':f,'Mgz':f}} +// LSM303DLH magnatometer record containing Mgx:the x field strength Mgy:the y field Mgz:ther z field +// +// {'MOG':{'Mox':f,'Moy':f,'Moz':f}} +// LSM303DLH magnatometer record containing Mox:x orientation Moy:y orientation Moz:z orientation +// This record is optional, by default its turned off (it can always be calculated later - Python) +// +// {'ACL':{'Acx':f,'Acy':f,'Acz':f}} +// LSM303DLH acclerometer record containing Acx:the x acceleration Acy:the y acceleration Acz:the z acceleration +// If this record immediatly follows a VIB record the fields were hardware latched when the g threshold was exceeded +// +// {'AOL':{'Aox':f,'Aoy':f,'Aoz':f}} +// LSM303DLH accelerometer record containing Aox:x orientation Aoy:y orientation Aoz:z orientation +// This record is optional, by default its turned off (it can always be calculated later - Python) +// +// {'LOC':{'Lat':f,'Lon':f,'Alt':f}} +// GPS location record containing Lat:latitude in degrees Lon:longitude in degrees Alt:altitude in meters +// +// {'TIM':{'Upt':i,'Frq':i,'Sec':i}} +// Time record containing Upt:up time seconds Frq:counter frequency Sec:time string +// +// {'STS':{'Qsz':i,'Mis':i,'Ter':i,'Htu':i,'Bmp':i,'Acl':i,'Mag':i, 'Gps':i}} +// Status record containing Qsz:events on queue Mis:missed events Ter:buffer error +// Htu:status Bmp:status Acl:status Mag:status Gps:ststus +// +// {'EVT':{'Evt':i,'Frq':i,'Tks':i,'Etm':f,'Adc':[[i,i,i,i,i,i,i,i][i,i,i,i,i,i,i,i]]}} +// Event record containing Evt:event number in second Frq:timer frequency Tks:ticks since last event in second +// Etm:event time stamp to 100ns Adc:[[Channel 0 values][Channel 1 values]] + +// N.B. These records pass the data to a python monitor over the serial line. Python has awsome string handling and looks them up in +// associative arrays to build records of any arbitary format you want. So this is only the start of the story of record processing. +// N.B. Also some of these records are sent out at regular intervals and or when an event occurs. + +// This program also accepts commands sent to it on the serial line. +// When a command arrives it is immediatly executed. + #include #include -#include -#include "Adafruit_BMP085_U.h" // Barrometric pressure -#include "Adafruit_HTU21DF.h" // Humidity and temperature sensor -#include "Adafruit_GPS.h" // GPS chip -#include "Adafruit_L3GD20_U.h" // Magoscope +#include "Adafruit_BMP085_U.h" // Barrometric pressure + +#include "Adafruit_HTU21DF.h" // Humidity and temperature sensor + +// GPS chips typically return NMEA strings over a serial line. +// They can be programmed to send different NMEA strings according to what you configure. +// The string RMCGGA has the altitude but misses the yy/mm/dd from the date, it only has hh/mm/ss. +// This is easilly made up for in the Python monitor which gets this information from it system time. + +#include "Adafruit_GPS.h" // GPS chip +#define GPSECHO true +#define RMCGGA // Altitude on, yy/mm/dd off + +#include "Adafruit_L3GD20_U.h" // Magoscope + // WARNING: I had to modify this library, its no longer standard -#include "Adafruit_LSM303_U.h" // Accelerometer and magnentometer/compass -#include "Adafruit_10DOF.h" // 10DOF breakout driver - scale to SI units +#include "Adafruit_LSM303_U.h" // Accelerometer and magnentometer/compass +#include "Adafruit_10DOF.h" // 10DOF breakout driver - scale to SI units +// Configuration constants -//setup variables -#define VERS "2016/Apr/24" -#define ADC_BUF_LEN 300 // Number of ADC values per event -#define PPS_EVENTS 10 // The maximum number of events stored per second -#define EVENT_QSIZE 32 // The number of events that can be queued for serial output -#define ACL_PIN 10 // Accelarometer INT1 interrupt pin -#define GPS_BAUD_RATE 9600 // GPS and Serial1 line -#define BMPID 18001 //barometric pressure sensor -#define ACLID 30301 //accelerometer definition -#define MAGID 30302 //magnetometer definition -#define SERIAL_BAUD_RATE 9600 // Serial line -int textctr = 0; +// The size of the one second event buffer +#define PPS_EVENTS 8 // The maximum number of events stored per second +#define ADC_BUF_LEN 8 // Number of ADC values per event +// This is the event queue size +#define EVENT_QSIZE 32 // The number of events that can be queued for serial output +// Handle text buffer serial output overflow errors +// When the output buffer overflows due to data comming too fast, we just stop printing due +// to insufficient bandwidth or slow things down if HANDLE_OVERFLOW is set (not recomended) +// #define HANDLE_OVERFLOW -//setup hardware breakouts +// This is the text ring buffer for real time output to serial line with interrupt on +#define TBLEN 4096 // Serial line output ring buffer size -Adafruit_GPS gps(&Serial1); // GPS Serial1 on pins RX1 and TX1 +// Define some output debug pins to monitor whats going on via an oscilloscope +#define PPS_PIN 13 // PPS (Pulse Per Second) and LED +#define EVT_PIN 12 // Cosmic ray event detected +#define FLG_PIN 11 // Debug flag -Adafruit_HTU21DF htu = Adafruit_HTU21DF(); // Humidity and temperature measurment -boolean htu_ok = false; // Chip OK +// For siesmic event input +#define ACL_PIN 10 // Accelarometer INT1 interrupt pin -Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(BMPID); // Barometric pressure -boolean bmp_ok = false; +// Baud rates +#define SERIAL_BAUD_RATE 9600 // Serial line +#define GPS_BAUD_RATE 9600 // GPS and Serial1 line + +// Instantiate external hardware breakouts + +Adafruit_GPS gps(&Serial1); // GPS Serial1 on pins RX1 and TX1 +boolean gps_ok = false; // Chip OK flag + +Adafruit_HTU21DF htu = Adafruit_HTU21DF(); // Humidity and temperature measurment +boolean htu_ok = false; // Chip OK + +#define BMPID 18001 +Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(BMPID); // Barometric pressure +boolean bmp_ok = false; // The 10DOF isn't a chip, its just a utility to convert say mago values into headings etc -Adafruit_10DOF dof = Adafruit_10DOF(); // The 10 Degrees-Of-Freedom DOF breakout -boolean dof_ok = false; // board driver, scales units to SI -Adafruit_LSM303_Accel_Unified acl = Adafruit_LSM303_Accel_Unified(ACLID); // Accelerometer Compass - boolean acl_ok = false; -Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(MAGID); // Magoscope -boolean mag_ok = false; +Adafruit_10DOF dof = Adafruit_10DOF(); // The 10 Degrees-Of-Freedom DOF breakout +boolean dof_ok = false; // board driver, scales units to SI -//init measurement values and rates -uint32_t latlon_display_rate = 12; // Display latitude and longitude each X seconds -uint32_t humtmp_display_rate = 12; // Display humidity and HTU temperature each X seconds -uint32_t alttmp_display_rate = 12; // Display altitude and BMP temperature each X seconds -uint32_t frqutc_display_rate = 1; // Display frequency and UTC time each X seconds -uint32_t status_display_rate = 4; // Display status (UpTime, QueueSize, MissedEvents, HardwareOK) -uint32_t accelr_display_rate = 1; // Display accelarometer x,y,z -uint32_t magnot_display_rate = 12; // Display magnotometer data x,y,z -uint32_t events_display_size = 20; // Display events after recieving X events +#define ACLID 30301 +Adafruit_LSM303_Accel_Unified acl = Adafruit_LSM303_Accel_Unified(ACLID); // Accelerometer Compass + boolean acl_ok = false; + +#define MAGID 30302 +Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(MAGID); // Magoscope +boolean mag_ok = false; + +// Control the output data rates by setting defaults, these values can be modified at run time +// via commands from the serial interface. Some output like position isn't supposed to be changing +// very fast if at all, so no need to clutter up the serial line with it. The Python monitor keeps +// the last sent values when it builds event messages to be sent over the internet to the server +// or logged to a file. + +uint32_t latlon_display_rate = 12; // Display latitude and longitude each X seconds +uint32_t humtmp_display_rate = 12; // Display humidity and HTU temperature each X seconds +uint32_t alttmp_display_rate = 12; // Display altitude and BMP temperature each X seconds +uint32_t frqutc_display_rate = 1; // Display frequency and UTC time each X seconds +uint32_t status_display_rate = 4; // Display status (UpTime, QueueSize, MissedEvents, HardwareOK) +uint32_t accelr_display_rate = 1; // Display accelarometer x,y,z +uint32_t magnot_display_rate = 12; // Display magnotometer data x,y,z + +uint32_t events_display_size = 20; // Display events after recieving X events // Siesmic event trigger parameters -uint32_t accelr_event_threshold = 2; // Trigger level for siesmic events in milli-g -uint32_t accelr_event_cutoff_fr = 30; // Siesmic event cutoff frequency +uint32_t accelr_event_threshold = 2; // Trigger level for siesmic events in milli-g +uint32_t accelr_event_cutoff_fr = 30; // Siesmic event cutoff frequency + +// Commands can be sent over the serial line to configure the display rates or whatever + +typedef enum { + NOOP, // No-operation + HELP, // Help + HTUX, // Reset HTU chip + HTUD, // HUT display rate + BMPD, // BMP display rate + LOCD, // Location display rate + TIMD, // Timing display rate + STSD, // Status display rate + EVQT, // Event queue dump threshold + + ACLD, // Accelerometer display rate + MAGD, // Magnetometer display rate + + ACLT, // Accelerometer event threshold + + CMDS }; // Command count + +typedef struct { + int Id; // Command ID number + void (*proc)(int arg); // Function to call + char *Name; // Command name + char *Help; // Command help text + int Par; // Command parameter flag +} CmdStruct; + +// Function forward references + +void noop(int arg); +void help(int arg); +void htux(int arg); +void htud(int arg); +void bmpd(int arg); +void locd(int arg); +void timd(int arg); +void stsd(int arg); +void evqt(int arg); +void acld(int arg); +void magd(int arg); +void aclt(int arg); + +// Command table + +CmdStruct cmd_table[CMDS] = { + { NOOP, noop, "NOOP", "Do nothing", 0 }, + { HELP, help, "HELP", "Display commands", 0 }, + { HTUX, htux, "HTUX", "Reset the HTU chip", 0 }, + { HTUD, htud, "HTUD", "HTU Temperature-Humidity display rate", 1 }, + { BMPD, bmpd, "BMPD", "BMP Temperature-Altitude display rate", 1 }, + { LOCD, locd, "LOCD", "Location latitude-longitude display rate", 1 }, + { TIMD, timd, "TIMD", "Timing uptime-frequency-utc display rate", 1 }, + { STSD, stsd, "STSD", "Status info display rate", 1 }, + { EVQT, evqt, "EVQT", "Event queue dump threshold", 1 }, + { ACLD, acld, "ACLD", "Accelerometer display rate", 1 }, + { MAGD, magd, "MAGD", "Magnatometer display rate", 1 }, + { ACLT, aclt, "ACLT", "Accelerometer event trigger threshold", 1 } +}; + +#define CMDLEN 32 +static char cmd[CMDLEN]; // Command input buffer +static int irdp=0, irdy=0, istp=0; // Read, ready, stop + +static char txtb[TBLEN]; // Text ring buffer +static uint32_t txtw = 0, txtr = 0, // Write and Read indexes + tsze = 0, terr = 0; // Buffer size and error code -//define serial output ringbuffer -#define TBLEN 4096 // Serial line output ring buffer size -static char txtb[TBLEN]; // Text ring buffer -static uint32_t txtw = 0, txtr = 0, // Write and Read indexes - tsze = 0, terr = 0; // Buffer size and error code typedef enum { TXT_NOERR=0, TXT_TOOBIG=1, TXT_OVERFL=2 } TxtErr; -//serial output buffers #define TXTLEN 256 -static byte txt[TXTLEN]; // For writing to serial - -//timer initialisation -//PPS input to D2 -//Trigger input to D5 -//Clock rate is 83MHz/2, so approx 41.5Mhz +static char txt[TXTLEN]; // For writing to serial + +// Initialize the timer chips to measure time between the PPS pulses and the EVENT pulse +// The PPS enters pin D2, the PPS is forwarded accross an isolating diode to pin D5 +// The event pulse is also connected to pin D5. So D5 sees the LOR of the PPS and the +// event, while D2 sees only the PPS. In this way we measure the frequency of the +// clock MCLK/2 each second on the first counter, and the time between EVENTs on the second void TimersStart() { -uint32_t config = 0; -// Set up the power management controller for TC0 and TC2 -pmc_set_writeprotect(false); // Enable write access to power management chip -pmc_enable_periph_clk(ID_TC0); // Turn on power for timer block 0 channel 0 -pmc_enable_periph_clk(ID_TC6); // Turn on power for timer block 2 channel 0 + uint32_t config = 0; - // Timer block zero channel zero is connected only to the PPS - // We set it up to load register RA on each PPS and reset - // So RA will contain the number of clock ticks between two PPS, this - // value should be very stable +/- one tick + // Set up the power management controller for TC0 and TC2 -config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz - TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA0 - TC_CMR_ABETRG | // Use the TIOA external input line - TC_CMR_LDRA_RISING; // Latch counter value into RA + pmc_set_writeprotect(false); // Enable write access to power management chip + pmc_enable_periph_clk(ID_TC0); // Turn on power for timer block 0 channel 0 + pmc_enable_periph_clk(ID_TC6); // Turn on power for timer block 2 channel 0 -TC_Configure(TC0, 0, config); // Configure channel 0 of TC0 -TC_Start(TC0, 0); // Start timer running + // Timer block zero channel zero is connected only to the PPS + // We set it up to load regester RA on each PPS and reset + // So RA will contain the number of clock ticks between two PPS, this + // value should be very stable +/- one tick -TC0->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS -TC0->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources -NVIC_EnableIRQ(TC0_IRQn); // Enable interrupt handler for channel 0 + config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz + TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA0 + TC_CMR_ABETRG | // Use the TIOA external input line + TC_CMR_LDRA_RISING; // Latch counter value into RA + TC_Configure(TC0, 0, config); // Configure channel 0 of TC0 + TC_Start(TC0, 0); // Start timer running -// Timer block 2 channel zero is connected to the Trigger signal from a cosmic ray event + TC0->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS + TC0->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources + NVIC_EnableIRQ(TC0_IRQn); // Enable interrupt handler for channel 0 + + // Timer block 2 channel zero is connected to the OR of the PPS and the RAY event - config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz - TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA1 - TC_CMR_ABETRG | // Use the TIOA external input line - TC_CMR_LDRA_RISING; // Latch counter value into RA - -TC_Configure(TC2, 0, config); // Configure channel 0 of TC2 -TC_Start(TC2, 0); // Start timer running + config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz + TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA1 + TC_CMR_ABETRG | // Use the TIOA external input line + TC_CMR_LDRA_RISING; // Latch counter value into RA + + TC_Configure(TC2, 0, config); // Configure channel 0 of TC2 + TC_Start(TC2, 0); // Start timer running -TC2->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS -TC2->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources -NVIC_EnableIRQ(TC6_IRQn); // Enable interrupt handler for channel 0 + TC2->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS + TC2->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources + NVIC_EnableIRQ(TC6_IRQn); // Enable interrupt handler for channel 0 -// Set up the PIO controller to route input pins for TC0 and TC2 -PIO_Configure(PIOC,PIO_INPUT, - PIO_PB25B_TIOA0, // D2 Input - PIO_DEFAULT); -PIO_Configure(PIOC,PIO_INPUT, - PIO_PC25B_TIOA6, // D5 Input - PIO_DEFAULT); + // Set up the PIO controller to route input pins for TC0 and TC2 + + PIO_Configure(PIOC,PIO_INPUT, + PIO_PB25B_TIOA0, // D2 Input + PIO_DEFAULT); + + PIO_Configure(PIOC,PIO_INPUT, + PIO_PC25B_TIOA6, // D5 Input + PIO_DEFAULT); } -// Timer0 system interrupt handlers and variables -static uint32_t ppsfl = LOW, // PPS Flag boolean - rega0 = 0, // RA reg - stsr0 = 0, // Interrupt status register - ppcnt = 0; // PPS count +// Timer chip interrupt handlers try to get time stamps to within 4 system clock ticks + +static uint32_t displ = 0; // Display values in loop + +static uint32_t ppsfl = LOW, // PPS Flag boolean + rega0 = 0, // RA reg + stsr0 = 0, // Interrupt status register + ppcnt = 0; // PPS count // Handle the PPS interrupt in counter block 0 ISR void TC0_Handler() { -// This ISR is run only when the PPS (Pulse Per Second) GPS event is detected -rega0 = TC0->TC_CHANNEL[0].TC_RA; // Read the RA reg (PPS period) and store in rega0 -stsr0 = TC_GetStatus(TC0, 0); // Read status and clear load bits, reset interrupt on exit -ppcnt++; // PPS count increase + + // This ISR is connected only to the PPS (Pulse Per Second) GPS event + // Each time this runs, set the flag to tell the TC6 ISR we have seen it + // This logic only works if the TC0 handler gets called before the TC6 handler + // hence the debug flag which I look at with a scope to be sure. + // I may introduce a small delay line to ensure this is true, so far it is. + + ppsfl = HIGH; // Seen a rising edge on the PPS +#if FLG_PIN + digitalWrite(FLG_PIN,ppsfl); // Flag set (for debug) +#endif + rega0 = TC0->TC_CHANNEL[0].TC_RA; // Read the RA reg (PPS period) + stsr0 = TC_GetStatus(TC0, 0); // Read status and clear load bits + + ppcnt++; // PPS count + displ = 1; // Display stuff in the loop } -// Timer0 system interrupt handlers and variables -static uint32_t rega1 = 0, //register for TC6 to be written into - eventcnt = 0, //event counter - stsr1 = 0; //Interrupt status register +// We need a double buffer, one is being written by the ISR while +// the other is read from user space within one second. + +struct Event { + uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values + uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values + uint32_t Tks; // Time since last event in ticks +}; + +static struct Event b1[PPS_EVENTS]; // Event ticks buffeer +static struct Event b2[PPS_EVENTS]; // Event ticks buffer +static struct Event *wbuf = b1; // Write event buffer pointer and its index +static struct Event *rbuf = b2; // Read event buffer pointer and its index +static int ridx, widx; + +// We also need a time value for the current and previous second + +#ifdef RMCGGA +#define DATE_TIME_LEN 9 +#else +#define DATE_TIME_LEN 17 +#endif + +static char t1[DATE_TIME_LEN]; // Date time buffer text string +static char t2[DATE_TIME_LEN]; +static char *wdtm = t1; // Write date/time pointer +static char *rdtm = t2; // Read date/time pointer + +// Swap read write event buffers and indexes along with their time strings +// each second, so we have the current and previous second numbers + +void SwapBufs() { + struct Event *tbuf; // Temp event buf pointer + char *tdtm; // Temp date/time string pointer + tbuf = rbuf; rbuf = wbuf; wbuf = tbuf; // Swap write with read buffer + ridx = widx; widx = 0; // Write count to read, reset the write count + tdtm = rdtm; rdtm = wdtm; wdtm = tdtm; // And swap asociated buffer date/time +} + +// Handle isolated PPS (via diode) LOR with the Event +// The diode is needed to block Event pulses getting back to TC0 +// LOR means Logical inclusive OR + +static uint32_t rega1, stsr1 = 0; void TC6_Handler() { -// This ISR is connected to the event trigger -//insert here the indexing of the timer for events and ADC reading -//dump the value of the timer at event into the event tcks variable -//wbuf[widx].Tks = TC0->TC_CHANNEL[0].TC_CV; -//get the ADC data, copy the DMA buffer into a new buffer -//AdcPullData(&wbuf[widx]); -// widx++; - eventcnt++; - stsr1 = TC_GetStatus(TC2, 0); // Read status clear load bits, reset interrupt + // This ISR is connected to the OR of the event and the PPS + // If the TC0 has seen the PPS it sets the flag high + // and if its high we are seeing the PPS here, but if the + // flag is not set then this is a cosmic ray event. + + if (ppsfl == HIGH) { // Was ther a PPS ? + ppsfl = LOW; // Yes so we have seen it here + SwapBufs(); // Every PPS swap the read/write buffers +#if EVT_PIN + digitalWrite(EVT_PIN,LOW); // Not an event +#endif + } else { +#if EVT_PIN + digitalWrite(EVT_PIN,HIGH); // Event detected +#endif + if (widx < PPS_EVENTS) { // Up to PPS_EVENTS stored per PPS + + // Read the latched tick count getting the event time + // and then pull the ADC pipe line + + wbuf[widx].Tks = TC2->TC_CHANNEL[0].TC_RA; + AdcPullData(&wbuf[widx]); + widx++; + } + } +#if FLG_PIN + digitalWrite(FLG_PIN,ppsfl); // Flag out +#endif + rega1 = TC2->TC_CHANNEL[0].TC_RA; // Read the RA on channel 1 (PPS period) + stsr1 = TC_GetStatus(TC2, 0); // Read status clear load bits } +// Accelerometer setup +// These settings come from reading the LSM303DLH doccumentation, which is more than the +// author of the Adda_fruit library did. It is a badly written load of rubbish, I have +// been forced to correct some bugs and export some private methods. It was a touch and go +// descision wether to just not use the library at all and do it all here. For now I will +// use it with the bug corrections. -//Accelerometer setup +// The following setup makes the accelarometer compare G-forces against a threshold value, and +// latch the output registers until they are read. To avoid excessive interrupt rates the high +// pass filter has been configured to keep the frequency low. + +// N.B. It took me a day to find out that I needed to use Active low and Open drain on the INT1 +// signal, otherwise the Adda_fruit module wont pass it on. Beware !!! + void AclSetup() { -uint8_t tmp, val; -//abort if accelerometer status if fault. -if (!acl_ok) return; + uint8_t tmp, val; -//define accelerometer setuo variables -#define PMD 0x20 // Normal power mode (PM0=1,PM1=0:Normal) -#define DRT 0x00 // Data rate 50 Hz (0x08 = 100Hz) -#define AEN 0x07 // XYZ Enabled + if (!acl_ok) return; -//concatenate and write to accelerometer chip -val = PMD | DRT | AEN; -acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, val); +#define PMD 0x20 // Normal power mode (PM0=1,PM1=0:Normal) +#define DRT 0x00 // Data rate 50 Hz (0x08 = 100Hz) +#define AEN 0x07 // XYZ Enabled -//setup frequency filters for acceleration events -#define HPE1 0x04 // High pass filter Int 1 on -#define HPCF 0x03 // High pass cut off frequency + val = PMD | DRT | AEN; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, val); -//concatenate and write to accelerometer chip -val = HPE1 | HPCF; -acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG2_A, val); +#define HPE1 0x04 // High pass filter Int 1 on +#define HPCF 0x03 // High pass cut off frequency -//Define latching behaviour -#define LIR1 0x06 // Latch Int1 bit Data ready -#define LIR2 0x00 // Latch Int2 bit Data ready (0x20 Latch On) -#define IHL_OD 0xC0 // Interrupt active low, open drain (Argh !!!) + val = HPE1 | HPCF; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG2_A, val); -//concatenate and write to accelerometer chip -val = LIR1 | LIR2 | IHL_OD; -acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG3_A, val); +#define LIR1 0x06 // Latch Int1 bit Data ready +#define LIR2 0x00 // Latch Int2 bit Data ready (0x20 Latch On) -//define internal gain of accelerometer -#define BDU_FS 0x80 // Block data and scale +-2g +#define IHL_OD 0xC0 // Interrupt active low, open drain (Argh !!!) -//concatenate and write to accelerometer chip -val = BDU_FS; -acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG4_A, val); + val = LIR1 | LIR2 | IHL_OD; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG3_A, val); -//define directions and maximum values -#define XYZ_HI 0x2A // Hi values ZHIE YHIE XHIE -#define AOI_6D 0x00 // 0xC0 would enable 6 directions +#define BDU_FS 0x80 // Block data and scale +-2g -//concatenate and write to accelerometer chip -val = XYZ_HI | AOI_6D; -acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_CFG_A, val); + val = BDU_FS; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG4_A, val); -//set threshold -val = accelr_event_threshold & 0x7F; -acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); +#define XYZ_HI 0x2A // Hi values ZHIE YHIE XHIE +#define AOI_6D 0x00 // 0xC0 would enable 6 directions + + val = XYZ_HI | AOI_6D; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_CFG_A, val); -//attach interrupt for acceleration events -attachInterrupt(digitalPinToInterrupt(ACL_PIN),Acl_ISR,RISING); -//accelerometer setup complete + val = accelr_event_threshold & 0x7F; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); + + // The chip make very wide pulses (100ms), the values on the rising and + // falling edges are different ! + + attachInterrupt(digitalPinToInterrupt(ACL_PIN),Acl_ISR,RISING); } // Magnatometer setup, again the Adda_fruit library was inadequate. void MagSetup() { -uint8_t val; -//abort if magnetometer status fault -if (!mag_ok) return; + uint8_t val; -//initialise variables on mag board -val = 0; -mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, val); + if (!mag_ok) return; -//set the gain -#define GAIN 0x80 // +- 4.0 Gauss -val = GAIN; -mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, val); + val = 0; + mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, val); -//define conversion mode -#define MODE 0x0 // 01=Single conversion mode -val = MODE; -mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, val); -//magnetometer setup completed +#define GAIN 0x80 // +- 4.0 Gauss + + val = GAIN; + mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, val); + +#define MODE 0x0 // 01=Single conversion mode + + val = MODE; + mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, val); } -//Accelerometer ISR +// This Accelerometer ISR + static uint32_t accl_icount = 0, accl_flag = 0; + void Acl_ISR() { -//Set the bitmask for accelerometer interrupt + #define IA 0x40 -//increment accelerometer event count -accl_icount++; -//read the device status into variable -accl_flag = AclReadStatus(); -//launch transmission routine for this variable -//PushVib(); + + accl_icount++; + accl_flag = AclReadStatus(); + PushVib(); } // Read accelerometer status // This just reads the interrupt source INT1 and the overrun status. // It returns 1 bit for X, Y, or Z (0..7) if the threshold value is exceeded. // This determins if the board is being shaken - Earth quake - or other reason + static uint8_t acl_sts = 0; static uint8_t acl_src = 0; -uint8_t AclReadStatus() { - uint8_t rval; - acl_src = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_SOURCE_A); -#define ZH 0x20 // Z High +uint8_t AclReadStatus() { + uint8_t rval; + + acl_src = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_SOURCE_A); + +#define ZH 0x20 // Z High #define YH 0x08 // Y High #define XH 0x02 // X High - rval = 0; - if (acl_src & IA) { - if (acl_src & ZH) rval |= 4; - if (acl_src & YH) rval |= 2; - if (acl_src & XH) rval |= 1; - } - if (rval) - acl_sts = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_STATUS_REG_A); - return rval; + rval = 0; + if (acl_src & IA) { + if (acl_src & ZH) rval |= 4; + if (acl_src & YH) rval |= 2; + if (acl_src & XH) rval |= 1; + } + if (rval) + acl_sts = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_STATUS_REG_A); + return rval; } - // Set up the ADC channels + void AdcSetup() { - ADC->ADC_MR = 0x10380180; // Free running maximum speed - ADC -> ADC_CHER = 0x03; // enable ADC on pin A6 and A7 + REG_ADC_MR = 0x10380080; // Free run as fast as you can + REG_ADC_CHER = 3; // Channels 0 and 1 } -// Extract NMEA data string from the GPS chip +// Pull all data (16 values) from the ADC into a buffer + +uint8_t AdcPullData(struct Event *b) { + + int i; + + for (i=0; iADC_ISR & 0x01)==0); // Wait for channel 0 (2.5us) + b->Ch0[i] = (uint16_t) ADC->ADC_CDR[0]; // Read ch 0 + while((ADC->ADC_ISR & 0x02)==0); // Wait for channel 1 (2.5us) + b->Ch1[i] = (uint16_t) ADC->ADC_CDR[1]; // Read ch 1 + } +} + +// This is the nmea data string from the GPS chip -//define required variables #define GPS_STRING_LEN 256 static char gps_string[GPS_STRING_LEN + 1]; + float latitude = 0.0, longitude = 0.0, altitude = 0.0; -// We also need a time value for the current and previous second -#ifdef RMCGGA -#define DATE_TIME_LEN 9 -#else -#define DATE_TIME_LEN 17 -#endif - -static char t1[DATE_TIME_LEN]; // Date time buffer text string -static char t2[DATE_TIME_LEN]; -static char *wdtm = t1; // Write date/time pointer -static char *rdtm = t2; // Read date/time pointer // This function is dependent on the GPS chip implementation // It should return a date time string as described above @@ -309,65 +567,60 @@ static char *rdtm = t2; // Read date/time pointer // Here I am using the addafruit GPS chip char *GetDateTime() { - int i = 0; - while (Serial1.available()) { - if (i < GPS_STRING_LEN) { - gps_string[i++] = (char) Serial1.read(); - gps_string[i] = 0; - } else i++; - } - if (gps.parse(gps_string)) { -#ifdef RMCGGA - // I choose RMCGGA by default, and get the altitude but no date. - // Its easy to get the date once the records arrive at the Python end. - // The GPS altitude is far more accurate than the barrometric altitude. - // Warning: The syntax can not be changed, we need an integer hhmmss -sprintf(wdtm, - "%02d%02d%02d", - gps.hour, - gps.minute, - gps.seconds); -altitude = gps.altitude; + int i = 0; + while (Serial1.available()) { + if (i < GPS_STRING_LEN) { + gps_string[i++] = (char) Serial1.read(); + gps_string[i] = 0; + } else i++; + } + if (gps.parse(gps_string)) { + +#ifdef RMCGGA + // I choose RMCGGA by default, and get the altitude but no date. + // Its easy to get the date once the records arrive at the Python end. + // The GPS altitude is far more accurate than the barrometric altitude. + // Warning: The syntax can not be changed, we need an integer hhmmss + + sprintf(wdtm,"%02d%02d%02d", + gps.hour,gps.minute,gps.seconds); + + altitude = gps.altitude; #else -sprintf(wdtm, - "%02d%02d%02d%02d%02d%02d%02d%02d", - gps.year, - gps.month, - gps.day, - gps.hour, - gps.minute, - gps.seconds); + sprintf(wdtm,"%02d%02d%02d%02d%02d%02d%02d%02d", + gps.year,gps.month,gps.day, + gps.hour,gps.minute,gps.seconds); -altitude = 0; -#endif - latitude = gps.latitudeDegrees; // Easy place to get location - longitude = gps.longitudeDegrees; // Works well in Google maps - return rdtm; - } else - return NULL; + altitude = 0; +#endif + latitude = gps.latitudeDegrees; // Easy place to get location + longitude = gps.longitudeDegrees; // Works well in Google maps + + return rdtm; + } else + return NULL; } - -//Create queue system // Implement queue access mechanism for events, each second the user space (loop) copies // any events it has read onto the queue + struct EventBuf { - char DateTime[DATE_TIME_LEN]; // The date and time string - uint32_t Frequency; // The current clock frequency - uint32_t Ticks; // The number of ticks since the last event or PPS if none - uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values - uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values - uint8_t Count; // The number of events since the PPS + char DateTime[DATE_TIME_LEN]; // The date and time string + uint32_t Frequency; // The current clock frequency + uint32_t Ticks; // The number of ticks since the last event or PPS if none + uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values + uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values + uint8_t Count; // The number of events since the PPS }; typedef struct { - uint8_t Size; // Current size of the queue - uint8_t RdPtr; // Read pointer - uint8_t WrPtr; // Write pointer - uint8_t Missed; // Missed events counter due to overflow - uint8_t Lock; // The queue spin lock (not needed here) - struct EventBuf Events[EVENT_QSIZE]; // Queued events + uint8_t Size; // Current size of the queue + uint8_t RdPtr; // Read pointer + uint8_t WrPtr; // Write pointer + uint8_t Missed; // Missed events counter due to overflow + uint8_t Lock; // The queue spin lock (not needed here) + struct EventBuf Events[EVENT_QSIZE]; // Queued events } EventQueue; static EventQueue event_queue; @@ -377,453 +630,484 @@ static EventQueue event_queue; uint8_t PutQueue(struct EventBuf *ebuf) { - EventQueue *q = &event_queue; + EventQueue *q = &event_queue; - while(q->Lock) {}; q->Lock = 1; // Spin lock on queue - q->Events[q->WrPtr] = *ebuf; // Write event to the queue - q->WrPtr = (q->WrPtr + 1) % EVENT_QSIZE;// Increment the write pointer - if (q->Size < EVENT_QSIZE) q->Size++; // If we are overwriting old enties that havnt been read - else { - q->Missed++; // Say we missed some events - q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; // and throw the oldest event away - } - q->Lock = 0; - return q->Missed; + while(q->Lock) {}; q->Lock = 1; // Spin lock on queue + q->Events[q->WrPtr] = *ebuf; // Write event to the queue + q->WrPtr = (q->WrPtr + 1) % EVENT_QSIZE;// Increment the write pointer + if (q->Size < EVENT_QSIZE) q->Size++; // If we are overwriting old enties that havnt been read + else { + q->Missed++; // Say we missed some events + q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; // and throw the oldest event away + } + q->Lock = 0; + return q->Missed; } // Pop an event off the queue, if the queue is empty nothing happens // the queue size is zero when the queue is empty, and this is the // return value -uint8_t PopQueue(struct EventBuf *ebuf) { // Points to where the caller wants the event stored +uint8_t PopQueue(struct EventBuf *ebuf) { // Points to where the caller wants the event stored - EventQueue *q = &event_queue; + EventQueue *q = &event_queue; - while(q->Lock) {}; q->Lock = 1; // Spin lock on queue - if (q->Size) { - *ebuf = q->Events[q->RdPtr]; - q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; - q->Size--; - } - q->Lock = 0; - return q->Size; + while(q->Lock) {}; q->Lock = 1; // Spin lock on queue + if (q->Size) { + *ebuf = q->Events[q->RdPtr]; + q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; + q->Size--; + } + q->Lock = 0; + return q->Size; } // Get the size of the queue uint8_t SzeQueue() { - EventQueue *q = &event_queue; + EventQueue *q = &event_queue; - return q->Size; + return q->Size; } // Initialize the queue void InitQueue() { - EventQueue *q = &event_queue; + EventQueue *q = &event_queue; - q->Lock = 1; - q->Size = 0; - q->RdPtr = 0; - q->WrPtr = 0; - q->Missed = 0; - q->Lock = 0; + q->Lock = 1; + q->Size = 0; + q->RdPtr = 0; + q->WrPtr = 0; + q->Missed = 0; + q->Lock = 0; } - +// Arduino setup function, initialize hardware and software +// This is the first function to be called when the sketch is started void setup() { -Serial.begin(9600); -SerialUSB.begin(0); // Start the serial line -Serial1.begin(GPS_BAUD_RATE); // and the second -gps.begin(GPS_BAUD_RATE); // Chip baud rate +#if FLG_PIN + pinMode(FLG_PIN, OUTPUT); // Pin for the ppsfl flag for debug +#endif +#if EVT_PIN + pinMode(EVT_PIN, OUTPUT); // Pin for the cosmic ray event +#endif +#if PPS_PIN + pinMode(PPS_PIN, OUTPUT); // Pin for the PPS (LED pin) +#endif + + Serial.begin(SERIAL_BAUD_RATE); // Start the serial line + Serial1.begin(GPS_BAUD_RATE); // and the second + + gps.begin(GPS_BAUD_RATE); // Chip baud rate #ifdef RMCGGA - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); // With altitude but no yy/mm/dd + gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); // With altitude but no yy/mm/dd #else - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); // With yy/mm/dd but no altitude + gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); // With yy/mm/dd but no altitude #endif - gps.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // each second + gps.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // each second -//InitQueue(); // Reset queue pointers, missed count, and size + InitQueue(); // Reset queue pointers, missed count, and size -strcpy(rdtm,""); // Set initial value for date/time -strcpy(wdtm,""); + strcpy(rdtm,""); // Set initial value for date/time + strcpy(wdtm,""); -htu_ok = htu.begin(); -bmp_ok = bmp.begin(); -acl_ok = acl.begin(); -mag_ok = mag.begin(); -dof_ok = dof.begin(); + htu_ok = htu.begin(); + bmp_ok = bmp.begin(); + acl_ok = acl.begin(); + mag_ok = mag.begin(); + dof_ok = dof.begin(); -AclSetup(); -MagSetup(); -//AdcSetup(); - -TimersStart(); // Start timers + AclSetup(); + MagSetup(); + AdcSetup(); + + TimersStart(); // Start timers } -void PushHtu(int flg) { // If flg is true always push +// These two routines are needed because the Serial.print method prints without using interrupts. +// Calls to Serial.print block interrupts and use a wait in kernel space causing all ISRs to +// be blocked and hence we could miss some timer interrupts. +// To avoid this problem call PushTxt to have stuff delivered to the serial line, PushTxt simply +// stores your text for future print out by PutChar. The PutChar routine removes one character +// from the stored text each time its called. By placing a call to PutChar in the outermost loop +// of the Arduino loop function, then for each loop one character is printed, avoiding blocking +// of interrupts and vastly improving the loops real time behaviour. - float temph = 0.0; - float humid = 0.0; -//Serial.println("testingHTU"); - if ((flg) || ((htu_ok) && ((ppcnt % humtmp_display_rate) == 0))) { - //Serial.println("reading"); - temph = htu.readTemperature(); - humid = htu.readHumidity(); - //sprintf(txt,"{'HTU':{'Tmh':%5.3f,'Hum':%4.1f}}\n",temph,humid); - txt[textctr] = 1;//code for htu - textctr++; - byte *bufbyteone = (byte *)&temph; - memcpy(&txt[textctr],&bufbyteone[0],sizeof(bufbyteone)); - textctr = textctr + sizeof(bufbyteone); - byte *bufbytetwo = (byte *)&humid; - memcpy(&txt[textctr],&bufbytetwo[0],sizeof(bufbytetwo)); - textctr = textctr + sizeof(bufbytetwo); - txt[textctr] = 10; - textctr++; - Serial.println(1); - Serial.println(temph); - Serial.println(humid); - Serial.println(); - } +// Copy text to the buffer for future printing + +void PushTxt(char *txt) { + + int i, l = strlen(txt); + + // If this happens there is a programming bug + + if (l > TBLEN) { // Can't handle more than TBLEN at a time + terr = TXT_TOOBIG; // say error and abort + return; + } + + // If the buffer is filling up to fast throw it away and return an error + + if ((l + tsze) >= TBLEN) { // If there is no room in the buffer + terr = TXT_OVERFL; // Buffer overflow + return; // Simply stop printing when txt comming too fast + } + + // Copy the new text onto the ring buffer for later output + // from the loop idle function + + for (i=0; i Earth Quake + +uint32_t old_icount = 0; + + if (accl_flag) { + if (accl_icount != old_icount) { + old_icount = accl_icount; + PushTim(1); // Push these first, and then vib + PushAcl(1); // This is the real latched value + PushMag(1); + sprintf(txt,"{'VIB':{'Vax':%d,'Vcn':%d}}\n",accl_flag,accl_icount); + PushTxt(txt); + } + } } -void PushTim(int flg) { -//time not really working - no pps for testing - if ((flg) || ((ppcnt % frqutc_display_rate) == 0)) { - //sprintf(txt,"{'TIM':{'Upt':%4d,'Frq':%7d,'Sec':%s}}\n",ppcnt,rega0,rdtm); - //PushTxt(txt); - txt[textctr] = 9;//code for htu - textctr++; - byte *bufbytenine = (byte *)&ppcnt; - memcpy(&txt[textctr],&bufbytenine[0],sizeof(bufbytenine)); - textctr = textctr + sizeof(bufbytenine); - byte *bufbyteten = (byte *)®a0; - memcpy(&txt[textctr],&bufbyteten[0],sizeof(bufbyteten)); - textctr = textctr + sizeof(bufbyteten); - byte *bufbyteeleven = (byte *)&rdtm; - memcpy(&txt[textctr],&bufbyteeleven[0],sizeof(bufbyteeleven)); - textctr = textctr + sizeof(bufbyteeleven); - txt[textctr] = 10; - textctr++; - Serial.println(9); - Serial.println(ppcnt); - Serial.println(rega0); - Serial.println(rdtm); - Serial.println(); - } -} - -void PushMag(int flg) { // Push the mago stuff - sensors_event_t mag_event; - sensors_vec_t xyz; - Serial.print("magsensstart"); - //if ((flg) || ((mag_ok) && ((ppcnt % magnot_display_rate) == 0))) { - mag.getEvent(&mag_event); - Serial.print("magread"); - // Micro Tesla - - //sprintf(txt,"{'MAG':{'Mgx':%f,'Mgy':%f,'Mgz':%f}}\n", - // mag_event.magnetic.x, - // mag_event.magnetic.y, - // mag_event.magnetic.z); -// PushTxt(txt); - txt[textctr] = 4;//code for htu - textctr++; - byte *bufbytetwelve = (byte *)&mag_event.magnetic.x; - memcpy(&txt[textctr],&bufbytetwelve[0],sizeof(bufbytetwelve)); - textctr = textctr + sizeof(bufbytetwelve); - byte *bufbytethirteen = (byte *)&mag_event.magnetic.y; - memcpy(&txt[textctr],&bufbytethirteen[0],sizeof(bufbytethirteen)); - textctr = textctr + sizeof(bufbytethirteen); - byte *bufbytefourteen = (byte *)&mag_event.magnetic.z; - memcpy(&txt[textctr],&bufbytefourteen[0],sizeof(bufbytefourteen)); - textctr = textctr + sizeof(bufbytefourteen); - txt[textctr] = 10; - textctr++; - Serial.println(4); - Serial.println(mag_event.magnetic.x); - Serial.println(mag_event.magnetic.y); - Serial.println(mag_event.magnetic.z); - Serial.println(); - - - // Orientation (Easy to calculate later in Python - dont waste resources) -//#ifdef ORIENTATION -// if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &xyz)) { -// sprintf(txt,"{'MOG':{'Mox':%f,'Moy':%f,'Moz':%f}}\n",xyz.x,xyz.y,xyz.z); -// PushTxt(txt); -// } -//#endif - // } +// Push the magnetic field strengths in all three axis in micro tesla (gauss) + +void PushMag(int flg) { // Push the mago stuff + sensors_event_t mag_event; + sensors_vec_t xyz; + + if ((flg) || ((mag_ok) && ((ppcnt % magnot_display_rate) == 0))) { + mag.getEvent(&mag_event); + + // Micro Tesla + + sprintf(txt,"{'MAG':{'Mgx':%f,'Mgy':%f,'Mgz':%f}}\n", + mag_event.magnetic.x, + mag_event.magnetic.y, + mag_event.magnetic.z); + PushTxt(txt); + + // Orientation (Easy to calculate later in Python - dont waste resources) +#ifdef ORIENTATION + if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &xyz)) { + sprintf(txt,"{'MOG':{'Mox':%f,'Moy':%f,'Moz':%f}}\n",xyz.x,xyz.y,xyz.z); + PushTxt(txt); + } +#endif + } } +// Push the acceleration values in xyz in meters per sec squared void PushAcl(int flg) { // Push the accelerometer and compass stuff - sensors_event_t acl_event; - sensors_vec_t xyz; -Serial.print("going to get accelerator values"); - if ((flg) || ((acl_ok) && ((ppcnt % accelr_display_rate) == 0))) { - acl.getEvent(&acl_event); + sensors_event_t acl_event; + sensors_vec_t xyz; - // Meters per second squared + if ((flg) || ((acl_ok) && ((ppcnt % accelr_display_rate) == 0))) { + acl.getEvent(&acl_event); - //sprintf(txt,"{'ACL':{'Acx':%f,'Acy':%f,'Acz':%f}}\n", - //acl_event.acceleration.x, - //acl_event.acceleration.y, - //acl_event.acceleration.z); - //PushTxt(txt); - txt[textctr] = 6;//code for htu - textctr++; - byte *bufbytefifteen = (byte *)&acl_event.acceleration.x; - memcpy(&txt[textctr],&bufbytefifteen[0],sizeof(bufbytefifteen)); - textctr = textctr + sizeof(bufbytefifteen); - byte *bufbytesixteen = (byte *)&acl_event.acceleration.y; - memcpy(&txt[textctr],&bufbytesixteen[0],sizeof(bufbytesixteen)); - textctr = textctr + sizeof(bufbytesixteen); - byte *bufbyteseventeen = (byte *)&acl_event.acceleration.z; - memcpy(&txt[textctr],&bufbyteseventeen[0],sizeof(bufbyteseventeen)); - textctr = textctr + sizeof(bufbyteseventeen); - txt[textctr] = 10; - textctr++; - Serial.println(6); - Serial.println(acl_event.acceleration.x); - Serial.println(acl_event.acceleration.y); - Serial.println(acl_event.acceleration.z); - Serial.println(); + // Meters per second squared - /* - // Orientation (Easy to calculate later in Python - dont waste resources) -#ifdef ORIENTATION - if (dof.accelGetOrientation(&acl_event, &xyz)) { - sprintf(txt,"{'AOL':{'Aox':%f,'Aoy':%f,'Aoz':%f}}\n",xyz.x,xyz.y,xyz.z); - PushTxt(txt); - } + sprintf(txt,"{'ACL':{'Acx':%f,'Acy':%f,'Acz':%f}}\n", + acl_event.acceleration.x, + acl_event.acceleration.y, + acl_event.acceleration.z); + PushTxt(txt); + + // Orientation (Easy to calculate later in Python - dont waste resources) +#ifdef ORIENTATION + if (dof.accelGetOrientation(&acl_event, &xyz)) { + sprintf(txt,"{'AOL':{'Aox':%f,'Aoy':%f,'Aoz':%f}}\n",xyz.x,xyz.y,xyz.z); + PushTxt(txt); + } #endif -*/ - - } + } } +// Push location latitude longitude in degrees so that google maps gets it correct + +void PushLoc(int flg) { + + if ((flg) || ((ppcnt % latlon_display_rate) == 0)) { + sprintf(txt,"{'LOC':{'Lat':%f,'Lon':%f,'Alt':%f}}\n",latitude,longitude,altitude); + PushTxt(txt); + } +} + +// Push timing + +void PushTim(int flg) { + + if ((flg) || ((ppcnt % frqutc_display_rate) == 0)) { + sprintf(txt,"{'TIM':{'Upt':%4d,'Frq':%7d,'Sec':%s}}\n",ppcnt,rega0,rdtm); + PushTxt(txt); + } +} + +// Push status + void PushSts(int flg, int qsize, int missed) { uint8_t res; - if ((flg) || ((ppcnt % status_display_rate) == 0)) { - //sprintf(txt,"{'STS':{'Qsz':%2d,'Mis':%2d,'Ter':%d,'Htu':%d,'Bmp':%d,'Acl':%d,'Mag':%d}}\n", - // qsize,missed,terr,htu_ok,bmp_ok,acl_ok,mag_ok); - //PushTxt(txt); - terr = 0; - - txt[textctr] = 10;//code for status - textctr++; - byte *bufbyteseventeenptone = (byte *)&qsize; - memcpy(&txt[textctr],&bufbyteseventeenptone[0],sizeof(bufbyteseventeenptone)); - textctr = textctr + sizeof(bufbyteseventeenptone); - byte *bufbyteseventeenpttwo = (byte *)&missed; - memcpy(&txt[textctr],&bufbyteseventeenpttwo[0],sizeof(bufbyteseventeenpttwo)); - textctr = textctr + sizeof(bufbyteseventeenpttwo); - byte *bufbyteeighteen = (byte *)&terr; - memcpy(&txt[textctr],&bufbyteeighteen[0],sizeof(bufbyteeighteen)); - textctr = textctr + sizeof(bufbyteeighteen); - - if (htu_ok) - { - txt[textctr] = B00000001; - textctr++; - } - else - { - txt[textctr] = B00000000; - textctr++; - } - - //byte *bufbytenineteen = (byte *)&htu_ok; - //memcpy(&txt[textctr],&bufbytenineteen[0],sizeof(bufbytenineteen)); - //textctr = textctr + sizeof(bufbytenineteen); - - if (bmp_ok) - { - txt[textctr] = B00000001; - textctr++; - } - else - { - txt[textctr] = B00000000; - textctr++; - } - //byte *bufbytetwenty = (byte *)&bmp_ok; - //memcpy(&txt[textctr],&bufbytetwenty[0],sizeof(bufbytetwenty)); - //textctr = textctr + sizeof(bufbytetwenty); - - if (acl_ok) - { - txt[textctr] = B00000001; - textctr++; - } - else - { - txt[textctr] = B00000000; - textctr++; - } - - //byte *bufbytetwentyone = (byte *)&acl_ok; - //memcpy(&txt[textctr],&bufbytetwentyone[0],sizeof(bufbytetwentyone)); - //textctr = textctr + sizeof(bufbytetwentyone); - - if (mag_ok) - { - txt[textctr] = B00000001; - textctr++; - } - else - { - txt[textctr] = B00000000; - textctr++; - } - //byte *bufbytetwentytwo = (byte *)&mag_ok; - //memcpy(&txt[textctr],&bufbytetwentytwo[0],sizeof(bufbytetwentytwo)); - //textctr = textctr + sizeof(bufbytetwentytwo); - - txt[textctr] = 10; - textctr++; - - Serial.println(10); - - Serial.println(qsize); - Serial.println(missed); - Serial.println(terr); - Serial.println(htu_ok); - Serial.println(bmp_ok); - Serial.println(acl_ok); -// Serial.println(bufbytetwentyone[3],DEC); - Serial.println(mag_ok); - // Serial.println(bufbytetwentytwo[3],DEC); - - Serial.println(textctr); - - //Serial.println(sizeof(bufbytetwentyone)); - //Serial.println(sizeof(bufbytetwentytwo)); - - } + if ((flg) || ((ppcnt % status_display_rate) == 0)) { + sprintf(txt,"{'STS':{'Qsz':%2d,'Mis':%2d,'Ter':%d,'Htu':%d,'Bmp':%d,'Acl':%d,'Mag':%d,'Gps':%d}}\n", + qsize,missed,terr,htu_ok,bmp_ok,acl_ok,mag_ok,gps_ok); + PushTxt(txt); + terr = 0; + } } +// Push event queue + +void PushEvq(int flg, int *qsize, int *missed) { + + struct EventBuf eb; // Temporary event buffer + double evtm = 0.0; // Time since last event or PPS in seconds (< 1.0) + char stx[16]; // Second text + int i,j; + + // If there are any events waiting in the event read buffer, put them on the queue + + for (i=0; i= events_display_size) { + + PushTxt("\n"); + while (PopQueue(&eb)) { // While ther are events on the queue + + // Calculate the time in seconds of this event in the second + // N.B. Ticks are since the last event, or from PPS + + if (eb.Count == 1) evtm = 0.0; // Start a new second + evtm += ((double) eb.Ticks / (double) eb.Frequency); // Add time since last event + sprintf(stx,"%9.7f",evtm); // It will be 0.something + + // Build string and push it out to the print buffer + + sprintf(txt, + "{'EVT':{'Evt':%1d,'Frq':%8d,'Tks':%8d,'Etm':%s%s," + "'Adc':[[%d,%d,%d,%d,%d,%d,%d,%d],[%d,%d,%d,%d,%d,%d,%d,%d]]}}\n", + eb.Count, eb.Frequency, eb.Ticks, eb.DateTime, index(stx,'.'), + eb.Ch0[0],eb.Ch0[1],eb.Ch0[2],eb.Ch0[3],eb.Ch0[4],eb.Ch0[5],eb.Ch0[6],eb.Ch0[7], + eb.Ch1[0],eb.Ch1[1],eb.Ch1[2],eb.Ch1[3],eb.Ch1[4],eb.Ch1[5],eb.Ch1[6],eb.Ch1[7]); + PushTxt(txt); + } + PushTxt("\n"); + } +} + +// Read one input character, we have exactly the same problem with +// the serial line read as with writing, so we need the same work around + +void ReadOneChar() { + char c; + + // Suck in all the characters available on the input stream + // put as many as will fit in the cmd buffer, and say ready + + if ((irdy == 0) && (Serial.available())) { // If buffer free + c = (char) Serial.read(); // Read one char + if (c == '\n') istp = 1; // Stop on '\n' + if ((!istp) && (irdp < (CMDLEN -1))) { + cmd[irdp] = c; + irdp = irdp + 1; + cmd[irdp] = 0; + } + } else irdy = 1; +} + +// Implement the command callback functions + +void noop(int arg) { }; // That was easy + +void help(int arg) { // Display the help + int i; + CmdStruct *cms; + + for (i=0; iName,cms->Par,cms->Help); + PushTxt(txt); + } +} + +void htux(int arg) { htu_ok = htu.begin(); } +void htud(int arg) { humtmp_display_rate = arg; } +void bmpd(int arg) { alttmp_display_rate = arg; } +void locd(int arg) { latlon_display_rate = arg; } +void timd(int arg) { frqutc_display_rate = arg; } +void stsd(int arg) { status_display_rate = arg; } + +void evqt(int arg) { + events_display_size = arg % EVENT_QSIZE; + if (events_display_size == 0) + events_display_size = 24; +} + +void acld(int arg) { accelr_display_rate = arg; } +void magd(int arg) { magnot_display_rate = arg; } + +void aclt(int arg) { + uint8_t val = 0; + accelr_event_threshold = arg & 0x7F; + val = accelr_event_threshold; + acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); +} + +// Look up a command in the command table for the given command string +// and call it with its single integer parameter + +void ParseCmd() { + int i, p=0, cl=0; + char *cp, *ep; + CmdStruct *cms; + + for (i=0; iName); + if (strncmp(cms->Name,cmd,cl) == 0) { + if ((cms->Par) && (strlen(cmd) > cl)) { + cp = &cmd[cl]; + p = (int) strtoul(cp,&ep,0); + } + cms->proc(p); + break; + } + } +} + +// This waits for a ready buffer from ReadOneChar. Once ready the buffer is +// locked until its been seen here + +void DoCmd() { + if (irdy) { + if (irdp) { + sprintf(txt,"{'CMD':%s}\n",cmd); + PushTxt(txt); + ParseCmd(); + } + bzero((void *) cmd, CMDLEN); + irdp = 0; irdy = 0; istp = 0; + } +} + +// Arduino main loop does all the user space work void loop() { -textctr=0; -//Serial.println("alive"); -//PushHtu(1); // Push HTU temperature and humidity -//PushBmp(1); // Push BMP temperature and barrometric altitude -//PushLoc(1); // Push location latitude and longitude -//PushTim(1); // Push timing data - //PushMag(1); // Push mago data - //PushAcl(1); // Push accelarometer data - //PushSts(1,0,0); // Push status -// GetDateTime(); // Read the next date/time from the GPS chip -//if (textctr>0){ - SerialUSB.write(txt,textctr); - //} + + int missed, qsize; // Queue vars + + if (displ) { // Displ is set in the PPS ISR, we will reset it here +#if PPS_PIN + digitalWrite(PPS_PIN,HIGH); // PPS arrived +#endif + DoCmd(); // Execute any incomming commands + PushEvq(0,&qsize,&missed); // Push any events + PushHtu(0); // Push HTU temperature and humidity + PushBmp(0); // Push BMP temperature and barrometric altitude + PushLoc(0); // Push location latitude and longitude + PushTim(0); // Push timing data + PushMag(0); // Push mago data + PushAcl(0); // Push accelarometer data + PushSts(0,qsize,missed); // Push status + GetDateTime(); // Read the next date/time from the GPS chip +#if PPS_PIN + digitalWrite(PPS_PIN,LOW); // Reset PPS +#endif + displ = 0; // Clear flag for next PPS + gps_ok = true; // Its OK because we got a PPS + } + + if (gps_ok == false) { + delay(1000); // One second sleep + PushSts(0,qsize,missed); // Push bad hardware status + } + + PutChar(); // Print one character per loop !!! + ReadOneChar(); // Get next input command char } - - -//status as of 9:30 Sunday night -//We've written a new protocol, transfering data at the bit level over USB for maximum efficiency and speed! -//writes in to a python script that justin's made -//output/input protocol working, using SerialUSB -//GPS not tested due to missing antenna, need to verify timestamp functionality and pps -//sensors working fine (baro, mag, accel, press, temp) -//Output for events: -//on trigger dump 300 samples per channel into buffer -//on pps dump buffer to SerialUSB. -//We can implement a character/byte passing system over SerialUSB if required, but it's very quick - so fingers crossed we don't need it. - - diff --git a/geo_cosmic_pi.cc b/geo_cosmic_pi.cc deleted file mode 100644 index a7439b4..0000000 --- a/geo_cosmic_pi.cc +++ /dev/null @@ -1,1113 +0,0 @@ -// This program collects up to PPS_EVENTS events each second into a double buffer. -// While the ISR fills one event buffer, the user space loop function reads from the other. -// Each PPS interrupt the read/write buffers are swapped over by the event ISR -// If any events are available in the read buffer the loop function puts them onto a queue -// along with the UTC time string where they get stored. Once there are at -// least DUMP_THRESHOLD entries on the queue, the loop function outputs them over the serial -// line for processing. - -// In this version interrupts come from the accelerometer chip, if the acceleration exceeds -// the threshold (in meters/sec/sec) in any direction the chip interrupts and its logged to -// the serial output stream. - -// Julian Lewis lewis.julian@gmail.com - -#define VERS "2016/Mar/29" - -// In this sketch I am using an Adafruite ultimate GPS breakout which exposes the PPS output -// The Addafruite Rx is connected to the DUE TX1 (Pin 18) and its Tx to DUE RX1 (Pin 19) -// The Adafruite 3.3V power is provided from the DUE 3.3V and ground pins -// N.B. Go to the Adafruit downloads page and copy Adafruit_GPS.h and Adafruit_GPS.cc to -// your sketch directory. - -// The output from this program is processed by a Python monitor on the other end of the -// serial line. There has to be mutual aggreement between this program and the monitor. - -// Output strings -// All fields in all output strings conform to the json standard - -// Here is the list of all records where 'f' denotes float and 'i' denotes integer ... -// {'HTU':{'Tmh':f,'Hum':f}} -// HTU21DF record containing Tmh:temperature in C Hum:humidity percent -// -// {'BMP':{'Tmb':f,'Prs':f,'Alb':f}} -// BMP085 record containing Tmb:temperature Prs:pressure Alb:Barrometric altitude -// -// {'VIB':{'Vax':i,'Vcn':i}} -// Vibration record containing Vax:3 bit xyz direction mask Vcn:vibration count -// This record is always immediatly followed by 3 more records, TIM, ACL, and MAG -// -// {'MAG':{'Mgx':f,'Mgy':f,'Mgz':f}} -// LSM303DLH magnatometer record containing Mgx:the x field strength Mgy:the y field Mgz:ther z field -// -// {'MOG':{'Mox':f,'Moy':f,'Moz':f}} -// LSM303DLH magnatometer record containing Mox:x orientation Moy:y orientation Moz:z orientation -// This record is optional, by default its turned off (it can always be calculated later - Python) -// -// {'ACL':{'Acx':f,'Acy':f,'Acz':f}} -// LSM303DLH acclerometer record containing Acx:the x acceleration Acy:the y acceleration Acz:the z acceleration -// If this record immediatly follows a VIB record the fields were hardware latched when the g threshold was exceeded -// -// {'AOL':{'Aox':f,'Aoy':f,'Aoz':f}} -// LSM303DLH accelerometer record containing Aox:x orientation Aoy:y orientation Aoz:z orientation -// This record is optional, by default its turned off (it can always be calculated later - Python) -// -// {'LOC':{'Lat':f,'Lon':f,'Alt':f}} -// GPS location record containing Lat:latitude in degrees Lon:longitude in degrees Alt:altitude in meters -// -// {'TIM':{'Upt':i,'Frq':i,'Sec':i}} -// Time record containing Upt:up time seconds Frq:counter frequency Sec:time string -// -// {'STS':{'Qsz':i,'Mis':i,'Ter':i,'Htu':i,'Bmp':i,'Acl':i,'Mag':i, 'Gps':i}} -// Status record containing Qsz:events on queue Mis:missed events Ter:buffer error -// Htu:status Bmp:status Acl:status Mag:status Gps:ststus -// -// {'EVT':{'Evt':i,'Frq':i,'Tks':i,'Etm':f,'Adc':[[i,i,i,i,i,i,i,i][i,i,i,i,i,i,i,i]]}} -// Event record containing Evt:event number in second Frq:timer frequency Tks:ticks since last event in second -// Etm:event time stamp to 100ns Adc:[[Channel 0 values][Channel 1 values]] - -// N.B. These records pass the data to a python monitor over the serial line. Python has awsome string handling and looks them up in -// associative arrays to build records of any arbitary format you want. So this is only the start of the story of record processing. -// N.B. Also some of these records are sent out at regular intervals and or when an event occurs. - -// This program also accepts commands sent to it on the serial line. -// When a command arrives it is immediatly executed. - -#include -#include - -#include "Adafruit_BMP085_U.h" // Barrometric pressure - -#include "Adafruit_HTU21DF.h" // Humidity and temperature sensor - -// GPS chips typically return NMEA strings over a serial line. -// They can be programmed to send different NMEA strings according to what you configure. -// The string RMCGGA has the altitude but misses the yy/mm/dd from the date, it only has hh/mm/ss. -// This is easilly made up for in the Python monitor which gets this information from it system time. - -#include "Adafruit_GPS.h" // GPS chip -#define GPSECHO true -#define RMCGGA // Altitude on, yy/mm/dd off - -#include "Adafruit_L3GD20_U.h" // Magoscope - -// WARNING: I had to modify this library, its no longer standard -#include "Adafruit_LSM303_U.h" // Accelerometer and magnentometer/compass - -#include "Adafruit_10DOF.h" // 10DOF breakout driver - scale to SI units - -// Configuration constants - -// The size of the one second event buffer -#define PPS_EVENTS 8 // The maximum number of events stored per second -#define ADC_BUF_LEN 8 // Number of ADC values per event - -// This is the event queue size -#define EVENT_QSIZE 32 // The number of events that can be queued for serial output - -// Handle text buffer serial output overflow errors -// When the output buffer overflows due to data comming too fast, we just stop printing due -// to insufficient bandwidth or slow things down if HANDLE_OVERFLOW is set (not recomended) -// #define HANDLE_OVERFLOW - -// This is the text ring buffer for real time output to serial line with interrupt on -#define TBLEN 4096 // Serial line output ring buffer size - -// Define some output debug pins to monitor whats going on via an oscilloscope -#define PPS_PIN 13 // PPS (Pulse Per Second) and LED -#define EVT_PIN 12 // Cosmic ray event detected -#define FLG_PIN 11 // Debug flag - -// For siesmic event input -#define ACL_PIN 10 // Accelarometer INT1 interrupt pin - -// Baud rates -#define SERIAL_BAUD_RATE 9600 // Serial line -#define GPS_BAUD_RATE 9600 // GPS and Serial1 line - -// Instantiate external hardware breakouts - -Adafruit_GPS gps(&Serial1); // GPS Serial1 on pins RX1 and TX1 -boolean gps_ok = false; // Chip OK flag - -Adafruit_HTU21DF htu = Adafruit_HTU21DF(); // Humidity and temperature measurment -boolean htu_ok = false; // Chip OK - -#define BMPID 18001 -Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(BMPID); // Barometric pressure -boolean bmp_ok = false; - -// The 10DOF isn't a chip, its just a utility to convert say mago values into headings etc - -Adafruit_10DOF dof = Adafruit_10DOF(); // The 10 Degrees-Of-Freedom DOF breakout -boolean dof_ok = false; // board driver, scales units to SI - -#define ACLID 30301 -Adafruit_LSM303_Accel_Unified acl = Adafruit_LSM303_Accel_Unified(ACLID); // Accelerometer Compass - boolean acl_ok = false; - -#define MAGID 30302 -Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(MAGID); // Magoscope -boolean mag_ok = false; - -// Control the output data rates by setting defaults, these values can be modified at run time -// via commands from the serial interface. Some output like position isn't supposed to be changing -// very fast if at all, so no need to clutter up the serial line with it. The Python monitor keeps -// the last sent values when it builds event messages to be sent over the internet to the server -// or logged to a file. - -uint32_t latlon_display_rate = 12; // Display latitude and longitude each X seconds -uint32_t humtmp_display_rate = 12; // Display humidity and HTU temperature each X seconds -uint32_t alttmp_display_rate = 12; // Display altitude and BMP temperature each X seconds -uint32_t frqutc_display_rate = 1; // Display frequency and UTC time each X seconds -uint32_t status_display_rate = 4; // Display status (UpTime, QueueSize, MissedEvents, HardwareOK) -uint32_t accelr_display_rate = 1; // Display accelarometer x,y,z -uint32_t magnot_display_rate = 12; // Display magnotometer data x,y,z - -uint32_t events_display_size = 20; // Display events after recieving X events - -// Siesmic event trigger parameters - -uint32_t accelr_event_threshold = 2; // Trigger level for siesmic events in milli-g -uint32_t accelr_event_cutoff_fr = 30; // Siesmic event cutoff frequency - -// Commands can be sent over the serial line to configure the display rates or whatever - -typedef enum { - NOOP, // No-operation - HELP, // Help - HTUX, // Reset HTU chip - HTUD, // HUT display rate - BMPD, // BMP display rate - LOCD, // Location display rate - TIMD, // Timing display rate - STSD, // Status display rate - EVQT, // Event queue dump threshold - - ACLD, // Accelerometer display rate - MAGD, // Magnetometer display rate - - ACLT, // Accelerometer event threshold - - CMDS }; // Command count - -typedef struct { - int Id; // Command ID number - void (*proc)(int arg); // Function to call - char *Name; // Command name - char *Help; // Command help text - int Par; // Command parameter flag -} CmdStruct; - -// Function forward references - -void noop(int arg); -void help(int arg); -void htux(int arg); -void htud(int arg); -void bmpd(int arg); -void locd(int arg); -void timd(int arg); -void stsd(int arg); -void evqt(int arg); -void acld(int arg); -void magd(int arg); -void aclt(int arg); - -// Command table - -CmdStruct cmd_table[CMDS] = { - { NOOP, noop, "NOOP", "Do nothing", 0 }, - { HELP, help, "HELP", "Display commands", 0 }, - { HTUX, htux, "HTUX", "Reset the HTU chip", 0 }, - { HTUD, htud, "HTUD", "HTU Temperature-Humidity display rate", 1 }, - { BMPD, bmpd, "BMPD", "BMP Temperature-Altitude display rate", 1 }, - { LOCD, locd, "LOCD", "Location latitude-longitude display rate", 1 }, - { TIMD, timd, "TIMD", "Timing uptime-frequency-utc display rate", 1 }, - { STSD, stsd, "STSD", "Status info display rate", 1 }, - { EVQT, evqt, "EVQT", "Event queue dump threshold", 1 }, - { ACLD, acld, "ACLD", "Accelerometer display rate", 1 }, - { MAGD, magd, "MAGD", "Magnatometer display rate", 1 }, - { ACLT, aclt, "ACLT", "Accelerometer event trigger threshold", 1 } -}; - -#define CMDLEN 32 -static char cmd[CMDLEN]; // Command input buffer -static int irdp=0, irdy=0, istp=0; // Read, ready, stop - -static char txtb[TBLEN]; // Text ring buffer -static uint32_t txtw = 0, txtr = 0, // Write and Read indexes - tsze = 0, terr = 0; // Buffer size and error code - -typedef enum { TXT_NOERR=0, TXT_TOOBIG=1, TXT_OVERFL=2 } TxtErr; - -#define TXTLEN 256 -static char txt[TXTLEN]; // For writing to serial - -// Initialize the timer chips to measure time between the PPS pulses and the EVENT pulse -// The PPS enters pin D2, the PPS is forwarded accross an isolating diode to pin D5 -// The event pulse is also connected to pin D5. So D5 sees the LOR of the PPS and the -// event, while D2 sees only the PPS. In this way we measure the frequency of the -// clock MCLK/2 each second on the first counter, and the time between EVENTs on the second - -void TimersStart() { - - uint32_t config = 0; - - // Set up the power management controller for TC0 and TC2 - - pmc_set_writeprotect(false); // Enable write access to power management chip - pmc_enable_periph_clk(ID_TC0); // Turn on power for timer block 0 channel 0 - pmc_enable_periph_clk(ID_TC6); // Turn on power for timer block 2 channel 0 - - // Timer block zero channel zero is connected only to the PPS - // We set it up to load regester RA on each PPS and reset - // So RA will contain the number of clock ticks between two PPS, this - // value should be very stable +/- one tick - - config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz - TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA0 - TC_CMR_ABETRG | // Use the TIOA external input line - TC_CMR_LDRA_RISING; // Latch counter value into RA - - TC_Configure(TC0, 0, config); // Configure channel 0 of TC0 - TC_Start(TC0, 0); // Start timer running - - TC0->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS - TC0->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources - NVIC_EnableIRQ(TC0_IRQn); // Enable interrupt handler for channel 0 - - // Timer block 2 channel zero is connected to the OR of the PPS and the RAY event - - config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz - TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA1 - TC_CMR_ABETRG | // Use the TIOA external input line - TC_CMR_LDRA_RISING; // Latch counter value into RA - - TC_Configure(TC2, 0, config); // Configure channel 0 of TC2 - TC_Start(TC2, 0); // Start timer running - - TC2->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS - TC2->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources - NVIC_EnableIRQ(TC6_IRQn); // Enable interrupt handler for channel 0 - - // Set up the PIO controller to route input pins for TC0 and TC2 - - PIO_Configure(PIOC,PIO_INPUT, - PIO_PB25B_TIOA0, // D2 Input - PIO_DEFAULT); - - PIO_Configure(PIOC,PIO_INPUT, - PIO_PC25B_TIOA6, // D5 Input - PIO_DEFAULT); -} - -// Timer chip interrupt handlers try to get time stamps to within 4 system clock ticks - -static uint32_t displ = 0; // Display values in loop - -static uint32_t ppsfl = LOW, // PPS Flag boolean - rega0 = 0, // RA reg - stsr0 = 0, // Interrupt status register - ppcnt = 0; // PPS count - -// Handle the PPS interrupt in counter block 0 ISR - -void TC0_Handler() { - - // This ISR is connected only to the PPS (Pulse Per Second) GPS event - // Each time this runs, set the flag to tell the TC6 ISR we have seen it - // This logic only works if the TC0 handler gets called before the TC6 handler - // hence the debug flag which I look at with a scope to be sure. - // I may introduce a small delay line to ensure this is true, so far it is. - - ppsfl = HIGH; // Seen a rising edge on the PPS -#if FLG_PIN - digitalWrite(FLG_PIN,ppsfl); // Flag set (for debug) -#endif - rega0 = TC0->TC_CHANNEL[0].TC_RA; // Read the RA reg (PPS period) - stsr0 = TC_GetStatus(TC0, 0); // Read status and clear load bits - - ppcnt++; // PPS count - displ = 1; // Display stuff in the loop -} - -// We need a double buffer, one is being written by the ISR while -// the other is read from user space within one second. - -struct Event { - uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values - uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values - uint32_t Tks; // Time since last event in ticks -}; - -static struct Event b1[PPS_EVENTS]; // Event ticks buffeer -static struct Event b2[PPS_EVENTS]; // Event ticks buffer -static struct Event *wbuf = b1; // Write event buffer pointer and its index -static struct Event *rbuf = b2; // Read event buffer pointer and its index -static int ridx, widx; - -// We also need a time value for the current and previous second - -#ifdef RMCGGA -#define DATE_TIME_LEN 9 -#else -#define DATE_TIME_LEN 17 -#endif - -static char t1[DATE_TIME_LEN]; // Date time buffer text string -static char t2[DATE_TIME_LEN]; -static char *wdtm = t1; // Write date/time pointer -static char *rdtm = t2; // Read date/time pointer - -// Swap read write event buffers and indexes along with their time strings -// each second, so we have the current and previous second numbers - -void SwapBufs() { - struct Event *tbuf; // Temp event buf pointer - char *tdtm; // Temp date/time string pointer - tbuf = rbuf; rbuf = wbuf; wbuf = tbuf; // Swap write with read buffer - ridx = widx; widx = 0; // Write count to read, reset the write count - tdtm = rdtm; rdtm = wdtm; wdtm = tdtm; // And swap asociated buffer date/time -} - -// Handle isolated PPS (via diode) LOR with the Event -// The diode is needed to block Event pulses getting back to TC0 -// LOR means Logical inclusive OR - -static uint32_t rega1, stsr1 = 0; - -void TC6_Handler() { - - // This ISR is connected to the OR of the event and the PPS - // If the TC0 has seen the PPS it sets the flag high - // and if its high we are seeing the PPS here, but if the - // flag is not set then this is a cosmic ray event. - - if (ppsfl == HIGH) { // Was ther a PPS ? - ppsfl = LOW; // Yes so we have seen it here - SwapBufs(); // Every PPS swap the read/write buffers -#if EVT_PIN - digitalWrite(EVT_PIN,LOW); // Not an event -#endif - } else { -#if EVT_PIN - digitalWrite(EVT_PIN,HIGH); // Event detected -#endif - if (widx < PPS_EVENTS) { // Up to PPS_EVENTS stored per PPS - - // Read the latched tick count getting the event time - // and then pull the ADC pipe line - - wbuf[widx].Tks = TC2->TC_CHANNEL[0].TC_RA; - AdcPullData(&wbuf[widx]); - widx++; - } - } -#if FLG_PIN - digitalWrite(FLG_PIN,ppsfl); // Flag out -#endif - rega1 = TC2->TC_CHANNEL[0].TC_RA; // Read the RA on channel 1 (PPS period) - stsr1 = TC_GetStatus(TC2, 0); // Read status clear load bits -} - -// Accelerometer setup -// These settings come from reading the LSM303DLH doccumentation, which is more than the -// author of the Adda_fruit library did. It is a badly written load of rubbish, I have -// been forced to correct some bugs and export some private methods. It was a touch and go -// descision wether to just not use the library at all and do it all here. For now I will -// use it with the bug corrections. - -// The following setup makes the accelarometer compare G-forces against a threshold value, and -// latch the output registers until they are read. To avoid excessive interrupt rates the high -// pass filter has been configured to keep the frequency low. - -// N.B. It took me a day to find out that I needed to use Active low and Open drain on the INT1 -// signal, otherwise the Adda_fruit module wont pass it on. Beware !!! - -void AclSetup() { - uint8_t tmp, val; - - if (!acl_ok) return; - -#define PMD 0x20 // Normal power mode (PM0=1,PM1=0:Normal) -#define DRT 0x00 // Data rate 50 Hz (0x08 = 100Hz) -#define AEN 0x07 // XYZ Enabled - - val = PMD | DRT | AEN; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, val); - -#define HPE1 0x04 // High pass filter Int 1 on -#define HPCF 0x03 // High pass cut off frequency - - val = HPE1 | HPCF; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG2_A, val); - -#define LIR1 0x06 // Latch Int1 bit Data ready -#define LIR2 0x00 // Latch Int2 bit Data ready (0x20 Latch On) - -#define IHL_OD 0xC0 // Interrupt active low, open drain (Argh !!!) - - val = LIR1 | LIR2 | IHL_OD; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG3_A, val); - -#define BDU_FS 0x80 // Block data and scale +-2g - - val = BDU_FS; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG4_A, val); - -#define XYZ_HI 0x2A // Hi values ZHIE YHIE XHIE -#define AOI_6D 0x00 // 0xC0 would enable 6 directions - - val = XYZ_HI | AOI_6D; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_CFG_A, val); - - val = accelr_event_threshold & 0x7F; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); - - // The chip make very wide pulses (100ms), the values on the rising and - // falling edges are different ! - - attachInterrupt(digitalPinToInterrupt(ACL_PIN),Acl_ISR,RISING); -} - -// Magnatometer setup, again the Adda_fruit library was inadequate. - -void MagSetup() { - uint8_t val; - - if (!mag_ok) return; - - val = 0; - mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, val); - -#define GAIN 0x80 // +- 4.0 Gauss - - val = GAIN; - mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, val); - -#define MODE 0x0 // 01=Single conversion mode - - val = MODE; - mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, val); -} - -// This Accelerometer ISR - -static uint32_t accl_icount = 0, accl_flag = 0; - -void Acl_ISR() { - -#define IA 0x40 - - accl_icount++; - accl_flag = AclReadStatus(); - PushVib(); -} - -// Read accelerometer status -// This just reads the interrupt source INT1 and the overrun status. -// It returns 1 bit for X, Y, or Z (0..7) if the threshold value is exceeded. -// This determins if the board is being shaken - Earth quake - or other reason - -static uint8_t acl_sts = 0; -static uint8_t acl_src = 0; - -uint8_t AclReadStatus() { - uint8_t rval; - - acl_src = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_SOURCE_A); - -#define ZH 0x20 // Z High -#define YH 0x08 // Y High -#define XH 0x02 // X High - - rval = 0; - if (acl_src & IA) { - if (acl_src & ZH) rval |= 4; - if (acl_src & YH) rval |= 2; - if (acl_src & XH) rval |= 1; - } - if (rval) - acl_sts = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_STATUS_REG_A); - return rval; -} - -// Set up the ADC channels - -void AdcSetup() { - REG_ADC_MR = 0x10380080; // Free run as fast as you can - REG_ADC_CHER = 3; // Channels 0 and 1 -} - -// Pull all data (16 values) from the ADC into a buffer - -uint8_t AdcPullData(struct Event *b) { - - int i; - - for (i=0; iADC_ISR & 0x01)==0); // Wait for channel 0 (2.5us) - b->Ch0[i] = (uint16_t) ADC->ADC_CDR[0]; // Read ch 0 - while((ADC->ADC_ISR & 0x02)==0); // Wait for channel 1 (2.5us) - b->Ch1[i] = (uint16_t) ADC->ADC_CDR[1]; // Read ch 1 - } -} - -// This is the nmea data string from the GPS chip - -#define GPS_STRING_LEN 256 -static char gps_string[GPS_STRING_LEN + 1]; - -float latitude = 0.0, longitude = 0.0, altitude = 0.0; - -// This function is dependent on the GPS chip implementation -// It should return a date time string as described above -// So you need to re-implements this for whichever chip you are using -// Here I am using the addafruit GPS chip - -char *GetDateTime() { - - int i = 0; - while (Serial1.available()) { - if (i < GPS_STRING_LEN) { - gps_string[i++] = (char) Serial1.read(); - gps_string[i] = 0; - } else i++; - } - if (gps.parse(gps_string)) { - -#ifdef RMCGGA - // I choose RMCGGA by default, and get the altitude but no date. - // Its easy to get the date once the records arrive at the Python end. - // The GPS altitude is far more accurate than the barrometric altitude. - // Warning: The syntax can not be changed, we need an integer hhmmss - - sprintf(wdtm,"%02d%02d%02d", - gps.hour,gps.minute,gps.seconds); - - altitude = gps.altitude; -#else - sprintf(wdtm,"%02d%02d%02d%02d%02d%02d%02d%02d", - gps.year,gps.month,gps.day, - gps.hour,gps.minute,gps.seconds); - - altitude = 0; -#endif - latitude = gps.latitudeDegrees; // Easy place to get location - longitude = gps.longitudeDegrees; // Works well in Google maps - - return rdtm; - } else - return NULL; -} - -// Implement queue access mechanism for events, each second the user space (loop) copies -// any events it has read onto the queue - -struct EventBuf { - char DateTime[DATE_TIME_LEN]; // The date and time string - uint32_t Frequency; // The current clock frequency - uint32_t Ticks; // The number of ticks since the last event or PPS if none - uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values - uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values - uint8_t Count; // The number of events since the PPS -}; - -typedef struct { - uint8_t Size; // Current size of the queue - uint8_t RdPtr; // Read pointer - uint8_t WrPtr; // Write pointer - uint8_t Missed; // Missed events counter due to overflow - uint8_t Lock; // The queue spin lock (not needed here) - struct EventBuf Events[EVENT_QSIZE]; // Queued events -} EventQueue; - -static EventQueue event_queue; - -// Put an event in an EventBuf on the queue, if the queue is full then the oldest event -// is thrown away and the "missed" event count is incremented - -uint8_t PutQueue(struct EventBuf *ebuf) { - - EventQueue *q = &event_queue; - - while(q->Lock) {}; q->Lock = 1; // Spin lock on queue - q->Events[q->WrPtr] = *ebuf; // Write event to the queue - q->WrPtr = (q->WrPtr + 1) % EVENT_QSIZE;// Increment the write pointer - if (q->Size < EVENT_QSIZE) q->Size++; // If we are overwriting old enties that havnt been read - else { - q->Missed++; // Say we missed some events - q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; // and throw the oldest event away - } - q->Lock = 0; - return q->Missed; -} - -// Pop an event off the queue, if the queue is empty nothing happens -// the queue size is zero when the queue is empty, and this is the -// return value - -uint8_t PopQueue(struct EventBuf *ebuf) { // Points to where the caller wants the event stored - - EventQueue *q = &event_queue; - - while(q->Lock) {}; q->Lock = 1; // Spin lock on queue - if (q->Size) { - *ebuf = q->Events[q->RdPtr]; - q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; - q->Size--; - } - q->Lock = 0; - return q->Size; -} - -// Get the size of the queue - -uint8_t SzeQueue() { - - EventQueue *q = &event_queue; - - return q->Size; -} - -// Initialize the queue - -void InitQueue() { - - EventQueue *q = &event_queue; - - q->Lock = 1; - q->Size = 0; - q->RdPtr = 0; - q->WrPtr = 0; - q->Missed = 0; - q->Lock = 0; -} - -// Arduino setup function, initialize hardware and software -// This is the first function to be called when the sketch is started - -void setup() { - -#if FLG_PIN - pinMode(FLG_PIN, OUTPUT); // Pin for the ppsfl flag for debug -#endif -#if EVT_PIN - pinMode(EVT_PIN, OUTPUT); // Pin for the cosmic ray event -#endif -#if PPS_PIN - pinMode(PPS_PIN, OUTPUT); // Pin for the PPS (LED pin) -#endif - - Serial.begin(SERIAL_BAUD_RATE); // Start the serial line - Serial1.begin(GPS_BAUD_RATE); // and the second - - gps.begin(GPS_BAUD_RATE); // Chip baud rate - -#ifdef RMCGGA - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); // With altitude but no yy/mm/dd -#else - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); // With yy/mm/dd but no altitude -#endif - gps.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // each second - - - InitQueue(); // Reset queue pointers, missed count, and size - - strcpy(rdtm,""); // Set initial value for date/time - strcpy(wdtm,""); - - htu_ok = htu.begin(); - bmp_ok = bmp.begin(); - acl_ok = acl.begin(); - mag_ok = mag.begin(); - dof_ok = dof.begin(); - - AclSetup(); - MagSetup(); - AdcSetup(); - - TimersStart(); // Start timers -} - -// These two routines are needed because the Serial.print method prints without using interrupts. -// Calls to Serial.print block interrupts and use a wait in kernel space causing all ISRs to -// be blocked and hence we could miss some timer interrupts. -// To avoid this problem call PushTxt to have stuff delivered to the serial line, PushTxt simply -// stores your text for future print out by PutChar. The PutChar routine removes one character -// from the stored text each time its called. By placing a call to PutChar in the outermost loop -// of the Arduino loop function, then for each loop one character is printed, avoiding blocking -// of interrupts and vastly improving the loops real time behaviour. - -// Copy text to the buffer for future printing - -void PushTxt(char *txt) { - - int i, l = strlen(txt); - - // If this happens there is a programming bug - - if (l > TBLEN) { // Can't handle more than TBLEN at a time - terr = TXT_TOOBIG; // say error and abort - return; - } - - // If the buffer is filling up to fast throw it away and return an error - - if ((l + tsze) >= TBLEN) { // If there is no room in the buffer - terr = TXT_OVERFL; // Buffer overflow - return; // Simply stop printing when txt comming too fast - } - - // Copy the new text onto the ring buffer for later output - // from the loop idle function - - for (i=0; i Earth Quake - -uint32_t old_icount = 0; - - if (accl_flag) { - if (accl_icount != old_icount) { - old_icount = accl_icount; - PushTim(1); // Push these first, and then vib - PushAcl(1); // This is the real latched value - PushMag(1); - sprintf(txt,"{'VIB':{'Vax':%d,'Vcn':%d}}\n",accl_flag,accl_icount); - PushTxt(txt); - } - } -} - -// Push the magnetic field strengths in all three axis in micro tesla (gauss) - -void PushMag(int flg) { // Push the mago stuff - sensors_event_t mag_event; - sensors_vec_t xyz; - - if ((flg) || ((mag_ok) && ((ppcnt % magnot_display_rate) == 0))) { - mag.getEvent(&mag_event); - - // Micro Tesla - - sprintf(txt,"{'MAG':{'Mgx':%f,'Mgy':%f,'Mgz':%f}}\n", - mag_event.magnetic.x, - mag_event.magnetic.y, - mag_event.magnetic.z); - PushTxt(txt); - - // Orientation (Easy to calculate later in Python - dont waste resources) -#ifdef ORIENTATION - if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &xyz)) { - sprintf(txt,"{'MOG':{'Mox':%f,'Moy':%f,'Moz':%f}}\n",xyz.x,xyz.y,xyz.z); - PushTxt(txt); - } -#endif - } -} - -// Push the acceleration values in xyz in meters per sec squared - -void PushAcl(int flg) { // Push the accelerometer and compass stuff - sensors_event_t acl_event; - sensors_vec_t xyz; - - if ((flg) || ((acl_ok) && ((ppcnt % accelr_display_rate) == 0))) { - acl.getEvent(&acl_event); - - // Meters per second squared - - sprintf(txt,"{'ACL':{'Acx':%f,'Acy':%f,'Acz':%f}}\n", - acl_event.acceleration.x, - acl_event.acceleration.y, - acl_event.acceleration.z); - PushTxt(txt); - - // Orientation (Easy to calculate later in Python - dont waste resources) -#ifdef ORIENTATION - if (dof.accelGetOrientation(&acl_event, &xyz)) { - sprintf(txt,"{'AOL':{'Aox':%f,'Aoy':%f,'Aoz':%f}}\n",xyz.x,xyz.y,xyz.z); - PushTxt(txt); - } -#endif - } -} - -// Push location latitude longitude in degrees so that google maps gets it correct - -void PushLoc(int flg) { - - if ((flg) || ((ppcnt % latlon_display_rate) == 0)) { - sprintf(txt,"{'LOC':{'Lat':%f,'Lon':%f,'Alt':%f}}\n",latitude,longitude,altitude); - PushTxt(txt); - } -} - -// Push timing - -void PushTim(int flg) { - - if ((flg) || ((ppcnt % frqutc_display_rate) == 0)) { - sprintf(txt,"{'TIM':{'Upt':%4d,'Frq':%7d,'Sec':%s}}\n",ppcnt,rega0,rdtm); - PushTxt(txt); - } -} - -// Push status - -void PushSts(int flg, int qsize, int missed) { -uint8_t res; - - if ((flg) || ((ppcnt % status_display_rate) == 0)) { - sprintf(txt,"{'STS':{'Qsz':%2d,'Mis':%2d,'Ter':%d,'Htu':%d,'Bmp':%d,'Acl':%d,'Mag':%d,'Gps':%d}}\n", - qsize,missed,terr,htu_ok,bmp_ok,acl_ok,mag_ok,gps_ok); - PushTxt(txt); - terr = 0; - } -} - -// Push event queue - -void PushEvq(int flg, int *qsize, int *missed) { - - struct EventBuf eb; // Temporary event buffer - double evtm = 0.0; // Time since last event or PPS in seconds (< 1.0) - char stx[16]; // Second text - int i,j; - - // If there are any events waiting in the event read buffer, put them on the queue - - for (i=0; i= events_display_size) { - - PushTxt("\n"); - while (PopQueue(&eb)) { // While ther are events on the queue - - // Calculate the time in seconds of this event in the second - // N.B. Ticks are since the last event, or from PPS - - if (eb.Count == 1) evtm = 0.0; // Start a new second - evtm += ((double) eb.Ticks / (double) eb.Frequency); // Add time since last event - sprintf(stx,"%9.7f",evtm); // It will be 0.something - - // Build string and push it out to the print buffer - - sprintf(txt, - "{'EVT':{'Evt':%1d,'Frq':%8d,'Tks':%8d,'Etm':%s%s," - "'Adc':[[%d,%d,%d,%d,%d,%d,%d,%d],[%d,%d,%d,%d,%d,%d,%d,%d]]}}\n", - eb.Count, eb.Frequency, eb.Ticks, eb.DateTime, index(stx,'.'), - eb.Ch0[0],eb.Ch0[1],eb.Ch0[2],eb.Ch0[3],eb.Ch0[4],eb.Ch0[5],eb.Ch0[6],eb.Ch0[7], - eb.Ch1[0],eb.Ch1[1],eb.Ch1[2],eb.Ch1[3],eb.Ch1[4],eb.Ch1[5],eb.Ch1[6],eb.Ch1[7]); - PushTxt(txt); - } - PushTxt("\n"); - } -} - -// Read one input character, we have exactly the same problem with -// the serial line read as with writing, so we need the same work around - -void ReadOneChar() { - char c; - - // Suck in all the characters available on the input stream - // put as many as will fit in the cmd buffer, and say ready - - if ((irdy == 0) && (Serial.available())) { // If buffer free - c = (char) Serial.read(); // Read one char - if (c == '\n') istp = 1; // Stop on '\n' - if ((!istp) && (irdp < (CMDLEN -1))) { - cmd[irdp] = c; - irdp = irdp + 1; - cmd[irdp] = 0; - } - } else irdy = 1; -} - -// Implement the command callback functions - -void noop(int arg) { }; // That was easy - -void help(int arg) { // Display the help - int i; - CmdStruct *cms; - - for (i=0; iName,cms->Par,cms->Help); - PushTxt(txt); - } -} - -void htux(int arg) { htu_ok = htu.begin(); } -void htud(int arg) { humtmp_display_rate = arg; } -void bmpd(int arg) { alttmp_display_rate = arg; } -void locd(int arg) { latlon_display_rate = arg; } -void timd(int arg) { frqutc_display_rate = arg; } -void stsd(int arg) { status_display_rate = arg; } - -void evqt(int arg) { - events_display_size = arg % EVENT_QSIZE; - if (events_display_size == 0) - events_display_size = 24; -} - -void acld(int arg) { accelr_display_rate = arg; } -void magd(int arg) { magnot_display_rate = arg; } - -void aclt(int arg) { - uint8_t val = 0; - accelr_event_threshold = arg & 0x7F; - val = accelr_event_threshold; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); -} - -// Look up a command in the command table for the given command string -// and call it with its single integer parameter - -void ParseCmd() { - int i, p=0, cl=0; - char *cp, *ep; - CmdStruct *cms; - - for (i=0; iName); - if (strncmp(cms->Name,cmd,cl) == 0) { - if ((cms->Par) && (strlen(cmd) > cl)) { - cp = &cmd[cl]; - p = (int) strtoul(cp,&ep,0); - } - cms->proc(p); - break; - } - } -} - -// This waits for a ready buffer from ReadOneChar. Once ready the buffer is -// locked until its been seen here - -void DoCmd() { - if (irdy) { - if (irdp) { - sprintf(txt,"{'CMD':%s}\n",cmd); - PushTxt(txt); - ParseCmd(); - } - bzero((void *) cmd, CMDLEN); - irdp = 0; irdy = 0; istp = 0; - } -} - -// Arduino main loop does all the user space work - -void loop() { - - int missed, qsize; // Queue vars - - if (displ) { // Displ is set in the PPS ISR, we will reset it here -#if PPS_PIN - digitalWrite(PPS_PIN,HIGH); // PPS arrived -#endif - DoCmd(); // Execute any incomming commands - PushEvq(0,&qsize,&missed); // Push any events - PushHtu(0); // Push HTU temperature and humidity - PushBmp(0); // Push BMP temperature and barrometric altitude - PushLoc(0); // Push location latitude and longitude - PushTim(0); // Push timing data - PushMag(0); // Push mago data - PushAcl(0); // Push accelarometer data - PushSts(0,qsize,missed); // Push status - GetDateTime(); // Read the next date/time from the GPS chip -#if PPS_PIN - digitalWrite(PPS_PIN,LOW); // Reset PPS -#endif - displ = 0; // Clear flag for next PPS - gps_ok = true; // Its OK because we got a PPS - } - - if (gps_ok == false) { - delay(1000); // One second sleep - PushSts(0,qsize,missed); // Push bad hardware status - } - - PutChar(); // Print one character per loop !!! - ReadOneChar(); // Get next input command char -} diff --git a/geo_cosmic_pi.ino b/geo_cosmic_pi.ino deleted file mode 100644 index a7439b4..0000000 --- a/geo_cosmic_pi.ino +++ /dev/null @@ -1,1113 +0,0 @@ -// This program collects up to PPS_EVENTS events each second into a double buffer. -// While the ISR fills one event buffer, the user space loop function reads from the other. -// Each PPS interrupt the read/write buffers are swapped over by the event ISR -// If any events are available in the read buffer the loop function puts them onto a queue -// along with the UTC time string where they get stored. Once there are at -// least DUMP_THRESHOLD entries on the queue, the loop function outputs them over the serial -// line for processing. - -// In this version interrupts come from the accelerometer chip, if the acceleration exceeds -// the threshold (in meters/sec/sec) in any direction the chip interrupts and its logged to -// the serial output stream. - -// Julian Lewis lewis.julian@gmail.com - -#define VERS "2016/Mar/29" - -// In this sketch I am using an Adafruite ultimate GPS breakout which exposes the PPS output -// The Addafruite Rx is connected to the DUE TX1 (Pin 18) and its Tx to DUE RX1 (Pin 19) -// The Adafruite 3.3V power is provided from the DUE 3.3V and ground pins -// N.B. Go to the Adafruit downloads page and copy Adafruit_GPS.h and Adafruit_GPS.cc to -// your sketch directory. - -// The output from this program is processed by a Python monitor on the other end of the -// serial line. There has to be mutual aggreement between this program and the monitor. - -// Output strings -// All fields in all output strings conform to the json standard - -// Here is the list of all records where 'f' denotes float and 'i' denotes integer ... -// {'HTU':{'Tmh':f,'Hum':f}} -// HTU21DF record containing Tmh:temperature in C Hum:humidity percent -// -// {'BMP':{'Tmb':f,'Prs':f,'Alb':f}} -// BMP085 record containing Tmb:temperature Prs:pressure Alb:Barrometric altitude -// -// {'VIB':{'Vax':i,'Vcn':i}} -// Vibration record containing Vax:3 bit xyz direction mask Vcn:vibration count -// This record is always immediatly followed by 3 more records, TIM, ACL, and MAG -// -// {'MAG':{'Mgx':f,'Mgy':f,'Mgz':f}} -// LSM303DLH magnatometer record containing Mgx:the x field strength Mgy:the y field Mgz:ther z field -// -// {'MOG':{'Mox':f,'Moy':f,'Moz':f}} -// LSM303DLH magnatometer record containing Mox:x orientation Moy:y orientation Moz:z orientation -// This record is optional, by default its turned off (it can always be calculated later - Python) -// -// {'ACL':{'Acx':f,'Acy':f,'Acz':f}} -// LSM303DLH acclerometer record containing Acx:the x acceleration Acy:the y acceleration Acz:the z acceleration -// If this record immediatly follows a VIB record the fields were hardware latched when the g threshold was exceeded -// -// {'AOL':{'Aox':f,'Aoy':f,'Aoz':f}} -// LSM303DLH accelerometer record containing Aox:x orientation Aoy:y orientation Aoz:z orientation -// This record is optional, by default its turned off (it can always be calculated later - Python) -// -// {'LOC':{'Lat':f,'Lon':f,'Alt':f}} -// GPS location record containing Lat:latitude in degrees Lon:longitude in degrees Alt:altitude in meters -// -// {'TIM':{'Upt':i,'Frq':i,'Sec':i}} -// Time record containing Upt:up time seconds Frq:counter frequency Sec:time string -// -// {'STS':{'Qsz':i,'Mis':i,'Ter':i,'Htu':i,'Bmp':i,'Acl':i,'Mag':i, 'Gps':i}} -// Status record containing Qsz:events on queue Mis:missed events Ter:buffer error -// Htu:status Bmp:status Acl:status Mag:status Gps:ststus -// -// {'EVT':{'Evt':i,'Frq':i,'Tks':i,'Etm':f,'Adc':[[i,i,i,i,i,i,i,i][i,i,i,i,i,i,i,i]]}} -// Event record containing Evt:event number in second Frq:timer frequency Tks:ticks since last event in second -// Etm:event time stamp to 100ns Adc:[[Channel 0 values][Channel 1 values]] - -// N.B. These records pass the data to a python monitor over the serial line. Python has awsome string handling and looks them up in -// associative arrays to build records of any arbitary format you want. So this is only the start of the story of record processing. -// N.B. Also some of these records are sent out at regular intervals and or when an event occurs. - -// This program also accepts commands sent to it on the serial line. -// When a command arrives it is immediatly executed. - -#include -#include - -#include "Adafruit_BMP085_U.h" // Barrometric pressure - -#include "Adafruit_HTU21DF.h" // Humidity and temperature sensor - -// GPS chips typically return NMEA strings over a serial line. -// They can be programmed to send different NMEA strings according to what you configure. -// The string RMCGGA has the altitude but misses the yy/mm/dd from the date, it only has hh/mm/ss. -// This is easilly made up for in the Python monitor which gets this information from it system time. - -#include "Adafruit_GPS.h" // GPS chip -#define GPSECHO true -#define RMCGGA // Altitude on, yy/mm/dd off - -#include "Adafruit_L3GD20_U.h" // Magoscope - -// WARNING: I had to modify this library, its no longer standard -#include "Adafruit_LSM303_U.h" // Accelerometer and magnentometer/compass - -#include "Adafruit_10DOF.h" // 10DOF breakout driver - scale to SI units - -// Configuration constants - -// The size of the one second event buffer -#define PPS_EVENTS 8 // The maximum number of events stored per second -#define ADC_BUF_LEN 8 // Number of ADC values per event - -// This is the event queue size -#define EVENT_QSIZE 32 // The number of events that can be queued for serial output - -// Handle text buffer serial output overflow errors -// When the output buffer overflows due to data comming too fast, we just stop printing due -// to insufficient bandwidth or slow things down if HANDLE_OVERFLOW is set (not recomended) -// #define HANDLE_OVERFLOW - -// This is the text ring buffer for real time output to serial line with interrupt on -#define TBLEN 4096 // Serial line output ring buffer size - -// Define some output debug pins to monitor whats going on via an oscilloscope -#define PPS_PIN 13 // PPS (Pulse Per Second) and LED -#define EVT_PIN 12 // Cosmic ray event detected -#define FLG_PIN 11 // Debug flag - -// For siesmic event input -#define ACL_PIN 10 // Accelarometer INT1 interrupt pin - -// Baud rates -#define SERIAL_BAUD_RATE 9600 // Serial line -#define GPS_BAUD_RATE 9600 // GPS and Serial1 line - -// Instantiate external hardware breakouts - -Adafruit_GPS gps(&Serial1); // GPS Serial1 on pins RX1 and TX1 -boolean gps_ok = false; // Chip OK flag - -Adafruit_HTU21DF htu = Adafruit_HTU21DF(); // Humidity and temperature measurment -boolean htu_ok = false; // Chip OK - -#define BMPID 18001 -Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(BMPID); // Barometric pressure -boolean bmp_ok = false; - -// The 10DOF isn't a chip, its just a utility to convert say mago values into headings etc - -Adafruit_10DOF dof = Adafruit_10DOF(); // The 10 Degrees-Of-Freedom DOF breakout -boolean dof_ok = false; // board driver, scales units to SI - -#define ACLID 30301 -Adafruit_LSM303_Accel_Unified acl = Adafruit_LSM303_Accel_Unified(ACLID); // Accelerometer Compass - boolean acl_ok = false; - -#define MAGID 30302 -Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(MAGID); // Magoscope -boolean mag_ok = false; - -// Control the output data rates by setting defaults, these values can be modified at run time -// via commands from the serial interface. Some output like position isn't supposed to be changing -// very fast if at all, so no need to clutter up the serial line with it. The Python monitor keeps -// the last sent values when it builds event messages to be sent over the internet to the server -// or logged to a file. - -uint32_t latlon_display_rate = 12; // Display latitude and longitude each X seconds -uint32_t humtmp_display_rate = 12; // Display humidity and HTU temperature each X seconds -uint32_t alttmp_display_rate = 12; // Display altitude and BMP temperature each X seconds -uint32_t frqutc_display_rate = 1; // Display frequency and UTC time each X seconds -uint32_t status_display_rate = 4; // Display status (UpTime, QueueSize, MissedEvents, HardwareOK) -uint32_t accelr_display_rate = 1; // Display accelarometer x,y,z -uint32_t magnot_display_rate = 12; // Display magnotometer data x,y,z - -uint32_t events_display_size = 20; // Display events after recieving X events - -// Siesmic event trigger parameters - -uint32_t accelr_event_threshold = 2; // Trigger level for siesmic events in milli-g -uint32_t accelr_event_cutoff_fr = 30; // Siesmic event cutoff frequency - -// Commands can be sent over the serial line to configure the display rates or whatever - -typedef enum { - NOOP, // No-operation - HELP, // Help - HTUX, // Reset HTU chip - HTUD, // HUT display rate - BMPD, // BMP display rate - LOCD, // Location display rate - TIMD, // Timing display rate - STSD, // Status display rate - EVQT, // Event queue dump threshold - - ACLD, // Accelerometer display rate - MAGD, // Magnetometer display rate - - ACLT, // Accelerometer event threshold - - CMDS }; // Command count - -typedef struct { - int Id; // Command ID number - void (*proc)(int arg); // Function to call - char *Name; // Command name - char *Help; // Command help text - int Par; // Command parameter flag -} CmdStruct; - -// Function forward references - -void noop(int arg); -void help(int arg); -void htux(int arg); -void htud(int arg); -void bmpd(int arg); -void locd(int arg); -void timd(int arg); -void stsd(int arg); -void evqt(int arg); -void acld(int arg); -void magd(int arg); -void aclt(int arg); - -// Command table - -CmdStruct cmd_table[CMDS] = { - { NOOP, noop, "NOOP", "Do nothing", 0 }, - { HELP, help, "HELP", "Display commands", 0 }, - { HTUX, htux, "HTUX", "Reset the HTU chip", 0 }, - { HTUD, htud, "HTUD", "HTU Temperature-Humidity display rate", 1 }, - { BMPD, bmpd, "BMPD", "BMP Temperature-Altitude display rate", 1 }, - { LOCD, locd, "LOCD", "Location latitude-longitude display rate", 1 }, - { TIMD, timd, "TIMD", "Timing uptime-frequency-utc display rate", 1 }, - { STSD, stsd, "STSD", "Status info display rate", 1 }, - { EVQT, evqt, "EVQT", "Event queue dump threshold", 1 }, - { ACLD, acld, "ACLD", "Accelerometer display rate", 1 }, - { MAGD, magd, "MAGD", "Magnatometer display rate", 1 }, - { ACLT, aclt, "ACLT", "Accelerometer event trigger threshold", 1 } -}; - -#define CMDLEN 32 -static char cmd[CMDLEN]; // Command input buffer -static int irdp=0, irdy=0, istp=0; // Read, ready, stop - -static char txtb[TBLEN]; // Text ring buffer -static uint32_t txtw = 0, txtr = 0, // Write and Read indexes - tsze = 0, terr = 0; // Buffer size and error code - -typedef enum { TXT_NOERR=0, TXT_TOOBIG=1, TXT_OVERFL=2 } TxtErr; - -#define TXTLEN 256 -static char txt[TXTLEN]; // For writing to serial - -// Initialize the timer chips to measure time between the PPS pulses and the EVENT pulse -// The PPS enters pin D2, the PPS is forwarded accross an isolating diode to pin D5 -// The event pulse is also connected to pin D5. So D5 sees the LOR of the PPS and the -// event, while D2 sees only the PPS. In this way we measure the frequency of the -// clock MCLK/2 each second on the first counter, and the time between EVENTs on the second - -void TimersStart() { - - uint32_t config = 0; - - // Set up the power management controller for TC0 and TC2 - - pmc_set_writeprotect(false); // Enable write access to power management chip - pmc_enable_periph_clk(ID_TC0); // Turn on power for timer block 0 channel 0 - pmc_enable_periph_clk(ID_TC6); // Turn on power for timer block 2 channel 0 - - // Timer block zero channel zero is connected only to the PPS - // We set it up to load regester RA on each PPS and reset - // So RA will contain the number of clock ticks between two PPS, this - // value should be very stable +/- one tick - - config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz - TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA0 - TC_CMR_ABETRG | // Use the TIOA external input line - TC_CMR_LDRA_RISING; // Latch counter value into RA - - TC_Configure(TC0, 0, config); // Configure channel 0 of TC0 - TC_Start(TC0, 0); // Start timer running - - TC0->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS - TC0->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources - NVIC_EnableIRQ(TC0_IRQn); // Enable interrupt handler for channel 0 - - // Timer block 2 channel zero is connected to the OR of the PPS and the RAY event - - config = TC_CMR_TCCLKS_TIMER_CLOCK1 | // Select fast clock MCK/2 = 42 MHz - TC_CMR_ETRGEDG_RISING | // External trigger rising edge on TIOA1 - TC_CMR_ABETRG | // Use the TIOA external input line - TC_CMR_LDRA_RISING; // Latch counter value into RA - - TC_Configure(TC2, 0, config); // Configure channel 0 of TC2 - TC_Start(TC2, 0); // Start timer running - - TC2->TC_CHANNEL[0].TC_IER = TC_IER_LDRAS; // Enable the load AR channel 0 interrupt each PPS - TC2->TC_CHANNEL[0].TC_IDR = ~TC_IER_LDRAS; // and disable the rest of the interrupt sources - NVIC_EnableIRQ(TC6_IRQn); // Enable interrupt handler for channel 0 - - // Set up the PIO controller to route input pins for TC0 and TC2 - - PIO_Configure(PIOC,PIO_INPUT, - PIO_PB25B_TIOA0, // D2 Input - PIO_DEFAULT); - - PIO_Configure(PIOC,PIO_INPUT, - PIO_PC25B_TIOA6, // D5 Input - PIO_DEFAULT); -} - -// Timer chip interrupt handlers try to get time stamps to within 4 system clock ticks - -static uint32_t displ = 0; // Display values in loop - -static uint32_t ppsfl = LOW, // PPS Flag boolean - rega0 = 0, // RA reg - stsr0 = 0, // Interrupt status register - ppcnt = 0; // PPS count - -// Handle the PPS interrupt in counter block 0 ISR - -void TC0_Handler() { - - // This ISR is connected only to the PPS (Pulse Per Second) GPS event - // Each time this runs, set the flag to tell the TC6 ISR we have seen it - // This logic only works if the TC0 handler gets called before the TC6 handler - // hence the debug flag which I look at with a scope to be sure. - // I may introduce a small delay line to ensure this is true, so far it is. - - ppsfl = HIGH; // Seen a rising edge on the PPS -#if FLG_PIN - digitalWrite(FLG_PIN,ppsfl); // Flag set (for debug) -#endif - rega0 = TC0->TC_CHANNEL[0].TC_RA; // Read the RA reg (PPS period) - stsr0 = TC_GetStatus(TC0, 0); // Read status and clear load bits - - ppcnt++; // PPS count - displ = 1; // Display stuff in the loop -} - -// We need a double buffer, one is being written by the ISR while -// the other is read from user space within one second. - -struct Event { - uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values - uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values - uint32_t Tks; // Time since last event in ticks -}; - -static struct Event b1[PPS_EVENTS]; // Event ticks buffeer -static struct Event b2[PPS_EVENTS]; // Event ticks buffer -static struct Event *wbuf = b1; // Write event buffer pointer and its index -static struct Event *rbuf = b2; // Read event buffer pointer and its index -static int ridx, widx; - -// We also need a time value for the current and previous second - -#ifdef RMCGGA -#define DATE_TIME_LEN 9 -#else -#define DATE_TIME_LEN 17 -#endif - -static char t1[DATE_TIME_LEN]; // Date time buffer text string -static char t2[DATE_TIME_LEN]; -static char *wdtm = t1; // Write date/time pointer -static char *rdtm = t2; // Read date/time pointer - -// Swap read write event buffers and indexes along with their time strings -// each second, so we have the current and previous second numbers - -void SwapBufs() { - struct Event *tbuf; // Temp event buf pointer - char *tdtm; // Temp date/time string pointer - tbuf = rbuf; rbuf = wbuf; wbuf = tbuf; // Swap write with read buffer - ridx = widx; widx = 0; // Write count to read, reset the write count - tdtm = rdtm; rdtm = wdtm; wdtm = tdtm; // And swap asociated buffer date/time -} - -// Handle isolated PPS (via diode) LOR with the Event -// The diode is needed to block Event pulses getting back to TC0 -// LOR means Logical inclusive OR - -static uint32_t rega1, stsr1 = 0; - -void TC6_Handler() { - - // This ISR is connected to the OR of the event and the PPS - // If the TC0 has seen the PPS it sets the flag high - // and if its high we are seeing the PPS here, but if the - // flag is not set then this is a cosmic ray event. - - if (ppsfl == HIGH) { // Was ther a PPS ? - ppsfl = LOW; // Yes so we have seen it here - SwapBufs(); // Every PPS swap the read/write buffers -#if EVT_PIN - digitalWrite(EVT_PIN,LOW); // Not an event -#endif - } else { -#if EVT_PIN - digitalWrite(EVT_PIN,HIGH); // Event detected -#endif - if (widx < PPS_EVENTS) { // Up to PPS_EVENTS stored per PPS - - // Read the latched tick count getting the event time - // and then pull the ADC pipe line - - wbuf[widx].Tks = TC2->TC_CHANNEL[0].TC_RA; - AdcPullData(&wbuf[widx]); - widx++; - } - } -#if FLG_PIN - digitalWrite(FLG_PIN,ppsfl); // Flag out -#endif - rega1 = TC2->TC_CHANNEL[0].TC_RA; // Read the RA on channel 1 (PPS period) - stsr1 = TC_GetStatus(TC2, 0); // Read status clear load bits -} - -// Accelerometer setup -// These settings come from reading the LSM303DLH doccumentation, which is more than the -// author of the Adda_fruit library did. It is a badly written load of rubbish, I have -// been forced to correct some bugs and export some private methods. It was a touch and go -// descision wether to just not use the library at all and do it all here. For now I will -// use it with the bug corrections. - -// The following setup makes the accelarometer compare G-forces against a threshold value, and -// latch the output registers until they are read. To avoid excessive interrupt rates the high -// pass filter has been configured to keep the frequency low. - -// N.B. It took me a day to find out that I needed to use Active low and Open drain on the INT1 -// signal, otherwise the Adda_fruit module wont pass it on. Beware !!! - -void AclSetup() { - uint8_t tmp, val; - - if (!acl_ok) return; - -#define PMD 0x20 // Normal power mode (PM0=1,PM1=0:Normal) -#define DRT 0x00 // Data rate 50 Hz (0x08 = 100Hz) -#define AEN 0x07 // XYZ Enabled - - val = PMD | DRT | AEN; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, val); - -#define HPE1 0x04 // High pass filter Int 1 on -#define HPCF 0x03 // High pass cut off frequency - - val = HPE1 | HPCF; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG2_A, val); - -#define LIR1 0x06 // Latch Int1 bit Data ready -#define LIR2 0x00 // Latch Int2 bit Data ready (0x20 Latch On) - -#define IHL_OD 0xC0 // Interrupt active low, open drain (Argh !!!) - - val = LIR1 | LIR2 | IHL_OD; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG3_A, val); - -#define BDU_FS 0x80 // Block data and scale +-2g - - val = BDU_FS; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG4_A, val); - -#define XYZ_HI 0x2A // Hi values ZHIE YHIE XHIE -#define AOI_6D 0x00 // 0xC0 would enable 6 directions - - val = XYZ_HI | AOI_6D; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_CFG_A, val); - - val = accelr_event_threshold & 0x7F; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); - - // The chip make very wide pulses (100ms), the values on the rising and - // falling edges are different ! - - attachInterrupt(digitalPinToInterrupt(ACL_PIN),Acl_ISR,RISING); -} - -// Magnatometer setup, again the Adda_fruit library was inadequate. - -void MagSetup() { - uint8_t val; - - if (!mag_ok) return; - - val = 0; - mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, val); - -#define GAIN 0x80 // +- 4.0 Gauss - - val = GAIN; - mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, val); - -#define MODE 0x0 // 01=Single conversion mode - - val = MODE; - mag.write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, val); -} - -// This Accelerometer ISR - -static uint32_t accl_icount = 0, accl_flag = 0; - -void Acl_ISR() { - -#define IA 0x40 - - accl_icount++; - accl_flag = AclReadStatus(); - PushVib(); -} - -// Read accelerometer status -// This just reads the interrupt source INT1 and the overrun status. -// It returns 1 bit for X, Y, or Z (0..7) if the threshold value is exceeded. -// This determins if the board is being shaken - Earth quake - or other reason - -static uint8_t acl_sts = 0; -static uint8_t acl_src = 0; - -uint8_t AclReadStatus() { - uint8_t rval; - - acl_src = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_SOURCE_A); - -#define ZH 0x20 // Z High -#define YH 0x08 // Y High -#define XH 0x02 // X High - - rval = 0; - if (acl_src & IA) { - if (acl_src & ZH) rval |= 4; - if (acl_src & YH) rval |= 2; - if (acl_src & XH) rval |= 1; - } - if (rval) - acl_sts = acl.read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_STATUS_REG_A); - return rval; -} - -// Set up the ADC channels - -void AdcSetup() { - REG_ADC_MR = 0x10380080; // Free run as fast as you can - REG_ADC_CHER = 3; // Channels 0 and 1 -} - -// Pull all data (16 values) from the ADC into a buffer - -uint8_t AdcPullData(struct Event *b) { - - int i; - - for (i=0; iADC_ISR & 0x01)==0); // Wait for channel 0 (2.5us) - b->Ch0[i] = (uint16_t) ADC->ADC_CDR[0]; // Read ch 0 - while((ADC->ADC_ISR & 0x02)==0); // Wait for channel 1 (2.5us) - b->Ch1[i] = (uint16_t) ADC->ADC_CDR[1]; // Read ch 1 - } -} - -// This is the nmea data string from the GPS chip - -#define GPS_STRING_LEN 256 -static char gps_string[GPS_STRING_LEN + 1]; - -float latitude = 0.0, longitude = 0.0, altitude = 0.0; - -// This function is dependent on the GPS chip implementation -// It should return a date time string as described above -// So you need to re-implements this for whichever chip you are using -// Here I am using the addafruit GPS chip - -char *GetDateTime() { - - int i = 0; - while (Serial1.available()) { - if (i < GPS_STRING_LEN) { - gps_string[i++] = (char) Serial1.read(); - gps_string[i] = 0; - } else i++; - } - if (gps.parse(gps_string)) { - -#ifdef RMCGGA - // I choose RMCGGA by default, and get the altitude but no date. - // Its easy to get the date once the records arrive at the Python end. - // The GPS altitude is far more accurate than the barrometric altitude. - // Warning: The syntax can not be changed, we need an integer hhmmss - - sprintf(wdtm,"%02d%02d%02d", - gps.hour,gps.minute,gps.seconds); - - altitude = gps.altitude; -#else - sprintf(wdtm,"%02d%02d%02d%02d%02d%02d%02d%02d", - gps.year,gps.month,gps.day, - gps.hour,gps.minute,gps.seconds); - - altitude = 0; -#endif - latitude = gps.latitudeDegrees; // Easy place to get location - longitude = gps.longitudeDegrees; // Works well in Google maps - - return rdtm; - } else - return NULL; -} - -// Implement queue access mechanism for events, each second the user space (loop) copies -// any events it has read onto the queue - -struct EventBuf { - char DateTime[DATE_TIME_LEN]; // The date and time string - uint32_t Frequency; // The current clock frequency - uint32_t Ticks; // The number of ticks since the last event or PPS if none - uint16_t Ch0[ADC_BUF_LEN]; // ADC channel 0 values - uint16_t Ch1[ADC_BUF_LEN]; // ADC channel 1 values - uint8_t Count; // The number of events since the PPS -}; - -typedef struct { - uint8_t Size; // Current size of the queue - uint8_t RdPtr; // Read pointer - uint8_t WrPtr; // Write pointer - uint8_t Missed; // Missed events counter due to overflow - uint8_t Lock; // The queue spin lock (not needed here) - struct EventBuf Events[EVENT_QSIZE]; // Queued events -} EventQueue; - -static EventQueue event_queue; - -// Put an event in an EventBuf on the queue, if the queue is full then the oldest event -// is thrown away and the "missed" event count is incremented - -uint8_t PutQueue(struct EventBuf *ebuf) { - - EventQueue *q = &event_queue; - - while(q->Lock) {}; q->Lock = 1; // Spin lock on queue - q->Events[q->WrPtr] = *ebuf; // Write event to the queue - q->WrPtr = (q->WrPtr + 1) % EVENT_QSIZE;// Increment the write pointer - if (q->Size < EVENT_QSIZE) q->Size++; // If we are overwriting old enties that havnt been read - else { - q->Missed++; // Say we missed some events - q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; // and throw the oldest event away - } - q->Lock = 0; - return q->Missed; -} - -// Pop an event off the queue, if the queue is empty nothing happens -// the queue size is zero when the queue is empty, and this is the -// return value - -uint8_t PopQueue(struct EventBuf *ebuf) { // Points to where the caller wants the event stored - - EventQueue *q = &event_queue; - - while(q->Lock) {}; q->Lock = 1; // Spin lock on queue - if (q->Size) { - *ebuf = q->Events[q->RdPtr]; - q->RdPtr = (q->RdPtr + 1) % EVENT_QSIZE; - q->Size--; - } - q->Lock = 0; - return q->Size; -} - -// Get the size of the queue - -uint8_t SzeQueue() { - - EventQueue *q = &event_queue; - - return q->Size; -} - -// Initialize the queue - -void InitQueue() { - - EventQueue *q = &event_queue; - - q->Lock = 1; - q->Size = 0; - q->RdPtr = 0; - q->WrPtr = 0; - q->Missed = 0; - q->Lock = 0; -} - -// Arduino setup function, initialize hardware and software -// This is the first function to be called when the sketch is started - -void setup() { - -#if FLG_PIN - pinMode(FLG_PIN, OUTPUT); // Pin for the ppsfl flag for debug -#endif -#if EVT_PIN - pinMode(EVT_PIN, OUTPUT); // Pin for the cosmic ray event -#endif -#if PPS_PIN - pinMode(PPS_PIN, OUTPUT); // Pin for the PPS (LED pin) -#endif - - Serial.begin(SERIAL_BAUD_RATE); // Start the serial line - Serial1.begin(GPS_BAUD_RATE); // and the second - - gps.begin(GPS_BAUD_RATE); // Chip baud rate - -#ifdef RMCGGA - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); // With altitude but no yy/mm/dd -#else - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); // With yy/mm/dd but no altitude -#endif - gps.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // each second - - - InitQueue(); // Reset queue pointers, missed count, and size - - strcpy(rdtm,""); // Set initial value for date/time - strcpy(wdtm,""); - - htu_ok = htu.begin(); - bmp_ok = bmp.begin(); - acl_ok = acl.begin(); - mag_ok = mag.begin(); - dof_ok = dof.begin(); - - AclSetup(); - MagSetup(); - AdcSetup(); - - TimersStart(); // Start timers -} - -// These two routines are needed because the Serial.print method prints without using interrupts. -// Calls to Serial.print block interrupts and use a wait in kernel space causing all ISRs to -// be blocked and hence we could miss some timer interrupts. -// To avoid this problem call PushTxt to have stuff delivered to the serial line, PushTxt simply -// stores your text for future print out by PutChar. The PutChar routine removes one character -// from the stored text each time its called. By placing a call to PutChar in the outermost loop -// of the Arduino loop function, then for each loop one character is printed, avoiding blocking -// of interrupts and vastly improving the loops real time behaviour. - -// Copy text to the buffer for future printing - -void PushTxt(char *txt) { - - int i, l = strlen(txt); - - // If this happens there is a programming bug - - if (l > TBLEN) { // Can't handle more than TBLEN at a time - terr = TXT_TOOBIG; // say error and abort - return; - } - - // If the buffer is filling up to fast throw it away and return an error - - if ((l + tsze) >= TBLEN) { // If there is no room in the buffer - terr = TXT_OVERFL; // Buffer overflow - return; // Simply stop printing when txt comming too fast - } - - // Copy the new text onto the ring buffer for later output - // from the loop idle function - - for (i=0; i Earth Quake - -uint32_t old_icount = 0; - - if (accl_flag) { - if (accl_icount != old_icount) { - old_icount = accl_icount; - PushTim(1); // Push these first, and then vib - PushAcl(1); // This is the real latched value - PushMag(1); - sprintf(txt,"{'VIB':{'Vax':%d,'Vcn':%d}}\n",accl_flag,accl_icount); - PushTxt(txt); - } - } -} - -// Push the magnetic field strengths in all three axis in micro tesla (gauss) - -void PushMag(int flg) { // Push the mago stuff - sensors_event_t mag_event; - sensors_vec_t xyz; - - if ((flg) || ((mag_ok) && ((ppcnt % magnot_display_rate) == 0))) { - mag.getEvent(&mag_event); - - // Micro Tesla - - sprintf(txt,"{'MAG':{'Mgx':%f,'Mgy':%f,'Mgz':%f}}\n", - mag_event.magnetic.x, - mag_event.magnetic.y, - mag_event.magnetic.z); - PushTxt(txt); - - // Orientation (Easy to calculate later in Python - dont waste resources) -#ifdef ORIENTATION - if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &xyz)) { - sprintf(txt,"{'MOG':{'Mox':%f,'Moy':%f,'Moz':%f}}\n",xyz.x,xyz.y,xyz.z); - PushTxt(txt); - } -#endif - } -} - -// Push the acceleration values in xyz in meters per sec squared - -void PushAcl(int flg) { // Push the accelerometer and compass stuff - sensors_event_t acl_event; - sensors_vec_t xyz; - - if ((flg) || ((acl_ok) && ((ppcnt % accelr_display_rate) == 0))) { - acl.getEvent(&acl_event); - - // Meters per second squared - - sprintf(txt,"{'ACL':{'Acx':%f,'Acy':%f,'Acz':%f}}\n", - acl_event.acceleration.x, - acl_event.acceleration.y, - acl_event.acceleration.z); - PushTxt(txt); - - // Orientation (Easy to calculate later in Python - dont waste resources) -#ifdef ORIENTATION - if (dof.accelGetOrientation(&acl_event, &xyz)) { - sprintf(txt,"{'AOL':{'Aox':%f,'Aoy':%f,'Aoz':%f}}\n",xyz.x,xyz.y,xyz.z); - PushTxt(txt); - } -#endif - } -} - -// Push location latitude longitude in degrees so that google maps gets it correct - -void PushLoc(int flg) { - - if ((flg) || ((ppcnt % latlon_display_rate) == 0)) { - sprintf(txt,"{'LOC':{'Lat':%f,'Lon':%f,'Alt':%f}}\n",latitude,longitude,altitude); - PushTxt(txt); - } -} - -// Push timing - -void PushTim(int flg) { - - if ((flg) || ((ppcnt % frqutc_display_rate) == 0)) { - sprintf(txt,"{'TIM':{'Upt':%4d,'Frq':%7d,'Sec':%s}}\n",ppcnt,rega0,rdtm); - PushTxt(txt); - } -} - -// Push status - -void PushSts(int flg, int qsize, int missed) { -uint8_t res; - - if ((flg) || ((ppcnt % status_display_rate) == 0)) { - sprintf(txt,"{'STS':{'Qsz':%2d,'Mis':%2d,'Ter':%d,'Htu':%d,'Bmp':%d,'Acl':%d,'Mag':%d,'Gps':%d}}\n", - qsize,missed,terr,htu_ok,bmp_ok,acl_ok,mag_ok,gps_ok); - PushTxt(txt); - terr = 0; - } -} - -// Push event queue - -void PushEvq(int flg, int *qsize, int *missed) { - - struct EventBuf eb; // Temporary event buffer - double evtm = 0.0; // Time since last event or PPS in seconds (< 1.0) - char stx[16]; // Second text - int i,j; - - // If there are any events waiting in the event read buffer, put them on the queue - - for (i=0; i= events_display_size) { - - PushTxt("\n"); - while (PopQueue(&eb)) { // While ther are events on the queue - - // Calculate the time in seconds of this event in the second - // N.B. Ticks are since the last event, or from PPS - - if (eb.Count == 1) evtm = 0.0; // Start a new second - evtm += ((double) eb.Ticks / (double) eb.Frequency); // Add time since last event - sprintf(stx,"%9.7f",evtm); // It will be 0.something - - // Build string and push it out to the print buffer - - sprintf(txt, - "{'EVT':{'Evt':%1d,'Frq':%8d,'Tks':%8d,'Etm':%s%s," - "'Adc':[[%d,%d,%d,%d,%d,%d,%d,%d],[%d,%d,%d,%d,%d,%d,%d,%d]]}}\n", - eb.Count, eb.Frequency, eb.Ticks, eb.DateTime, index(stx,'.'), - eb.Ch0[0],eb.Ch0[1],eb.Ch0[2],eb.Ch0[3],eb.Ch0[4],eb.Ch0[5],eb.Ch0[6],eb.Ch0[7], - eb.Ch1[0],eb.Ch1[1],eb.Ch1[2],eb.Ch1[3],eb.Ch1[4],eb.Ch1[5],eb.Ch1[6],eb.Ch1[7]); - PushTxt(txt); - } - PushTxt("\n"); - } -} - -// Read one input character, we have exactly the same problem with -// the serial line read as with writing, so we need the same work around - -void ReadOneChar() { - char c; - - // Suck in all the characters available on the input stream - // put as many as will fit in the cmd buffer, and say ready - - if ((irdy == 0) && (Serial.available())) { // If buffer free - c = (char) Serial.read(); // Read one char - if (c == '\n') istp = 1; // Stop on '\n' - if ((!istp) && (irdp < (CMDLEN -1))) { - cmd[irdp] = c; - irdp = irdp + 1; - cmd[irdp] = 0; - } - } else irdy = 1; -} - -// Implement the command callback functions - -void noop(int arg) { }; // That was easy - -void help(int arg) { // Display the help - int i; - CmdStruct *cms; - - for (i=0; iName,cms->Par,cms->Help); - PushTxt(txt); - } -} - -void htux(int arg) { htu_ok = htu.begin(); } -void htud(int arg) { humtmp_display_rate = arg; } -void bmpd(int arg) { alttmp_display_rate = arg; } -void locd(int arg) { latlon_display_rate = arg; } -void timd(int arg) { frqutc_display_rate = arg; } -void stsd(int arg) { status_display_rate = arg; } - -void evqt(int arg) { - events_display_size = arg % EVENT_QSIZE; - if (events_display_size == 0) - events_display_size = 24; -} - -void acld(int arg) { accelr_display_rate = arg; } -void magd(int arg) { magnot_display_rate = arg; } - -void aclt(int arg) { - uint8_t val = 0; - accelr_event_threshold = arg & 0x7F; - val = accelr_event_threshold; - acl.write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_INT1_THS_A, val); -} - -// Look up a command in the command table for the given command string -// and call it with its single integer parameter - -void ParseCmd() { - int i, p=0, cl=0; - char *cp, *ep; - CmdStruct *cms; - - for (i=0; iName); - if (strncmp(cms->Name,cmd,cl) == 0) { - if ((cms->Par) && (strlen(cmd) > cl)) { - cp = &cmd[cl]; - p = (int) strtoul(cp,&ep,0); - } - cms->proc(p); - break; - } - } -} - -// This waits for a ready buffer from ReadOneChar. Once ready the buffer is -// locked until its been seen here - -void DoCmd() { - if (irdy) { - if (irdp) { - sprintf(txt,"{'CMD':%s}\n",cmd); - PushTxt(txt); - ParseCmd(); - } - bzero((void *) cmd, CMDLEN); - irdp = 0; irdy = 0; istp = 0; - } -} - -// Arduino main loop does all the user space work - -void loop() { - - int missed, qsize; // Queue vars - - if (displ) { // Displ is set in the PPS ISR, we will reset it here -#if PPS_PIN - digitalWrite(PPS_PIN,HIGH); // PPS arrived -#endif - DoCmd(); // Execute any incomming commands - PushEvq(0,&qsize,&missed); // Push any events - PushHtu(0); // Push HTU temperature and humidity - PushBmp(0); // Push BMP temperature and barrometric altitude - PushLoc(0); // Push location latitude and longitude - PushTim(0); // Push timing data - PushMag(0); // Push mago data - PushAcl(0); // Push accelarometer data - PushSts(0,qsize,missed); // Push status - GetDateTime(); // Read the next date/time from the GPS chip -#if PPS_PIN - digitalWrite(PPS_PIN,LOW); // Reset PPS -#endif - displ = 0; // Clear flag for next PPS - gps_ok = true; // Its OK because we got a PPS - } - - if (gps_ok == false) { - delay(1000); // One second sleep - PushSts(0,qsize,missed); // Push bad hardware status - } - - PutChar(); // Print one character per loop !!! - ReadOneChar(); // Get next input command char -} diff --git a/new_geo_cosmic_pi.py b/new_geo_cosmic_pi.py deleted file mode 100644 index b765dc1..0000000 --- a/new_geo_cosmic_pi.py +++ /dev/null @@ -1,606 +0,0 @@ -#! /usr/bin/python -# coding: utf8 - -""" -Talk to the CosmicPi Arduino DUE accross the serial USB link -This program has the following functions ... - -1) Build event messages and send them to a server or local port - - Events are any combination of Vibration, Weather and CosmicRays - Hence the Arduino can behave as a weather station, as a vibration/Siesmic monitor - and as a cosmic ray detector. - There is a gyroscope available but I don't use it - -2) Perform diagnostics and monitoring of the Arduino via commands - -3) Log events to the log file - -Typing the '>' character turns on command input - -It is important to keep the Python dictionary objects synchronised with the Arduino firmware -otherwise this monitor will not understand the data being sent to it - -julian.lewis lewis.julian@gmail.com 7/Apr/2016 - -""" - -import sys -import socket -import select -import serial -import time -import traceback -import os -import termios -import fcntl -import re -import ast -from optparse import OptionParser - -# Handle keyboard input, this tests to see if a '>' was typed - -class KeyBoard(object): - - def __init__(self): - self.fd = sys.stdin.fileno() - - def echo_off(self): - self.oldterm = termios.tcgetattr(self.fd) - self.newattr = termios.tcgetattr(self.fd) - self.newattr[3] = self.newattr[3] & ~termios.ICANON & ~termios.ECHO - termios.tcsetattr(self.fd, termios.TCSANOW, self.newattr) - self.oldflags = fcntl.fcntl(self.fd, fcntl.F_GETFL) - fcntl.fcntl(self.fd, fcntl.F_SETFL, self.oldflags | os.O_NONBLOCK) - - def echo_on(self): - termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.oldterm) - fcntl.fcntl(self.fd, fcntl.F_SETFL, self.oldflags) - - def test_input(self): - res = False - try: - c = sys.stdin.read(1) - if c == '>': - res = True - except IOError: pass - return res - -# This is the event object, it builds a dictionary from incomming jsom strings -# and provides access to the dictionary entries containing the data for each field. - -class Event(object): - - def __init__(self): - - # These are the json strings we are expecting from the arduino - - self.HTU = { "Tmh":"0.0","Hum":"0.0" } - self.BMP = { "Tmb":"0.0","Prs":"0.0","Alb":"0.0" } - self.VIB = { "Vax":"0" ,"Vcn":"0" } - self.MAG = { "Mgx":"0.0","Mgy":"0.0","Mgz":"0.0" } - self.MOG = { "Mox":"0.0","Moy":"0.0","Moz":"0.0" } - self.ACL = { "Acx":"0.0","Acy":"0.0","Acz":"0.0" } - self.AOL = { "Aox":"0.0","Aoy":"0.0","Aoz":"0.0" } - self.LOC = { "Lat":"0.0","Lon":"0.0","Alt":"0.0" } - self.TIM = { "Upt":"0" ,"Frq":"0" ,"Sec":"0" } - self.STS = { "Qsz":"0" ,"Mis":"0" ,"Ter":"0","Htu":"0" ,"Bmp":"0","Acl":"0","Mag":"0","Gps":"0" } - self.EVT = { "Evt":"0" ,"Frq":"0" ,"Tks":"0","Etm":"0.0","Adc":"[[0,0,0,0,0,0,0,0],[0,0,0,0,0,0,0,0]]" } - - # Add ons - - self.DAT = { "Dat":"s" } # Date - self.SQN = { "Sqn":"0" } # Sequence number - self.PAT = { "Pat":"s","Ntf":"0" } # Pushover application token - - # Now build the main dictionary with one entry for each json string we will process - - self.recd = { "HTU":self.HTU, "BMP":self.BMP, "VIB":self.VIB, "MAG":self.MAG, "MOG":self.MOG, - "ACL":self.ACL, "AOL":self.AOL, "LOC":self.LOC, "TIM":self.TIM, "STS":self.STS, - "EVT":self.EVT, "DAT":self.DAT, "SQN":self.SQN, "PAT":self.PAT } - - self.newvib = 0 # Vibration - self.newevt = 0 # Cosmic ray - self.newhtu = 0 # Weather report - - self.sqn = 0 # Packet sequenc number - - self.ohum = 0.0 # Old humidity value - self.otmb = 0.0 # Old barometric temperature value - self.oprs = 0.0 # Old barometric presure value - - # Convert the incomming json strings into entries in the dictionary - - def parse(self, line): # parse the incomming json strings from arduino - nstr = line.replace('\n','') # Throw away , we dont want them - try: - dic = ast.literal_eval(nstr) # Build a dictionary entry - kys = dic.keys() # Get key names, the first is the address - if self.recd.has_key(kys[0]): # Check we know about records with this key - self.recd[kys[0]] = dic[kys[0]] # and put it in the dictionary at that address - - if kys[0] == "VIB": - self.newvib = 1 - - if kys[0] == "EVT": - self.newevt = 1 - - if kys[0] == "HTU": - self.newhtu = 1 - - except Exception, e: - #print e - #print "BAD:%s" % line - pass # Didnt understand, throw it away - - def extract(self, entry): - if self.recd.has_key(entry): - nstr = "{\'%s\':%s}" % (entry,str(self.recd[entry])) - return nstr - else: - return "" - - # build weather, cosmic ray and vibration event strings suitable to be sent over the network to server - # these strings are self describing json format for easy decoding at the server end - - def get_weather(self): - if self.newhtu: - self.newhtu = 0 - try: - hum = float(self.recd["HTU"]["Hum"]) - tmb = float(self.recd["BMP"]["Tmb"]) - prs = float(self.recd["BMP"]["Prs"]) - - except Exception, e: - hum = 0.0 - tmb = 0.0 - prs = 0.0 - pass - - tol = abs(hum - self.ohum) + abs(tmb - self.otmb) + abs(prs - self.oprs) - if tol > 1.0: - self.ohum = hum - self.otmb = tmb - self.oprs = prs - - self.weather = self.extract("HTU") + \ - "*" + self.extract("BMP") + \ - "*" + self.extract("LOC") + \ - "*" + self.extract("TIM") + \ - "*" + self.extract("DAT") + \ - "*" + self.extract("SQN") - - return self.weather - - return "" - - def get_event(self): - if self.newevt: - self.newevt = 0 - self.evt = self.extract("EVT") + \ - "*" + self.extract("BMP") + \ - "*" + self.extract("ACL") + \ - "*" + self.extract("MAG") + \ - "*" + self.extract("HTU") + \ - "*" + self.extract("STS") + \ - "*" + self.extract("LOC") + \ - "*" + self.extract("TIM") + \ - "*" + self.extract("DAT") + \ - "*" + self.extract("SQN") - return self.evt - - return "" - - def get_vibration(self): - if self.newvib: - self.newvib = 0 - self.vib = self.extract("VIB") + \ - "*" + self.extract("ACL") + \ - "*" + self.extract("MAG") + \ - "*" + self.extract("LOC") + \ - "*" + self.extract("TIM") + \ - "*" + self.extract("DAT") + \ - "*" + self.extract("SQN") - return self.vib - - return "" - - def get_notification(self): - if len(self.recd["PAT"]["Pat"]) > 1: - return self.extract("PAT") - return "" - - def get_status(self): - return self.extract("STS") - - # Here we just return dictionaries - - def get_vib(self): - return self.recd["VIB"] - - def get_tim(self): - return self.recd["TIM"] - - def get_loc(self): - return self.recd["LOC"] - - def get_sts(self): - return self.recd["STS"] - - def get_bmp(self): - return self.recd["BMP"] - - def get_acl(self): - return self.recd["ACL"] - - def get_mag(self): - return self.recd["MAG"] - - def get_htu(self): - return self.recd["HTU"] - - def get_evt(self): - return self.recd["EVT"] - - def get_dat(self): - self.recd["DAT"]["Dat"] = time.asctime(time.gmtime(time.time())) - return self.recd["DAT"] - - def get_sqn(self): - return self.recd["SQN"] - - def nxt_sqn(self): - self.recd["SQN"]["Sqn"] = self.sqn - self.sqn = self.sqn + 1 - - def get_pat(self): - return self.recd["PAT"] - - def set_pat(self,token,flag): - self.recd["PAT"]["Pat"] = token - self.recd["PAT"]["Ntf"] = flag - -# Send UDP packets to the remote server - -class Socket_io(object): - - def __init__(self,ipaddr,ipport): - try: - self.sok = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - - except Exception, e: - msg = "Exception: Can't open Socket: %s" % (e) - print "Sending OFF:%s" % msg - udpflg = False - - def send_event_pkt(self,pkt,ipaddr,ipport): - try: - sent = 0 - while sent < len(pkt): - sent = sent + self.sok.sendto(pkt[sent:], (ipaddr, ipport)) - - except Exception, e: - msg = "Exception: Can't sendto: %s" % (e) - print "Sending OFF:%s" % msg - udpflg = False - - - def close(self): - self.sok.close() - -def main(): - use = "Usage: %prog [--ip=cosmicpi.ddns.net --port=4901 --usb=/dev/ttyACM0 --debug --dirnam=/tmp]" - parser = OptionParser(usage=use, version="cosmic_pi version 1.0") - - parser.add_option("-i", "--ip", help="Server IP address or name", dest="ipaddr", default="localhost") - parser.add_option("-p", "--port", help="Server portnumber", dest="ipport", type="int", default="4901") - parser.add_option("-u", "--usb", help="USB device name", dest="usbdev", default="/dev/ttyACM0") - parser.add_option("-d", "--debug", help="Debug Option", dest="debug", default=False, action="store_true") - parser.add_option("-o", "--odir", help="Path to log directory", dest="logdir", default="/tmp") - parser.add_option("-n", "--noip", help="IP Sending", dest="udpflg", default=True, action="store_false") - parser.add_option("-l", "--log", help="Event Logging", dest="logflg", default=False, action="store_true") - parser.add_option("-v", "--vib", help="Vibration monitor", dest="vibflg", default=False, action="store_true") - parser.add_option("-w", "--ws", help="Weather station", dest="wstflg", default=False, action="store_true") - parser.add_option("-c", "--cray", help="Cosmic ray sending", dest="evtflg", default=True, action="store_false") - parser.add_option("-k", "--patk", help="Server push notification token", dest="patok", default="") - - options, args = parser.parse_args() - - ipaddr = options.ipaddr - ipport = options.ipport - usbdev = options.usbdev - logdir = options.logdir - debug = options.debug - udpflg = options.udpflg - logflg = options.logflg - vibflg = options.vibflg - wstflg = options.wstflg - evtflg = options.evtflg - patok = options.patok - - pushflg = False - - print "\n" - print "options (Server IP address) ip : %s" % ipaddr - print "options (Server Port number) port : %d" % ipport - print "options (USB device name) usb : %s" % usbdev - print "options (Logging directory) odir : %s" % logdir - print "options (Event logging) log : %s" % logflg - print "options (UDP sending) udp : %s" % udpflg - print "options (Vibration monitor) vib : %s" % vibflg - print "options (Weather Station) wst : %s" % wstflg - print "options (Cosmic Ray Station) cray : %s" % evtflg - print "options (Push notifications) patk : %s" % patok - print "options (Debug Flag) debug: %s" % debug - - print "\ncosmic_pi monitor running, hit '>' for commands\n" - - ts = time.strftime("%d-%b-%Y-%H-%M-%S",time.gmtime(time.time())) - lgf = "%s/cosmicpi-logs/%s.log" % (logdir,ts) - dir = os.path.dirname(lgf) - if not os.path.exists(dir): - os.makedirs(dir) - try: - log = open(lgf, "w"); - except Exception, e: - msg = "Exception: Cant open log file: %s" % (e) - print "Fatal: %s" % msg - sys.exit(1) - - if options.debug: - print "\n" - print "Log file is: %s" % lgf - - try: - ser = serial.Serial(port=usbdev, baudrate=9600, timeout=60) - ser.flush() - except Exception, e: - msg = "Exception: Cant open USB device: %s" % (e) - print "Fatal: %s" % msg - sys.exit(1) - - kbrd = KeyBoard() - kbrd.echo_off() - - evt = Event() - events = 0 - vbrts = 0 - weathers = 0 - - sio = Socket_io(ipaddr,ipport) - - try: - while(True): - if kbrd.test_input(): - kbrd.echo_on() - print "\n" - cmd = raw_input(">") - - if cmd.find("q") != -1: - break - - elif cmd.find("d") != -1: - if debug: - debug = False - else: - debug = True - print "Debug:%s\n" % debug - - elif cmd.find("v") != -1: - if vibflg: - vibflg = False - else: - vibflg = True - print "Vibration:%s\n" % vibflg - - elif cmd.find("w") != -1: - if wstflg: - wstflg = False - else: - wstflg = True - print "WeatherStation:%s\n" % wstflg - - elif cmd.find("r") != -1: - if len(patok) > 0: - if pushflg: - pushflg = False - print "Unregister server notifications" - else: - pushflg = True - print "Register for server notifications" - - if udpflg: - evt.set_pat(patok,pushflg) - pbuf = evt.get_notification() - sio.send_event_pkt(pbuf,ipaddr,ipport) - sbuf = evt.get_status() - sio.send_event_pkt(sbuf,ipaddr,ipport) - print "Sent notification request:%s" % pbuf - else: - print "UDP sending is OFF, can not register with server" - pbuf = "" - else: - print "Token option is not set" - - elif cmd.find("s") != -1: - tim = evt.get_tim() - sts = evt.get_sts() - loc = evt.get_loc() - acl = evt.get_acl() - mag = evt.get_mag() - bmp = evt.get_bmp() - htu = evt.get_htu() - vib = evt.get_vib() - - print "ARDUINO STATUS" - print "Status........: Upt:%s Frq:%s Qsz:%s Mis:%s" % (tim["Upt"],tim["Frq"],sts["Qsz"],sts["Mis"]) - print "HardwareStatus: Htu:%s Bmp:%s Acl:%s Mag:%s Gps:%s" % (sts["Htu"],sts["Bmp"],sts["Acl"],sts["Mag"],sts["Gps"]) - print "Location......: Lat:%s Lon:%s Alt:%s" % (loc["Lat"],loc["Lon"],loc["Alt"]) - print "Accelarometer.: Acx:%s Acy:%s Acz:%s" % (acl["Acx"],acl["Acy"],acl["Acz"]) - print "Magnatometer..: Mgx:%s Mgy:%s Mgz:%s" % (mag["Mgx"],mag["Mgy"],mag["Mgz"]) - print "Barometer.....: Tmb:%s Prs:%s Alb:%s" % (bmp["Tmb"],bmp["Prs"],bmp["Alb"]) - print "Humidity......: Tmh:%s Hum:%s" % (htu["Tmh"],htu["Hum"]) - print "Vibration.....: Vax:%s Vcn:%s\n" % (vib["Vax"],vib["Vcn"]) - - print "MONITOR STATUS" - print "USB device....: %s" % (usbdev) - print "Remote........: Ip:%s Port:%s UdpFlag:%s" % (ipaddr,ipport,udpflg) - print "Notifications.: Flag:%s Token:%s" % (pushflg, patok) - print "Vibration.....: Sent:%d Flag:%s" % (vbrts,vibflg) - print "WeatherStation: Flag:%s" % (wstflg) - print "Events........: Sent:%d LogFlag:%s" % (events,logflg) - print "LogFile.......: %s\n" % (lgf) - - elif cmd.find("h") != -1: - print "MONITOR COMMANDS" - print " q=quit, s=status, d=toggle_debug, n=toggle_send, l=toggle_log" - print " v=vibration, w=weather, r=toggle_notifications h=help\n" - print "ARDUINO COMMANDS" - print " NOOP, Do nothing" - print " HELP, Display commands" - print " HTUX, Reset the HTH chip" - print " HTUD, HTU Temperature-Humidity display rate, " - print " BMPD, BMP Temperature-Altitude display rate, " - print " LOCD, Location latitude-longitude display rate, " - print " TIMD, Timing uptime-frequency-etm display rate, " - print " STSD, Status info display rate, " - print " EVQT, Event queue dump threshold, " - print " ACLD, Accelerometer display rate, " - print " MAGD, Magomagnatometer display rate, " - print " ACLT, Accelerometer event trigger threshold, " - print "" - - if debug: - ser.write("HELP") - - elif cmd.find("n") != -1: - if udpflg: - udpflg = False - else: - udpflg = True - print "Send:%s\n" % udpflg - - elif cmd.find("l") != -1: - if logflg: - logflg = False - else: - logflg = True - print "Log:%s\n" % logflg - - else: - print "Arduino < %s\n" % cmd - ser.write(cmd.upper()) - - kbrd.echo_off() - - # Process Arduino data json strings - - rc = ser.readline() - - if len(rc) == 0: - print "Serial input buffer empty" - ser.close() - time.sleep(1) - ser = serial.Serial(port=usbdev, baudrate=9600, timeout=60) - rc = ser.readline() - if len(rc) == 0: - break - print "Serial Reopened OK" - continue - else: - evt.parse(rc) - - if vibflg: - vbuf = evt.get_vibration() - if len(vbuf) > 0: - vbrts = vbrts + 1 - evt.nxt_sqn() - dat = evt.get_dat() - vib = evt.get_vib() - tim = evt.get_tim() - acl = evt.get_acl() - mag = evt.get_mag() - sqn = evt.get_sqn() - print "" - print "Vibration.....: Cnt:%d Vax:%s Vcn:%s " % (vbrts,vib["Vax"],vib["Vcn"]) - print "Accelarometer.: Acx:%s Acy:%s Acz:%s" % (acl["Acx"],acl["Acy"],acl["Acz"]) - print "Magnatometer..: Mgx:%s Mgy:%s Mgz:%s" % (mag["Mgx"],mag["Mgy"],mag["Mgz"]) - print "Time..........: Upt:%s Sec:%s Sqn:%d\n" % (tim["Upt"],tim["Sec"],sqn["Sqn"]) - - if udpflg: - sio.send_event_pkt(vbuf,ipaddr,ipport) - if logflg: - log.write(vbuf) - - continue - if wstflg: - wbuf = evt.get_weather() - if len(wbuf) > 0: - weathers = weathers + 1 - evt.nxt_sqn() - dat = evt.get_dat() - tim = evt.get_tim() - bmp = evt.get_bmp() - htu = evt.get_htu() - loc = evt.get_loc() - sqn = evt.get_sqn() - print "" - print "Barometer.....: Tmb:%s Prs:%s Alb:%s" % (bmp["Tmb"],bmp["Prs"],bmp["Alb"]) - print "Humidity......: Tmh:%s Hum:%s Alt:%s" % (htu["Tmh"],htu["Hum"],loc["Alt"]) - print "Time..........: Upt:%s Sec:%s Sqn:%d\n" % (tim["Upt"],tim["Sec"],sqn["Sqn"]) - - if udpflg: - sio.send_event_pkt(wbuf,ipaddr,ipport) - if logflg: - log.write(wbuf) - - continue - if evtflg: - ebuf = evt.get_event() - if len(ebuf) > 1: - events = events + 1 - evt.nxt_sqn() - dat = evt.get_dat() - evd = evt.get_evt() - tim = evt.get_tim() - sqn = evt.get_sqn() - print "" - print "Cosmic Event..: Evt:%s Frq:%s Tks:%s Etm:%s" % (evd["Evt"],evd["Frq"],evd["Tks"],evd["Etm"]) - print "Adc[[Ch0][Ch1]: Adc:%s" % (str(evd["Adc"])) - print "Time..........: Upt:%s Sec:%s Sqn:%d\n" % (tim["Upt"],tim["Sec"],sqn["Sqn"]) - - if udpflg: - sio.send_event_pkt(ebuf,ipaddr,ipport) - if logflg: - log.write(ebuf) - - continue - if debug: - sys.stdout.write(rc) - else: - ts = time.strftime("%d/%b/%Y %H:%M:%S",time.gmtime(time.time())) - tim = evt.get_tim(); - sts = evt.get_sts(); - s = "cosmic_pi:Upt:%s :Qsz:%s Tim:[%s] %s \r" % (tim["Upt"],sts["Qsz"],ts,tim["Sec"]) - sys.stdout.write(s) - sys.stdout.flush() - - except Exception, e: - msg = "Exception: main: %s" % (e) - print "Fatal: %s" % msg - traceback.print_exc() - - - finally: - kbrd.echo_on() - tim = evt.get_tim() - print "\nUp time:%s Quitting ..." % tim["Upt"] - ser.close() - log.close() - sio.close() - time.sleep(1) - sys.exit(0) - -if __name__ == '__main__': - - main()