Yearly Archives: 2018

Integrating Time, Memory, and Heading Capability, Part V

Posted 10 August 2018

Well, it appears I spoke too soon about having solved the I2C hangup problem on my Wall-E2 wall-following robot. In my last post on this subject, I described all the troubleshooting efforts I employed to nail down the cause of intermittent hangups when trying to use the MPU6050 6DOF IMU on the robot, along with several other I2C devices (a Teensy 3.5 used for IR homing, and Adafruit RTC, and FRAM modules).

After (I thought) figuring out that the I2C SCL/SDA line lengths were the root problem of the hangups I had been experiencing, my grandson Danny and I spent some quality time reworking Wall-E2’s layout to accommodate shorter line lengths. Instead of mounting the IMU and it’s companion sensors on the second deck as before, we 3D printed a small plastic plate to attach to one of the hexagonal 2nd deck standoff posts and provide a 1st deck mounting area for the sensors. The previous and new mounting locations are shown below:

2nd deck mounting location. The MPU6050 is the module with the illuminated blue LED toward the rear of the robot

1st deck mounting location for I2C sensors (lower right-hand corner of the photo). The Teensy 3.5 IR homing module is shown mounted on the IR detector housing (above the red plastic plate)

Unfortunately, as I was doing some final tests on this setup, I started experiencing hangups again. After a day or so moping and some very choice words, I started all over again trying to figure out what happened.

On previous searches through the i-verse, I had run across several posts indicating that the Arduino Wire library had some basic problems with I2C bus edge conditions; there were several places where it uses several blocking ‘while()’ loops to transmit and receive data on the I2C bus, and there was no way to recover from a ‘while()’ loop where the exit condition was never satisfied. After literally exhausting all the other possibilities, it was becoming apparent that this must be what was happening – the MPU6050 must occasionally fail to respond correctly to a I2C transaction, causing the associated ‘while()’ loop to never exit.

So, I started looking for solutions to this problem. Again, I found some posts where folks had modified the low-level I2C bus handling code found in twi.c/.h, the code underlying the Android Wire class. I found a post by ‘unaie’ (http://forum.arduino.cc/index.php/topic,19624.0.html) with the same complaint, but he also posted modified versions of twi.c and twi.h that solved these problems by forcing the ‘while()’ loops to exit after a set number of iterations, and resetting the I2C bus when this happens. His modified versions can be downloaded at:

http://liken.otsoa.net/pub/ntwi/twi.h

http://liken.otsoa.net/pub/ntwi/twi.c

I downloaded these files and tried to replace the ‘stock’ twi.c/h with the modified versions. Unfortunately, unaie’s modifications were made on a quite old version of the files, and conflicted with the later ‘repeated start’ versions of these files that are in the current ‘wire’ library.

So, I did a ‘diff’ between the ‘repeated start’ version and unaie’s version, and created a modified version of the latest ‘repeated start’ twi.c/h. In addition, I added a couple of functions to allow monitoring of the number of times a bus reset was required due to a ‘while()’ loop timeout. When I was finished, I ran the sensor for over 24 hours with no failures, but in that time there were three instances where a ‘while()’ loop timed out and a I2C bus reset was required. A small snippet of this run is shown below. The blue line is the yaw value, and the plot snippet shows where I manually rotated the sensor just after 24 hours, and the horizontal orange line shows the number of bus resets.

Small snippet of 24-hour sensor run. blue line is reported yaw value; orange shows the I2C bus reset counter

So it is clear that, absent the lockup recovery modifications, the I2C bus would have locked up long before, and that with the modifications ‘while()’ loop deadlocks have been successfully handled.

11 August 2018 Update:

The sensor is still going strong after 44 hours with no hangups, and the reset counter is still holding at 3.

The complete twi.c & twi.h codes are included below:

/*
  twi.c - TWI/I2C library for Wiring & Arduino
  Copyright (c) 2006 Nicholas Zambetti.  All right reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA


  Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
*/

//08/09/18 this file has been merged with unaie's version of twi.c, modified to recover from infinite hang conditions
//see http://forum.arduino.cc/index.php/topic,19624.0.html for details
//this file should be renamed 'twi.c' and placed so that it gets used instead of the normal one.


#include <math.h>
#include <stdlib.h>
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <compat/twi.h>
#include "Arduino.h" // for digitalWrite

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif

#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

#include "pins_arduino.h"
#include "twi.h"

static volatile uint8_t trcase;//Nir

static volatile uint8_t twi_state;
static volatile uint8_t twi_slarw;
static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start

static void (*twi_onSlaveTransmit)(void);
static void (*twi_onSlaveReceive)(uint8_t*, int);

static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH];
static volatile uint8_t twi_masterBufferIndex;
static volatile uint8_t twi_masterBufferLength;

static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH];
static volatile uint8_t twi_txBufferIndex;
static volatile uint8_t twi_txBufferLength;

static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH];
static volatile uint8_t twi_rxBufferIndex;

static volatile uint8_t twi_error;

//08/09/18 gfp - added to monitor lockup recoveries
static uint16_t lockup_recovery_count = 0;

/* 
 * Function twi_init
 * Desc     readys twi pins and sets twi bitrate
 * Input    none
 * Output   none
 */
void twi_init(void)
{
	// disable twi module (to allow for reinitialize)
  TWCR = 0; //08/09/18 - required to get TWI to re-init
  
  // initialize state
  twi_state = TWI_READY;
  twi_sendStop = true;		// default value
  twi_inRepStart = false;
  
  // activate internal pullups for twi.
  digitalWrite(SDA, 1);
  digitalWrite(SCL, 1);

  // initialize twi prescaler and bit rate
  cbi(TWSR, TWPS0);
  cbi(TWSR, TWPS1);
  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;

  /* twi bit rate formula from atmega128 manual pg 204
  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
  note: TWBR should be 10 or higher for master mode
  It is 72 for a 16mhz Wiring board with 100kHz TWI */

  // enable twi module, acks, and twi interrupt
  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
}

/* 
 * Function twi_disable
 * Desc     disables twi pins
 * Input    none
 * Output   none
 */
void twi_disable(void)
{
  // disable twi module, acks, and twi interrupt
  TWCR &= ~(_BV(TWEN) | _BV(TWIE) | _BV(TWEA));

  // deactivate internal pullups for twi.
  digitalWrite(SDA, 0);
  digitalWrite(SCL, 0);
}

/* 
 * Function twi_slaveInit
 * Desc     sets slave address and enables interrupt
 * Input    none
 * Output   none
 */
void twi_setAddress(uint8_t address)
{
  // set twi slave address (skip over TWGCE bit)
  TWAR = address << 1;
}

/* 
 * Function twi_setClock
 * Desc     sets twi bit rate
 * Input    Clock Frequency
 * Output   none
 */
void twi_setFrequency(uint32_t frequency)
{
  TWBR = ((F_CPU / frequency) - 16) / 2;
  
  /* twi bit rate formula from atmega128 manual pg 204
  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
  note: TWBR should be 10 or higher for master mode
  It is 72 for a 16mhz Wiring board with 100kHz TWI */
}

/* 
 * Function twi_readFrom
 * Desc     attempts to become twi bus master and read a
 *          series of bytes from a device on the bus
 * Input    address: 7bit i2c device address
 *          data: pointer to byte array
 *          length: number of bytes to read into array
 *          sendStop: Boolean indicating whether to send a stop at the end
 * Output   number of bytes read
 */
uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)
{
  uint8_t i;

  // ensure data will fit into buffer
  if(TWI_BUFFER_LENGTH < length){
    return 0;
  }

  // wait until twi is ready, become master receiver
  twi_tout(1);//Ini TimeOut
  while(TWI_READY != twi_state){
  	if (twi_tout(0)) break;
    continue;
  }
  twi_state = TWI_MRX;
  twi_sendStop = sendStop;
  // reset error state (0xFF.. no error occured)
  twi_error = 0xFF;

  // initialize buffer iteration vars
  twi_masterBufferIndex = 0;
  twi_masterBufferLength = length-1;  // This is not intuitive, read on...
  // On receive, the previously configured ACK/NACK setting is transmitted in
  // response to the received byte before the interrupt is signalled. 
  // Therefor we must actually set NACK when the _next_ to last byte is
  // received, causing that NACK to be sent in response to receiving the last
  // expected byte of data.

  // build sla+w, slave device address + w bit
  twi_slarw = TW_READ;
  twi_slarw |= address << 1;

  if (true == twi_inRepStart) {
    // if we're in the repeated start state, then we've already sent the start,
    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
    // We need to remove ourselves from the repeated start state before we enable interrupts,
    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
    // up. Also, don't enable the START interrupt. There may be one pending from the 
    // repeated start that we sent ourselves, and that would really confuse things.
    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
    do {
      TWDR = twi_slarw;
    } while(TWCR & _BV(TWWC));
    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
  }
  else
    // send start condition
    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);

  // wait for read operation to complete
  twi_tout(1);
  while(TWI_MRX == twi_state){
  	if (twi_tout(0)) break;
    continue;
  }
  if (twi_masterBufferIndex < length)
    length = twi_masterBufferIndex;

  // copy twi buffer to data
  for(i = 0; i < length; ++i){
    data[i] = twi_masterBuffer[i];
  }
	
  return length;
}

/* 
 * Function twi_writeTo
 * Desc     attempts to become twi bus master and write a
 *          series of bytes to a device on the bus
 * Input    address: 7bit i2c device address
 *          data: pointer to byte array
 *          length: number of bytes in array
 *          wait: boolean indicating to wait for write or not
 *          sendStop: boolean indicating whether or not to send a stop at the end
 * Output   0 .. success
 *          1 .. length to long for buffer
 *          2 .. address send, NACK received
 *          3 .. data send, NACK received
 *          4 .. other twi error (lost bus arbitration, bus error, ..)
 */
uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop)
{
  uint8_t i;

  // ensure data will fit into buffer
  if(TWI_BUFFER_LENGTH < length){
    return 1;
  }

  // wait until twi is ready, become master transmitter
  twi_tout(1);
  while(TWI_READY != twi_state){
  	if (twi_tout(0)) return 5;
    continue;
  }
  twi_state = TWI_MTX;
  twi_sendStop = sendStop;
  // reset error state (0xFF.. no error occured)
  twi_error = 0xFF;

  // initialize buffer iteration vars
  twi_masterBufferIndex = 0;
  twi_masterBufferLength = length;
  
  // copy data to twi buffer
  for(i = 0; i < length; ++i){
    twi_masterBuffer[i] = data[i];
  }
  
  // build sla+w, slave device address + w bit
  twi_slarw = TW_WRITE;
  twi_slarw |= address << 1;
  
  // if we're in a repeated start, then we've already sent the START
  // in the ISR. Don't do it again.
  //
  if (true == twi_inRepStart) {
    // if we're in the repeated start state, then we've already sent the start,
    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
    // We need to remove ourselves from the repeated start state before we enable interrupts,
    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
    // up. Also, don't enable the START interrupt. There may be one pending from the 
    // repeated start that we sent outselves, and that would really confuse things.
    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
    do {
      TWDR = twi_slarw;				
    } while(TWCR & _BV(TWWC));
    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
  }
  else
    // send start condition
    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs

  // wait for write operation to complete
 twi_tout(1);
  while(wait && (TWI_MTX == twi_state)){
  	if (twi_tout(0)) return 5;
    continue;
  }
  
  if (twi_error == 0xFF)
    return 0;	// success
  else if (twi_error == TW_MT_SLA_NACK)
    return 2;	// error: address send, nack received
  else if (twi_error == TW_MT_DATA_NACK)
    return 3;	// error: data send, nack received
  else
    return 4;	// other twi error
}

/* 
 * Function twi_transmit
 * Desc     fills slave tx buffer with data
 *          must be called in slave tx event callback
 * Input    data: pointer to byte array
 *          length: number of bytes in array
 * Output   1 length too long for buffer
 *          2 not slave transmitter
 *          0 ok
 */
uint8_t twi_transmit(const uint8_t* data, uint8_t length)
{
  uint8_t i;

  // ensure data will fit into buffer
  if(TWI_BUFFER_LENGTH < (twi_txBufferLength+length)){
    return 1;
  }
  
  // ensure we are currently a slave transmitter
  if(TWI_STX != twi_state){
    return 2;
  }
  
  // set length and copy data into tx buffer
  for(i = 0; i < length; ++i){
    twi_txBuffer[twi_txBufferLength+i] = data[i];
  }
  twi_txBufferLength += length;
  
  return 0;
}

/* 
 * Function twi_attachSlaveRxEvent
 * Desc     sets function called before a slave read operation
 * Input    function: callback function to use
 * Output   none
 */
void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) )
{
  twi_onSlaveReceive = function;
}

/* 
 * Function twi_attachSlaveTxEvent
 * Desc     sets function called before a slave write operation
 * Input    function: callback function to use
 * Output   none
 */
void twi_attachSlaveTxEvent( void (*function)(void) )
{
  twi_onSlaveTransmit = function;
}

/* 
 * Function twi_reply
 * Desc     sends byte or readys receive line
 * Input    ack: byte indicating to ack or to nack
 * Output   none
 */
void twi_reply(uint8_t ack)
{
  // transmit master read ready signal, with or without ack
  if(ack){
    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
  }else{
	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
  }
}

/* 
 * Function twi_stop
 * Desc     relinquishes bus master status
 * Input    none
 * Output   none
 */
void twi_stop(void)
{
  // send stop condition
  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);

  // wait for stop condition to be exectued on bus
  // TWINT is not set after a stop condition!
  twi_tout(1);
  while(TWCR & _BV(TWSTO)){
  	if (twi_tout(0)) return;
    continue;
  }
  // update twi state
  twi_state = TWI_READY;
}

/* 
 * Function twi_releaseBus
 * Desc     releases bus control
 * Input    none
 * Output   none
 */
void twi_releaseBus(void)
{
  // release bus
  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);

  // update twi state
  twi_state = TWI_READY;
}


//Nirea. Time Out
static volatile uint32_t twi_toutc;
uint8_t twi_tout(uint8_t ini)
{
	if (ini) twi_toutc=0; else twi_toutc++;	
	if (twi_toutc>=100000UL) {
		twi_toutc=0;
		twi_init();
		lockup_recovery_count++;
		return 1;
	}
  return 0;  
}

void twi_clearRecoveryCount() //08/09/18 gfp for monitoring purposes
{
	lockup_recovery_count = 0;
}

uint16_t twi_getRecoveryCount() //08/09/18 gfp for monitoring purposes
{
	return lockup_recovery_count;
}

