Show Posts
Pages: [1]
1  Using Arduino / Programming Questions / Re: Trouble implementing Madgwick AHRS (with MATLAB sample) on: April 21, 2014, 12:28:35 pm

That is a bad initialisation choice for a quaternion.    The x,y,z components represent a scaled unit vector ( multiplied by the sine of the rotation ),   but 0,0,0    cannot be scaled to a unit vector by any means.


This is wrong, and the reason why it's wrong is right in front of you: "multiplied by the sine of the rotation." When the rotation is zero, the sine is zero, and you get 1,0,0,0. Plug in zero roll, pitch, and yaw into any Euler angle to quaternion calculator and see what you get.

wilhem: That data doesn't look right. Try printing out the inputs to the filter instead of the outputs, and make sure the inputs are continuous and otherwise look as expected.
2  Using Arduino / Programming Questions / Re: Trouble implementing Madgwick AHRS (with MATLAB sample) on: April 20, 2014, 02:43:09 pm
b_z and b_x represent the filter's estimate of the direction of Earth's magnetic flux. I initialize b_x to zero and b_z to 1 and the quaternion to 1,0,0,0.
3  Using Arduino / Programming Questions / Re: Trouble implementing Madgwick AHRS (with MATLAB sample) on: April 19, 2014, 11:23:00 am
It sounds like you have the right idea. The filter doesn't need the accelerations or magnetometer measurements in any specific units because it uses them to compute a unit vector's direction. As long as the three components have the same scale factor, the actual value of that scale factor doesn't matter. Depending on the quality of your sensors and what you're trying to achieve, you may not need to remove an offset from the accels/mags, but you'll get the best results if you calibrate them to find where zero is for each axis.
4  Using Arduino / Programming Questions / Re: Trouble implementing Madgwick AHRS (with MATLAB sample) on: October 27, 2013, 06:39:28 pm
I found the problem! It actually seems to be a mistake in the paper. twom_x, twom_y, and twom_z need to be computed after normalizing the magnetometer reading. In the paper, they are computed before the normalization. Here's the corrected function: http://pastebin.com/KiwcEgXS

and the new results:



That took a long time.
5  Using Arduino / Programming Questions / Re: Trouble implementing Madgwick AHRS (with MATLAB sample) on: October 27, 2013, 05:05:59 pm
I'm trying to get this to work on an Arduino. I moved it into MATLAB to make it easier to debug.

I didn't really want to post the Arduino code because I gave links to it, and it's already all over the internet, but here it is for each filter:

Common version: http://pastebin.com/XUF7bpWu
White paper version: http://pastebin.com/jmGVs4Ri
6  Using Arduino / Programming Questions / Trouble implementing Madgwick AHRS (with MATLAB sample) on: October 27, 2013, 03:46:40 pm
I'm trying to implement the MARG AHRS filter given at the end of Madgwick's paper: http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf

I have already successfully implemented the more common filter also by Madgwick here: http://www.x-io.co.uk/res/sw/madgwick_algorithm_c.zip

For some reason the version in the paper diverges when I use it. I want to use the paper version because it should be more accurate and it estimates gyro biases. Below are the results from the common version and the white paper version:



I attached a zip file with the MATLAB data/scripts to reproduce those results. I have tried changing which axes are which and varying the tuning parameters, but it always seems to diverge. Has anyone successfully used both versions of the filter?

The sensors are an ADIS16355 for accel/gyro and LSM303 for mag.
7  Using Arduino / Networking, Protocols, and Devices / Re: Trouble communicating with Analog Devices ADIS16355 IMU over SPI with Teensy 3.0 on: November 10, 2012, 10:56:26 pm
I'm totally having a conversation with myself here, but there is a tDATASTALL called on in the datasheet which is exactly what I need. It says 9 microseconds minimum. Does that already include some kind of safety factor, or should I add my own, say by using a 15 microsecond stall time?
8  Using Arduino / Networking, Protocols, and Devices / Re: Trouble communicating with Analog Devices ADIS16355 IMU over SPI with Teensy 3.0 on: November 10, 2012, 10:51:51 pm
The code is working exactly as intended after I put 1 millisecond delays between data frames, and the link I provided in the OP has the corrected code. I do need to find out how small to make that delay, but other than that, I have to say I'm rather proud of doing this in one day without using SPI before.
9  Using Arduino / Networking, Protocols, and Devices / Re: Trouble communicating with Analog Devices ADIS16355 IMU over SPI with Teensy 3.0 on: November 10, 2012, 10:36:21 pm
Now I put a delay(1) between the two data frames in the readAddress function, and it's giving me the correct data. I assume that delay doesn't need to be nearly so long, but I can't find a minimum value for it in the datasheet. How do I know how long that delay needs to be?
10  Using Arduino / Networking, Protocols, and Devices / Re: Trouble communicating with Analog Devices ADIS16355 IMU over SPI with Teensy 3.0 on: November 10, 2012, 10:20:54 pm
Made some progress, I think... I moved SPI.begin() to before the SPI.setBitOrder(), SPI.setClockDivider(), and SPI.setDataMode() commands. Now the LED does light up and I get different output, though still not correct:

