diff --git a/src/shared/BusTemplate.h b/src/shared/BusTemplate.h
index 47e6df14ae9b44fbb6a8ecf0964bce6087184e19..9e0eb51bcf14fc8d01ba4e2391d338018750b307 100644
--- a/src/shared/BusTemplate.h
+++ b/src/shared/BusTemplate.h
@@ -73,7 +73,8 @@ private:
inline void _write(uint8_t byte) const {
getSPIAddr(N)->DR=byte;
while((getSPIAddr(N)->SR & SPI_SR_RXNE)==0);
- byte=getSPIAddr(N)->DR;
+ volatile uint8_t temp;
+ temp = getSPIAddr(N)->DR;
}
inline int _read(void* buffer, size_t max_len) const {
diff --git a/src/shared/boards/AnakinBoard.cpp b/src/shared/boards/AnakinBoard.cpp
index 93420571952f20fab184e8a0b5ba9121140ac2f3..c4f009f037f7c33472cb970e53413ad6b4bd1a9f 100644
--- a/src/shared/boards/AnakinBoard.cpp
+++ b/src/shared/boards/AnakinBoard.cpp
@@ -4,10 +4,6 @@
if (!x->init()) { sLog->logString("=== ERR: CANNOT INIT " #x); } \
} while(0)
-#define ADD_SAMPLER(type, name, rate) \
- sEventScheduler->add(std::bind(& type ## SensorSampler::Update,name), \
- rate, #name "-" #rate "ms")
-
AnakinBoard::AnakinBoard()
{
mInited = false;
@@ -78,6 +74,10 @@ bool AnakinBoard::init()
m10HzSimple.AddSensor(mS_MS580);
sLog->logString("Adding samplers to scheduler\n");
+ #define ADD_SAMPLER(type, name, rate) \
+ sEventScheduler->add(std::bind(& type ## SensorSampler::Update,name),\
+ rate, #name "-" #rate "ms")
+
ADD_SAMPLER(DMA, m100HzDMA, 10); // 10ms
ADD_SAMPLER(DMA, m25HzDMA, 40); // 25ms
ADD_SAMPLER(Simple, m100HzSimple, 10); // 10ms
diff --git a/src/shared/log/Log.h b/src/shared/log/Log.h
index 9e11e6f01a819a9c7d6f65c320a1c4a53f394175..3be1e16ffdc9add2275d41651034f5e51f7f2f1d 100644
--- a/src/shared/log/Log.h
+++ b/src/shared/log/Log.h
@@ -44,7 +44,7 @@ public:
std::vector<uint8_t> buf(2 + sizeof(float));
buf[0] = DATA_FLOAT;
buf[1] = id;
- memcpy(&buf[2], &data, sizeof(float));
+ *((float *)&buf[2]) = data;
queue(std::move(buf));
}
@@ -56,9 +56,9 @@ public:
buf[0] = DATA_VEC3;
buf[1] = id;
- tmp = data.getX(); memcpy(&buf[2], &tmp, sizeof(float));
- tmp = data.getY(); memcpy(&buf[6], &tmp, sizeof(float));
- tmp = data.getZ(); memcpy(&buf[10], &tmp, sizeof(float));
+ tmp = data.getX(); *((float *)&buf[2]) = tmp;
+ tmp = data.getY(); *((float *)&buf[6]) = tmp;
+ tmp = data.getZ(); *((float *)&buf[10]) = tmp;
queue(std::move(buf));
}
@@ -89,15 +89,17 @@ private:
Log() : ActiveObject(1024)
{
+ /*
struct termios t;
tcgetattr(STDIN_FILENO, &t);
t.c_lflag &= ~(ISIG | ICANON | ECHO);
tcsetattr(STDIN_FILENO,TCSANOW, &t);
+ */
}
void write(const std::vector<uint8_t>& data)
{
- const char map[]="0123456789abcdef\r\n";
+ static const char map[]="0123456789abcdef";
for(size_t i=0;i<data.size();i++)
{
const char& di = data[i];
@@ -106,7 +108,7 @@ private:
out[1] = map[di & 0x0f];
::write(1, out, 2);
}
- ::write(1, &map[16], 2);
+ ::write(1, "\r\n", 2);
}
void queue(std::vector<uint8_t>&& data)
diff --git a/src/shared/sensors/FXAS21002.h b/src/shared/sensors/FXAS21002.h
index 4a564af7bdb74ad4490383b4ef3238f95dbfad6f..cfba4bf2807a26b4b412bff882cd46576d39b965 100644
--- a/src/shared/sensors/FXAS21002.h
+++ b/src/shared/sensors/FXAS21002.h
@@ -125,7 +125,7 @@ public:
std::vector<SPIRequest> buildDMARequest() override
{
std::vector<uint8_t> data = {
- REG_OUT_X_MSB | 0x80, 0,
+ REG_OUT_X_MSB | 0x80,
0,0,0,0,0,0
};
@@ -136,7 +136,7 @@ public:
{
const auto& r = req.readResponseFromPeripheral();
int16_t data[3];
- memcpy(data, &r[2], sizeof(data));
+ memcpy(data, &r[1], sizeof(data));
for(int i=0;i<3;i++)
data[i] = fromBigEndian16(data[i]);
diff --git a/src/shared/sensors/LPS331AP.h b/src/shared/sensors/LPS331AP.h
index 41ebf69d2ebed22d58833966488069c68454c8c1..5af67789b16baa3dd73893a3c124c888d28824c5 100644
--- a/src/shared/sensors/LPS331AP.h
+++ b/src/shared/sensors/LPS331AP.h
@@ -71,7 +71,7 @@ public:
{
return {
SPIRequest(0, Bus::getCSPin(), {
- REG_STATUS | 0xc0, 0,
+ REG_STATUS | 0xc0,
0,0,0,0, // pressure (int32_t)
0,0 // temperature (int16_t)
})
@@ -81,7 +81,7 @@ public:
void onDMAUpdate(const SPIRequest& req) override
{
const auto& r = req.readResponseFromPeripheral();
- const data_t *data = (const data_t*) &r[2];
+ const data_t *data = (const data_t*) &r[1];
// Remove status and realign bytes
int32_t pressure = data->press >> 8;
diff --git a/src/shared/sensors/MAX21105.h b/src/shared/sensors/MAX21105.h
index 66f48879c9cebabf664d072db77888d509cdd338..e78b09469f969e28980ae86d0ae9e73c0e9ca2fa 100644
--- a/src/shared/sensors/MAX21105.h
+++ b/src/shared/sensors/MAX21105.h
@@ -109,7 +109,7 @@ public:
{
return { SPIRequest(0, Bus::getCSPin(),
{
- (GYRO_X_H | 0x80), 0,
+ (GYRO_X_H | 0x80),
0,0,0,0,0,0, // gyro
0,0, // accel
0,0,0,0,0,0, // temp
@@ -120,7 +120,7 @@ public:
void onDMAUpdate(const SPIRequest& req) override
{
const auto& r = req.readResponseFromPeripheral();
- const int16_t* ptr = (const int16_t*) &r[2];
+ const int16_t* ptr = (const int16_t*) &r[1];
rawdata_t raw_data;
constexpr size_t dSize = 7; // 3 (gyro) + 3 (accel) + 1 (temp)
diff --git a/src/shared/sensors/MPU9250.h b/src/shared/sensors/MPU9250.h
index 4cba3202a5691f3007c8fefd147e48f1db12cdeb..9c7d2d96f19092b37939ebd4ed195e250f438661 100644
--- a/src/shared/sensors/MPU9250.h
+++ b/src/shared/sensors/MPU9250.h
@@ -38,7 +38,6 @@ class MPU9250 : public GyroSensor, public AccelSensor,
typedef union {
struct {
uint8_t status1;
- uint8_t padding;
int16_t mag[3];
uint8_t status2;
};
@@ -72,7 +71,7 @@ public:
{
std::vector<uint8_t> v =
{
- (REG_ACCEL_XOUT_H | 0x80), 0,
+ (REG_ACCEL_XOUT_H | 0x80),
0,0,0,0,0,0, // accel
0,0, // temp
0,0,0,0,0,0, // gyro
@@ -86,7 +85,7 @@ public:
const auto& r = req.readResponseFromPeripheral();
mpudata_t raw_data;
- memcpy(&raw_data.buf, &(r[2]), sizeof(raw_data));
+ memcpy(&raw_data.buf, &(r[1]), sizeof(raw_data));
for(int i=0;i<3;i++)
{
raw_data.accel[i] = fromBigEndian16(raw_data.accel[i]);
diff --git a/src/shared/sensors/iNemo.h b/src/shared/sensors/iNemo.h
index 952f86a6b8ce012b5699313f5d0b6a5d4166dcf1..baa9301c15f3f21eb53cd56a1bbc593c86ddbc02 100644
--- a/src/shared/sensors/iNemo.h
+++ b/src/shared/sensors/iNemo.h
@@ -102,13 +102,13 @@ public:
{
return {
SPIRequest(DMA_GYRO, BusG::getCSPin(),
- { OUT_X_L_G | 0xc0, 0,0,0,0,0,0,0}),
+ { OUT_X_L_G | 0xc0, 0,0,0,0,0,0}),
SPIRequest(DMA_ACC, BusXM::getCSPin(),
- { OUT_X_L_A | 0xc0, 0,0,0,0,0,0,0}),
+ { OUT_X_L_A | 0xc0, 0,0,0,0,0,0}),
SPIRequest(DMA_COMP, BusXM::getCSPin(),
- { OUT_X_L_M | 0xc0,0,0,0,0,0,0,0}),
+ { OUT_X_L_M | 0xc0, 0,0,0,0,0,0}),
SPIRequest(DMA_TEMP, BusXM::getCSPin(),
- { OUT_TEMP_L_XM | 0xc0, 0,0,0}),
+ { OUT_TEMP_L_XM | 0xc0, 0,0}),
};
}
@@ -117,7 +117,7 @@ public:
const auto& r = req.readResponseFromPeripheral();
int16_t data[3];
- memcpy(data, &r[2], r.size()-2);
+ memcpy(data, &r[1], r.size()-1);
// printf("ID: %d --> ", req.id()); memDump(r.data(), r.size());
switch(req.id())