ISR(TWI_vect)
{
  switch(TW_STATUS){
    // All Master
    case TW_START:     // sent start condition
    case TW_REP_START: // sent repeated start condition
      // copy device address and r/w bit to output register and ack
      TWDR = twi_slarw;
      twi_reply(1);
      break;

    // Master Transmitter
    case TW_MT_SLA_ACK:  // slave receiver acked address
    case TW_MT_DATA_ACK: // slave receiver acked data
      // if there is data to send, send it, otherwise stop 
      if(twi_masterBufferIndex < twi_masterBufferLength){
        // copy data to output register and ack
        TWDR = twi_masterBuffer[twi_masterBufferIndex++];
        twi_reply(1);
      }else{
	if (twi_sendStop)
          twi_stop();
	else {
	  twi_inRepStart = true;	// we're gonna send the START
	  // don't enable the interrupt. We'll generate the start, but we 
	  // avoid handling the interrupt until we're in the next transaction,
	  // at the point where we would normally issue the start.
	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
	  twi_state = TWI_READY;
	}
      }
      break;
    case TW_MT_SLA_NACK:  // address sent, nack received
      twi_error = TW_MT_SLA_NACK;
      twi_stop();
      break;
    case TW_MT_DATA_NACK: // data sent, nack received
      twi_error = TW_MT_DATA_NACK;
      twi_stop();
      break;
    case TW_MT_ARB_LOST: // lost bus arbitration
      twi_error = TW_MT_ARB_LOST;
      twi_releaseBus();
      break;

    // Master Receiver
    case TW_MR_DATA_ACK: // data received, ack sent
      // put byte into buffer
      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
    case TW_MR_SLA_ACK:  // address sent, ack received
      // ack if more bytes are expected, otherwise nack
      if(twi_masterBufferIndex < twi_masterBufferLength){
        twi_reply(1);
      }else{
        twi_reply(0);
      }
      break;
    case TW_MR_DATA_NACK: // data received, nack sent
      // put final byte into buffer
      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
	if (twi_sendStop)
          twi_stop();
	else {
	  twi_inRepStart = true;	// we're gonna send the START
	  // don't enable the interrupt. We'll generate the start, but we 
	  // avoid handling the interrupt until we're in the next transaction,
	  // at the point where we would normally issue the start.
	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
	  twi_state = TWI_READY;
	}    
	break;
    case TW_MR_SLA_NACK: // address sent, nack received
      twi_stop();
      break;
    // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case

    // Slave Receiver
    case TW_SR_SLA_ACK:   // addressed, returned ack
    case TW_SR_GCALL_ACK: // addressed generally, returned ack
    case TW_SR_ARB_LOST_SLA_ACK:   // lost arbitration, returned ack
    case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack
      // enter slave receiver mode
      twi_state = TWI_SRX;
      // indicate that rx buffer can be overwritten and ack
      twi_rxBufferIndex = 0;
      twi_reply(1);
      break;
    case TW_SR_DATA_ACK:       // data received, returned ack
    case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack
      // if there is still room in the rx buffer
      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
        // put byte in buffer and ack
        twi_rxBuffer[twi_rxBufferIndex++] = TWDR;
        twi_reply(1);
      }else{
        // otherwise nack
        twi_reply(0);
      }
      break;
    case TW_SR_STOP: // stop or repeated start condition received
      // ack future responses and leave slave receiver state
      twi_releaseBus();
      // put a null char after data if there's room
      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
        twi_rxBuffer[twi_rxBufferIndex] = '\0';
      }
      // callback to user defined callback
      twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);
      // since we submit rx buffer to "wire" library, we can reset it
      twi_rxBufferIndex = 0;
      break;
    case TW_SR_DATA_NACK:       // data received, returned nack
    case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack
      // nack back at master
      twi_reply(0);
      break;
    
    // Slave Transmitter
    case TW_ST_SLA_ACK:          // addressed, returned ack
    case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack
      // enter slave transmitter mode
      twi_state = TWI_STX;
      // ready the tx buffer index for iteration
      twi_txBufferIndex = 0;
      // set tx buffer length to be zero, to verify if user changes it
      twi_txBufferLength = 0;
      // request for txBuffer to be filled and length to be set
      // note: user must call twi_transmit(bytes, length) to do this
      twi_onSlaveTransmit();
      // if they didn't change buffer & length, initialize it
      if(0 == twi_txBufferLength){
        twi_txBufferLength = 1;
        twi_txBuffer[0] = 0x00;
      }
      // transmit first byte from buffer, fall
    case TW_ST_DATA_ACK: // byte sent, ack returned
      // copy data to output register
      TWDR = twi_txBuffer[twi_txBufferIndex++];
      // if there is more to send, ack, otherwise nack
      if(twi_txBufferIndex < twi_txBufferLength){
        twi_reply(1);
      }else{
        twi_reply(0);
      }
      break;
    case TW_ST_DATA_NACK: // received nack, we are done 
    case TW_ST_LAST_DATA: // received ack, but we are done already!
      // ack future responses
      twi_reply(1);
      // leave slave receiver state
      twi_state = TWI_READY;
      break;

    // All
    case TW_NO_INFO:   // no state information
      break;
    case TW_BUS_ERROR: // bus error, illegal stop/start
      twi_error = TW_BUS_ERROR;
      twi_stop();
      break;
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

twi.c - TWI/I2C library for Wiring & Arduino

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public

License along with this library; if not, write to the Free Software

Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts

//08/09/18 this file has been merged with unaie's version of twi.c, modified to recover from infinite hang conditions

//see http://forum.arduino.cc/index.php/topic,19624.0.html for details

//this file should be renamed 'twi.c' and placed so that it gets used instead of the normal one.

#include <math.h>

#include <stdlib.h>

#include <inttypes.h>

#include <avr/io.h>

#include <avr/interrupt.h>

#include <compat/twi.h>

#include "Arduino.h" // for digitalWrite

#ifndef cbi

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))

#endif

#ifndef sbi

#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

#endif

#include "pins_arduino.h"

#include "twi.h"

static volatile uint8_t trcase;//Nir

static volatile uint8_t twi_state;

static volatile uint8_t twi_slarw;

static volatile uint8_t twi_sendStop; // should the transaction end with a stop

static volatile uint8_t twi_inRepStart; // in the middle of a repeated start

static void (*twi_onSlaveTransmit)(void);

static void (*twi_onSlaveReceive)(uint8_t*, int);

static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH];

static volatile uint8_t twi_masterBufferIndex;

static volatile uint8_t twi_masterBufferLength;

static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH];

static volatile uint8_t twi_txBufferIndex;

static volatile uint8_t twi_txBufferLength;

static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH];

static volatile uint8_t twi_rxBufferIndex;

static volatile uint8_t twi_error;

//08/09/18 gfp - added to monitor lockup recoveries

static uint16_t lockup_recovery_count = 0;

* Function twi_init

* Desc readys twi pins and sets twi bitrate

* Input none

* Output none

void twi_init(void)

{

// disable twi module (to allow for reinitialize)

TWCR = 0; //08/09/18 - required to get TWI to re-init

// initialize state

twi_state = TWI_READY;

twi_sendStop = true; // default value

twi_inRepStart = false;

// activate internal pullups for twi.

digitalWrite(SDA, 1);

digitalWrite(SCL, 1);

// initialize twi prescaler and bit rate

cbi(TWSR, TWPS0);

cbi(TWSR, TWPS1);

TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;

/* twi bit rate formula from atmega128 manual pg 204

SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))

note: TWBR should be 10 or higher for master mode

It is 72 for a 16mhz Wiring board with 100kHz TWI */

// enable twi module, acks, and twi interrupt

TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);

}

* Function twi_disable

* Desc disables twi pins

* Input none

* Output none

void twi_disable(void)

{

// disable twi module, acks, and twi interrupt

TWCR &= ~(_BV(TWEN) | _BV(TWIE) | _BV(TWEA));

// deactivate internal pullups for twi.

digitalWrite(SDA, 0);

digitalWrite(SCL, 0);

}

* Function twi_slaveInit

* Desc sets slave address and enables interrupt

* Input none

* Output none

void twi_setAddress(uint8_t address)

{

// set twi slave address (skip over TWGCE bit)

TWAR = address << 1;

}

* Function twi_setClock

* Desc sets twi bit rate

* Input Clock Frequency

* Output none

void twi_setFrequency(uint32_t frequency)

{

TWBR = ((F_CPU / frequency) - 16) / 2;

/* twi bit rate formula from atmega128 manual pg 204

SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))

note: TWBR should be 10 or higher for master mode

It is 72 for a 16mhz Wiring board with 100kHz TWI */

}

* Function twi_readFrom

* Desc attempts to become twi bus master and read a

* series of bytes from a device on the bus

* Input address: 7bit i2c device address

* data: pointer to byte array

* length: number of bytes to read into array

* sendStop: Boolean indicating whether to send a stop at the end

* Output number of bytes read

uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)

{

uint8_t i;

// ensure data will fit into buffer

if(TWI_BUFFER_LENGTH < length){

return 0;

}

// wait until twi is ready, become master receiver

twi_tout(1);//Ini TimeOut

while(TWI_READY != twi_state){

if (twi_tout(0)) break;

continue;

}

twi_state = TWI_MRX;

twi_sendStop = sendStop;

// reset error state (0xFF.. no error occured)

twi_error = 0xFF;

// initialize buffer iteration vars

twi_masterBufferIndex = 0;

twi_masterBufferLength = length-1; // This is not intuitive, read on...

// On receive, the previously configured ACK/NACK setting is transmitted in

// response to the received byte before the interrupt is signalled.

// Therefor we must actually set NACK when the _next_ to last byte is

// received, causing that NACK to be sent in response to receiving the last

// expected byte of data.

// build sla+w, slave device address + w bit

twi_slarw = TW_READ;

twi_slarw |= address << 1;

if (true == twi_inRepStart) {

// if we're in the repeated start state, then we've already sent the start,

// (@@@ we hope), and the TWI statemachine is just waiting for the address byte.

// We need to remove ourselves from the repeated start state before we enable interrupts,

// since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning

// up. Also, don't enable the START interrupt. There may be one pending from the

// repeated start that we sent ourselves, and that would really confuse things.

twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR

do {

TWDR = twi_slarw;

} while(TWCR & _BV(TWWC));

TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START

}

else

// send start condition

TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);

// wait for read operation to complete

twi_tout(1);

while(TWI_MRX == twi_state){

if (twi_tout(0)) break;

continue;

}

if (twi_masterBufferIndex < length)

length = twi_masterBufferIndex;

// copy twi buffer to data

for(i = 0; i < length; ++i){

data[i] = twi_masterBuffer[i];

}

return length;

}

* Function twi_writeTo

* Desc attempts to become twi bus master and write a

* series of bytes to a device on the bus

* Input address: 7bit i2c device address

* data: pointer to byte array

* length: number of bytes in array

* wait: boolean indicating to wait for write or not

* sendStop: boolean indicating whether or not to send a stop at the end

* Output 0 .. success

* 1 .. length to long for buffer

* 2 .. address send, NACK received

* 3 .. data send, NACK received

* 4 .. other twi error (lost bus arbitration, bus error, ..)

uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop)

{

uint8_t i;

// ensure data will fit into buffer

if(TWI_BUFFER_LENGTH < length){

return 1;

}

// wait until twi is ready, become master transmitter

twi_tout(1);

while(TWI_READY != twi_state){

if (twi_tout(0)) return 5;

continue;

}

twi_state = TWI_MTX;

twi_sendStop = sendStop;

// reset error state (0xFF.. no error occured)

twi_error = 0xFF;

// initialize buffer iteration vars

twi_masterBufferIndex = 0;

twi_masterBufferLength = length;

// copy data to twi buffer

for(i = 0; i < length; ++i){

twi_masterBuffer[i] = data[i];

}

// build sla+w, slave device address + w bit

twi_slarw = TW_WRITE;

twi_slarw |= address << 1;

// if we're in a repeated start, then we've already sent the START

// in the ISR. Don't do it again.

if (true == twi_inRepStart) {

// if we're in the repeated start state, then we've already sent the start,

// (@@@ we hope), and the TWI statemachine is just waiting for the address byte.

// We need to remove ourselves from the repeated start state before we enable interrupts,

// since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning

// up. Also, don't enable the START interrupt. There may be one pending from the

// repeated start that we sent outselves, and that would really confuse things.

twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR

do {

TWDR = twi_slarw;

} while(TWCR & _BV(TWWC));

TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START

}

else

// send start condition

TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA); // enable INTs

// wait for write operation to complete

twi_tout(1);

while(wait && (TWI_MTX == twi_state)){

if (twi_tout(0)) return 5;

continue;

}

if (twi_error == 0xFF)

return 0; // success

else if (twi_error == TW_MT_SLA_NACK)

return 2; // error: address send, nack received

else if (twi_error == TW_MT_DATA_NACK)

return 3; // error: data send, nack received

else

return 4; // other twi error

}

* Function twi_transmit

* Desc fills slave tx buffer with data

* must be called in slave tx event callback

* Input data: pointer to byte array

* length: number of bytes in array

* Output 1 length too long for buffer

* 2 not slave transmitter

* 0 ok

uint8_t twi_transmit(const uint8_t* data, uint8_t length)

{

uint8_t i;

// ensure data will fit into buffer

if(TWI_BUFFER_LENGTH < (twi_txBufferLength+length)){

return 1;

}

// ensure we are currently a slave transmitter

if(TWI_STX != twi_state){

return 2;

}

// set length and copy data into tx buffer

for(i = 0; i < length; ++i){

twi_txBuffer[twi_txBufferLength+i] = data[i];

}

twi_txBufferLength += length;

return 0;

}

* Function twi_attachSlaveRxEvent

* Desc sets function called before a slave read operation

* Input function: callback function to use

* Output none

void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) )

{

twi_onSlaveReceive = function;

}

* Function twi_attachSlaveTxEvent

* Desc sets function called before a slave write operation

* Input function: callback function to use

* Output none

void twi_attachSlaveTxEvent( void (*function)(void) )

{

twi_onSlaveTransmit = function;

}

* Function twi_reply

* Desc sends byte or readys receive line

* Input ack: byte indicating to ack or to nack

* Output none

void twi_reply(uint8_t ack)

{

// transmit master read ready signal, with or without ack

if(ack){

TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);

}else{

TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);

}

* Function twi_stop

* Desc relinquishes bus master status

* Input none

* Output none

void twi_stop(void)

{

// send stop condition

TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);

// wait for stop condition to be exectued on bus

// TWINT is not set after a stop condition!

twi_tout(1);

while(TWCR & _BV(TWSTO)){

if (twi_tout(0)) return;

continue;

}

// update twi state

twi_state = TWI_READY;

}

* Function twi_releaseBus

* Desc releases bus control

* Input none

* Output none

void twi_releaseBus(void)

{

// release bus

TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);

// update twi state

twi_state = TWI_READY;

}

//Nirea. Time Out

static volatile uint32_t twi_toutc;

uint8_t twi_tout(uint8_t ini)

{

if (ini) twi_toutc=0; else twi_toutc++;

if (twi_toutc>=100000UL) {

twi_toutc=0;

twi_init();

lockup_recovery_count++;

return 1;

}

return 0;

}

void twi_clearRecoveryCount() //08/09/18 gfp for monitoring purposes

{

lockup_recovery_count = 0;

}

uint16_t twi_getRecoveryCount() //08/09/18 gfp for monitoring purposes

{

return lockup_recovery_count;

}

ISR(TWI_vect)

{

switch(TW_STATUS){

// All Master

case TW_START: // sent start condition

case TW_REP_START: // sent repeated start condition

// copy device address and r/w bit to output register and ack

TWDR = twi_slarw;

twi_reply(1);

break;

// Master Transmitter

case TW_MT_SLA_ACK: // slave receiver acked address

case TW_MT_DATA_ACK: // slave receiver acked data

// if there is data to send, send it, otherwise stop

if(twi_masterBufferIndex < twi_masterBufferLength){

// copy data to output register and ack

TWDR = twi_masterBuffer[twi_masterBufferIndex++];

twi_reply(1);

}else{

if (twi_sendStop)

twi_stop();

else {

twi_inRepStart = true; // we're gonna send the START

// don't enable the interrupt. We'll generate the start, but we

// avoid handling the interrupt until we're in the next transaction,

// at the point where we would normally issue the start.

TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;

twi_state = TWI_READY;

}

break;

case TW_MT_SLA_NACK: // address sent, nack received

twi_error = TW_MT_SLA_NACK;

twi_stop();

break;

case TW_MT_DATA_NACK: // data sent, nack received

twi_error = TW_MT_DATA_NACK;

twi_stop();

break;

case TW_MT_ARB_LOST: // lost bus arbitration

twi_error = TW_MT_ARB_LOST;

twi_releaseBus();

break;

// Master Receiver

case TW_MR_DATA_ACK: // data received, ack sent

// put byte into buffer

twi_masterBuffer[twi_masterBufferIndex++] = TWDR;

case TW_MR_SLA_ACK: // address sent, ack received

// ack if more bytes are expected, otherwise nack

if(twi_masterBufferIndex < twi_masterBufferLength){

twi_reply(1);

}else{

twi_reply(0);

}

break;

case TW_MR_DATA_NACK: // data received, nack sent

// put final byte into buffer

twi_masterBuffer[twi_masterBufferIndex++] = TWDR;

if (twi_sendStop)

twi_stop();

else {

twi_inRepStart = true; // we're gonna send the START

// don't enable the interrupt. We'll generate the start, but we

// avoid handling the interrupt until we're in the next transaction,

// at the point where we would normally issue the start.

TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;

twi_state = TWI_READY;

}

break;

case TW_MR_SLA_NACK: // address sent, nack received

twi_stop();

break;

// TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case

// Slave Receiver

case TW_SR_SLA_ACK: // addressed, returned ack

case TW_SR_GCALL_ACK: // addressed generally, returned ack

case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack

case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack

// enter slave receiver mode

twi_state = TWI_SRX;

// indicate that rx buffer can be overwritten and ack

twi_rxBufferIndex = 0;

twi_reply(1);

break;

case TW_SR_DATA_ACK: // data received, returned ack

case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack

// if there is still room in the rx buffer

if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){

// put byte in buffer and ack

twi_rxBuffer[twi_rxBufferIndex++] = TWDR;

twi_reply(1);

}else{

// otherwise nack

twi_reply(0);

}

break;

case TW_SR_STOP: // stop or repeated start condition received

// ack future responses and leave slave receiver state

twi_releaseBus();

// put a null char after data if there's room

if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){

twi_rxBuffer[twi_rxBufferIndex] = '\0';

}

// callback to user defined callback

twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);

// since we submit rx buffer to "wire" library, we can reset it

twi_rxBufferIndex = 0;

break;

case TW_SR_DATA_NACK: // data received, returned nack

case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack

// nack back at master

twi_reply(0);

break;

// Slave Transmitter

case TW_ST_SLA_ACK: // addressed, returned ack

case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack

// enter slave transmitter mode

twi_state = TWI_STX;

// ready the tx buffer index for iteration

twi_txBufferIndex = 0;

// set tx buffer length to be zero, to verify if user changes it

twi_txBufferLength = 0;

// request for txBuffer to be filled and length to be set

// note: user must call twi_transmit(bytes, length) to do this

twi_onSlaveTransmit();

// if they didn't change buffer & length, initialize it