Code:
Z Accel (g): 0.64
Z Accel (g): 0.64
Z Accel (g): 0.48
Z Accel (g): 0.48

It seems to only give one of those two values at a time depending on how I tilt the IMU. Better than nothing...
11  Using Arduino / Networking, Protocols, and Devices / Trouble communicating with Analog Devices ADIS16355 IMU over SPI with Teensy 3.0 on: November 10, 2012, 10:02:15 pm
I am trying to make a simple sketch that just reads data from the ADIS16355 IMU. I am using the new Teensy 3.0. I am using USB for power. The IMU is using the same 5V power, and all the other connections to it should be 3.3V since that's the operating voltage of the Teensy 3.0.

These are all the connections:

Teensy 3.0 pin 10 (CS0) to ADIS16355 pin 6 (CS)
Teensy 3.0 pin 11 (DOUT) to ADIS16355 pin 15 (DIN)
Teensy 3.0 pin 12 (DIN) to ADIS16355 pin 4 (DOUT)
Teensy 3.0 pin 13 (SCK) to ADIS16355 pin 3 (SCK)
Teensy 3.0 pin 25 (AGND) to ADIS16355 pin 13 (GND)
Teensy 3.0 pin 26 (Vin) to ADIS16355 pin 11 (VCC)

The loop function of my sketch just prints the XACCL_OUT value every 5 ms. Unfortunately the output is just:

Z Accel (g): 0.00
Z Accel (g): 0.00
...ad infinitum

Here is the code that handles the SPI stuff: (EDIT: Replaced with working code for posterity)

Code:
// Copyright 2012 Jacob Niehus
// jacob.niehus@gmail.com
// Do not distribute without permission.

#include "ADIS16355_Interface.h"
#include <Streaming.h>

// Pin definitions
const int SSpin = 10;

// SPI address definitions (upper; // lower)
const uint8_t SUPPLY_OUT = 0x03; // 0x02
const uint8_t XGYRO_OUT  = 0x05; // 0x04
const uint8_t YGYRO_OUT  = 0x07; // 0x06
const uint8_t ZGYRO_OUT  = 0x09; // 0x08
const uint8_t XACCL_OUT  = 0x0B; // 0x0A
const uint8_t YACCL_OUT  = 0x0D; // 0x0C
const uint8_t ZACCL_OUT  = 0x0F; // 0x0E
const uint8_t XTEMP_OUT  = 0x11; // 0x10
const uint8_t YTEMP_OUT  = 0x13; // 0x12
const uint8_t ZTEMP_OUT  = 0x15; // 0x14
const uint8_t AUX_ADC    = 0x17; // 0x16

// LSB definitions
const float SUPPLY_LSB  = 0.0018315;  // V per LSB
const float GYRO_LSB    = 0.07326;    // deg/sec per LSB (300 deg/sec dynamic range)
const float ACCL_LSB    = 0.002522;   // g per LSB
const float AUX_ADC_LSB = 0.0006105;  // V per LSB

// Other constants
const int T_DATA_STALL_MICROSEC = 15;

void ADIS16355::Initialize()
{
  pinMode(SSpin, OUTPUT);
  Deselect();
  SPI.begin();
  SPI.setBitOrder(MSBFIRST);
  SPI.setClockDivider(SPI_CLOCK_DIV32);
  SPI.setDataMode(SPI_MODE3);
}

float ADIS16355::getXaccel()
{
  uint16_t data = readRegister(XACCL_OUT);
  int16_t dataLSBs = twosComplementToInt(data, 14);
  return dataLSBs * ACCL_LSB;
}

