Martijn Grootens / AS5048

Library for SPI communication with the AMS AS5048 rotary sensor

Dependents:   heros_leg_readout_torque_addition heros_leg_readout_torque_addition heros_leg_readout_torque_addition_V3

As5048.h@1:94b48453d13a, 2016-05-13 (annotated)

Committer:
megrootens
Date:
Fri May 13 09:43:57 2016 +0000
Revision:
1:94b48453d13a
Parent:
0:723d48642d5c
Child:
2:111641f7e672
added directions and offsets

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:723d48642d5c 1 #ifndef _AS5048_H_
megrootens 0:723d48642d5c 2 #define _AS5048_H_
megrootens 0:723d48642d5c 3
megrootens 0:723d48642d5c 4 #include "mbed.h"
megrootens 0:723d48642d5c 5
megrootens 0:723d48642d5c 6 const int kNumSensorBits = 14; // 14-bits sensor
megrootens 0:723d48642d5c 7 const uint16_t kCountsPerRev = 0x4000; // 2**NUM_SENSOR_BITS
megrootens 0:723d48642d5c 8 const uint16_t kMask = 0x3FFF; // 2**NUM_SENSOR_BITS - 1
megrootens 0:723d48642d5c 9 const int kParity = 1; // even parity
megrootens 0:723d48642d5c 10
megrootens 0:723d48642d5c 11 const int kSpiFrequency = 1000000; // AS5048 max 10 MHz
megrootens 0:723d48642d5c 12 const int kSpiBitsPerTransfer = 8;
megrootens 0:723d48642d5c 13 const int kSpiMode = 1;
megrootens 0:723d48642d5c 14
megrootens 0:723d48642d5c 15 const float kDegPerRev = 360.0f; // 360 degrees/rev
megrootens 0:723d48642d5c 16 const float kRadPerRev = 6.28318530718f; // 2*pi rad/rev
megrootens 0:723d48642d5c 17
megrootens 0:723d48642d5c 18 // AS5048 flags
megrootens 0:723d48642d5c 19 typedef enum {
megrootens 0:723d48642d5c 20 AS_FLAG_PARITY = 0x8000,
megrootens 0:723d48642d5c 21 AS_FLAG_READ = 0x4000,
megrootens 0:723d48642d5c 22 } As5048Flag;
megrootens 0:723d48642d5c 23
megrootens 0:723d48642d5c 24 // AS5048 commands
megrootens 0:723d48642d5c 25 typedef enum {
megrootens 0:723d48642d5c 26 AS_CMD_NOP = 0x0000,
megrootens 0:723d48642d5c 27 AS_CMD_ERROR = 0x0001 | AS_FLAG_READ, // Reads error register of sensor and clear error flags
megrootens 0:723d48642d5c 28 AS_CMD_DIAGNOSTICS = 0x3FFD | AS_FLAG_READ, // Reads automatic gain control and diagnostics info
megrootens 0:723d48642d5c 29 AS_CMD_MAGNITUDE = 0x3FFE | AS_FLAG_READ,
megrootens 0:723d48642d5c 30 AS_CMD_ANGLE = 0x3FFF | AS_FLAG_PARITY | AS_FLAG_READ,
megrootens 0:723d48642d5c 31 } As5048Command;
megrootens 0:723d48642d5c 32
megrootens 0:723d48642d5c 33 // AS5048 diagnostics
megrootens 0:723d48642d5c 34 typedef enum {
megrootens 0:723d48642d5c 35 AS_DIAG_CORDIC_OVERFLOW = 0x0200,
megrootens 0:723d48642d5c 36 AS_DIAG_HIGH_MAGNETIC = 0x0400,
megrootens 0:723d48642d5c 37 AS_DIAG_LOW_MAGNETIC = 0x0800,
megrootens 0:723d48642d5c 38 } As5048Diagnostics;
megrootens 0:723d48642d5c 39
megrootens 0:723d48642d5c 40
megrootens 0:723d48642d5c 41
megrootens 0:723d48642d5c 42
megrootens 0:723d48642d5c 43 /**
megrootens 0:723d48642d5c 44 * Interfacing with the AMS AS5048A magnetic rotary sensor using SPI protocol
megrootens 0:723d48642d5c 45 * AS5048 uses 16-bit transfer;
megrootens 0:723d48642d5c 46 * We use two 8-bit transfers for compatibility with 8-bit SPI master devices
megrootens 0:723d48642d5c 47 * SPI protocol:
megrootens 0:723d48642d5c 48 * Mode = 1:
megrootens 0:723d48642d5c 49 * clock polarity = 0 --> clock pulse is high
megrootens 0:723d48642d5c 50 * clock phase = 1 --> sample on falling edge of clock pulse
megrootens 0:723d48642d5c 51 * Code was succesfully tested on the FRDM KL25Z and K22F. The same code fails
megrootens 0:723d48642d5c 52 * on the K64F for some reason. Sampling using a logic analyzer does however
megrootens 0:723d48642d5c 53 * show the same results for al three boards.
megrootens 0:723d48642d5c 54 */
megrootens 0:723d48642d5c 55 class As5048 {
megrootens 0:723d48642d5c 56
megrootens 0:723d48642d5c 57 public:
megrootens 0:723d48642d5c 58
megrootens 0:723d48642d5c 59 /**
megrootens 0:723d48642d5c 60 * Creates an object of num_sensors daisy chained AS5048 sensors;
megrootens 0:723d48642d5c 61 * default number of sensors in chain is 1
megrootens 0:723d48642d5c 62 * @param mosi: pinname of the mosi pin of the spi communication
megrootens 0:723d48642d5c 63 * @param miso: pinname of the miso pin of the spi communication
megrootens 0:723d48642d5c 64 * @param sck: pinname of the clock pin of the spi communication
megrootens 0:723d48642d5c 65 * @param cs: pinname of the chip select pin of the spi communication
megrootens 0:723d48642d5c 66 * @param num_sensors = 1: number of sensors in daisy chain
megrootens 0:723d48642d5c 67 */
megrootens 0:723d48642d5c 68 As5048(PinName mosi, PinName miso, PinName sck, PinName cs, int num_sensors = 1):
megrootens 0:723d48642d5c 69 kNumSensors_(num_sensors),
megrootens 0:723d48642d5c 70 chip_(cs),
megrootens 0:723d48642d5c 71 spi_(mosi, miso, sck)
megrootens 0:723d48642d5c 72 {
megrootens 0:723d48642d5c 73 DeselectChip();
megrootens 0:723d48642d5c 74
megrootens 0:723d48642d5c 75 spi_.format(kSpiBitsPerTransfer, kSpiMode);
megrootens 0:723d48642d5c 76 spi_.frequency(kSpiFrequency);
megrootens 0:723d48642d5c 77
megrootens 0:723d48642d5c 78 read_buffer_ = new uint16_t[kNumSensors_];
megrootens 0:723d48642d5c 79 angle_buffer_ = new uint16_t[kNumSensors_];
megrootens 0:723d48642d5c 80 angle_offset_ = new uint16_t[kNumSensors_];
megrootens 1:94b48453d13a 81 directions_ = new bool[kNumSensors_];
megrootens 0:723d48642d5c 82
megrootens 0:723d48642d5c 83 for (int i=0; i<kNumSensors_; ++i) {
megrootens 0:723d48642d5c 84 read_buffer_[i] = 0;
megrootens 0:723d48642d5c 85 angle_buffer_[i] = 0;
megrootens 0:723d48642d5c 86 angle_offset_[i] = 0;
megrootens 1:94b48453d13a 87 directions_[i] = true;
megrootens 0:723d48642d5c 88 }
megrootens 0:723d48642d5c 89
megrootens 0:723d48642d5c 90 last_command_ = AS_CMD_NOP;
megrootens 0:723d48642d5c 91 }
megrootens 0:723d48642d5c 92
megrootens 0:723d48642d5c 93
megrootens 0:723d48642d5c 94 /**
megrootens 0:723d48642d5c 95 * Destructor, memory deallocation
megrootens 0:723d48642d5c 96 */
megrootens 0:723d48642d5c 97 ~As5048() {
megrootens 0:723d48642d5c 98 delete [] read_buffer_;
megrootens 0:723d48642d5c 99 delete [] angle_buffer_;
megrootens 0:723d48642d5c 100 delete [] angle_offset_;
megrootens 1:94b48453d13a 101 delete [] directions_;
megrootens 0:723d48642d5c 102 }
megrootens 0:723d48642d5c 103
megrootens 0:723d48642d5c 104 /**
megrootens 0:723d48642d5c 105 * Parity check
megrootens 0:723d48642d5c 106 * @param n: integer to check
megrootens 0:723d48642d5c 107 * @return: true if ok
megrootens 0:723d48642d5c 108 */
megrootens 0:723d48642d5c 109 static bool CheckParity(int n) {
megrootens 0:723d48642d5c 110 int parity = n;
megrootens 0:723d48642d5c 111 for(int i=1; i <= kNumSensorBits+1; ++i) {
megrootens 0:723d48642d5c 112 n >>= 1;
megrootens 0:723d48642d5c 113 parity ^= n;
megrootens 0:723d48642d5c 114 }
megrootens 0:723d48642d5c 115 return (parity & kParity) == 0;
megrootens 0:723d48642d5c 116 }
megrootens 0:723d48642d5c 117
megrootens 0:723d48642d5c 118 /**
megrootens 0:723d48642d5c 119 * Update the buffer with angular measurements
megrootens 0:723d48642d5c 120 * NOTE 1:
megrootens 0:723d48642d5c 121 * If the last command sent through Transfer was *not* AS_CMD_ANGLE
megrootens 0:723d48642d5c 122 * then we need an additional Transfer; this takes more time!
megrootens 0:723d48642d5c 123 * This should not occur, since Transfer is not *yet* used elsewhere.
megrootens 0:723d48642d5c 124 * NOTE 2:
megrootens 0:723d48642d5c 125 * We run a parity check on the results from the transfer. We only
megrootens 0:723d48642d5c 126 * update the angle_buffer_ with values that pass the parity check.
megrootens 0:723d48642d5c 127 * Measurement using Timer on K64F for last_command_ == AS_CMD_ANGLE
megrootens 0:723d48642d5c 128 * shows this function takes 87 or 88 us.
megrootens 0:723d48642d5c 129 */
megrootens 0:723d48642d5c 130 void UpdateAngleBuffer() {
megrootens 0:723d48642d5c 131 // ensure that the new results indeed will be angles
megrootens 0:723d48642d5c 132 if (last_command_ != AS_CMD_ANGLE) {
megrootens 0:723d48642d5c 133 Transfer(AS_CMD_ANGLE);
megrootens 0:723d48642d5c 134 }
megrootens 0:723d48642d5c 135
megrootens 0:723d48642d5c 136 // update the read buffer
megrootens 0:723d48642d5c 137 Transfer(AS_CMD_ANGLE);
megrootens 0:723d48642d5c 138
megrootens 0:723d48642d5c 139 // update the angle buffer with parity checked values
megrootens 0:723d48642d5c 140 for (int i=0; i<kNumSensors_; ++i) {
megrootens 0:723d48642d5c 141 if (CheckParity(read_buffer_[i])) {
megrootens 0:723d48642d5c 142 // only update angles when parity is correct
megrootens 0:723d48642d5c 143 angle_buffer_[i] = read_buffer_[i];
megrootens 0:723d48642d5c 144 }
megrootens 0:723d48642d5c 145 }
megrootens 0:723d48642d5c 146 }
megrootens 0:723d48642d5c 147
megrootens 0:723d48642d5c 148 /**
megrootens 0:723d48642d5c 149 * @return: pointer to read_buffer_
megrootens 0:723d48642d5c 150 */
megrootens 0:723d48642d5c 151 const uint16_t* get_read_buffer() { return read_buffer_; }
megrootens 0:723d48642d5c 152
megrootens 0:723d48642d5c 153 /**
megrootens 0:723d48642d5c 154 * @return: pointer to angle_buffer_
megrootens 0:723d48642d5c 155 */
megrootens 0:723d48642d5c 156 const uint16_t* get_angle_buffer() { return angle_buffer_; }
megrootens 0:723d48642d5c 157
megrootens 0:723d48642d5c 158 /**
megrootens 0:723d48642d5c 159 * @return: pointer to angle_offet_
megrootens 0:723d48642d5c 160 */
megrootens 0:723d48642d5c 161 const uint16_t* get_angle_offset() { return angle_offset_; }
megrootens 0:723d48642d5c 162
megrootens 0:723d48642d5c 163 /**
megrootens 1:94b48453d13a 164 * @return: pointer to directions_
megrootens 1:94b48453d13a 165 */
megrootens 1:94b48453d13a 166 const bool * get_directions_() { return directions_;}
megrootens 1:94b48453d13a 167
megrootens 1:94b48453d13a 168 /**
megrootens 0:723d48642d5c 169 * You get the angles from two UpdateAngleBuffer() calls before
megrootens 0:723d48642d5c 170 * @return: 14 bits absolute position
megrootens 0:723d48642d5c 171 */
megrootens 0:723d48642d5c 172 int getAngle(int i_sensor=0){
megrootens 1:94b48453d13a 173 int ans = ((int) (angle_buffer_[i_sensor] & kMask)) - angle_offset_[i_sensor];
megrootens 1:94b48453d13a 174 return directions_[i_sensor]?ans:-ans;
megrootens 0:723d48642d5c 175 }
megrootens 0:723d48642d5c 176
megrootens 0:723d48642d5c 177 /**
megrootens 0:723d48642d5c 178 * You get the angles from two UpdateAngleBuffer() calls before
megrootens 0:723d48642d5c 179 * @return: revolution ratio in [0,1]
megrootens 0:723d48642d5c 180 */
megrootens 0:723d48642d5c 181 float getAngleRatio(int i_sensor=0) { return (float) getAngle(i_sensor) / kCountsPerRev; }
megrootens 0:723d48642d5c 182
megrootens 0:723d48642d5c 183 /**
megrootens 0:723d48642d5c 184 * You get the angles from two UpdateAngleBuffer() calls before
megrootens 0:723d48642d5c 185 * @return: angle in degrees
megrootens 0:723d48642d5c 186 */
megrootens 0:723d48642d5c 187 float getAngleDegrees(int i_sensor=0) { return getAngleRatio(i_sensor) * kDegPerRev; }
megrootens 0:723d48642d5c 188
megrootens 0:723d48642d5c 189 /**
megrootens 0:723d48642d5c 190 * You get the angles from two UpdateAngleBuffer() calls before
megrootens 0:723d48642d5c 191 * @return: angle in radians
megrootens 0:723d48642d5c 192 */
megrootens 0:723d48642d5c 193 float getAngleRadians(int i_sensor=0) { return getAngleRatio(i_sensor) * kRadPerRev; }
megrootens 0:723d48642d5c 194
megrootens 0:723d48642d5c 195 /**
megrootens 1:94b48453d13a 196 * Set direction for a sensor
megrootens 1:94b48453d13a 197 * @param i_sensor: id of sensor for which the offset is to be set
megrootens 1:94b48453d13a 198 * @param dir: true positive, false negative
megrootens 1:94b48453d13a 199 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 1:94b48453d13a 200 */
megrootens 1:94b48453d13a 201 bool setDirection(int i_sensor, bool dir) {
megrootens 1:94b48453d13a 202 if (i_sensor>-1 and i_sensor<kNumSensors_) {
megrootens 1:94b48453d13a 203 directions_[i_sensor] = dir;
megrootens 1:94b48453d13a 204 return true;
megrootens 1:94b48453d13a 205 }
megrootens 1:94b48453d13a 206 return false;
megrootens 1:94b48453d13a 207 }
megrootens 1:94b48453d13a 208
megrootens 1:94b48453d13a 209 /**
megrootens 1:94b48453d13a 210 * Set direction for the first sensor
megrootens 1:94b48453d13a 211 * @param dir: true positive, false negative
megrootens 1:94b48453d13a 212 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 1:94b48453d13a 213 */
megrootens 1:94b48453d13a 214 bool setDirection(bool dir) {
megrootens 1:94b48453d13a 215 return setDirection(0,dir);
megrootens 1:94b48453d13a 216 }
megrootens 1:94b48453d13a 217
megrootens 1:94b48453d13a 218
megrootens 1:94b48453d13a 219 /**
megrootens 0:723d48642d5c 220 * Set offset for a sensor
megrootens 0:723d48642d5c 221 * @param i_sensor: id of sensor for which the offset is to be set
megrootens 0:723d48642d5c 222 * @param offset: offset in counts [0,2**14-1]
megrootens 0:723d48642d5c 223 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 224 */
megrootens 0:723d48642d5c 225 bool setOffset(int i_sensor, uint16_t offset) {
megrootens 0:723d48642d5c 226 if (i_sensor>-1 and i_sensor<kNumSensors_) {
megrootens 0:723d48642d5c 227 angle_offset_[i_sensor] = offset;
megrootens 0:723d48642d5c 228 return true;
megrootens 0:723d48642d5c 229 }
megrootens 0:723d48642d5c 230 return false;
megrootens 0:723d48642d5c 231 }
megrootens 0:723d48642d5c 232
megrootens 0:723d48642d5c 233 /**
megrootens 0:723d48642d5c 234 * Set offset for the first sensor
megrootens 0:723d48642d5c 235 * @param offset: offset in counts [0,2**14-1]
megrootens 0:723d48642d5c 236 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 237 */
megrootens 0:723d48642d5c 238 bool setOffset(uint16_t offset) { return setOffset(0,offset); }
megrootens 0:723d48642d5c 239
megrootens 0:723d48642d5c 240 /**
megrootens 0:723d48642d5c 241 * Set offset for a sensor
megrootens 0:723d48642d5c 242 * @param i_sensor: id of sensor for which the offset is to be set
megrootens 0:723d48642d5c 243 * @param offset_ratio: offset in ratio in [0,1]
megrootens 0:723d48642d5c 244 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 245 */
megrootens 0:723d48642d5c 246 bool setOffsetRatio (int i_sensor, float offset_ratio) {
megrootens 0:723d48642d5c 247 return setOffset(i_sensor,offset_ratio*kCountsPerRev);
megrootens 0:723d48642d5c 248 }
megrootens 0:723d48642d5c 249
megrootens 0:723d48642d5c 250 /**
megrootens 0:723d48642d5c 251 * Set offset for the first sensor
megrootens 0:723d48642d5c 252 * @param offset_ratio: offset in ratio in [0,1]
megrootens 0:723d48642d5c 253 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 254 */
megrootens 0:723d48642d5c 255 bool setOffsetRatio(float offset_ratio) {
megrootens 0:723d48642d5c 256 return setOffsetRatio(0,offset_ratio);
megrootens 0:723d48642d5c 257 }
megrootens 0:723d48642d5c 258
megrootens 0:723d48642d5c 259 /**
megrootens 0:723d48642d5c 260 * Set offset for a sensor
megrootens 0:723d48642d5c 261 * @param i_sensor: id of sensor for which the offset is to be set
megrootens 0:723d48642d5c 262 * @param offset_degrees: offset in degrees in [0,360]
megrootens 0:723d48642d5c 263 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 264 */
megrootens 0:723d48642d5c 265 bool setOffsetDegrees(int i_sensor, float offset_degrees) {
megrootens 0:723d48642d5c 266 return setOffsetRatio(i_sensor,offset_degrees / kDegPerRev);
megrootens 0:723d48642d5c 267 }
megrootens 0:723d48642d5c 268
megrootens 0:723d48642d5c 269 /**
megrootens 0:723d48642d5c 270 * Set offset for the first sensor
megrootens 0:723d48642d5c 271 * @param offset_degrees: offset in degrees in [0,360]
megrootens 0:723d48642d5c 272 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 273 */
megrootens 0:723d48642d5c 274 bool setOffsetDegrees(float offset_degrees) {
megrootens 0:723d48642d5c 275 return setOffsetDegrees(0, offset_degrees);
megrootens 0:723d48642d5c 276 }
megrootens 0:723d48642d5c 277
megrootens 0:723d48642d5c 278 /**
megrootens 0:723d48642d5c 279 * Set offset for a sensor
megrootens 0:723d48642d5c 280 * @param i_sensor: id of sensor for which the offset is to be set
megrootens 0:723d48642d5c 281 * @param offset_radians: offset in radians in [0,2*pi]
megrootens 0:723d48642d5c 282 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 283 */
megrootens 0:723d48642d5c 284 bool setOffsetRadians(int i_sensor, float offset_radians) {
megrootens 0:723d48642d5c 285 return setOffsetRatio(i_sensor, offset_radians / kRadPerRev);
megrootens 0:723d48642d5c 286 }
megrootens 0:723d48642d5c 287
megrootens 0:723d48642d5c 288 /**
megrootens 0:723d48642d5c 289 * Set offset for the first sensor
megrootens 0:723d48642d5c 290 * @param offset_radians: offset in radians in [0,2*pi]
megrootens 0:723d48642d5c 291 * @return: true if i_sensor in [0,kNumSensor_)
megrootens 0:723d48642d5c 292 */
megrootens 0:723d48642d5c 293 bool setOffsetRadians(float offset_radians) {
megrootens 0:723d48642d5c 294 return setOffsetRadians(0, offset_radians);
megrootens 0:723d48642d5c 295 }
megrootens 1:94b48453d13a 296
megrootens 1:94b48453d13a 297
megrootens 0:723d48642d5c 298
megrootens 0:723d48642d5c 299
megrootens 0:723d48642d5c 300 protected:
megrootens 0:723d48642d5c 301
megrootens 0:723d48642d5c 302 /**
megrootens 0:723d48642d5c 303 * Select (low) chip, and wait 1 us (at least 350 ns)
megrootens 0:723d48642d5c 304 */
megrootens 0:723d48642d5c 305 void SelectChip() { chip_.write(0); wait_us(1); }
megrootens 0:723d48642d5c 306
megrootens 0:723d48642d5c 307 /**
megrootens 0:723d48642d5c 308 * Deselect (high) chip, and wait 1 us (at least 350 ns)
megrootens 0:723d48642d5c 309 */
megrootens 0:723d48642d5c 310 void DeselectChip() { chip_.write(1); wait_us(1); }
megrootens 0:723d48642d5c 311
megrootens 0:723d48642d5c 312 /**
megrootens 0:723d48642d5c 313 * SPI transfer between each of the daisy chained sensors
megrootens 0:723d48642d5c 314 * @param cmd: Command to send
megrootens 0:723d48642d5c 315 */
megrootens 0:723d48642d5c 316 void Transfer(As5048Command cmd) {
megrootens 0:723d48642d5c 317 SelectChip();
megrootens 0:723d48642d5c 318 for(int i=0; i<kNumSensors_; ++i){
megrootens 0:723d48642d5c 319 read_buffer_[i] = spi_.write(cmd>>8) << 8;
megrootens 0:723d48642d5c 320 read_buffer_[i] |= spi_.write(cmd & 0x00FF);
megrootens 0:723d48642d5c 321 }
megrootens 0:723d48642d5c 322 DeselectChip();
megrootens 0:723d48642d5c 323 last_command_ = cmd;
megrootens 0:723d48642d5c 324 }
megrootens 0:723d48642d5c 325
megrootens 0:723d48642d5c 326 const int kNumSensors_; // number of sensors in daisy chain
megrootens 0:723d48642d5c 327 DigitalOut chip_; // chip select port
megrootens 0:723d48642d5c 328 SPI spi_; // mbed spi communiation object
megrootens 0:723d48642d5c 329
megrootens 0:723d48642d5c 330 uint16_t* read_buffer_; // buffer for results from last transfer
megrootens 0:723d48642d5c 331 uint16_t* angle_buffer_; // buffer for angle results from last transfer
megrootens 0:723d48642d5c 332 uint16_t* angle_offset_; // offset array for each sensor
megrootens 1:94b48453d13a 333 bool* directions_; // direction true positive, false negative
megrootens 0:723d48642d5c 334
megrootens 0:723d48642d5c 335 As5048Command last_command_;// command sent during last Transfer
megrootens 0:723d48642d5c 336
megrootens 0:723d48642d5c 337 };
megrootens 0:723d48642d5c 338 #endif