if(0 == twi_txBufferLength){

twi_txBufferLength = 1;

twi_txBuffer[0] = 0x00;

}

// transmit first byte from buffer, fall

case TW_ST_DATA_ACK: // byte sent, ack returned

// copy data to output register

TWDR = twi_txBuffer[twi_txBufferIndex++];

// if there is more to send, ack, otherwise nack

if(twi_txBufferIndex < twi_txBufferLength){

twi_reply(1);

}else{

twi_reply(0);

}

break;

case TW_ST_DATA_NACK: // received nack, we are done

case TW_ST_LAST_DATA: // received ack, but we are done already!

// ack future responses

twi_reply(1);

// leave slave receiver state

twi_state = TWI_READY;

break;

// All

case TW_NO_INFO: // no state information

break;

case TW_BUS_ERROR: // bus error, illegal stop/start

twi_error = TW_BUS_ERROR;

twi_stop();

break;

}

/*
  twi.h - TWI/I2C library for Wiring & Arduino
  Copyright (c) 2006 Nicholas Zambetti.  All right reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/


//08/09/18 this has been merged with unaie's version of twi.h, modified to recover from infinite hang conditions
//see http://forum.arduino.cc/index.php/topic,19624.0.html for details
//this file should be renamed 'twi.h' and placed so that it gets used instead of the normal one.

#ifndef twi_h
#define twi_h

  #include <inttypes.h>

  //#define ATMEGA8

  #ifndef TWI_FREQ
  #define TWI_FREQ 100000L
  #endif

  #ifndef TWI_BUFFER_LENGTH
  #define TWI_BUFFER_LENGTH 32
  #endif

  #define TWI_READY 0
  #define TWI_MRX   1
  #define TWI_MTX   2
  #define TWI_SRX   3
  #define TWI_STX   4
  
  void twi_init(void);
  void twi_disable(void);
  void twi_setAddress(uint8_t);
  void twi_setFrequency(uint32_t);
  uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t);
  uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t);
  uint8_t twi_transmit(const uint8_t*, uint8_t);
  void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) );
  void twi_attachSlaveTxEvent( void (*)(void) );
  void twi_reply(uint8_t);
  void twi_stop(void);
  void twi_releaseBus(void);
  uint8_t twi_tout(uint8_t); //unaie's timeout function
  uint16_t twi_getRecoveryCount(); //08/09/18 gfp for monitoring purposes
  void twi_clearRecoveryCount(); //08/09/18 gfp for monitoring purposes
#endif

twi.h - TWI/I2C library for Wiring & Arduino

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public

License along with this library; if not, write to the Free Software

Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

//08/09/18 this has been merged with unaie's version of twi.h, modified to recover from infinite hang conditions

//see http://forum.arduino.cc/index.php/topic,19624.0.html for details

//this file should be renamed 'twi.h' and placed so that it gets used instead of the normal one.

#ifndef twi_h

#define twi_h

#include <inttypes.h>

//#define ATMEGA8

#ifndef TWI_FREQ

#define TWI_FREQ 100000L

#endif

#ifndef TWI_BUFFER_LENGTH

#define TWI_BUFFER_LENGTH 32

#endif

#define TWI_READY 0

#define TWI_MRX 1

#define TWI_MTX 2

#define TWI_SRX 3

#define TWI_STX 4

void twi_init(void);

void twi_disable(void);

void twi_setAddress(uint8_t);

void twi_setFrequency(uint32_t);

uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t);

uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t);

uint8_t twi_transmit(const uint8_t*, uint8_t);

void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) );

void twi_attachSlaveTxEvent( void (*)(void) );

void twi_reply(uint8_t);

void twi_stop(void);

void twi_releaseBus(void);

uint8_t twi_tout(uint8_t); //unaie's timeout function

uint16_t twi_getRecoveryCount(); //08/09/18 gfp for monitoring purposes

void twi_clearRecoveryCount(); //08/09/18 gfp for monitoring purposes

#endif

Stay tuned!

Frank

Integrating Time, Memory, and Heading Capability, Part IV

1 Reply

Posted 07/26/18,

In my last post on this subject, I described my efforts to troubleshoot an intermittent ‘bad data’ problem I experienced with the Inversense MPU6050/DMP breakout board from DFRobots. The calculated yaw value would occasionally ‘lose synch with reality’ and start varying wildly. I finally tracked this down to occasional bad reads from the DMP FIFO; the read would extract bytes from two different packets, producing an invalid yaw value. The fix for this problem is to monitor the FIFO ‘bytes remaining’ count and reset the FIFO whenever the ‘bytes remaining’ count is not an integral multiple of the packet size.

After demonstrating that I could run the MPU6050 for days (literally) without any bad data occurrences, I thought I was home free in my effort to provide relative heading information for precise turns, but I ran into yet another problem when I tried to integrate the MPU6050 back onto the robot. This time the problem was a hangup problem; after some tens of minutes, the program would stop responding at all – as if it had gone into an infinite loop somewhere (which is exactly what was happening, but I’m getting ahead of myself…).

This problem turned out to be a ‘simple’ I2C wire-length issue; I was able to demonstrate that the problem would occur any time the I2C ICL/SDA wire length went beyond about 42 cm, and would not occur with wire lengths below about 30 cm. The problem did not appear to be sensitive to I2C bus speed (at least not for the default 100KHz or for a reduced clock speed of 50KHz) or pullup resistor value – just to wire length.

The rest of this post is a copy of my troubleshooting notes from the effort to track this problem down and solve it. I have found in the past that when facing a non-trivial problem with multiple possible causal factors, a troubleshooting journal is an absolute must. In the pre-computer days (you do remember there was a time before ubiquitous computing don’t you?), I used a real MIT engineering notebook for this purpose, and then later on a 3-ring binder into which I added quadrille-ruled sheets covered with my notes and drawings. Then I moved on to Word documents – nicer because I could include Excel plots, Visio drawings, photos, and other mixed media. Now that I have graduated to a WordPress blog, I can use it as a repository of my working notes, while also allowing others to see the inner workings of a mad scientist’s mind ;-).

I2C Hangup problem with Inversense MPU6050

Download

Stay tuned,

Frank

Integrating Time, Memory, and Heading Capability, Part III

Summary:

The Invensense MPU6050/DMP/FIFO combination is sensitive to delays in the main processing loop, even when using the INT line with an Interrupt Service Routine (ISR). When the main loop processing delays get beyond about 40mSec, the ‘mpu.getFIFOBytes(fifoBuffer, packetSize);’ call will occasionally not remove the correct number of bytes from the FIFO, leaving a non-modulo (packetsize) number of bytes remaining in the FIFO. When this happens, the next read will get (and process) bytes from two different packets, resulting in wildly varying yaw value outputs. This condition is (now, after knowing what’s going on) fairly easy to recognize, as the error condition generally causes adjacent yaw values to have different signs, resulting in a classic sawtooth output.

The way to eliminate this artifact is to check for non-modulo (packetsize) FIFO bytes remaining value each time, and reset the FIFO when this happens. Whatever good data is still in the FIFO will be lost, but the data that you do get will be valid.

I have included below my test program, with the FIFO modulo check and FIFO reset mechanism. Note that this program also includes my motor control code, which obviously will not work with your setup.

/*
    Name:       Arduino_IMU6050_Test3.ino
    Created:	6/22/2018 9:25:22 PM
    Author:     FPWIN7LAPTOP\Frank
*/
#include<elapsedMillis/elapsedMillis.h>
#include <PrintEx.h>
StreamEx mySerial = Serial;

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h" //06/23/18 modified for 20Hz interrupt freq

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 mpu;
MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr

/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050's INT pin being connected to the Arduino's
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
* ========================================================================= */

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

						// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//07/05/18 copied from FourWD_WallE2.ino
//02/29/16 hardware defines
#define NO_MOTORS

#pragma region MOTORCONSTS
//drive wheel speed parameters
const int MOTOR_SPEED_FULL = 200; //range is 0-255
const int MOTOR_SPEED_MAX = 255; //range is 0-255
const int MOTOR_SPEED_HALF = 127; //range is 0-255
const int MOTOR_SPEED_LOW = 50; //added 01/22/15
const int MOTOR_SPEED_OFF = 0; //range is 0-255
const int MOTOR_SPEED_ADJ_FACTOR = 40; //chg to 40 at 5:55pm
const int LEFT_SPEED_COMP_VAL_FWD = 15; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_FWD = -LEFT_SPEED_COMP_VAL_FWD; //right speed compensation value
const int LEFT_SPEED_COMP_VAL_REV = 5; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_REV = -LEFT_SPEED_COMP_VAL_REV; //right speed compensation value

															   //drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;

//drive wheel rotation constants - used for calculating time required to rotate X degrees
const double DRIVE_WHEEL_FULL_SPEED_RPM = 200.0; //at MOTOR_SPEED_FULL - rev 05/20/19
const double DRIVE_WHEEL_DIAM_MM = 66.0;
const double DriveWheelSpdMMPerSec = PI * DRIVE_WHEEL_DIAM_MM * DRIVE_WHEEL_FULL_SPEED_RPM / 60;
const double DriveWheelDegPerSec = 360 * DRIVE_WHEEL_FULL_SPEED_RPM / 60.0;
const int NUM_DEG_FOR_90_DEG_TURN = 90; //replace 'magic numbers' with program const
const int MOTOR_RAMPUP_MSEC = 500; //added 05/20/17 - used in RotateCW()

								   //Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
#pragma endregion Motor Parameters


//vvvvvvvvvvvvvvvvvvv  PIN ASSIGNMENTS vvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR_PINS
//Right Motors
int enA_R = 46;
int enB_R = 44;
int in1_R = 42;
int in2_R = 40;

int in3_R = 38;
int in4_R = 36;

// left motors
int enA_L = 8;
int in1_L = 9;
int in2_L = 10;

int in3_L = 11;
int in4_L = 12;
int enB_L = 13;
#pragma endregion Motor Pin Assignments

#pragma region LOOP_VARS
int leftspeednum = MOTOR_SPEED_HALF;
int rightspeednum = MOTOR_SPEED_HALF;

//02/13/16 added for 'pause' debug
int m_FinalLeftSpeed = 0;
int m_FinalRightSpeed = 0;
#pragma endregion Loop Variables

elapsedMillis mSecSinceLastReset;
elapsedMillis mSecSinceLastHeartbeat;
const int _6050_RESET_INTERVAL_MSEC = 1000;
const int HEARTBEAT_INTERVAL_MSEC = 250;
const int MAX_AD_VALUE = 1023;
const float ADC_REF_VOLTS = 5.0; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF 

const int HEARTBEAT_LED = 35;

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
volatile float yawval = 0;

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() 
{
	// join I2C bus (I2Cdev library doesn't do this automatically)
	Wire.begin();

	// initialize serial communication
	Serial.begin(115200);
	while (!Serial); // wait for Leonardo enumeration, others continue immediately

	//07/06/18 copied from FourWD_WallE2.ino

	//12/06/15 set up right motor pins as outputs
	pinMode(enA_R, OUTPUT);
	pinMode(enB_R, OUTPUT);
	pinMode(in1_R, OUTPUT);
	pinMode(in2_R, OUTPUT);

	//12/06/15 set up left motor pins as outputs
	pinMode(enA_L, OUTPUT);
	pinMode(enB_L, OUTPUT);
	pinMode(in1_L, OUTPUT);
	pinMode(in2_L, OUTPUT);

	//heartbeat LED
	pinMode(HEARTBEAT_LED, OUTPUT);
	pinMode(HEARTBEAT_LED-2, OUTPUT);


	// initialize device
	Serial.println(F("Initializing I2C devices..."));
	mpu.initialize();

	// verify connection
	Serial.println(F("Testing device connections..."));
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0) 
	{
		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// enable Arduino interrupt detection
		Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
		attachInterrupt(0, dmpDataReady, RISING);
		mpuIntStatus = mpu.getIntStatus();

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		Serial.println(F("DMP ready! Waiting for first interrupt..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
	}
	else 
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));
	}


	//07/06/18 copied from FourWD_WallE2.ino
	//mySerial.printf("initializing motors\n");

	//10/08/17 make sure motors control lines are all in STOP state
	StopLeftMotors();
	StopRightMotors();

	//01/26/15 start the robot going straight
	//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);
	MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_HALF);
	mySerial.printf("Setup Accomplished - entering loop()\n");

	mSecSinceLastReset = 0; //should be last statement in Setup()
}

void dmpDataReady()
{
	mpuInterrupt = true;
}


// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
long whilecount = 0;
int resetcount = 0;

void loop() 
{
	//visible heartbeat added 07/15/18 (can't use pin 13 - its a motor control pin)
	if (mSecSinceLastHeartbeat >= HEARTBEAT_INTERVAL_MSEC)
	{
		mSecSinceLastHeartbeat -= HEARTBEAT_INTERVAL_MSEC;
		bool ledState = digitalRead(HEARTBEAT_LED);
		digitalWrite(HEARTBEAT_LED, !ledState);
	}

	// if programming failed, don't try to do anything
	if (!dmpReady) return;


	// wait for MPU interrupt or extra packet(s) available
	while (!mpuInterrupt && fifoCount < packetSize) 
	{
		whilecount++;

		bool readval = digitalRead(HEARTBEAT_LED - 2);
		digitalWrite(HEARTBEAT_LED - 2, !readval);
		// other program behavior stuff here
		// .
		// .
		// .
		// if you are really paranoid you can frequently test in between other
		// stuff to see if mpuInterrupt is true, and if so, "break;" from the
		// while() loop to immediately process the MPU data
		// .
		// .
		// .

		//10/08/17 make sure motors control lines are all in STOP state
		//StopLeftMotors();
		//StopRightMotors();
		//if (mpuInterrupt)
		//{
		//	mySerial.printf("break 1\n");
		//	break;
		//}

		//fwd 1 sec
		//SetLeftMotorDir(FWD_DIR);
		//SetRightMotorDir(FWD_DIR);

		//if (mpuInterrupt)
		//{
		//	mySerial.printf("break 2\n");
		//	break;
		//}

		//delay(50);
		//delay(50);
		//delay(50);
		//delay(100);
		//delay(10);
		//delay(10);
		//delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		//01/26/15 start the robot going straight
		//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

		if (mpuInterrupt)
		{
			fifoCount = mpu.getFIFOCount();
			//mySerial.printf("%ld\t%d\n", whilecount, fifoCount);
			whilecount = 0;
			break;
		}



	}

	// reset interrupt flag and get INT_STATUS byte
	mpuIntStatus = mpu.getIntStatus();

	// get current FIFO count
	fifoCount = mpu.getFIFOCount();

	// check for overflow (this should never happen unless our code is too inefficient)
	if ((mpuIntStatus & 0x10) || fifoCount == 1024)
	{
		// reset so we can continue cleanly
		mpu.resetFIFO();
		Serial.println(F("FIFO overflow!"));

		// otherwise, check for DMP data ready interrupt (this should happen frequently)
	}
	else if (mpuIntStatus & 0x02)
	{
		// wait for correct available data length, should be a VERY short wait
		while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

		//// read a packet from FIFO
		//mpu.getFIFOBytes(fifoBuffer, packetSize);

		//// track FIFO count here in case there is > 1 packet available
		//// (this lets us immediately read more without waiting for an interrupt)
		//fifoCount -= packetSize;


		//07/08/18 added to watch for non-modulo FIFO counts
		if (fifoCount == 0 || fifoCount%packetSize != 0)
		{
			mpu.resetFIFO();
			resetcount++;
		}
		else
		{
			//07/07/18 modified to read all outstanding packets
			// read a packet from FIFO
			while (fifoCount >= packetSize)
			{
				mpu.getFIFOBytes(fifoBuffer, packetSize);
				fifoCount -= packetSize;
			}

			// display Euler angles in degrees
			mpu.dmpGetQuaternion(&q, fifoBuffer);
			mpu.dmpGetGravity(&gravity, &q);
			mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

			yawval =  ypr[0] * 180 / M_PI;
		}
	}

	if (mpuInterrupt)
	{
		mpuInterrupt = false; //reset for next time
		long msec = millis();
		float hours = (float)msec / 3600000.;
		float volts = GetVolts();
		//mySerial.printf("%ld\t%3.2f V\t%3.2f\t%d\t%d\n", msec, volts, yawval, fifoCount, resetcount);
		mySerial.printf("%2.2f\t%3.2f V\t%3.2f\t%d\t%d\n", hours, volts, yawval, fifoCount, resetcount);
	}
}

float GetVolts()
{
	int voltreading = analogRead(A1);
	float v = ((float)voltreading / MAX_AD_VALUE)*ADC_REF_VOLTS;
	return v;
}

//11/25/15 added for symmetry ;-).
void StopBothMotors()
{
	StopLeftMotors();
	StopRightMotors();
}

void StopLeftMotors()
{
	//10/08/17 added analogWrite calls to ensure speed set to zero
	analogWrite(enA_L, MOTOR_SPEED_OFF);
	analogWrite(enB_L, MOTOR_SPEED_OFF);

	digitalWrite(in1_L, LOW);
	digitalWrite(in2_L, LOW);
	digitalWrite(in3_L, LOW);
	digitalWrite(in4_L, LOW);
}

