KM3NeT CLB  2.0
KM3NeT CLB v2 Embedded Software
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
ahrs.c
1 /*
2  * KM3NeT CLB v2 Firmware
3  * ----------------------
4  *
5  * Copyright 2013 KM3NeT Collaboration
6  *
7  * All Rights Reserved.
8  *
9  *
10  * File : ahrs.c
11  * Created : 12 jun. 2013
12  * Author : Antonio Orzelli
13  */
14 
15 
16 #include <stdio.h>
17 #include <assert.h>
18 
19 #include "kernel/err.h"
20 
21 #include "drv/i2c/ahrs.h"
22 #include "drv/wb/i2c.h"
23 #include "drv/wb/gpio.h"
24 
25 // #include "util/float.h"
26 
27 
28 typedef union {
29  struct {
30  uint8_t __res[3]; // align 32 bits
31  uint8_t regAddr;
32  f32_t val;
33  } v;
34  uint8_t d[8];
35 } AHRSReg;
36 
37 #define REG_OFFSET 3
38 #define TRX_SIZE 5
39 
40 
41 void ahrsOn(){
44 }
45 
46 void ahrsOff(){
48 }
49 
50 
51 
52 
53 bool ahrsWriteReg(I2C_Device * dev, uint8_t addr, uint8_t reg_addr, f32_t newValue, f32_t * readback)
54 {
55  AHRSReg cmd,rpl;
56  cmd.v.regAddr = reg_addr;
57  cmd.v.val = newValue;
58  if (!i2cSendCmdAlt(dev, addr, &cmd.d[REG_OFFSET], TRX_SIZE, &rpl.d[REG_OFFSET], TRX_SIZE)) return errRebase("AHRS");
59 
60  // basic sanity check
61  if (rpl.v.regAddr != reg_addr) return errSet(ERROR(E_AHRS_REGSDIFFER));
62 
63  if (readback != NULL) *readback = rpl.v.val;
64 
65  return true;
66 }
67 
68 bool ahrsReadReg(I2C_Device * dev, uint8_t addr, uint8_t reg_addr, f32_t * value)
69 {
70  assert(value != NULL);
71 
72  AHRSReg rpl;
73 
74  if (!i2cReadRegAlt(dev, addr, reg_addr, &rpl.d[REG_OFFSET], TRX_SIZE)) return errRebase("AHRS");
75 
76  // basic sanity check
77  if (rpl.v.regAddr != reg_addr) return errSet(ERROR(E_AHRS_REGSDIFFER));
78 
79  *value = rpl.v.val;
80 
81  return true;
82 }
83 
84 
85 
86 bool ahrsGetVersion(I2C_Device * dev, uint8_t addr, uint8_t * version)
87 {
88  f32_t f = 0;
89  if (!ahrsReadReg(dev, addr, AHRS_REG_VERSION, &f)) return false;
90 
91 
92  *version = (uint8_t) ((fltToI32(f, 100) + 5) / 10);
93 
94  return true;
95 }
96 
97 
98 
99 bool ahrsRead(I2C_Device * dev, uint8_t addr, CompassData * data)
100 {
101  uint8_t ahrsdata[sizeof(CompassData) + 1];
102 
103  if (!i2cReadRegAlt(dev, addr, AHRS_REG_DATA, ahrsdata, sizeof(ahrsdata))) return errRebase("AHRS");
104 
105  memcpy((void *)data, ahrsdata + 1, sizeof(CompassData));
106 
107  return true;
108 
109 
110 }
void ahrsOff()
Switch off the AHRS.
Definition: ahrs.c:46
This driver is to read and configure the AHRS I2C sensor.
void ahrsOn()
Switch on the AHRS.
Definition: ahrs.c:41
bool ahrsGetVersion(I2C_Device *dev, uint8_t addr, uint8_t *version)
Reads the AHRS version.
Definition: ahrs.c:86
GPIO Driver.
void gpioPinConf(int pin, GpioPinDir dir)
Configure PIN directionality.
Definition: gpio.c:26
Structure defines OpenCores I2C Device.
Definition: dev_i2c.h:55
bool i2cSendCmdAlt(I2C_Device *dev, i2cAddr addr, uint8_t *cmd, int cmd_len, uint8_t *answer, int answer_len)
Writes a command to the I2C device and receive the answer.
Definition: i2c.c:287
void gpioPinSet(int pin, bool high)
Sets the pin state.
Definition: gpio.c:35
GPIO is output.
Definition: gpio.h:36
Definition: ahrs.c:28
Structure defines data from a compass/tilt/gyro sensor.
Definition: types.h:22
Manages the global system error.
bool ahrsWriteReg(I2C_Device *dev, uint8_t addr, uint8_t reg_addr, f32_t newvalue, f32_t *readback)
Writes an AHRS register.
Definition: ahrs.c:53
bool i2cReadRegAlt(I2C_Device *dev, i2cAddr addr, uint8_t reg_addr, uint8_t *answer, int len)
Reads a register from a I2C device.
Definition: i2c.c:356
bool ahrsRead(I2C_Device *dev, uint8_t addr, CompassData *data)
Reads all data from the AHRS.
Definition: ahrs.c:99
uint32_t f32_t
32 bit representation for float.
Definition: float.h:30
#define GPIO_AHRS_ENABLE
Defines GPIO pins.
Definition: gpio.h:42
static bool errRebase(const char *name)
Rebases the cause of the error message.
Definition: err.h:104
bool errSet(uint32_t code, const char *error, const char *name)
Sets an error.
#define ERROR(CODE,...)
Expands an error code to an error code with a description (if ERROR_W_DESCR is declared).
bool ahrsReadReg(I2C_Device *dev, uint8_t addr, uint8_t reg_addr, f32_t *value)
Reads an AHRS register.
Definition: ahrs.c:68
OpenCores I2C device driver.
int32_t fltToI32(f32_t value, uint32_t multiplier)
This takes a f32_t IEEE 754 single precision floating point, and converts it to a multiplied integer...
Definition: float.c:90