float ADIS16355::getYaccel()
{
  uint16_t data = readRegister(YACCL_OUT);
  int16_t dataLSBs = twosComplementToInt(data, 14);
  return dataLSBs * ACCL_LSB;
}

float ADIS16355::getZaccel()
{
  uint16_t data = readRegister(ZACCL_OUT);
  int16_t dataLSBs = twosComplementToInt(data, 14);
  return dataLSBs * ACCL_LSB;
}

uint16_t ADIS16355::readRegister(uint8_t address)
{
  uint8_t upper, lower;

  // First data frame sends address in bits 8-13
  Select();
  SPI.transfer(address);
  SPI.transfer(0);
  Deselect();

  // Second data frame receives upper then lower bytes of data
  Select();
  upper = SPI.transfer(0);
  lower = SPI.transfer(0);
  Deselect();

  if ((upper & (1 << 6)) == 1)
  {
    Serial << "WARNING: Error/alarm accessing address " << address << endl;
  }
  if ((upper & (1 << 7)) == 1)
  {
    Serial << "WARNING: Using old data from address " << address << endl;
  }

  // Combine upper and lower bytes to form output
  return (upper << 8) | lower;
}

void ADIS16355::writeRegister(uint8_t address, uint8_t value)
{
  // Turn on left-most bit to give a write command
  uint8_t command = address | 1 << 7;

  Select();
  // Transfer R/W command and register first,
  // followed by the value to be written
  SPI.transfer(command);
  SPI.transfer(value);
  Deselect();
}

int16_t ADIS16355::twosComplementToInt(uint16_t data, uint8_t bits)
{
  if ((data & (1 << (bits - 1))) == 0)
  {
    for (uint8_t iter = bits; iter < 16; iter++)
    {
      data &= ~(1 << iter);
    }
    return (int16_t)data;
  }
  else
  {
    data = ~data + 1;
    for (uint8_t iter = bits; iter < 16; iter++)
    {
      data &= ~(1 << iter);
    }
    return -(int16_t)data;
  }
}

void ADIS16355::Select()
{
  digitalWrite(SSpin, LOW);
}

void ADIS16355::Deselect()
{
  digitalWrite(SSpin, HIGH);
  delayMicroseconds(T_DATA_STALL_MICROSEC);
}

You can download the full sketch here: http://dropcanvas.com/4ujy7

Datasheet for the IMU is here: http://www.analog.com/static/imported-files/data_sheets/ADIS16350_16355.pdf

The SCK pin is the pin connected to the LED pin built into the Teensy 3.0. The LED doesn't seem to light up at all, and it seems like it should appear to be at half-brightness since it turns on and off quickly. I don't have an oscilloscope or logic analyzer, though this may be what finally forces me to get one.

I tried putting some print statements in the readAddress function to print the values stored in "upper" and "lower" and they are always zero.

Can anyone take a look at the code and let me know if I'm doing something obviously wrong?
12  Development / Other Software Development / Possible error in SPI documentation? on: November 07, 2012, 09:38:38 pm
The SPI documentation says that on the Uno, MOSI = pin 13, MISO = pin 11, and SCK = pin 12. I think the actual configuration is MOSI = pin 11, MISO = pin 12, and SCK = pin 13. That's how they are used in the two examples given in the documentation, and those are the pins connected to the right ICSP header pins (ICSP 1 = Arduino 12, ICSP 3 = Arduino 13, and ICSP 4 = Arduino 11).

Can anyone confirm?
13  Using Arduino / Motors, Mechanics, and Power / Closed loop control of 29:1 Metal Gearmotor 37Dx52L mm with 64 CPR Encoder on: August 22, 2012, 09:16:04 pm
I'm trying to control these motors for a balancing robot project, and the problem is I'm finding that open loop control and even closed loop PID control gives insufficient performance. In particular, it has problems following commands for low speeds, probably due to static friction. Does anyone have some code for this or a similar motor that includes friction feedforward compensation?

I'm driving the motors with the Arduino Motor Shield R3 powered by a 3S LiPo battery. I'm using the AdaEncoder library to read the encoders and the PID library on the Arduino website for my PID control.
14  Using Arduino / Motors, Mechanics, and Power / Plug in USB and battery simultaneously? on: May 18, 2012, 11:11:55 pm
This is a basic question but I can't find the answer. Am I going to fry anything if I plug in USB and a 7.4V LiPo battery (in Vin/GND) at the same time? The board is the Uno R2. Thanks!
Pages: [1]