void StopRightMotors()
{
	//10/08/17 added analogWrite calls to ensure speed set to zero
	analogWrite(enA_R, MOTOR_SPEED_OFF);
	analogWrite(enB_R, MOTOR_SPEED_OFF);

	digitalWrite(in1_R, LOW);
	digitalWrite(in2_R, LOW);
	digitalWrite(in3_R, LOW);
	digitalWrite(in4_R, LOW);
}

//01/24/15 removed time from MoveAhead sig, added MoveAheadMsec
void MoveAhead(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move ahead continuously
	//Provenance: G. Frank Paynter and Danny Frank 01/24/2014
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set forward direction for both wheels
	//    Step 2: Run both Motors at specified speeds

	//Step 1: Set forward direction for both wheels
	SetLeftMotorDir(true);
	SetRightMotorDir(true);

	//Step 2: Run both myMotors for timsec seconds
	//01/24/15 - removed timing - now continuous run
	RunBothMotors(leftspeednum, rightspeednum);
}



//
////12/26/14 added for independent motor speed
void RunBothMotors(int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors at left/rightspeednum speeds
	//Inputs:
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//    01/24/15 - revised for continuous run - no timing
	//  01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()
	//	12/07/15 - chg args from &int to int
	//Step 1: Apply drive to both wheels

	SetLeftMotorSpeed(leftspeednum);
	SetRightMotorSpeed(rightspeednum);
}

//12/26/14 added for independent motor speed
void RunBothMotorsMsec(int timeMsec, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors for timesec seconds at speednum speed
	//Inputs:
	//    timesec = time in seconds to run the Motors
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//    Step 2: Delay timsec seconds
	//    Step 3: Remove drive from both wheels.
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//	11/25/15 - rev to use motor driver class object

	RunBothMotors(leftspeednum, rightspeednum);

	//Step 2: Delay timsec seconds
	delay(timeMsec);
}
//11/25/15 added to utilize new private member variable bLeftMotorDirIsFwd
void SetLeftMotorSpeed(int speed)
{
	//if (bLeftMotorDirIsFwd)
	//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode
	{
		if (bLeftMotorDirIsFwd)
		{
			speed = speed + LEFT_SPEED_COMP_VAL_FWD;
		}
		else //must be REV
		{
			speed = speed + LEFT_SPEED_COMP_VAL_REV;
		}
	}

	//12/12/15 added here to make sure speed vals always within valid ranges
	//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (255)
	speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
	speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

#ifndef NO_MOTORS
	analogWrite(enA_L, speed);
	analogWrite(enB_L, speed);
#endif

	//02/13/16 added for 'pause' debug
	m_FinalLeftSpeed = speed;
	//Serial.print(", Left Speed = "); Serial.println(speed);
}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd
void SetRightMotorSpeed(int speed)
{
	//Serial.print("SetRightMotorSpeed: "); Serial.print("Input = "); Serial.print(speed);

	//if (bRightMotorDirIsFwd)
	//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode
	{
		if (bRightMotorDirIsFwd)
		{
			speed = speed + RIGHT_SPEED_COMP_VAL_FWD;
		}
		else //must be REV
		{
			speed = speed + RIGHT_SPEED_COMP_VAL_REV;
		}
	}

	//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (2550
	speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
	speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

#ifndef NO_MOTORS
	analogWrite(enA_R, speed);
	analogWrite(enB_R, speed);
#endif

	//02/13/16 added for 'pause' debug
	m_FinalRightSpeed = speed;
	//Serial.print(", Right Speed = "); Serial.println(speed);
}


void SetLeftMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(in1_L, HIGH);
		digitalWrite(in2_L, LOW);
		digitalWrite(in3_L, HIGH);
		digitalWrite(in4_L, LOW);

	}
	else //must be REV
	{
		digitalWrite(in1_L, LOW);
		digitalWrite(in2_L, HIGH);
		digitalWrite(in3_L, LOW);
		digitalWrite(in4_L, HIGH);
	}

	//added 11/25/15
	bLeftMotorDirIsFwd = bIsFwdDir;
}

void SetRightMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(in1_R, HIGH);
		digitalWrite(in2_R, LOW);
		digitalWrite(in3_R, HIGH);
		digitalWrite(in4_R, LOW);

	}
	else //must be REV
	{
		digitalWrite(in1_R, LOW);
		digitalWrite(in2_R, HIGH);
		digitalWrite(in3_R, LOW);
		digitalWrite(in4_R, HIGH);
	}
http://gfpbridge.com/wp-admin/post.php?post=3212&action=edit#
	//added 11/25/15
	bRightMotorDirIsFwd = bIsFwdDir;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

Name: Arduino_IMU6050_Test3.ino

Created: 6/22/2018 9:25:22 PM

Author: FPWIN7LAPTOP\Frank

#include<elapsedMillis/elapsedMillis.h>

#include <PrintEx.h>

StreamEx mySerial = Serial;

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)

// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>

// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h

#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files

// for both classes must be in the include path of your project

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h" //06/23/18 modified for 20Hz interrupt freq

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)

// AD0 high = 0x69

//MPU6050 mpu;

MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr

/* =========================================================================

NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch

depends on the MPU-6050's INT pin being connected to the Arduino's

external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is

digital I/O pin 2.

* ========================================================================= */

// MPU control/status vars

bool dmpReady = false; // set true if DMP init was successful

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

//07/05/18 copied from FourWD_WallE2.ino

//02/29/16 hardware defines

#define NO_MOTORS

#pragma region MOTORCONSTS

//drive wheel speed parameters

const int MOTOR_SPEED_FULL = 200; //range is 0-255

const int MOTOR_SPEED_MAX = 255; //range is 0-255

const int MOTOR_SPEED_HALF = 127; //range is 0-255

const int MOTOR_SPEED_LOW = 50; //added 01/22/15

const int MOTOR_SPEED_OFF = 0; //range is 0-255

const int MOTOR_SPEED_ADJ_FACTOR = 40; //chg to 40 at 5:55pm

const int LEFT_SPEED_COMP_VAL_FWD = 15; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_FWD = -LEFT_SPEED_COMP_VAL_FWD; //right speed compensation value

const int LEFT_SPEED_COMP_VAL_REV = 5; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_REV = -LEFT_SPEED_COMP_VAL_REV; //right speed compensation value

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//drive wheel rotation constants - used for calculating time required to rotate X degrees

const double DRIVE_WHEEL_FULL_SPEED_RPM = 200.0; //at MOTOR_SPEED_FULL - rev 05/20/19

const double DRIVE_WHEEL_DIAM_MM = 66.0;

const double DriveWheelSpdMMPerSec = PI * DRIVE_WHEEL_DIAM_MM * DRIVE_WHEEL_FULL_SPEED_RPM / 60;

const double DriveWheelDegPerSec = 360 * DRIVE_WHEEL_FULL_SPEED_RPM / 60.0;

const int NUM_DEG_FOR_90_DEG_TURN = 90; //replace 'magic numbers' with program const

const int MOTOR_RAMPUP_MSEC = 500; //added 05/20/17 - used in RotateCW()

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

#pragma endregion Motor Parameters

//vvvvvvvvvvvvvvvvvvv PIN ASSIGNMENTS vvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR_PINS

//Right Motors

int enA_R = 46;

int enB_R = 44;

int in1_R = 42;

int in2_R = 40;

int in3_R = 38;

int in4_R = 36;

// left motors

int enA_L = 8;

int in1_L = 9;

int in2_L = 10;

int in3_L = 11;

int in4_L = 12;

int enB_L = 13;

#pragma endregion Motor Pin Assignments

#pragma region LOOP_VARS

int leftspeednum = MOTOR_SPEED_HALF;

int rightspeednum = MOTOR_SPEED_HALF;

//02/13/16 added for 'pause' debug

int m_FinalLeftSpeed = 0;

int m_FinalRightSpeed = 0;

#pragma endregion Loop Variables

elapsedMillis mSecSinceLastReset;

elapsedMillis mSecSinceLastHeartbeat;

const int _6050_RESET_INTERVAL_MSEC = 1000;

const int HEARTBEAT_INTERVAL_MSEC = 250;

const int MAX_AD_VALUE = 1023;

const float ADC_REF_VOLTS = 5.0; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF

const int HEARTBEAT_LED = 35;

// ================================================================

// === INTERRUPT DETECTION ROUTINE ===

// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

volatile float yawval = 0;

// ================================================================

// === INITIAL SETUP ===

// ================================================================

void setup()

{

// join I2C bus (I2Cdev library doesn't do this automatically)

Wire.begin();

// initialize serial communication

Serial.begin(115200);

while (!Serial); // wait for Leonardo enumeration, others continue immediately

//07/06/18 copied from FourWD_WallE2.ino

//12/06/15 set up right motor pins as outputs

pinMode(enA_R, OUTPUT);

pinMode(enB_R, OUTPUT);

pinMode(in1_R, OUTPUT);

pinMode(in2_R, OUTPUT);

//12/06/15 set up left motor pins as outputs

pinMode(enA_L, OUTPUT);

pinMode(enB_L, OUTPUT);

pinMode(in1_L, OUTPUT);

pinMode(in2_L, OUTPUT);

//heartbeat LED

pinMode(HEARTBEAT_LED, OUTPUT);

pinMode(HEARTBEAT_LED-2, OUTPUT);

// initialize device

Serial.println(F("Initializing I2C devices..."));

mpu.initialize();

// verify connection

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if successful)

if (devStatus == 0)

{

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// enable Arduino interrupt detection

Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));

attachInterrupt(0, dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for first interrupt..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

}

//07/06/18 copied from FourWD_WallE2.ino

//mySerial.printf("initializing motors\n");

//10/08/17 make sure motors control lines are all in STOP state

StopLeftMotors();

StopRightMotors();

//01/26/15 start the robot going straight

//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_HALF);

mySerial.printf("Setup Accomplished - entering loop()\n");

mSecSinceLastReset = 0; //should be last statement in Setup()

}

void dmpDataReady()

{

mpuInterrupt = true;

}

// ================================================================

// === MAIN PROGRAM LOOP ===

// ================================================================

long whilecount = 0;

int resetcount = 0;

void loop()

{

//visible heartbeat added 07/15/18 (can't use pin 13 - its a motor control pin)

if (mSecSinceLastHeartbeat >= HEARTBEAT_INTERVAL_MSEC)

{

mSecSinceLastHeartbeat -= HEARTBEAT_INTERVAL_MSEC;

bool ledState = digitalRead(HEARTBEAT_LED);

digitalWrite(HEARTBEAT_LED, !ledState);

}

// if programming failed, don't try to do anything

if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available

while (!mpuInterrupt && fifoCount < packetSize)

{

whilecount++;

bool readval = digitalRead(HEARTBEAT_LED - 2);

digitalWrite(HEARTBEAT_LED - 2, !readval);

// other program behavior stuff here

// .

// if you are really paranoid you can frequently test in between other

// stuff to see if mpuInterrupt is true, and if so, "break;" from the

// while() loop to immediately process the MPU data

// .

//10/08/17 make sure motors control lines are all in STOP state

//StopLeftMotors();

//StopRightMotors();

//if (mpuInterrupt)

//{

// mySerial.printf("break 1\n");

// break;

//}

//fwd 1 sec

//SetLeftMotorDir(FWD_DIR);

//SetRightMotorDir(FWD_DIR);

//if (mpuInterrupt)

//{

// mySerial.printf("break 2\n");

// break;

//}

//delay(50);

//delay(100);

//delay(10);

delay(10);

//01/26/15 start the robot going straight

//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

if (mpuInterrupt)

{

fifoCount = mpu.getFIFOCount();

//mySerial.printf("%ld\t%d\n", whilecount, fifoCount);

whilecount = 0;

break;

}

// reset interrupt flag and get INT_STATUS byte

mpuIntStatus = mpu.getIntStatus();

// get current FIFO count

fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & 0x10) || fifoCount == 1024)

{

// reset so we can continue cleanly

mpu.resetFIFO();

Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)

}

else if (mpuIntStatus & 0x02)

{

// wait for correct available data length, should be a VERY short wait

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

//// read a packet from FIFO

//mpu.getFIFOBytes(fifoBuffer, packetSize);

//// track FIFO count here in case there is > 1 packet available

//// (this lets us immediately read more without waiting for an interrupt)

//fifoCount -= packetSize;

//07/08/18 added to watch for non-modulo FIFO counts

if (fifoCount == 0 || fifoCount%packetSize != 0)

{

mpu.resetFIFO();

resetcount++;

}

else

{

//07/07/18 modified to read all outstanding packets

// read a packet from FIFO

while (fifoCount >= packetSize)

{

mpu.getFIFOBytes(fifoBuffer, packetSize);

fifoCount -= packetSize;

}

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

yawval = ypr[0] * 180 / M_PI;

}

if (mpuInterrupt)

{

mpuInterrupt = false; //reset for next time

long msec = millis();

float hours = (float)msec / 3600000.;

float volts = GetVolts();

//mySerial.printf("%ld\t%3.2f V\t%3.2f\t%d\t%d\n", msec, volts, yawval, fifoCount, resetcount);

mySerial.printf("%2.2f\t%3.2f V\t%3.2f\t%d\t%d\n", hours, volts, yawval, fifoCount, resetcount);

}

float GetVolts()

{

int voltreading = analogRead(A1);

float v = ((float)voltreading / MAX_AD_VALUE)*ADC_REF_VOLTS;

return v;

}

//11/25/15 added for symmetry ;-).

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

void StopLeftMotors()

{

//10/08/17 added analogWrite calls to ensure speed set to zero

analogWrite(enA_L, MOTOR_SPEED_OFF);

analogWrite(enB_L, MOTOR_SPEED_OFF);

digitalWrite(in1_L, LOW);

digitalWrite(in2_L, LOW);

digitalWrite(in3_L, LOW);

digitalWrite(in4_L, LOW);

}

void StopRightMotors()

{

//10/08/17 added analogWrite calls to ensure speed set to zero

analogWrite(enA_R, MOTOR_SPEED_OFF);

analogWrite(enB_R, MOTOR_SPEED_OFF);

digitalWrite(in1_R, LOW);

digitalWrite(in2_R, LOW);

digitalWrite(in3_R, LOW);

digitalWrite(in4_R, LOW);

}

//01/24/15 removed time from MoveAhead sig, added MoveAheadMsec

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//Provenance: G. Frank Paynter and Danny Frank 01/24/2014

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Step 1: Set forward direction for both wheels

SetLeftMotorDir(true);

SetRightMotorDir(true);

//Step 2: Run both myMotors for timsec seconds

//01/24/15 - removed timing - now continuous run

RunBothMotors(leftspeednum, rightspeednum);

}

////12/26/14 added for independent motor speed

void RunBothMotors(int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors at left/rightspeednum speeds

//Inputs:

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 01/24/15 - revised for continuous run - no timing

// 01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()

// 12/07/15 - chg args from &int to int

//Step 1: Apply drive to both wheels

SetLeftMotorSpeed(leftspeednum);

SetRightMotorSpeed(rightspeednum);

}

//12/26/14 added for independent motor speed

void RunBothMotorsMsec(int timeMsec, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors for timesec seconds at speednum speed

//Inputs:

// timesec = time in seconds to run the Motors

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

// Step 2: Delay timsec seconds

// Step 3: Remove drive from both wheels.

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 11/25/15 - rev to use motor driver class object

RunBothMotors(leftspeednum, rightspeednum);

//Step 2: Delay timsec seconds

delay(timeMsec);

}

//11/25/15 added to utilize new private member variable bLeftMotorDirIsFwd

void SetLeftMotorSpeed(int speed)

{

//if (bLeftMotorDirIsFwd)

//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode

{

if (bLeftMotorDirIsFwd)

{

speed = speed + LEFT_SPEED_COMP_VAL_FWD;

}

else //must be REV

{

speed = speed + LEFT_SPEED_COMP_VAL_REV;

}

//12/12/15 added here to make sure speed vals always within valid ranges

//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (255)

speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

#ifndef NO_MOTORS

analogWrite(enA_L, speed);

analogWrite(enB_L, speed);

#endif

//02/13/16 added for 'pause' debug

m_FinalLeftSpeed = speed;

//Serial.print(", Left Speed = "); Serial.println(speed);

}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd

void SetRightMotorSpeed(int speed)

{

//Serial.print("SetRightMotorSpeed: "); Serial.print("Input = "); Serial.print(speed);

//if (bRightMotorDirIsFwd)

//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode

{

if (bRightMotorDirIsFwd)

{

speed = speed + RIGHT_SPEED_COMP_VAL_FWD;

}

else //must be REV

{

speed = speed + RIGHT_SPEED_COMP_VAL_REV;

}

//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (2550

speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

#ifndef NO_MOTORS

analogWrite(enA_R, speed);

analogWrite(enB_R, speed);

#endif

//02/13/16 added for 'pause' debug

m_FinalRightSpeed = speed;

//Serial.print(", Right Speed = "); Serial.println(speed);

}

void SetLeftMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(in1_L, HIGH);

digitalWrite(in2_L, LOW);

digitalWrite(in3_L, HIGH);

digitalWrite(in4_L, LOW);

}

else //must be REV

{

digitalWrite(in1_L, LOW);

digitalWrite(in2_L, HIGH);

digitalWrite(in3_L, LOW);

digitalWrite(in4_L, HIGH);

}

//added 11/25/15

bLeftMotorDirIsFwd = bIsFwdDir;

}

void SetRightMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(in1_R, HIGH);

digitalWrite(in2_R, LOW);

digitalWrite(in3_R, HIGH);

digitalWrite(in4_R, LOW);

}

else //must be REV

{

digitalWrite(in1_R, LOW);

digitalWrite(in2_R, HIGH);

digitalWrite(in3_R, LOW);

digitalWrite(in4_R, HIGH);

}

http://gfpbridge.com/wp-admin/post.php?post=3212&action=edit#

//added 11/25/15

bRightMotorDirIsFwd = bIsFwdDir;

}

Stay tuned,

Frank

Dawes Lightning 1500 Stem Bracket

Printing Notes:

I printed the PLA part with

layer height: 0.2mm

extruder temp: 205 for layer 1, 195 for 3 & up

print bed temp: 70C

speed: 30mm/sec

Z-offset: -0.05mm

I had to work a bit to get the PLA to stick to the bed, and the right combination was the above settings, plus attention to getting the bed levelled properly and the correct Z-axis offset so the first layer was ‘squished’ significantly.

Integrating Time, Memory, and Heading Capability, Part II

1 Reply

Posted 06 June 2018

About a month ago in early May I started another try at adding time, memory, and heading super-powers to Wall-E2’s repertoire. In this post, I described the effort to add a FRAM board, an RTC board, and a 9DOF MPU9050 IMU board to Wall-E2’s already-existing I2C bus, and in this post I described fabricating a permanent ‘tri-sensor board’ for installation on Wall-E2’s second deck.

After installing the tri-sensor board on the second deck, I did some tests to confirm Wall-E2’s new found ability to make accurate turns, and instead of confirming success, I witnessed complete failure – Wall-E2 was even worse at making turns now than in the past when he was just doing timed turns – WTF!!

Backing up and doing some more tests, it quickly became apparent that the MPU9050 IMU worked fine until Wall-E2’s wheel motors started running, but went completely ga-ga after that. I tried some quick tests to try and isolate the problem, but didn’t get anywhere. So, I did what I always do when faced with an apparently intractable problem – I ran away ;-). OK, I didn’t really run away, but I decided to put the whole thing on the back burner for a while and let my subconscious work on it for a while (I have discovered over and over again that my subconscious is a lot smarter than I am). In the meantime I had some other stuff to do, like going to a duplicate bridge tournament and going to a grand-daughter’s HS graduation.

After doing some web research, I began to suspect that the problem might be associated with the complexity associated with the Sparkfun 9DOF MPU9050 chip and the associated yaw/pitch/roll computation software, so I decided to see if there were other IMU solutions out there. I ran across the DFRobots MPU-6050 6DOF IMU breakout board.

Since I wasn’t planning to use this for anything but yaw (z-axis rotation) information, the 6DOF limitation wasn’t a problem, and the integrated Digital Motion Processorâ„¢ (DMP) should simplify the control software. In addition, the IMU6050 board is 5V compatible, so I can eliminate the level-shifting hardware required for the Sparkfun 9250 unit. So, I ordered a couple of units and started playing with them. Using the supplied libraries from DFRobots and their little example program, I was able to get the unit up and running fairly quickly. To test the ability to report yaw excursions, I modified the test program to report only yaw, and manually turned the sensor around its Z-axis. As shown below, the yaw progressed smoothly.

Manually turned sensor while recording yaw

Next, I mounted just the IMU6050 board on the robot’s 2nd deck, as shown below, and ran some tests with and without the motor running.

MPU6050 mounted on robot with double-sided tape

Sensor mounted on robot with double-sided tape, no motors running

As shown above, with the sensor mounted on the 2nd deck and with motors not running, I got a smooth response from the yaw sensor as the robot was manually rotated. However, when I tried the same trick with the motors running, I got the following plot:

Sensor mounted on robot with double-sided tape, left rear motor running

Then I mounted the sensor on an ‘air pillow’ (one section of a piece of air-filled packing material) and tried again, once again getting a smooth plot, even with one motor running.

MPU 6050 mounted on robot with an ‘air pillow’ (piece of some air-filled packing material)

6050 sensor mounted on air pillow, left rear wheel motor running

Integrating Time, Memory, and Heading Capability

2 Replies

Posted 06 May 2018

For the last two months I have been working on adding some secondary, but still important capabilities to Wall-E2, my wall-following autonomous robot. As I noted back in April, Wall-E2 still can’t tell which way he is heading, which (among other problems) means he can’t make accurate turns. In addition, Wall-E2 can’t tell how long (or even if) he has been turned off, making it impossible to tell how long it has been since he last was charged.

In the intervening months, I have been able to obtain and test individual modules to address the above issues; the Sparkfun MPU9250 9DOF IMU breakout board to obtain heading information, and the combination of an Adafruit MB85RC256V FRAM breakout board and an Adafruit DS3221 RTC breakout board to capture the date/time of any power interruptions.

Since all three of the above breakout boards are I2C-capable, it should be feasible to run all three from a single I2C bus on Wall-E2’s main controller – an Arduino Mega 2560. Unfortunately, the Sparkfun MPU9250 module isn’t 5V tolerant, so integrating all three isn’t as simple as just daisy-chaining them all together. Back to Google for some more research, where I eventually uncovered Philips Application Note AN97055 by Herman Schutte of The Netherlands dealing with bidirectional level-shifters for just this problem. Turns out that Philips introduced the I2C bus back in 1980, so they may know a thing or two about the issues ;-). In any case, this 1997 paper described a 2-MOSFET bidirectional level-shifter that completely addresses the issue, and seemed to be pretty straightforward to implement. It took me a couple of tries, but I got it working, with the result that I can now run all three modules (RTC, FRAM and IMU) from an Arduino Mega 2560, with the level-shifting MOSFET’s placed between the IMU and the rest of the circuit, as shown in the photo below.

All three sensors operating a the same time, controlled over a single I2C channel.

In the above photo, the modules are (from left to right): Adafruit MB85RC256K FRAM breakout, Adafruit DS3231 RTC breakout, and Sparkfun MPU9250 9DOF IMU breakout. The two 2N7000 level-shifter MOSFETs can be seen at the top left-hand corner of the Sparkfun IMU breakout board.

I put together a small test program, combining pieces from the software used to test the power-down date/time capture idea, and the software used to test the ability to use the Sparkfun IMU to accurately manage rotations. This program doesn’t do much at all, but it does demonstrate that all three modules can be controlled at the same time over a single I2C channel, as shown in the following output.

Opening port
Port open
Starting program, Scanning I2C addresses...
Scanning...
I2C device found at address 0x0C  !
I2C device found at address 0x50  !
I2C device found at address 0x68  !
I2C device found at address 0x69  !
I2C device found at address 0x7C  !
done

Initializing 9250 IMU...
MPU9250 I AM 71 I should be 71
MPU9250 is online - doing self-test...
Starting gyro/accel calibration - keep IMU motionless until finished...
gyro/accel calibration complete. Calibration took 381 Msec
MPU9250 initialized for active data mode....


Done...
Found I2C FRAM
Restarted 8 times
Displaying contents of first 50 memory locations...

0x0: 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
 0x20: 0x00 0x00 0x00 0x00 0x00 0xBE 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 

In Loop()...
2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.79
2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.62
2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.42
2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.18
2018/5/6 (Sunday) 22:5:24 IMU Hdg = 20.93
2018/5/6 (Sunday) 22:5:24 IMU Hdg = 20.64
2018/5/6 (Sunday) 22:5:24 IMU Hdg = 20.33
2018/5/6 (Sunday) 22:5:24 IMU Hdg = 19.98
2018/5/6 (Sunday) 22:5:24 IMU Hdg = 19.57
2018/5/6 (Sunday) 22:5:25 IMU Hdg = 19.12

0x0: 0x9 0x00 0x00 0x3 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:25 IMU Hdg = 18.59
2018/5/6 (Sunday) 22:5:25 IMU Hdg = 18.02
2018/5/6 (Sunday) 22:5:25 IMU Hdg = 17.33
2018/5/6 (Sunday) 22:5:25 IMU Hdg = 16.59
2018/5/6 (Sunday) 22:5:26 IMU Hdg = 15.75
2018/5/6 (Sunday) 22:5:26 IMU Hdg = 14.84
2018/5/6 (Sunday) 22:5:26 IMU Hdg = 13.78
2018/5/6 (Sunday) 22:5:26 IMU Hdg = 12.59
2018/5/6 (Sunday) 22:5:26 IMU Hdg = 11.29
2018/5/6 (Sunday) 22:5:27 IMU Hdg = 9.86

0x0: 0x9 0x00 0x00 0x3 0x4 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:27 IMU Hdg = 8.32
2018/5/6 (Sunday) 22:5:27 IMU Hdg = 6.70
2018/5/6 (Sunday) 22:5:27 IMU Hdg = 5.00
2018/5/6 (Sunday) 22:5:27 IMU Hdg = 3.32
2018/5/6 (Sunday) 22:5:28 IMU Hdg = 1.71
2018/5/6 (Sunday) 22:5:28 IMU Hdg = 0.33
2018/5/6 (Sunday) 22:5:28 IMU Hdg = -0.70
2018/5/6 (Sunday) 22:5:28 IMU Hdg = -1.16
2018/5/6 (Sunday) 22:5:28 IMU Hdg = -0.92
2018/5/6 (Sunday) 22:5:29 IMU Hdg = -1.12

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:29 IMU Hdg = -0.92
2018/5/6 (Sunday) 22:5:29 IMU Hdg = -1.06
2018/5/6 (Sunday) 22:5:29 IMU Hdg = -0.78
2018/5/6 (Sunday) 22:5:29 IMU Hdg = -1.03
2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.79
2018/5/6 (Sunday) 22:5:30 IMU Hdg = -1.01
2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.75
2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.94
2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.68
2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.93

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.66
2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.89
2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.62
2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.87
2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.62
2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.84
2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.62
2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.83
2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.62
2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.83

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.54
2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.80
2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.58
2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.76
2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.52
2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.73
2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.49
2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.70
2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.44
2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.69

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.45
2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.68
2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.39
2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.67
2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.43
2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.63
2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.36
2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.59
2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.36
2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.56

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.42
2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.57
2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.45
2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.56
2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.43
2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.52
2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.41
2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.49
2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.33
2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.44

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.32
2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.46
2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.41
2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.47
2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.42
2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.45
2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.45
2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.41
2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.37
2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.38

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.34
2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.37
2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.30
2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.35
2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.32
2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.33
2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.32
2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.32
2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.34
2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.32

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.34
2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.30
2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.37
2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.26
2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.29
2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.23
2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.34
2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.20
2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.28
2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.17

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.27
2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.15
2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.26
2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.12
2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.23
2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.08
2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.18
2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.10
2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.26
2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.09

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.26
2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.12
2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.35
2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.14
2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.39
2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.12
2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.34
2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.07
2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.33
2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.07

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.33
2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.02
2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.27
2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.01
2018/5/6 (Sunday) 22:5:50 IMU Hdg = -0.27
2018/5/6 (Sunday) 22:5:50 IMU Hdg = 0.00
2018/5/6 (Sunday) 22:5:50 IMU Hdg = -0.26
2018/5/6 (Sunday) 22:5:50 IMU Hdg = 0.04
2018/5/6 (Sunday) 22:5:50 IMU Hdg = -0.18
2018/5/6 (Sunday) 22:5:51 IMU Hdg = 0.09

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x00 0x00 0x00
2018/5/6 (Sunday) 22:5:51 IMU Hdg = -0.16
2018/5/6 (Sunday) 22:5:51 IMU Hdg = 0.13
2018/5/6 (Sunday) 22:5:51 IMU Hdg = -0.15
2018/5/6 (Sunday) 22:5:51 IMU Hdg = 0.14
2018/5/6 (Sunday) 22:5:52 IMU Hdg = -0.12
2018/5/6 (Sunday) 22:5:52 IMU Hdg = 0.16
2018/5/6 (Sunday) 22:5:52 IMU Hdg = -0.11
2018/5/6 (Sunday) 22:5:52 IMU Hdg = 0.17
2018/5/6 (Sunday) 22:5:52 IMU Hdg = -0.11
2018/5/6 (Sunday) 22:5:53 IMU Hdg = 0.15

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x00 0x00
2018/5/6 (Sunday) 22:5:53 IMU Hdg = -0.13
2018/5/6 (Sunday) 22:5:53 IMU Hdg = 0.18
2018/5/6 (Sunday) 22:5:53 IMU Hdg = -0.10
2018/5/6 (Sunday) 22:5:53 IMU Hdg = 0.23
2018/5/6 (Sunday) 22:5:54 IMU Hdg = -0.05
2018/5/6 (Sunday) 22:5:54 IMU Hdg = 0.26
2018/5/6 (Sunday) 22:5:54 IMU Hdg = -0.07
2018/5/6 (Sunday) 22:5:54 IMU Hdg = 0.26
2018/5/6 (Sunday) 22:5:54 IMU Hdg = -0.04
2018/5/6 (Sunday) 22:5:55 IMU Hdg = 0.25

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x00
2018/5/6 (Sunday) 22:5:55 IMU Hdg = -0.05
2018/5/6 (Sunday) 22:5:55 IMU Hdg = 0.26
2018/5/6 (Sunday) 22:5:55 IMU Hdg = -0.01
2018/5/6 (Sunday) 22:5:55 IMU Hdg = 0.31
2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.03
2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.32
2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.02
2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.31
2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.04
2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.33

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x13
2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.08
2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.35
2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.09
2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.38
2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.11
2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.37
2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.11
2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.40
2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.15
2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.41

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x13
2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.16
2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.42
2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.18
2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.43
2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.19
2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.43
2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.22
2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.47
2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.24
2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.47

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x13
2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.25
2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.49
2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.25
2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.47
2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.24
2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.52
2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.30
2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.52
2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.30
2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.52

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x00
2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.33
2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.52
2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.33
2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.53
2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.33
2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.54
2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.36
2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.56
2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.39
2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.59

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x00 0x00
2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.43
2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.64
2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.48
2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.65
2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.52
2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.73
2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.60
2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.77
2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.64
2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.80

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.66
2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.80
2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.69
2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.81
2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.69
2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.85
2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.73
2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.85
2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.76
2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.88

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.80
2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.91
2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.81
2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.91
2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.82
2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.93
2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.84
2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.92
2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.82
2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.94

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.83
2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.92
2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.83
2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.94
2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.87
2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.96
2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.89
2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.98
2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.94
2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.99

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.91
2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.97
2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.91
2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.99
2018/5/6 (Sunday) 22:6:14 IMU Hdg = 0.96
2018/5/6 (Sunday) 22:6:14 IMU Hdg = 1.01
2018/5/6 (Sunday) 22:6:14 IMU Hdg = 0.98
2018/5/6 (Sunday) 22:6:14 IMU Hdg = 1.02
2018/5/6 (Sunday) 22:6:14 IMU Hdg = 0.95
2018/5/6 (Sunday) 22:6:15 IMU Hdg = 0.99

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:15 IMU Hdg = 0.99
2018/5/6 (Sunday) 22:6:15 IMU Hdg = 1.04
2018/5/6 (Sunday) 22:6:15 IMU Hdg = 1.00
2018/5/6 (Sunday) 22:6:15 IMU Hdg = 1.05
2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.07
2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.07
2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.04
2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.08
2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.07
2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.10

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.09
2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.12
2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.13
2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.14
2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.17
2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.18
2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.20
2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.18
2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.21
2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.20

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.25
2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.22
2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.27
2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.26
2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.32
2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.27
2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.33
2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.28
2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.34
2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.31

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.36
2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.33
2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.39
2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.33
2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.39
2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.32
2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.39
2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.33
2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.41
2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.36

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.45
2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.38
2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.48
2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.39
2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.45
2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.39
2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.48
2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.40
2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.54
2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.44

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.54
2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.44
2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.59
2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.49
2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.61
2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.48
2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.61
2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.48
2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.62
2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.47

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.64
2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.51
2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.65
2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.50
2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.64
2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.50
2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.68
2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.52
2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.71
2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.55

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.74
2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.59
2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.77
2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.62
2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.82
2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.65
2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.82
2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.72
2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.89
2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.74

