KM3NeT CLB  2.0
KM3NeT CLB v2 Embedded Software
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
msg_ins.c
1 /*
2  * KM3NeT CLB v2 Firmware
3  * ----------------------
4  *
5  * Copyright 2012-2015 KM3NeT Collaboration
6  *
7  * All Rights Reserved.
8  *
9  *
10  * File : msg_ins.c
11  * Created : 15 jan. 2015
12  * Author : Vincent van Beveren
13  */
14 
15 #include <stdint.h>
16 #include <stdbool.h>
17 #include "net/msg.h"
18 #include "cfg_board.h"
19 #include "drv/i2c/ahrs.h"
20 #include "util/float.h"
21 /**
22  * Reads a register of the AHRS
23  */
24 bool _msgInsAHRSGetReg(MsgId * msgId, DataBuffer * cmd)
25 {
26  uint8_t buf[5];
27  uint8_t ahrs_reg;
28  f32_t answer;
29  DataBuffer rsp = DB_BUF_INIT(buf, 5);
30  dbReadU8(cmd, &ahrs_reg);
31 
32  if (!msgRxBufCheck(cmd)) return false;
33 
34  if (AHRS_I2C_ADDR != 0)
35  {
36  // Read the AHRS register
37  if(!ahrsReadReg(INS_I2C_DEV, AHRS_I2C_ADDR, ahrs_reg, &answer)){
38  errPrint(false);
39  return msgTxCurError(msgId);
40  }
41  } else {
42  answer = 0;
43  }
44 
45  if (msgId->msgId == MSG_INS_AHRS_GET_REG_EX)
46  dbWriteU8(&rsp, ahrs_reg);
47 
48  dbWriteF32(&rsp, answer);
49 
50  return msgTxReply(msgId, &rsp);
51 }
52 
53 bool _msgInsAHRSGetRegEx(MsgId * msgId, DataBuffer * cmd) __attribute__((alias("_msgInsAHRSGetReg")));
54 
55 /**
56  * Writes a register of the AHRS
57  */
58 bool _msgInsAHRSSetReg(MsgId * msgId, DataBuffer * cmd)
59 {
60  uint8_t ahrs_reg;
61  f32_t val;
62 
63  dbReadU8(cmd, &ahrs_reg);
64  dbReadF32(cmd, &val);
65 
66  if (!msgRxBufCheck(cmd)) return false;
67 
68  if (AHRS_I2C_ADDR != 0)
69  {
70  // Write the AHRS register
71  if(!ahrsWriteReg(INS_I2C_DEV, AHRS_I2C_ADDR, ahrs_reg, val, NULL)){
72  // error
73  errPrint(false);
74  return msgTxCurError(msgId);
75  }
76  }
77 
78  return msgTxReply(msgId, NULL);
79 }
80 
bool _msgInsAHRSSetReg(MsgId *id, DataBuffer *buf)
Writes a register of the AHRS.
Definition: msg_ins.c:58
This driver is to read and configure the AHRS I2C sensor.
bool dbReadU8(DataBuffer *buf, uint8_t *byte)
Reads an unsigned byte.
Definition: databuffer.c:153
Special library for primitive IEEE 754 floating point handling without dragging all float support alo...
bool dbReadF32(DataBuffer *buf, f32_t *flt)
Reads an 32-bit floating point.
Definition: databuffer.c:340
static bool msgTxCurError(MsgId *id)
Invoke to reply the current global error.
Definition: msg.h:378
Defines a DataBuffer structure.
Definition: databuffer.h:45
Handles MCF packed messages from the higher protocol layer.
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
void errPrint(bool clear)
Prints the last error.
Definition: err.c:79
bool msgTxReply(MsgId *id, DataBuffer *buf)
Invoke to send a reply.
Definition: msg.c:286
uint32_t f32_t
32 bit representation for float.
Definition: float.h:30
bool dbWriteF32(DataBuffer *buf, f32_t flt)
Writes a 32-bit floating point.
Definition: databuffer.c:318
bool msgRxBufCheck(DataBuffer *buf)
Checks the received buffer, and logs an error if there is something wrong.
Definition: msg.c:141
bool dbWriteU8(DataBuffer *buf, uint8_t byte)
Writes a unsigned byte.
Definition: databuffer.c:146
If defined, events will not require an acknowledge.
Definition: msg.h:274
#define DB_BUF_INIT(PTR, LEN)
Simple buffer initialization.
Definition: databuffer.h:63
bool _msgInsAHRSGetReg(MsgId *id, DataBuffer *buf)
Reads a register of the AHRS.
Definition: msg_ins.c:24
bool ahrsReadReg(I2C_Device *dev, uint8_t addr, uint8_t reg_addr, f32_t *value)
Reads an AHRS register.
Definition: ahrs.c:68
uint16_t msgId
the message identifier
Definition: msg.h:277
Configures the board-specific peripherals, like I2C, SPI etc...