0x0: 0x9 0x00 0x00 0x3 0x4 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.95
2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.81
2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.97
2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.87
2018/5/6 (Sunday) 22:6:32 IMU Hdg = 2.04
2018/5/6 (Sunday) 22:6:32 IMU Hdg = 1.88
2018/5/6 (Sunday) 22:6:32 IMU Hdg = 2.06
2018/5/6 (Sunday) 22:6:32 IMU Hdg = 1.93
2018/5/6 (Sunday) 22:6:32 IMU Hdg = 2.07
2018/5/6 (Sunday) 22:6:33 IMU Hdg = 1.95

0x0: 0x9 0x00 0x00 0x3 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:33 IMU Hdg = 2.11
2018/5/6 (Sunday) 22:6:33 IMU Hdg = 1.96
2018/5/6 (Sunday) 22:6:33 IMU Hdg = 2.11
2018/5/6 (Sunday) 22:6:33 IMU Hdg = 1.97
2018/5/6 (Sunday) 22:6:34 IMU Hdg = 2.11
2018/5/6 (Sunday) 22:6:34 IMU Hdg = 1.97
2018/5/6 (Sunday) 22:6:34 IMU Hdg = 2.11
2018/5/6 (Sunday) 22:6:34 IMU Hdg = 1.93
2018/5/6 (Sunday) 22:6:34 IMU Hdg = 2.14
2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.01

0x0: 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.17
2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.00
2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.21
2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.09
2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.26
2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.13
2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.27
2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.09
2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.30
2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.12

0x0: 0x9 0x00 0x00 0x3 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.32
2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.17
2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.38
2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.27
2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.41
2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.29
2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.45
2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.31
2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.49
2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.32

0x0: 0x9 0x00 0x00 0x3 0x4 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.49
2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.34
2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.52
2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.37
2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.56
2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.46
2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.59
2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.45
2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.61
2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.51

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.64
2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.50
2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.64
2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.48
2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.67
2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.58
2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.71
2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.61
2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.72
2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.62

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.73
2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.64
2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.75
2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.61
2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.76
2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.57
2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.74
2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.57
2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.74
2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.55

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.73
2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.58
2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.78
2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.65
2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.80
2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.67
2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.82
2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.73
2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.85
2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.76

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.91
2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.79
2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.95
2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.86
2018/5/6 (Sunday) 22:6:48 IMU Hdg = 3.00
2018/5/6 (Sunday) 22:6:48 IMU Hdg = 2.90
2018/5/6 (Sunday) 22:6:48 IMU Hdg = 3.03
2018/5/6 (Sunday) 22:6:48 IMU Hdg = 2.95
2018/5/6 (Sunday) 22:6:48 IMU Hdg = 3.05
2018/5/6 (Sunday) 22:6:49 IMU Hdg = 2.99

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.08
2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.06
2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.11
2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.07
2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.09
2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.07
2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.13
2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.08
2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.11
2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.05

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.13
2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.07
2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.18
2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.10
2018/5/6 (Sunday) 22:6:52 IMU Hdg = 3.20
2018/5/6 (Sunday) 22:6:52 IMU Hdg = 3.13
2018/5/6 (Sunday) 22:6:52 IMU Hdg = 3.26

Port closed

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

Opening port

Port open

Starting program, Scanning I2C addresses...

Scanning...

I2C device found at address 0x0C !

I2C device found at address 0x50 !

I2C device found at address 0x68 !

I2C device found at address 0x69 !

I2C device found at address 0x7C !

done

Initializing 9250 IMU...

MPU9250 I AM 71 I should be 71

MPU9250 is online - doing self-test...

Starting gyro/accel calibration - keep IMU motionless until finished...

gyro/accel calibration complete. Calibration took 381 Msec

MPU9250 initialized for active data mode....

Done...

Found I2C FRAM

Restarted 8 times

Displaying contents of first 50 memory locations...

0x0: 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

0x20: 0x00 0x00 0x00 0x00 0x00 0xBE 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

In Loop()...

2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.79

2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.62

2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.42

2018/5/6 (Sunday) 22:5:23 IMU Hdg = 21.18

2018/5/6 (Sunday) 22:5:24 IMU Hdg = 20.93

2018/5/6 (Sunday) 22:5:24 IMU Hdg = 20.64

2018/5/6 (Sunday) 22:5:24 IMU Hdg = 20.33

2018/5/6 (Sunday) 22:5:24 IMU Hdg = 19.98

2018/5/6 (Sunday) 22:5:24 IMU Hdg = 19.57

2018/5/6 (Sunday) 22:5:25 IMU Hdg = 19.12

0x0: 0x9 0x00 0x00 0x3 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:25 IMU Hdg = 18.59

2018/5/6 (Sunday) 22:5:25 IMU Hdg = 18.02

2018/5/6 (Sunday) 22:5:25 IMU Hdg = 17.33

2018/5/6 (Sunday) 22:5:25 IMU Hdg = 16.59

2018/5/6 (Sunday) 22:5:26 IMU Hdg = 15.75

2018/5/6 (Sunday) 22:5:26 IMU Hdg = 14.84

2018/5/6 (Sunday) 22:5:26 IMU Hdg = 13.78

2018/5/6 (Sunday) 22:5:26 IMU Hdg = 12.59

2018/5/6 (Sunday) 22:5:26 IMU Hdg = 11.29

2018/5/6 (Sunday) 22:5:27 IMU Hdg = 9.86

0x0: 0x9 0x00 0x00 0x3 0x4 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:27 IMU Hdg = 8.32

2018/5/6 (Sunday) 22:5:27 IMU Hdg = 6.70

2018/5/6 (Sunday) 22:5:27 IMU Hdg = 5.00

2018/5/6 (Sunday) 22:5:27 IMU Hdg = 3.32

2018/5/6 (Sunday) 22:5:28 IMU Hdg = 1.71

2018/5/6 (Sunday) 22:5:28 IMU Hdg = 0.33

2018/5/6 (Sunday) 22:5:28 IMU Hdg = -0.70

2018/5/6 (Sunday) 22:5:28 IMU Hdg = -1.16

2018/5/6 (Sunday) 22:5:28 IMU Hdg = -0.92

2018/5/6 (Sunday) 22:5:29 IMU Hdg = -1.12

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:29 IMU Hdg = -0.92

2018/5/6 (Sunday) 22:5:29 IMU Hdg = -1.06

2018/5/6 (Sunday) 22:5:29 IMU Hdg = -0.78

2018/5/6 (Sunday) 22:5:29 IMU Hdg = -1.03

2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.79

2018/5/6 (Sunday) 22:5:30 IMU Hdg = -1.01

2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.75

2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.94

2018/5/6 (Sunday) 22:5:30 IMU Hdg = -0.68

2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.93

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.66

2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.89

2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.62

2018/5/6 (Sunday) 22:5:31 IMU Hdg = -0.87

2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.62

2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.84

2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.62

2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.83

2018/5/6 (Sunday) 22:5:32 IMU Hdg = -0.62

2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.83

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.54

2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.80

2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.58

2018/5/6 (Sunday) 22:5:33 IMU Hdg = -0.76

2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.52

2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.73

2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.49

2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.70

2018/5/6 (Sunday) 22:5:34 IMU Hdg = -0.44

2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.69

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.45

2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.68

2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.39

2018/5/6 (Sunday) 22:5:35 IMU Hdg = -0.67

2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.43

2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.63

2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.36

2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.59

2018/5/6 (Sunday) 22:5:36 IMU Hdg = -0.36

2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.56

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.42

2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.57

2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.45

2018/5/6 (Sunday) 22:5:37 IMU Hdg = -0.56

2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.43

2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.52

2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.41

2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.49

2018/5/6 (Sunday) 22:5:38 IMU Hdg = -0.33

2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.44

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.32

2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.46

2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.41

2018/5/6 (Sunday) 22:5:39 IMU Hdg = -0.47

2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.42

2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.45

2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.41

2018/5/6 (Sunday) 22:5:40 IMU Hdg = -0.37

2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.38

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.34

2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.37

2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.30

2018/5/6 (Sunday) 22:5:41 IMU Hdg = -0.35

2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.32

2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.33

2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.32

2018/5/6 (Sunday) 22:5:42 IMU Hdg = -0.34

2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.32

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.34

2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.30

2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.37

2018/5/6 (Sunday) 22:5:43 IMU Hdg = -0.26

2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.29

2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.23

2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.34

2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.20

2018/5/6 (Sunday) 22:5:44 IMU Hdg = -0.28

2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.17

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.27

2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.15

2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.26

2018/5/6 (Sunday) 22:5:45 IMU Hdg = -0.12

2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.23

2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.08

2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.18

2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.10

2018/5/6 (Sunday) 22:5:46 IMU Hdg = -0.26

2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.09

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.26

2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.12

2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.35

2018/5/6 (Sunday) 22:5:47 IMU Hdg = -0.14

2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.39

2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.12

2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.34

2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.07

2018/5/6 (Sunday) 22:5:48 IMU Hdg = -0.33

2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.07

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.33

2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.02

2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.27

2018/5/6 (Sunday) 22:5:49 IMU Hdg = -0.01

2018/5/6 (Sunday) 22:5:50 IMU Hdg = -0.27

2018/5/6 (Sunday) 22:5:50 IMU Hdg = 0.00

2018/5/6 (Sunday) 22:5:50 IMU Hdg = -0.26

2018/5/6 (Sunday) 22:5:50 IMU Hdg = 0.04

2018/5/6 (Sunday) 22:5:50 IMU Hdg = -0.18

2018/5/6 (Sunday) 22:5:51 IMU Hdg = 0.09

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x00 0x00 0x00

2018/5/6 (Sunday) 22:5:51 IMU Hdg = -0.16

2018/5/6 (Sunday) 22:5:51 IMU Hdg = 0.13

2018/5/6 (Sunday) 22:5:51 IMU Hdg = -0.15

2018/5/6 (Sunday) 22:5:51 IMU Hdg = 0.14

2018/5/6 (Sunday) 22:5:52 IMU Hdg = -0.12

2018/5/6 (Sunday) 22:5:52 IMU Hdg = 0.16

2018/5/6 (Sunday) 22:5:52 IMU Hdg = -0.11

2018/5/6 (Sunday) 22:5:52 IMU Hdg = 0.17

2018/5/6 (Sunday) 22:5:52 IMU Hdg = -0.11

2018/5/6 (Sunday) 22:5:53 IMU Hdg = 0.15

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x00 0x00

2018/5/6 (Sunday) 22:5:53 IMU Hdg = -0.13

2018/5/6 (Sunday) 22:5:53 IMU Hdg = 0.18

2018/5/6 (Sunday) 22:5:53 IMU Hdg = -0.10

2018/5/6 (Sunday) 22:5:53 IMU Hdg = 0.23

2018/5/6 (Sunday) 22:5:54 IMU Hdg = -0.05

2018/5/6 (Sunday) 22:5:54 IMU Hdg = 0.26

2018/5/6 (Sunday) 22:5:54 IMU Hdg = -0.07

2018/5/6 (Sunday) 22:5:54 IMU Hdg = 0.26

2018/5/6 (Sunday) 22:5:54 IMU Hdg = -0.04

2018/5/6 (Sunday) 22:5:55 IMU Hdg = 0.25

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x00

2018/5/6 (Sunday) 22:5:55 IMU Hdg = -0.05

2018/5/6 (Sunday) 22:5:55 IMU Hdg = 0.26

2018/5/6 (Sunday) 22:5:55 IMU Hdg = -0.01

2018/5/6 (Sunday) 22:5:55 IMU Hdg = 0.31

2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.03

2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.32

2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.02

2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.31

2018/5/6 (Sunday) 22:5:56 IMU Hdg = 0.04

2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.33

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x13

2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.08

2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.35

2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.09

2018/5/6 (Sunday) 22:5:57 IMU Hdg = 0.38

2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.11

2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.37

2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.11

2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.40

2018/5/6 (Sunday) 22:5:58 IMU Hdg = 0.15

2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.41

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x13

2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.16

2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.42

2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.18

2018/5/6 (Sunday) 22:5:59 IMU Hdg = 0.43

2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.19

2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.43

2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.22

2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.47

2018/5/6 (Sunday) 22:6:0 IMU Hdg = 0.24

2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.47

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x13

2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.25

2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.49

2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.25

2018/5/6 (Sunday) 22:6:1 IMU Hdg = 0.47

2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.24

2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.52

2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.30

2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.52

2018/5/6 (Sunday) 22:6:2 IMU Hdg = 0.30

2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.52

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x12 0x00

2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.33

2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.52

2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.33

2018/5/6 (Sunday) 22:6:3 IMU Hdg = 0.53

2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.33

2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.54

2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.36

2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.56

2018/5/6 (Sunday) 22:6:4 IMU Hdg = 0.39

2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.59

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x11 0x00 0x00

2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.43

2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.64

2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.48

2018/5/6 (Sunday) 22:6:5 IMU Hdg = 0.65

2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.52

2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.73

2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.60

2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.77

2018/5/6 (Sunday) 22:6:6 IMU Hdg = 0.64

2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.80

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.66

2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.80

2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.69

2018/5/6 (Sunday) 22:6:7 IMU Hdg = 0.81

2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.69

2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.85

2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.73

2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.85

2018/5/6 (Sunday) 22:6:8 IMU Hdg = 0.76

2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.88

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.80

2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.91

2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.81

2018/5/6 (Sunday) 22:6:9 IMU Hdg = 0.91

2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.82

2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.93

2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.84

2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.92

2018/5/6 (Sunday) 22:6:10 IMU Hdg = 0.82

2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.94

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.83

2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.92

2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.83

2018/5/6 (Sunday) 22:6:11 IMU Hdg = 0.94

2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.87

2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.96

2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.89

2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.98

2018/5/6 (Sunday) 22:6:12 IMU Hdg = 0.94

2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.99

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.91

2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.97

2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.91

2018/5/6 (Sunday) 22:6:13 IMU Hdg = 0.99

2018/5/6 (Sunday) 22:6:14 IMU Hdg = 0.96

2018/5/6 (Sunday) 22:6:14 IMU Hdg = 1.01

2018/5/6 (Sunday) 22:6:14 IMU Hdg = 0.98

2018/5/6 (Sunday) 22:6:14 IMU Hdg = 1.02

2018/5/6 (Sunday) 22:6:14 IMU Hdg = 0.95

2018/5/6 (Sunday) 22:6:15 IMU Hdg = 0.99

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:15 IMU Hdg = 0.99

2018/5/6 (Sunday) 22:6:15 IMU Hdg = 1.04

2018/5/6 (Sunday) 22:6:15 IMU Hdg = 1.00

2018/5/6 (Sunday) 22:6:15 IMU Hdg = 1.05

2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.07

2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.04

2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.08

2018/5/6 (Sunday) 22:6:16 IMU Hdg = 1.07

2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.10

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.09

2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.12

2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.13

2018/5/6 (Sunday) 22:6:17 IMU Hdg = 1.14

2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.17

2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.18

2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.20

2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.18

2018/5/6 (Sunday) 22:6:18 IMU Hdg = 1.21

2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.20

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.25

2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.22

2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.27

2018/5/6 (Sunday) 22:6:19 IMU Hdg = 1.26

2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.32

2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.27

2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.33

2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.28

2018/5/6 (Sunday) 22:6:20 IMU Hdg = 1.34

2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.31

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.36

2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.33

2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.39

2018/5/6 (Sunday) 22:6:21 IMU Hdg = 1.33

2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.39

2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.32

2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.39

2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.33

2018/5/6 (Sunday) 22:6:22 IMU Hdg = 1.41

2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.36

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.45

2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.38

2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.48

2018/5/6 (Sunday) 22:6:23 IMU Hdg = 1.39

2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.45

2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.39

2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.48

2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.40

2018/5/6 (Sunday) 22:6:24 IMU Hdg = 1.54

2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.44

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.54

2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.44

2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.59

2018/5/6 (Sunday) 22:6:25 IMU Hdg = 1.49

2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.61

2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.48

2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.61

2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.48

2018/5/6 (Sunday) 22:6:26 IMU Hdg = 1.62

2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.47

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.64

2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.51

2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.65

2018/5/6 (Sunday) 22:6:27 IMU Hdg = 1.50

2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.64

2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.50

2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.68

2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.52

2018/5/6 (Sunday) 22:6:28 IMU Hdg = 1.71

2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.55

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.74

2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.59

2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.77

2018/5/6 (Sunday) 22:6:29 IMU Hdg = 1.62

2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.82

2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.65

2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.82

2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.72

2018/5/6 (Sunday) 22:6:30 IMU Hdg = 1.89

2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.74

0x0: 0x9 0x00 0x00 0x3 0x4 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.95

2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.81

2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.97

2018/5/6 (Sunday) 22:6:31 IMU Hdg = 1.87

2018/5/6 (Sunday) 22:6:32 IMU Hdg = 2.04

2018/5/6 (Sunday) 22:6:32 IMU Hdg = 1.88

2018/5/6 (Sunday) 22:6:32 IMU Hdg = 2.06

2018/5/6 (Sunday) 22:6:32 IMU Hdg = 1.93

2018/5/6 (Sunday) 22:6:32 IMU Hdg = 2.07

2018/5/6 (Sunday) 22:6:33 IMU Hdg = 1.95

0x0: 0x9 0x00 0x00 0x3 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:33 IMU Hdg = 2.11

2018/5/6 (Sunday) 22:6:33 IMU Hdg = 1.96

2018/5/6 (Sunday) 22:6:33 IMU Hdg = 2.11

2018/5/6 (Sunday) 22:6:33 IMU Hdg = 1.97

2018/5/6 (Sunday) 22:6:34 IMU Hdg = 2.11

2018/5/6 (Sunday) 22:6:34 IMU Hdg = 1.97

2018/5/6 (Sunday) 22:6:34 IMU Hdg = 2.11

2018/5/6 (Sunday) 22:6:34 IMU Hdg = 1.93

2018/5/6 (Sunday) 22:6:34 IMU Hdg = 2.14

2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.01

0x0: 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.17

2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.00

2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.21

2018/5/6 (Sunday) 22:6:35 IMU Hdg = 2.09

2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.26

2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.13

2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.27

2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.09

2018/5/6 (Sunday) 22:6:36 IMU Hdg = 2.30

2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.12

0x0: 0x9 0x00 0x00 0x3 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.32

2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.17

2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.38

2018/5/6 (Sunday) 22:6:37 IMU Hdg = 2.27

2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.41

2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.29

2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.45

2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.31

2018/5/6 (Sunday) 22:6:38 IMU Hdg = 2.49

2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.32

0x0: 0x9 0x00 0x00 0x3 0x4 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.49

2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.34

2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.52

2018/5/6 (Sunday) 22:6:39 IMU Hdg = 2.37

2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.56

2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.46

2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.59

2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.45

2018/5/6 (Sunday) 22:6:40 IMU Hdg = 2.61

2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.51

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.64

2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.50

2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.64

2018/5/6 (Sunday) 22:6:41 IMU Hdg = 2.48

2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.67

2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.58

2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.71

2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.61

2018/5/6 (Sunday) 22:6:42 IMU Hdg = 2.72

2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.62

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.73

2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.64

2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.75

2018/5/6 (Sunday) 22:6:43 IMU Hdg = 2.61

2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.76

2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.57

2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.74

2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.57

2018/5/6 (Sunday) 22:6:44 IMU Hdg = 2.74

2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.55

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.73

2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.58

2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.78

2018/5/6 (Sunday) 22:6:45 IMU Hdg = 2.65

2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.80

2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.67

2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.82

2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.73

2018/5/6 (Sunday) 22:6:46 IMU Hdg = 2.85

2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.76

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.91

2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.79

2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.95

2018/5/6 (Sunday) 22:6:47 IMU Hdg = 2.86

2018/5/6 (Sunday) 22:6:48 IMU Hdg = 3.00

2018/5/6 (Sunday) 22:6:48 IMU Hdg = 2.90

2018/5/6 (Sunday) 22:6:48 IMU Hdg = 3.03

2018/5/6 (Sunday) 22:6:48 IMU Hdg = 2.95

2018/5/6 (Sunday) 22:6:48 IMU Hdg = 3.05

2018/5/6 (Sunday) 22:6:49 IMU Hdg = 2.99

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.08

2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.06

2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.11

2018/5/6 (Sunday) 22:6:49 IMU Hdg = 3.07

2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.09

2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.07

2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.13

2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.08

2018/5/6 (Sunday) 22:6:50 IMU Hdg = 3.11

2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.05

0x0: 0x9 0x00 0x00 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.13

2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.07

2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.18

2018/5/6 (Sunday) 22:6:51 IMU Hdg = 3.10

2018/5/6 (Sunday) 22:6:52 IMU Hdg = 3.20

2018/5/6 (Sunday) 22:6:52 IMU Hdg = 3.13

2018/5/6 (Sunday) 22:6:52 IMU Hdg = 3.26

Port closed

As shown in the above output, all three modules are initialized, and then the program enters a loop where the current time is displayed from the RTC, and the current IMU heading is displayed from the IMU. Then every 10 times through the loop, a new value is written to the FRAM until the first 20 or so locations have been written. Then the program starts ‘un-writing’ the values until all the written locations have been cleared, at which point the cycle starts all over again.

So, now that I have conclusively demonstrated the ability to add (relative) heading and FRAM-based non-volatile power-cycle date/time recording to Wall-E2, the next trick will be to actually mount the modules on the robot. This will entail solving yet another set of problems, as it turns out (naturally) that although I have plenty of room on Wall-E2’s second deck, I have run out of available pins on the inter-deck connector. This could be addressed by putting the modules somewhere on the first deck, but finding that ‘somewhere’ is going to be a real trick. Time for either a second inter-deck connector, or to replace the current 8-pin model with a larger one.

10 May 2018 Update:

After verifying the proper operation of all three modules one one of my new high-quality ASP protoboards, I transferred everything to a permanent perfboard rendition more suitable for mounting on Wall-E2’s second deck, as shown below

Stay tuned!

Frank

Another try at heading information for Wall-E2

2 Replies

Posted 24 April 2018

A little over two years ago I started a project to give Wall-E2 ‘a sense of direction‘ by integrating a Mongoose 9DOF IMU board onto the robot chassis. I worked on the idea for about six months before I finally determined that the magnetometer idea was not going to work in my indoor environment, as there was just too much magnetic interference due to indoor wiring, air handling motors, and the like.

However, it recently occurred to me that although the absolute magnetic heading problem was intractable, I might be able to use one of the newer single-board IMU products (like the Sparkfun 9250) to generate relative heading information, which I could use to solve a different problem. Wall-E2 occasionally needs to perform sharp turns, on the order of 90 º, either as part of what I call an ‘open room step turn’ (like what happens when Wall-E2 exits a hallway into an open room, and needs to turn 90 º in one direction or the other to continue wall-following), or as part of an evasion maneuver or as part of a recovery from a stuck condition. In my ‘field’ testing to date I have noticed that Wall-E2’s effective turn rate varies considerably depending on the surface condition; very slow on shag carpeting, OK on tightly-woven rugs, and very fast on hard flooring. This means that my current strategy of timed turns only works OK on the medium surfaces, but sucks the big weenie on the other two types. So, if I had even relative heading information, I could use it to make sure Wall-E2’s 90 º turns are actually 90 º, as opposed to 45 º or 180 º.

It turns out there are a lot of single-board IMU solutions out there new, probably due to the wildly popular quad-copter market; they all need attitude-control (AHRS) module of some kind to make them flyable, and those modules need to be small and lightweight. The one I started playing with first is the Sparkfun MPU-9250 breakout board, with 3-axis accelerometer, gyro, and magnetometer capability in a really small package for an incredibly cheap price. And, as a bonus, there is a lot of good Arduino/Teensyduino software out there to help us mere mortals run the thing. In particular, Kris Winer (of Pesky Products fame) has done a lot of work with sensor fusion software for the Arduino and Teensy line, and actively supports both his products and his software.

So, I got a board from Sparkfun and started playing around with it. The first problem I ran into is that the MPU9250 is a 3.3V board, so I couldn’t run it from an Arduino without either potentially frying the board or having to implement a bidirectional level shifter. So, I grabbed a Teensy 3.2 from my stash and used it instead. This, in turn, required some judicious sifting through the available software to find the Teensy 3.x compatible versions, but I eventually got that accomplished and started collecting data.

As usual, there were a lot of mis-steps along the way. The biggest obstacle was figuring out how to avoid using the magnetometer data; almost all the software, including the all-important Madgwick quaternion routine for converting raw sensor data into yaw/pitch/roll values assumes the use of magnetometer data to obtain geo-magnetically referenced heading and to minimize/eliminate gyro drift. In my case, I specifically wanted to decouple heading calculations from magnetometer input, as I already knew the magnetic environment in my house was too variable for reliable measurements. With some guidance from Kris, I eventually found a 6DOF (3D rate gyro and 3D accelerometer) version of the Madgwick routine.

Once I started getting reliable yaw (heading) measurements from the sensor, I decided to modify my Teensy-based stepper motor rotary table measurement system to rotate the sensor in a uniform way to obtain constant-rate heading scan data. The basic idea of the rotary table system is to rotate the unit under test X degrees, then stop, take some measurements, and then repeat. So, I got it all set up, and got the following plot.

This data made no sense at all – for a full 360 º rotation I should have recorded a full 360 º heading change, regardless of the rate of rotation. Instead, the sensor seemed numb to rotation rates below about 20 rpm (120dps) and even at 60 rpm (360 dps) I wasn’t seeing a full 360 º rotation – what the heck??

After beating on this problem for waaaayyy too long, it finally occurred to me that my test program had a fatal flaw; my measurement system stepped to each desired heading, then stopped, took a heading measurement, and then moved on to the next heading. The operative word here being stopped. While this works great for units under test whose performance varies with rotation, the Sparkfun sensor performance varies with rotation rate, not the rotation position! Well, doh – it’s an accelerometer! So, my carefully assembled test setup wasn’t measuring the sensor’s rotation rate at all – the rotation rate was being reduced to almost zero at each measurement position – oops!

After this “aha!” (or maybe, “doh!”) moment, I realized I needed to modify my rotary test system to make sure that heading data acquisition from the sensor was accomplished ‘on the fly’. Once I did this, I started getting more reasonable data, like the following plot:

Nice, linear data – great!

At this point I started thinking about how I could integrate this capability into my robot to monitor/manage turn operations, and thought that I could maybe average the first few heading values before the start of a turn to create a stable reference to be used to terminate the turn appropriately. However, when I tried this trick, with a 10-sample average at the start, I got the following plot

I performed the ‘with’ and ‘without’ initial measurement average experiments several times, and convinced myself that the above phenomenon was real, but I still have no real idea why it occurs. It has to have something to due with the Madgwick quaternion manipulations, as that is the only place in the entire system where there is any state memory (the 4-element quaternion array itself). For my purposes, it was sufficient to realize I couldn’t do what I wanted to do, and to “run away!” from this idea.

Next, I modified my stepper motor rotator test system again to simulate the process of setting up a turn direction and angle and terminating the turn when the correct turn angle change had been achieved. Here’s a short video of the result, where the sensor is turned through +/- 30, 60, 90, and 180 º angles. The motor is run at a constant rate until the target angle is approached, and then run at 1/5 normal speed to fine-tune turn termination.

So, at this point I confident that I can use the Sparkfun MPU9250 9DOF sensor (used in 6DOF mode) to accurately turn my robot – cool!

With the addition of the Sparkfun accelerometer and the FRAM/RTC combination to his sensor suite, Wall-E2 is set to get significantly smarter; he’ll be able to remember when he was last turned off so he can more accurately report run times and charge times, and he’ll be able to make real 90 º turns instead of having to fake it with timing. Now if I could only get him to take out the trash! ;-).

Stay tuned,

Frank

The Evolution of an Outside Shot – Part V

May 2018 Update:

Here is a short clip taken on my home court, showing a couple of nice shots in slo-mo, taken with my new (to me) iphone SE.

Capture Power Loss Data Using Adafruit FRAM Breakout Board

2 Replies

Posted 06 April 2018,

As an enhancement to Wall-E2, my wall-following robot I wanted a way to capture the run time at power-down so I could determine how long it has been since Wall-E2 last charged its battery, even through power cycles.

At first I tried to do this using the Arduino Mega 2560’s onboard EEPROM, but this proved infeasible, as the time required to write to EEPROM exceeded the time available from the time power was removed to the time the Mega died. I played around with extra capacitance on the power bus, but this still wasn’t enough to hold the power up long enough to write to EEPROM. And, even if I finally succeeded, there was still the problem of EEPROM wear-out to deal with.

So, after some more research time with Google, I found this nice Adafruit FRAM breakout board, featuring the Fujitsu M85RC256V 32KB FRAM part

Adafruit provides a simple, but effective I2C interface library and an example sketch, and I was able to use these with a spare Arduino Uno board to verify that I could indeed write to and read from the FRAM. However, what I really wanted to determine was whether or not the FRAM was fast enough to allow me to write data to it after power was removed from the Arduino but before the processor actually died.

I did some poking around in the Arduinio Uno schematic and determined there was a 47Î¼F capacitor on the output of the +5V voltage regulator, and so I thought I might be able to trigger off power loss at the input to the regulator and accomplish the writes while the capacitor was still holding up the Arduino processor. I put a 1/3:2/3 voltage divider on the +12V input line and used my trusty Tektronix 2236 scope to monitor it and the +5V regulated output. With the scope I was able to see that the +5V output stayed up for about 5msec, and stayed above 4V for 8msec, as shown in the following scope photo.

Scope photo of Arduino Uno +5V regulated output, triggered by removal of +12V input power. Time scale 1msec/div.

The next step was to modify Adafruit’s FRAM read/write example program to implement a power-down interrupt routine to test my idea. Here’s the code (the software is also available here)

#include <Wire.h>
#include "MyAdafruit_FRAM_I2C.h"

/* Example code for the Adafruit I2C FRAM breakout */

/* Connect SCL    to analog 5
Connect SDA    to analog 4
Connect VDD    to 5.0V DC
Connect GROUND to common ground */

MyAdafruit_FRAM_I2C fram = MyAdafruit_FRAM_I2C();
uint16_t          framAddr = 0;
volatile uint8_t testval;
const int InterruptPin = 2;
const int RestartCountAddr = 0x0;
volatile byte state = LOW;
byte last_state = state;
const byte ledPin = 13;
const int CLEAR_FRAM_PIN = 3; 
const int NUM_LOCS_TO_CLEAR = 100;
int nextFramWriteAddr = 2; //addr 0 reserved for restart count, addr 1 is unusable (don't know why)
const int NUM_TIMES_TO_DISPLAY = 10;
bool InSetup = true;

void setup(void) 
{
	Serial.begin(2000000);

	if (fram.begin()) {  // you can stick the new i2c addr in here, e.g. begin(0x51);
		Serial.println("Found I2C FRAM");
	}
	else {
		Serial.println("I2C FRAM not identified ... check your connections?\r\n");
		Serial.println("Will continue in case this processor doesn't support repeated start\r\n");
	}

	// Read the first byte
	fram.FRAM_I2C_readAnything(RestartCountAddr, testval);
	Serial.print("Restarted "); Serial.print(testval); Serial.println(" times");
	fram.FRAM_I2C_writeAnything(RestartCountAddr, testval + 1);

	Serial.println("Displaying contents of first 50 memory locations... "); 

	uint8_t value;
	for (uint16_t a = 0; a < 50; a++) 
	{
		fram.FRAM_I2C_readAnything(a, value);
		if ((a % 32) == 0) {
			Serial.print("\n 0x"); Serial.print(a, HEX); Serial.print(": ");
		}
		Serial.print("0x");
		if (value < 0x1)
			Serial.print('0');
		Serial.print(value, HEX); Serial.print(" ");
	}
	Serial.println();

	pinMode(InterruptPin, INPUT_PULLUP);
	pinMode(ledPin, OUTPUT);
	pinMode(CLEAR_FRAM_PIN, INPUT_PULLUP);
	int intnum = digitalPinToInterrupt(InterruptPin);
	Serial.print("Using interrupt number "); Serial.println(intnum);
	attachInterrupt(intnum, FRAM_ISR, FALLING);
	interrupts();

	//optionally clear FRAM locations
	if (digitalRead(CLEAR_FRAM_PIN) == LOW)
	{
		Serial.print("Clearing "); Serial.print(NUM_LOCS_TO_CLEAR); Serial.println(" FRAM addresses...");
		byte clearval = 0;
		for (size_t i = 0; i < NUM_LOCS_TO_CLEAR; i++)
		{
			fram.FRAM_I2C_writeAnything(i, clearval);
		}

		//now read them back
		DisplayMemory(NUM_LOCS_TO_CLEAR);
	}
	else
	{
		DisplayTimes(nextFramWriteAddr, NUM_TIMES_TO_DISPLAY);
	}

	Serial.print("Enabling Power-down ISR, Entering Loop() with millis() = "); Serial.println(millis());
	InSetup = false; //enable power-down ISR
}

void loop(void) 
{
	digitalWrite(ledPin, state);
	if (state != last_state)
	{
		Serial.println("Changed...");
		last_state = state;
		DisplayMemory(32);
		DisplayTimes(nextFramWriteAddr, 10);
	}

	delay(100);
	//Serial.print(millis()); Serial.print(": nextFramWriteAddr = "); Serial.println(nextFramWriteAddr);
}

void DisplayMemory(int lastAddr)
{
	Serial.print("\nDisplaying memory locations 0 to "); Serial.println(lastAddr);
	uint8_t value;
	for (uint16_t a = 0; a < lastAddr; a++)
	{
		//value = fram.read8(a);
		fram.FRAM_I2C_readAnything(a, value);
		if ((a % 16) == 0)
		{
			Serial.print("\n 0x"); Serial.print(a, HEX); Serial.print(": ");
		}
		Serial.print("0x");
		if (value < 0x1)
			Serial.print('0');
		Serial.print(value, HEX); Serial.print(" ");
	}
	Serial.println();
}
void DisplayTimes(int startaddr, int numtimes)
{
	Serial.print("\nDisplaying first "); Serial.print(numtimes); Serial.print(" stored times starting at "); Serial.println(nextFramWriteAddr);
	long value = 0;
	for (uint16_t i = 0; i < numtimes; i++)
	{
		//value = fram.read8(a);
		int addr = nextFramWriteAddr + i * sizeof(value);
		int numbytes = fram.FRAM_I2C_readAnything(addr, value); //starts at addr 1
		//Serial.print(i + 1); Serial.print(": "); Serial.println(value);
		Serial.print(addr); Serial.print(": "); Serial.println(value);
	}
}

void FRAM_ISR()
{
	static boolean IsRunning = false;
	if (IsRunning || InSetup) return; //we have been interrupted while already processing an interrupt - ignore this ocurrence
	IsRunning = true;

	//Depending on the interrupt source, you may need to clear the interrupt flag here. Most Arduino interrupts are self-clearing.
	state = !state;
	interrupts(); //re-enable interrupts so that interrupt-based functions can be used inside this function

	//'infinite' loop (that is, until the Uno dies....)
	while(1)
	{ 
		for (size_t i = 0; i < 1000; i++)
		{
			long timeMs = millis(); //interrupts are enabled, so this should work...
			int numbyteswritten = fram.FRAM_I2C_writeAnything(nextFramWriteAddr, timeMs);
			nextFramWriteAddr += numbyteswritten;
		}
	}
	noInterrupts(); //turn off interrupts so we can't be interrupted while resetting our special variable
	IsRunning = false;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

#include <Wire.h>

#include "MyAdafruit_FRAM_I2C.h"

/* Example code for the Adafruit I2C FRAM breakout */

/* Connect SCL to analog 5

Connect SDA to analog 4

Connect VDD to 5.0V DC

Connect GROUND to common ground */

MyAdafruit_FRAM_I2C fram = MyAdafruit_FRAM_I2C();

uint16_t framAddr = 0;

volatile uint8_t testval;

const int InterruptPin = 2;

const int RestartCountAddr = 0x0;

volatile byte state = LOW;

byte last_state = state;

const byte ledPin = 13;

const int CLEAR_FRAM_PIN = 3;

const int NUM_LOCS_TO_CLEAR = 100;

int nextFramWriteAddr = 2; //addr 0 reserved for restart count, addr 1 is unusable (don't know why)

const int NUM_TIMES_TO_DISPLAY = 10;

bool InSetup = true;

void setup(void)

{

Serial.begin(2000000);

if (fram.begin()) { // you can stick the new i2c addr in here, e.g. begin(0x51);

Serial.println("Found I2C FRAM");

}

else {

Serial.println("I2C FRAM not identified ... check your connections?\r\n");

Serial.println("Will continue in case this processor doesn't support repeated start\r\n");

}

// Read the first byte

fram.FRAM_I2C_readAnything(RestartCountAddr, testval);

Serial.print("Restarted "); Serial.print(testval); Serial.println(" times");

fram.FRAM_I2C_writeAnything(RestartCountAddr, testval + 1);

Serial.println("Displaying contents of first 50 memory locations... ");

uint8_t value;

for (uint16_t a = 0; a < 50; a++)

{

fram.FRAM_I2C_readAnything(a, value);

if ((a % 32) == 0) {

Serial.print("\n 0x"); Serial.print(a, HEX); Serial.print(": ");

}

Serial.print("0x");

if (value < 0x1)

Serial.print('0');

Serial.print(value, HEX); Serial.print(" ");

}

Serial.println();

pinMode(InterruptPin, INPUT_PULLUP);

pinMode(ledPin, OUTPUT);

pinMode(CLEAR_FRAM_PIN, INPUT_PULLUP);

int intnum = digitalPinToInterrupt(InterruptPin);

Serial.print("Using interrupt number "); Serial.println(intnum);

attachInterrupt(intnum, FRAM_ISR, FALLING);

interrupts();

//optionally clear FRAM locations

if (digitalRead(CLEAR_FRAM_PIN) == LOW)

{

Serial.print("Clearing "); Serial.print(NUM_LOCS_TO_CLEAR); Serial.println(" FRAM addresses...");

byte clearval = 0;

for (size_t i = 0; i < NUM_LOCS_TO_CLEAR; i++)

{

fram.FRAM_I2C_writeAnything(i, clearval);

}

//now read them back

DisplayMemory(NUM_LOCS_TO_CLEAR);

}

else

{

DisplayTimes(nextFramWriteAddr, NUM_TIMES_TO_DISPLAY);

}

Serial.print("Enabling Power-down ISR, Entering Loop() with millis() = "); Serial.println(millis());

InSetup = false; //enable power-down ISR

}

void loop(void)

{

digitalWrite(ledPin, state);

if (state != last_state)

{

Serial.println("Changed...");

last_state = state;

DisplayMemory(32);

DisplayTimes(nextFramWriteAddr, 10);

}

delay(100);

//Serial.print(millis()); Serial.print(": nextFramWriteAddr = "); Serial.println(nextFramWriteAddr);

}

void DisplayMemory(int lastAddr)

{

Serial.print("\nDisplaying memory locations 0 to "); Serial.println(lastAddr);

uint8_t value;

for (uint16_t a = 0; a < lastAddr; a++)

{

//value = fram.read8(a);

fram.FRAM_I2C_readAnything(a, value);

if ((a % 16) == 0)

{

Serial.print("\n 0x"); Serial.print(a, HEX); Serial.print(": ");

}

Serial.print("0x");

if (value < 0x1)

Serial.print('0');

Serial.print(value, HEX); Serial.print(" ");

}

Serial.println();

}

void DisplayTimes(int startaddr, int numtimes)

{

Serial.print("\nDisplaying first "); Serial.print(numtimes); Serial.print(" stored times starting at "); Serial.println(nextFramWriteAddr);

long value = 0;

for (uint16_t i = 0; i < numtimes; i++)

{

//value = fram.read8(a);

int addr = nextFramWriteAddr + i * sizeof(value);

int numbytes = fram.FRAM_I2C_readAnything(addr, value); //starts at addr 1

//Serial.print(i + 1); Serial.print(": "); Serial.println(value);

Serial.print(addr); Serial.print(": "); Serial.println(value);

}

void FRAM_ISR()

{

static boolean IsRunning = false;

if (IsRunning || InSetup) return; //we have been interrupted while already processing an interrupt - ignore this ocurrence

IsRunning = true;

//Depending on the interrupt source, you may need to clear the interrupt flag here. Most Arduino interrupts are self-clearing.

state = !state;

interrupts(); //re-enable interrupts so that interrupt-based functions can be used inside this function

//'infinite' loop (that is, until the Uno dies....)

while(1)

{

for (size_t i = 0; i < 1000; i++)

{

long timeMs = millis(); //interrupts are enabled, so this should work...

int numbyteswritten = fram.FRAM_I2C_writeAnything(nextFramWriteAddr, timeMs);

nextFramWriteAddr += numbyteswritten;

}

noInterrupts(); //turn off interrupts so we can't be interrupted while resetting our special variable

IsRunning = false;

}

The above code takes advantage of a modified version of Adafruit’s I2C FRAM library that facilitates writing and reading of arbitrary data types like int, long, float, etc. The modifications were cribbed from Nick Gammon’s wonderful ‘IC2_Anything’ library – thanks Nick!

Here’s the modified ‘Adafruit_FRAM_I2C.h’ file

/**************************************************************************/
/*! 
    @file     Adafruit_FRAM_I2C.h
    @author   KTOWN (Adafruit Industries)

    @section LICENSE

    Software License Agreement (BSD License)

    Copyright (c) 2013, Adafruit Industries
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
    1. Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
    3. Neither the name of the copyright holders nor the
    names of its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/**************************************************************************/
#ifndef _ADAFRUIT_FRAM_I2C_H_
#define _ADAFRUIT_FRAM_I2C_H_

#if ARDUINO >= 100
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif

#include <Wire.h>

#define MB85RC_DEFAULT_ADDRESS        (0x50) /* 1010 + A2 + A1 + A0 = 0x50 default */
#define MB85RC_SLAVE_ID       (0xF8)

class MyAdafruit_FRAM_I2C {
 public:
  MyAdafruit_FRAM_I2C(void);
  
  boolean  begin(uint8_t addr = MB85RC_DEFAULT_ADDRESS);
  void     write8 (uint16_t framAddr, uint8_t value);
  uint8_t  read8  (uint16_t framAddr);
  void     getDeviceID(uint16_t *manufacturerID, uint16_t *productID);

  //Additions to facilitate read/write of arbitrary data types
  //G. Frank Paynter, April 2018
  // Original I2C_Anything.h written by Nick Gammon, May 2012
  template <typename T> unsigned int FRAM_I2C_writeAnything(uint16_t framAddr, const T& value)
 {
	const byte *p = (const byte*)&value;
	unsigned int i;
	for (i = 0; i < sizeof value; i++)
	{
		write8(framAddr++,*p++);
	 }
	return i;
  }  // end of I2C_writeAnything

  template <typename T> unsigned int FRAM_I2C_readAnything(uint16_t framAddr, T& value)
  {
	  byte * p = (byte*)&value;
	  unsigned int i;
	  for (i = 0; i < sizeof value; i++)
		  *p++ = read8(framAddr++);
	  return i;
  }  // end of I2C_readAnything


 private:
  uint8_t i2c_addr;
  boolean _framInitialised;
};

#endif

/**************************************************************************/

/*!

@file Adafruit_FRAM_I2C.h

@author KTOWN (Adafruit Industries)

@section LICENSE

Software License Agreement (BSD License)

Redistribution and use in source and binary forms, with or without

modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright

notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright

notice, this list of conditions and the following disclaimer in the

documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holders nor the

names of its contributors may be used to endorse or promote products

derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY

EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY

DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/**************************************************************************/

#ifndef _ADAFRUIT_FRAM_I2C_H_

#define _ADAFRUIT_FRAM_I2C_H_

#if ARDUINO >= 100

#include <Arduino.h>

#else

#include <WProgram.h>

#endif

#include <Wire.h>

#define MB85RC_DEFAULT_ADDRESS (0x50) /* 1010 + A2 + A1 + A0 = 0x50 default */

#define MB85RC_SLAVE_ID (0xF8)

class MyAdafruit_FRAM_I2C {

public:

MyAdafruit_FRAM_I2C(void);

boolean begin(uint8_t addr = MB85RC_DEFAULT_ADDRESS);

void write8 (uint16_t framAddr, uint8_t value);

uint8_t read8 (uint16_t framAddr);

void getDeviceID(uint16_t *manufacturerID, uint16_t *productID);

//Additions to facilitate read/write of arbitrary data types

//G. Frank Paynter, April 2018

// Original I2C_Anything.h written by Nick Gammon, May 2012

template <typename T> unsigned int FRAM_I2C_writeAnything(uint16_t framAddr, const T& value)

{

const byte *p = (const byte*)&value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

{

write8(framAddr++,*p++);

}

return i;

} // end of I2C_writeAnything

template <typename T> unsigned int FRAM_I2C_readAnything(uint16_t framAddr, T& value)

{

byte * p = (byte*)&value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

*p++ = read8(framAddr++);

return i;

} // end of I2C_readAnything

private:

uint8_t i2c_addr;

boolean _framInitialised;

};

#endif

With this test setup, I was able to repeatedly clear the FRAM memory and see the effect of pulling the power plug. A typical result is shown below:

Program response when restarted with CLEAR_FRAM_PIN (pin 3) grounded

Program response after the power plug was pulled at about 8.9 sec after startup.

In the first figure above, the CLEAR_FRAM_PIN (pin 3) was held LOW while the Arduino was restarted, resulting in zeroes being written to the first 100 FRAM locations. In the second figure, the +12V power plug was pulled after about 9 seconds, then reconnected. When the Arduino restarted, the power-down ISR had written program timer values from 8976 to 8990 into the first nine 4-byte segments. This shows that the Arduino continued to operate for about 14msec after the power plug was removed. This is very good news, as it implies that I should be able to write the current date/time value from a real-time clock (when I get it running, that is) into FRAM whenever there is a power interruption, allowing me to accurately track battery usage history.

The hardware used to perform these tests is shown in the following photos:

Experimental setup moved to new ASP solderless breadboard

Arduino Board Modification:

I ran into a problem when I started testing the power-down interrupt idea; I wanted to keep thE Arduino board connected to my PC via the USB cable, but if I did that, the Arduino would automatically switch over to USB power when I disconnected the +12V power cable. Fortunately, if you have a problem, it’s almost certain that someone else has had and solved the same problem. In my case I found this post, that explained that T1, the automatic power crossover MOSFET switch had to be removed, as shown in the following photo. This modification allows the USB cable to continue to supply USBVCC power to U3, the ATMEGA8U2-MU chip, which in turn allows the PC to recognize the Arduino for firmware uploads.

Taking too long?

Reload document

Open in new tab

Download

So, now that I have demonstrated the practicality of a power-down interrupt routine to capture time-of-shutdown information (at least on an Arduino Uno), the next step is to integrate this capability with the Adafruit RS3231 Precision RTC breakout board. Stay tuned!

09 April Update: Adafruit DS3231 RTC breakout board added:

As I mentioned in my ‘Time and Memory for Wall-E2‘ post, I planned to complement the FRAM capability with the addition of an RTC module so that I could record the time & date of any power interruptions. As I mentioned in that post, I planned to use the Adafruit DS3231 Breakout Board for this purpose. After receiving the module, I added it to the system by daisy-chaining the I2C SCL & SDA lines from the FRAM module, and modified the software to incorporate the RTC. I changed the FRAM_ISR() function to acquire ‘Unixtime’ from the RTC (i.e. the number of seconds since 00:00:00 UTC on January 1, 1970) and write it to the FRAM.

I tested this by uploading this firmware to the Arduino Uno, and then pulling the power plug a few seconds after the program entered the main loop. Here’s the printout from the run

Program run with Adafruit DS3231 RTC added. Note ‘unixtime’ printout at top, and first 5 FRAM locations at bottom

As can be seen from the above printout, the power-down ISR successfully recorded the current time (in ‘Unixtime’ format) into five successive FRAM memory locations before the Arduino died. Note also that the recorded time was eleven seconds after the time shown at the top of the printout – about right, as the upper value is the time retrieved using the PC’s __DATE__ and ___TIME___ environment variables at the time the program was compiled and uploaded to the Arduino board.

Here are some photos of the new hardware setup:

Adafruit FRAM module on the left, DS3231 RTC module on the right

Frank

Time and Memory for Wall-E2

2 Replies

Posted 01 April 2018 (not an April Fool’s joke)

Now that I have Wall-E2’s charging module and charging station working, I’ve moved on to some secondary, but still important, issues that need addressing.

Wall-E2 still can’t tell which way he is heading. This isn’t necessarily a killer, as his current technique of simply following walls does fairly well. However, when it comes to a human figuring out where Wall-E2 is or has been, distance to the nearest wall just doesn’t cut it. A while ago I attempted to solve this problem using an onboard magnetometer, but ultimately concluded a magnetometer-based navigation system is doomed to failure in an indoor environment, due to too many interfering magnetic signals. I have decided to try this again, but this time using a cheap solid-state gyroscope module from Sparkfun. I should be able to initialize the gyroscope z-axis to an absolute heading each time Wall-E2 connects to a charging station, as their location(s) and headings are known. As a bonus, I should be able to use the gyro to generate accurate 90 º turns, instead of the current open-loop timing method.
Wall-E2 remembers how long it has been since its last recharge, but only if the main power hasn’t been interrupted. If I turn him off for any reason, that information goes away. I dicked around with writing the current value of millis() to EEPROM on power-down, but it turns out that EEPROM writes are way too slow for that. After Googling around for a bit, I found a nice little FRAM (Ferro-magnetic Random Access Memory) module from Adafruit, and I believe I will be able to implement a power-down memory save feature using it.
Once I have a fast non-volatile FRAM solution, it occurs to me that I may want to write telemetry information to it, so it isn’t lost when Wall-E2 is out of range of the current Wixel link to my PC. Maybe even set the FRAM (or at least part of it) up as a rate buffer between Wall-E2 and the Wixel. The idea would be that telemetry data always goes to the FRAM at some rate A, and is then read from the FRAM to my PC via the Wixel link at rate B, where B > A. When the link isn’t available, the telemetry data continues to be written into the FRAM, and is read back out again when the link becomes available. As long as the link isn’t interrupted for too long, I won’t lose any telemetry data. As part of this implementation, I would like to time-stamp the data with real date-time information, which requires a battery-backed RTC (real-time clock). As it happens, Adafruit has one of these too, so I may be able to implement it quickly and easily.

I chose the I2C versions of all these modules, as I already have I2C implemented for acquiring steering cues from my IR Homing Module. In theory, at least, adding three more I2C slave devices to an already existing setup should be trivial.

Stay tuned!

Frank