Separate a float into 4 uint8_ts and then merge back together - c++

I'm working on developing both the client(C) and server(C++) side of an RF connection. I need to send a float value, but the way the architecture is set up I have to arrange my message in a struct that limits me to 3 uint8t parameters: p0, p1, p2. My solution was to break the float into an array of 4 uint8_ts and send in 2 separate messages and use p0 as an identifier whether the message contains the first or second half.
So far I have something like this:
Server (C++):
sendFloat(float f)
{
messageStruct msg1, msg2;
uint8_t* array = (uint8_t*)(&f);
msg1.p0 = 1; //1 means it's the first half
msg1.p1 = array[0];
msg1.p2 = array[1];
msg2.p0 = 0; //0 means it's the second half
msg2.p1 = array[2];
msg2.p2 = array[3];
sendOverRf(msg1);
sendOverRf(msg2);
}
Client(C):
processReceivedMessage (uint32_t id, uint32_t byteA, uint32_t byteB) //(p0,p1,p2) are routed here
{
static uint32_t firsHalfOfFloat;
uint32_t ondHalfOfFloat;
float combinedFloat;
if(id == 1) //first half
{
firstHalfOfFloat = (byteA << 8) | byteB;
}
else //second half
{
secondHalfOfFloat = (byteA << 8) | byteB;
combinedFloat = (float)((firstHalfOfFloat << 16) | secondHalfOfFloat);
}
writeFloatToFile(combinedFloat);
}
then on request the client must then send that float back
Client(C):
sendFloatBack(uint8_t firstHalfIdentifier) // is commanded twice by server with both 0 and 1 ids
{
messageStruct msg;
float f = getFloatFromFile();
uint8_t* array = (uint8_t*)(&f);
msg.p0 = firstHalfIdentifier;
if(firstHalfIdentifier == 1) //First half
{
msg.p1 = array[0];
msg.p2 = array[1];
}
else //Second half
{
msg.p1 = array[2];
msg.p2 = array[3];
}
sendOverRf(msg);
}
and finally the Server (C++) gets the value back:
retrieveFunc()
{
float f;
uint32_t firstHalf;
uint32_t secondHalf;
messageStruct msg = recieveOverRf();
firstHalf = (msg.p1 << 8) | msg.p2;
msg = receiveOverRf();
firstHalf = (msg.p1 << 8) | msg.p2;
f = (firstHalf << 16) | secondHalf;
}
but I'm getting really wrong values back. Any help would be great.

Unions are a very convenient way to disassemble a float into individual bytes and later put the bytes back together again. Here's some example code showing how you can do it:
#include <stdio.h>
#include <stdint.h>
typedef union {
uint8_t _asBytes[4];
float _asFloat;
} FloatBytesConverter;
int main(int argc, char** argv)
{
FloatBytesConverter fbc;
fbc._asFloat = 3.14159;
printf("Original float value is: %f\n", fbc._asFloat);
printf("The bytes of the float are: %u, %u, %u, %u\n"
, fbc._asBytes[0]
, fbc._asBytes[1]
, fbc._asBytes[2]
, fbc._asBytes[3]);
// Now let's put the float back together from the individual bytes
FloatBytesConverter ac;
ac._asBytes[0] = fbc._asBytes[0];
ac._asBytes[1] = fbc._asBytes[1];
ac._asBytes[2] = fbc._asBytes[2];
ac._asBytes[3] = fbc._asBytes[3];
printf("Restored float is %f\n", ac._asFloat);
return 0;
}

memcpy is your friend.
float toFloat(const uint8_t *arr)
{
float result;
memcpy(&result, arr, sizeof(result));
return result;
}
uint8_t *toArray(const float x, uint8_t * const arr)
{
memcpy(arr, &x, sizeof(x));
return arr;
}
void sendFloat(float f)
{
messageStruct msg1, msg2;
uint8_t array[4];
toArray(f, array);
msg1.p0 = 1; //1 means it's the first half
msg1.p1 = array[0];
msg1.p2 = array[1];
msg2.p0 = 0; //0 means it's the second half
msg2.p1 = array[2];
msg2.p2 = array[3];
sendOverRf(msg1);
sendOverRf(msg2);
}
float retrieveFunc(void)
{
float f;
unit8_t array[4]
messageStruct msg = recieveOverRf();
array[0] = msg.p1;
array[1] = msg.p2;
msg = receiveOverRf();
array[2] = msg.p1;
array[3] = msg.p2;
return toFloat(array);
}

Well, bytes are bytes as far as architectures are concerned.
We assume that we're using IEEE 754 (or whatever) on both sides.
We can put one float (4 bytes) into/outof one uint32_t with memcpy
But, we have to deal with processor endianness.
Edit:
problem with this is that p0, p1, p2 are all uint8_ts. – 
TheBigJabronie
Oops, my bad. I've updated the code to use only bytes [I've left my original/incorrect answer below for reference].
Here is the updated code. The functions will work on either host, regardless of the endianness of each architecture.
Note that your main issue is the encoding of the float. So, the code below assumes that the packets arrive intact (i.e. retry/resend is done in the lower RF layer)
void
sendFloat(float f)
{
messageStruct msg;
uint32_t i32;
assert(sizeof(float) == sizeof(uint32_t));
// get bytes of the float in native endian order
memcpy(&i32,&f,sizeof(i32));
// handle endianness
i32 = htonl(i32);
// send MSW half
msg.p0 = 1;
msg.p1 = i32 >> 24;
msg.p2 = i32 >> 16;
sendOverRf(msg);
// send LSW half
msg.p0 = 2;
msg.p1 = i32 >> 8;
msg.p2 = i32 >> 0;
sendOverRf(msg);
}
float
recvFloat(void)
{
uint32_t i32 = 0;
float f;
messageStruct msg;
// NOTE: the two packets _should_ come in the same order as the sender, but
// we'll handle out of order packets to be complete
for (int rcount = 0; rcount < 2; ++rcount) {
msg = recieveOverRf();
uint32_t tmp = msg.p1;
tmp <<= 8;
tmp |= msg.p2;
switch (msg.p0) {
case 1:
i32 |= tmp << 16;
break;
case 2:
i32 |= tmp << 0;
break;
}
}
// handle endianness
i32 = ntohl(i32);
// get bytes into float
memcpy(&f,&i32,sizeof(float));
return f;
}
My original code and further assumptions.
We can do this in a single message with room to spare.
Here is the code I would use.
void
sendFloat(float f)
{
messageStruct msg;
uint32_t i32;
assert(sizeof(float) == sizeof(uint32_t));
// get bytes of the float in native endian order
memcpy(&i32,&f,sizeof(i32));
// handle endianness
i32 = htonl(i32);
// means we're sending a float
msg.p0 = CMD_FLOAT;
msg.p1 = i32;
msg.p2 = 0;
sendOverRf(msg);
}
float
recvFloat(void)
{
uint32_t i32;
float f;
messageStruct msg = recieveOverRf();
// ensure we got correct message
if (msg.p0 != CMD_FLOAT)
exit(1);
// get int in network order
i32 = msg.p1;
// handle endianness
i32 = ntohl(i32);
// get bytes into float
memcpy(&f,&i32,sizeof(float));
return f;
}

Related

Arduino Wire requestFrom freezes

I'm trying to program a class to control the MPU6050 with the Arduino Wire library but when I run the code in my Arduino mini it freezes after a few seconds.
There is the code of the library and a test sketch:
// Include Wire Library for I2C
#include <Wire.h>
enum MPU6050_filter {_256Hz, _188Hz, _98Hz, _42Hz, _20Hz, _10Hz, _5Hz};
enum MPU6050_gyro {_250dps, _500dps, _1000dps, _2000dps};
enum MPU6050_accel {_2g, _4g, _8g, _16Hz};
class MPU6050
{
public:
MPU6050 ();
bool start (bool AD0_value);
void goToSleep ();
void stopSleeping ();
void setFilterVal (MPU6050_filter filter_val);
void setGyroRange (MPU6050_gyro range);
void setAccelRange (MPU6050_accel range);
bool dataAvailable ();
void getLastGyroData (float& gx, float& gy, float& gz);
void getRawGyroData (int& gx, int& gy, int& gz);
private:
void writeRegister (byte address, byte data);
byte readRegister (byte address);
void readData (byte start_address, byte bytes, byte* data);
float convertGyroToDPS (int gyro);
bool AD0_val;
MPU6050_filter filter;
MPU6050_accel accel_range;
MPU6050_gyro gyro_range;
unsigned long last_read;
const unsigned long min_read_time = 1;
};
MPU6050::MPU6050 () : AD0_val(false),
filter(_256Hz),
accel_range(_2g),
gyro_range(_250dps) {}
bool MPU6050::start (bool AD0_value)
{
AD0_val = AD0_value;
// init sample rate div to 0 (max sample rate)
writeRegister(0x19, 0);
// activate FIFO for gyroscope data
writeRegister(0x23, 0x70);
// clear config setup register
writeRegister(0x6B, 0);
// setup the register
writeRegister(0x37, 0x10);
// set interrupt by data ready
writeRegister(0x38, 0x01);
}
void MPU6050::goToSleep ()
{
byte prev_data = readRegister(0x6B);
prev_data = (prev_data | 0x40);
writeRegister(0x6B, prev_data);
}
void MPU6050::stopSleeping ()
{
byte prev_data = readRegister(0x6B);
prev_data = (prev_data & 0xBF);
writeRegister(0x6B, prev_data);
}
void MPU6050::setFilterVal (MPU6050_filter filter_val)
{
int val;
if (filter_val == _256Hz) val = 0;
else if (filter_val == _188Hz) val = 1;
else if (filter_val == _98Hz) val = 2;
else if (filter_val == _42Hz) val = 3;
else if (filter_val == _20Hz) val = 4;
else if (filter_val == _10Hz) val = 5;
else val = 6;
byte data = readRegister(0x1A);
data = (data & 0xF8) | (val & 0x07);
writeRegister(0x1A, data);
filter = filter_val;
}
void MPU6050::setAccelRange (MPU6050_accel range)
{
byte value;
if (range == _2g) value = 0;
else if (range == _4g) value = 1;
else if (range == _8g) value = 2;
else value = 3;
byte reg_value = readRegister(0x1C);
reg_value = (reg_value & 0xE0) | (value << 3);
writeRegister(0x1C, reg_value);
accel_range = range;
}
void MPU6050::setGyroRange (MPU6050_gyro range)
{
byte value;
if (range == _250dps) value = 0;
else if (range == _500dps) value = 1;
else if (range == _1000dps) value = 2;
else value = 3;
byte reg_value = readRegister(0x1B);
reg_value = (reg_value & 0xE0) | (value << 3);
writeRegister(0x1B, reg_value);
gyro_range = range;
}
bool MPU6050::dataAvailable ()
{
return (readRegister(0x3A) & 0x01);
}
void MPU6050::getLastGyroData (float& gx, float& gy, float& gz)
{
int raw_x, raw_y, raw_z;
getRawGyroData(raw_x, raw_y, raw_z);
gx = convertGyroToDPS(raw_x);
gy = convertGyroToDPS(raw_y);
gz = convertGyroToDPS(raw_z);
}
void MPU6050::getRawGyroData (int& gx, int& gy, int& gz)
{
byte* data = new byte[6];
readData(0x43, 6, data);
gx = data[0] << 8 | data[1];
gy = data[2] << 8 | data[3];
gz = data[4] << 8 | data[5];
delete data;
}
void MPU6050::writeRegister (byte address, byte data)
{
Wire.beginTransmission(0x68 + AD0_val);
Wire.write(address);
Wire.write(data);
Wire.endTransmission();
}
byte MPU6050::readRegister (byte address)
{
byte data_buff = 0x00;
Wire.beginTransmission(byte(0x68 + AD0_val));
//Send the requested starting register
Wire.write(address);
//End the transmission
Wire.endTransmission(false);
//Request 14 bytes from the MPU-6050
Wire.requestFrom(byte(0x68 + AD0_val), byte(0x01), byte(true));
unsigned long initial_time = millis();
//Wait until all the bytes are received
while(Wire.available() == 0 and millis() < initial_time + 5);
if (millis() < initial_time + 5)
{
// read the data
data_buff = Wire.read();
}
// end the transmission
Wire.endTransmission();
return data_buff;
}
void MPU6050::readData (byte start_address, byte bytes, byte* data)
{
Wire.beginTransmission(byte(0x68 + AD0_val));
//Send the requested starting register
Wire.write(start_address);
//End the transmission
Wire.endTransmission(false);
//Request 14 bytes from the MPU-6050
Wire.requestFrom(byte(0x68 + AD0_val), bytes, byte(true));
//Wait until all the bytes are received
while(Wire.available() < bytes);
for (int i = 0; i < bytes; i++)
data[i] = Wire.read();
Wire.endTransmission();
}
float MPU6050::convertGyroToDPS (int gyro)
{
if (gyro_range == _250dps) return float(gyro)/131.0;
else if (gyro_range == _500dps) return float(gyro)/65.5;
else if (gyro_range == _1000dps) return float(gyro)/32.8;
else return float(gyro)/16.4;
}
#define SHOW_EACH 50
MPU6050 chip;
unsigned long last_shown = 0;
unsigned this_fps = 0;
unsigned last_fps = 0;
unsigned last_time = 0;
unsigned total_fps = 0;
float g_x, g_y, g_z;
void setup()
{
Serial.begin(115200);
Serial.println("--------");
chip.setFilterVal(_256Hz);
chip.setGyroRange(_250dps);
chip.start(false);
}
void loop()
{
if (chip.dataAvailable())
chip.getLastGyroData(g_x, g_y, g_z);
++this_fps;
++total_fps;
if (millis()/1000 != last_time)
{
last_time = millis()/1000;
last_fps = this_fps;
this_fps = 0;
}
if (millis() - last_shown >= SHOW_EACH)
{
last_shown = millis();
Serial.print(g_x);
Serial.print(" ");
Serial.print(g_y);
Serial.print(" ");
Serial.print(g_y);
Serial.print(" ");
Serial.print(last_fps);
Serial.print(" ");
Serial.println(total_fps);
}
}
Some testing with Serial.println points to the function requestFrom from the Wire library. What can be the cause?
Sorry that i write this as an answer, but I can't write comments yet.
1st. There are multiple requestFrom() calls in your code, so it would be better to specify where does the problem occure exactly (if you can).
2nd. Are you completely sure that, it's the requestFrom() where your code hang. In readData() there is a while() just after requestFrom(). Maybe it hangs there, as the other device don't send enough bytes (for some reasons).
Anyway this might help a litle (link), here they recommend to always check the return value of endTransmission().

how to fill the "data field" of wavfile

Hi i am trying to record from a board and i have successfully record 4 seconds. Problem is when i try to record for more time, i got an error telling me that there not enough memory. my target is to record a 5 minutes file. Until now i have create a buffer named snIn[256] where are the samples. i send it to a big buffer of [16K * 4sec] and when it is full, i create the wav file.
#include "SAI_InOut.hpp"
#include "F746_GUI.hpp"
#include "Delay.hpp"
#include "WaveformDisplay.hpp"
#include "SDFileSystem.h"
#include "wavfile.h"
using namespace Mikami;
#define RES_STR_SIZE 0x20
#define WAVFILE_SAMPLES_PER_SECOND 16000
#define REC_TIME 4
//Create an SDFileSystem object
SDFileSystem sd("sd");
bool flag = 1;
int count = 0;
char *res_buf;
int rp = 0;
const int NUM_SAMPLES = WAVFILE_SAMPLES_PER_SECOND * REC_TIME;
Array<int16_t> my_buffer(NUM_SAMPLES);
int j = 0;
static const char *target_filename = "/sd/rectest.wav";
const int SEG_SIZE = 256;
int sent_array = 0;
int rec(const char *filename, Array<int16_t> my_buffer)
{
j = 0;
flag = 0;
sent_array = 0;
WavFileResult result;
wavfile_info_t info;
wavfile_data_t data;
WAVFILE_INFO_AUDIO_FORMAT(&info) = 1;
WAVFILE_INFO_NUM_CHANNELS(&info) = 1;
WAVFILE_INFO_SAMPLE_RATE(&info) = WAVFILE_SAMPLES_PER_SECOND;
WAVFILE_INFO_BITS_PER_SAMPLE(&info) = 16;
WAVFILE_INFO_BYTE_RATE(&info) = WAVFILE_INFO_NUM_CHANNELS(&info) * WAVFILE_INFO_SAMPLE_RATE(&info) * (WAVFILE_INFO_BITS_PER_SAMPLE(&info) / 8);
WAVFILE_INFO_BLOCK_ALIGN(&info) = 2;
WAVFILE *wf = wavfile_open(filename, WavFileModeWrite, &result);
if (result != WavFileResultOK) {
wavfile_result_string(result, res_buf, RES_STR_SIZE);
printf("%s", res_buf);
return result;
} else printf ("Open file success \r\n");
rp = 0;
WAVFILE_DATA_NUM_CHANNELS(&data) = 1;
result = wavfile_write_info(wf, &info);
if (result != WavFileResultOK) {
wavfile_result_string(result, res_buf, RES_STR_SIZE);
printf("%s", res_buf);
return result; } else printf ("Write info success \r\n");
while ( rp < NUM_SAMPLES ) {
WAVFILE_DATA_CHANNEL_DATA(&data, 0) = my_buffer[rp];
result = wavfile_write_data(wf, &data);
rp += 1;
}
if (result != WavFileResultOK) {
wavfile_result_string(result, res_buf, RES_STR_SIZE);
printf("%s", res_buf);
return result; } else printf ("Write Data file success \r\n");
result = wavfile_close(wf);
if (result != WavFileResultOK) {
wavfile_result_string(result, res_buf , RES_STR_SIZE);
printf("%s", res_buf);
return result; } else printf ("Close file success \r\n");
//UnMount the filesystem
sd.unmount();
printf("Success rec !\r\n");
return 0;
}
int main()
{
//Mount the filesystem
sd.mount();
const float MAX_DELAY = 0.5f; // 最大遅延,単位:秒
const int FS = I2S_AUDIOFREQ_16K; // 標本化周波数: 16 kHz
const uint32_t MAX_ARRAY_SIZE = (uint32_t)(MAX_DELAY*FS);
SaiIO mySai(SaiIO::BOTH, 256, FS, INPUT_DEVICE_DIGITAL_MICROPHONE_2);
Label myLabel(185, 10, "Delay System", Label::CENTER, Font16);
// ButtonGroup: "ON", "OFF"
const uint16_t BG_LEFT = 370;
const uint16_t BG_WIDTH = 100;
const uint16_t BG_HEIGHT = 45;
ButtonGroup onOff(BG_LEFT, 40, BG_WIDTH/2, BG_HEIGHT,
2, (string[]){"ON", "OFF"}, 0, 0, 2, 1);
const uint16_t SB_LEFT = BG_LEFT - 320;
const uint16_t SB_WIDTH = 270;
const uint16_t SB_Y0 = 240;
char str[20];
sprintf(str, " %3.1f [s]", MAX_DELAY);
SeekBar barDelay(SB_LEFT, SB_Y0, SB_WIDTH,
0, MAX_ARRAY_SIZE, 0, "0", "", str);
NumericLabel<float> labelDelay(SB_LEFT+SB_WIDTH/2, SB_Y0-40, "DELEY: %4.2f", 0, Label::CENTER);
DelaySystem delaySystem(MAX_ARRAY_SIZE);
WaveformDisplay displayIn(*GuiBase::GetLcdPtr(), SB_LEFT+7, 70, 256, 9,LCD_COLOR_WHITE, LCD_COLOR_CYAN,GuiBase::ENUM_BACK);
Label inLabel(SB_LEFT-30, 65, "IN");
WaveformDisplay displayOut(*GuiBase::GetLcdPtr(), SB_LEFT+7, 130, 256, 9,LCD_COLOR_WHITE, LCD_COLOR_CYAN,GuiBase::ENUM_BACK);
Label outLabel(SB_LEFT-30, 125, "OUT");
int runStop = 1;
Array<int16_t> snIn(mySai.GetLength());
Array<int16_t> snOut(mySai.GetLength());
mySai.RecordIn();
mySai.PlayOut();
mySai.PauseOut();
while (true)
{
// On/OFF
int num;
if (onOff.GetTouchedNumber(num))
if (runStop != num)
{
if (num == 0) mySai.ResumeOut();
else mySai.PauseOut();
runStop = num;
}
if (mySai.IsCompleted())
{
for (int n=0; n<mySai.GetLength() ; n++)
{
int16_t xL, xR;
mySai.Input(xL,xR);
int16_t xn = xL + xR;
snIn[n] = xn;
my_buffer[j] = xn;
j++;
if (j == NUM_SAMPLES && flag == 1) {
rec (target_filename , my_buffer); }
int16_t yn = delaySystem.Execute(xn);
mySai.Output(yn, yn);
snOut[n] = yn;
}
mySai.Reset();
displayIn.Execute(snIn);
}
}
}
I thought about a possible solution, to fill directly the "data field" of the wavefile with the snIn[256] buffer (instead of using my_buffer) again and again and at the end close the wavfile. Please let me know what you think about that and other solutions
things to note: 1) while a write operation is being performed, more data is still coming in.
At the very least I would double buffer that data, so can be writing one buffer while the other one fills.
Usually this means using an interrupt to collect the samples (into which ever buffer is currently being filed.)
the foreground program waits for the current buffer to be 'full', then initiates write operation.,
then waits again for a buffer to be 'full'
The interrupt function tracks which buffer is being filled and the current index into that buffer. When a buffer is full, set a 'global' status to let the foreground program know which buffer is ready to be written.
The foreground program writes the buffer, then resets the status for that buffer.

HMC5883L library not showing float's decimals

I'm using HMC5883L sensor to measure magnetic field but when I look to the data on my ESP8266 it misses the decimals... The value should be, for example, "-234.54" and I got only "-234.00". I'm trying to fix this looking into the library but it didn't work.
My structures in HMC5883L.h:
struct MagnetometerScaled
{
float XAxis;
float YAxis;
float ZAxis;
};
struct MagnetometerRaw
{
float XAxis;
float YAxis;
float ZAxis;
};
Parts of HMC5883L.cpp in use:
MagnetometerRaw HMC5883L::ReadRawAxis()
{
const int8_t* buffer = Read(DataRegisterBegin, 6);
MagnetometerRaw raw = MagnetometerRaw();
raw.XAxis = (buffer[0] << 8) | buffer[1];
raw.ZAxis = (buffer[2] << 8) | buffer[3];
raw.YAxis = (buffer[4] << 8) | buffer[5];
delete [] buffer;
return raw;
}
MagnetometerScaled HMC5883L::ReadScaledAxis()
{
MagnetometerRaw raw = ReadRawAxis();
MagnetometerScaled scaled = MagnetometerScaled();
scaled.XAxis = raw.XAxis * m_Scale; // m_scale = 0.92 -> A float value
scaled.ZAxis = raw.ZAxis * m_Scale; // m_scale = 0.92 -> A float value
scaled.YAxis = raw.YAxis * m_Scale; // m_scale = 0.92 -> A float value
return scaled;
}
int8_t* HMC5883L::Read(int address, int length)
{
Wire.beginTransmission(HMC5883L_Address);
Wire.write(address);
Wire.endTransmission();
Wire.beginTransmission(HMC5883L_Address);
Wire.requestFrom(HMC5883L_Address, length);
// uint8_t buffer[length];
int8_t *buffer = new int8_t[length];
if(Wire.available() == length)
{
for(uint8_t i = 0; i < length; i++)
{
buffer[i] = Wire.read();
}
}
Wire.endTransmission();
return buffer;
}
In run this code (basically) using:
HMC5883L magnetometer;
MagnetometerScaled scaledVals;
scaledVals = magnetometer.ReadScaledAxis();
Serial.println(scaledVals.XAxis);
If I left some important part of the code behind, just ask for it :)
EDIT1: If I run Serial.println(1.01); it prints the value correctly (output = 1.01). So, Serial.println() is not truncating float values...
EDIT2: As I see, the problem seems to be with the type returned by Read() function. Maybe if I change it to char* instead of int8_t I'll be able to receive the decimals on the right way. My question related to this is with the ReadRawAxis() function. How to get the data from buffer?

Read "varint" from linux sockets

I need to read a VarInts from linux sockets in C/C++. Any library, idea or something?
I tried reading and casting char to bool[8] to try without success to read a VarInt...
Also, this is for compatibility with new Minecraft 1.7.2 communication protocol, so, the documentation of the protocol may also help.
Let me explain my project: I'm making a Minecraft server software to run in my VPS (because java is too slow...) and I got stuck with the protocol. One thread waits for the connections and when it has a new connection, it creates a new Client object and starts the Client thread that starts communicating with the client.
I think that there is no need to show code. In case I'm wrong, tell me and I'll edit with some code.
First off, note that varints are sent as actual bytes, not strings of the characters 1 and 0.
For an unsigned varint, I believe the following will decode it for you, assuming you've got the varint data in a buffer pointed to by data. This example function returns the number of bytes decoded in the reference argument int decoded_bytes.
uint64_t decode_unsigned_varint( const uint8_t *const data, int &decoded_bytes )
{
int i = 0;
uint64_t decoded_value = 0;
int shift_amount = 0;
do
{
decoded_value |= (uint64_t)(data[i] & 0x7F) << shift_amount;
shift_amount += 7;
} while ( (data[i++] & 0x80) != 0 );
decoded_bytes = i;
return decoded_value;
}
To decode a signed varint, you can use this second function that calls the first:
int64_t decode_signed_varint( const uint8_t *const data, int &decoded_bytes )
{
uint64_t unsigned_value = decode_unsigned_varint(data, decoded_bytes);
return (int64_t)( unsigned_value & 1 ? ~(unsigned_value >> 1)
: (unsigned_value >> 1) );
}
I believe both of these functions are correct. I did some basic testing with the code below to verify a couple datapoints from the Google page. The output is correct.
#include <stdint.h>
#include <iostream>
uint64_t decode_unsigned_varint( const uint8_t *const data, int &decoded_bytes )
{
int i = 0;
uint64_t decoded_value = 0;
int shift_amount = 0;
do
{
decoded_value |= (uint64_t)(data[i] & 0x7F) << shift_amount;
shift_amount += 7;
} while ( (data[i++] & 0x80) != 0 );
decoded_bytes = i;
return decoded_value;
}
int64_t decode_signed_varint( const uint8_t *const data, int &decoded_bytes )
{
uint64_t unsigned_value = decode_unsigned_varint(data, decoded_bytes);
return (int64_t)( unsigned_value & 1 ? ~(unsigned_value >> 1)
: (unsigned_value >> 1) );
}
uint8_t ex_p300[] = { 0xAC, 0x02 };
uint8_t ex_n1 [] = { 0x01 };
using namespace std;
int main()
{
int decoded_bytes_p300;
uint64_t p300;
p300 = decode_unsigned_varint( ex_p300, decoded_bytes_p300 );
int decoded_bytes_n1;
int64_t n1;
n1 = decode_signed_varint( ex_n1, decoded_bytes_n1 );
cout << "p300 = " << p300
<< " decoded_bytes_p300 = " << decoded_bytes_p300 << endl;
cout << "n1 = " << n1
<< " decoded_bytes_n1 = " << decoded_bytes_n1 << endl;
return 0;
}
To encode varints, you could use the following functions. Note that the buffer uint8_t *const data should have room for at least 10 bytes, as the largest varint is 10 bytes long.
#include
// Encode an unsigned 64-bit varint. Returns number of encoded bytes.
// 'buffer' must have room for up to 10 bytes.
int encode_unsigned_varint(uint8_t *const buffer, uint64_t value)
{
int encoded = 0;
do
{
uint8_t next_byte = value & 0x7F;
value >>= 7;
if (value)
next_byte |= 0x80;
buffer[encoded++] = next_byte;
} while (value);
return encoded;
}
// Encode a signed 64-bit varint. Works by first zig-zag transforming
// signed value into an unsigned value, and then reusing the unsigned
// encoder. 'buffer' must have room for up to 10 bytes.
int encode_signed_varint(uint8_t *const buffer, int64_t value)
{
uint64_t uvalue;
uvalue = uint64_t( value < 0 ? ~(value << 1) : (value << 1) );
return encode_unsigned_varint( buffer, uvalue );
}

SD.h not compatible with other libraries in Arduino/C++ environment

I'm having some very weird issues using the following hardware elements:
Arduino Uno
Wi-Fi shield
GPS receiver
Accelerometer
Barometer
I wanted to off-load the sensor readings to an SD card as needed, but before I can even code the SD functions, the mere inclusion of the SD.h library renders my code useless.
My code is as follows:
#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <SD.h>
/* This sample code demonstrates the normal use of a TinyGPS object.
It requires the use of SoftwareSerial, and assumes that you have a
4800-baud serial GPS device hooked up on pins 3(rx) and 4(tx).
*/
//For baraometer
#include <Wire.h>
#define BMP085_ADDRESS 0x77 // I2C address of BMP085
const unsigned char OSS = 2; // Oversampling Setting
// Calibration values
int ac1;
int ac2;
int ac3;
unsigned int ac4;
unsigned int ac5;
unsigned int ac6;
int b1;
int b2;
int mb;
int mc;
int md;
// b5 is calculated in bmp085GetTemperature(...), this variable is also used in bmp085GetPressure(...)
// So ...Temperature(...) must be called before ...Pressure(...).
long b5;
//End of baraometer
//ACcelerometer
// These constants describe the pins. They won't change:
const int xpin = A1; // x-axis of the accelerometer
const int ypin = A2; // y-axis
const int zpin = A3; // z-axis (only on 3-axis models)
//end of accel
TinyGPS gps;
SoftwareSerial nss(3, 4);
static void gpsdump(TinyGPS &gps);
static bool feedgps();
static void print_float(float val, float invalid, int len, int prec);
static void print_int(unsigned long val, unsigned long invalid, int len);
static void print_date(TinyGPS &gps);
static void print_str(const char *str, int len);
void setup()
{
//Make sure the analog-to-digital converter takes its reference voltage from
// the AREF pin
analogReference(EXTERNAL);
pinMode(xpin, INPUT);
pinMode(ypin, INPUT);
pinMode(zpin, INPUT);
//Barometer
Wire.begin();
bmp085Calibration();
//GPS
Serial.begin(115200);
nss.begin(57600);
Serial.print("Testing TinyGPS library v. "); Serial.println(TinyGPS::library_version());
Serial.println("by Mikal Hart");
Serial.println();
Serial.print("Sizeof(gpsobject) = "); Serial.println(sizeof(TinyGPS));
Serial.println();
Serial.println("Sats HDOP Latitude Longitude Fix Date Time Date Alt Course Speed Card Distance Course Card Chars Sentences Checksum");
Serial.println(" (deg) (deg) Age Age (m) --- from GPS ---- ---- to London ---- RX RX Fail");
Serial.println("--------------------------------------------------------------------------------------------------------------------------------------");
}
void loop()
{
//Accelerometer
Serial.print( analogRead(xpin));
Serial.print("\t");
//Add a small delay between pin readings. I read that you should
//do this but haven't tested the importance
delay(1);
Serial.print( analogRead(ypin));
Serial.print("\t");
//add a small delay between pin readings. I read that you should
//do this but haven't tested the importance
delay(1);
Serial.print( analogRead(zpin));
Serial.print("\n"); // delay before next reading:
bool newdata = false;
unsigned long start = millis();
// Every second we print an update
while (millis() - start < 1000)
{
if (feedgps())
newdata = true;
}
//barometer
float temperature = bmp085GetTemperature(bmp085ReadUT()); //MUST be called first
float pressure = bmp085GetPressure(bmp085ReadUP());
float atm = pressure / 101325; // "standard atmosphere"
float altitude = calcAltitude(pressure); //Uncompensated caculation - in Meters
Serial.print("Temperature: ");
Serial.print(temperature, 2); //display 2 decimal places
Serial.println(" C");
Serial.print("Pressure: ");
Serial.print(pressure, 0); //whole number only.
Serial.println(" Pa");
Serial.print("Standard Atmosphere: ");
Serial.println(atm, 4); //display 4 decimal places
Serial.print("Altitude: ");
Serial.print(altitude, 2); //display 2 decimal places
Serial.println(" M");
Serial.println();//line break
//end of barometer
gpsdump(gps);
}
static void gpsdump(TinyGPS &gps)
{
float flat, flon;
unsigned long age, date, time, chars = 0;
unsigned short sentences = 0, failed = 0;
static const float LONDON_LAT = 51.508131, LONDON_LON = -0.128002;
print_int(gps.satellites(), TinyGPS::GPS_INVALID_SATELLITES, 5);
print_int(gps.hdop(), TinyGPS::GPS_INVALID_HDOP, 5);
gps.f_get_position(&flat, &flon, &age);
print_float(flat, TinyGPS::GPS_INVALID_F_ANGLE, 9, 5);
print_float(flon, TinyGPS::GPS_INVALID_F_ANGLE, 10, 5);
print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
print_date(gps);
print_float(gps.f_altitude(), TinyGPS::GPS_INVALID_F_ALTITUDE, 8, 2);
print_float(gps.f_course(), TinyGPS::GPS_INVALID_F_ANGLE, 7, 2);
print_float(gps.f_speed_kmph(), TinyGPS::GPS_INVALID_F_SPEED, 6, 2);
print_str(gps.f_course() == TinyGPS::GPS_INVALID_F_ANGLE ? "*** " : TinyGPS::cardinal(gps.f_course()), 6);
print_int(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0UL : (unsigned long)TinyGPS::distance_between(flat, flon, LONDON_LAT, LONDON_LON) / 1000, 0xFFFFFFFF, 9);
print_float(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : TinyGPS::course_to(flat, flon, 51.508131, -0.128002), TinyGPS::GPS_INVALID_F_ANGLE, 7, 2);
print_str(flat == TinyGPS::GPS_INVALID_F_ANGLE ? "*** " : TinyGPS::cardinal(TinyGPS::course_to(flat, flon, LONDON_LAT, LONDON_LON)), 6);
gps.stats(&chars, &sentences, &failed);
print_int(chars, 0xFFFFFFFF, 6);
print_int(sentences, 0xFFFFFFFF, 10);
print_int(failed, 0xFFFFFFFF, 9);
Serial.println();
}
static void print_int(unsigned long val, unsigned long invalid, int len)
{
char sz[32];
if (val == invalid)
strcpy(sz, "*******");
else
sprintf(sz, "%ld", val);
sz[len] = 0;
for (int i=strlen(sz); i<len; ++i)
sz[i] = ' ';
if (len > 0)
sz[len-1] = ' ';
Serial.print(sz);
feedgps();
}
static void print_float(float val, float invalid, int len, int prec)
{
char sz[32];
if (val == invalid)
{
strcpy(sz, "*******");
sz[len] = 0;
if (len > 0)
sz[len-1] = ' ';
for (int i=7; i<len; ++i)
sz[i] = ' ';
Serial.print(sz);
}
else
{
Serial.print(val, prec);
int vi = abs((int)val);
int flen = prec + (val < 0.0 ? 2 : 1);
flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
for (int i=flen; i<len; ++i)
Serial.print(" ");
}
feedgps();
}
static void print_date(TinyGPS &gps)
{
int year;
byte month, day, hour, minute, second, hundredths;
unsigned long age;
gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
if (age == TinyGPS::GPS_INVALID_AGE)
Serial.print("******* ******* ");
else
{
char sz[32];
sprintf(sz, "%02d/%02d/%02d %02d:%02d:%02d ",
month, day, year, hour, minute, second);
Serial.print(sz);
}
print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
feedgps();
}
static void print_str(const char *str, int len)
{
int slen = strlen(str);
for (int i=0; i<len; ++i)
Serial.print(i<slen ? str[i] : ' ');
feedgps();
}
static bool feedgps()
{
while (nss.available())
{
if (gps.encode(nss.read()))
return true;
}
return false;
}
// Stores all of the bmp085's calibration values into global variables
// Calibration values are required to calculate temp and pressure
// This function should be called at the beginning of the program
void bmp085Calibration()
{
Serial.write("\n\nCalibrating ... ");
ac1 = bmp085ReadInt(0xAA);
ac2 = bmp085ReadInt(0xAC);
ac3 = bmp085ReadInt(0xAE);
ac4 = bmp085ReadInt(0xB0);
ac5 = bmp085ReadInt(0xB2);
ac6 = bmp085ReadInt(0xB4);
b1 = bmp085ReadInt(0xB6);
b2 = bmp085ReadInt(0xB8);
mb = bmp085ReadInt(0xBA);
mc = bmp085ReadInt(0xBC);
md = bmp085ReadInt(0xBE);
Serial.write("Calibrated\n\n");
}
// Calculate temperature in deg C
float bmp085GetTemperature(unsigned int ut){
long x1, x2;
x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
x2 = ((long)mc << 11)/(x1 + md);
b5 = x1 + x2;
float temp = ((b5 + 8)>>4);
temp = temp /10;
return temp;
}
// Calculate pressure given up
// calibration values must be known
// b5 is also required so bmp085GetTemperature(...) must be called first.
// Value returned will be pressure in units of Pa.
long bmp085GetPressure(unsigned long up){
long x1, x2, x3, b3, b6, p;
unsigned long b4, b7;
b6 = b5 - 4000;
// Calculate B3
x1 = (b2 * (b6 * b6)>>12)>>11;
x2 = (ac2 * b6)>>11;
x3 = x1 + x2;
b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;
// Calculate B4
x1 = (ac3 * b6)>>13;
x2 = (b1 * ((b6 * b6)>>12))>>16;
x3 = ((x1 + x2) + 2)>>2;
b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;
b7 = ((unsigned long)(up - b3) * (50000>>OSS));
if (b7 < 0x80000000)
p = (b7<<1)/b4;
else
p = (b7/b4)<<1;
x1 = (p>>8) * (p>>8);
x1 = (x1 * 3038)>>16;
x2 = (-7357 * p)>>16;
p += (x1 + x2 + 3791)>>4;
long temp = p;
return temp;
}
// Read 1 byte from the BMP085 at 'address'
char bmp085Read(byte address)
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.write(address);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 1);
while(!Wire.available()) {};
return Wire.read();
}
// Read 2 bytes from the BMP085
// First byte will be from 'address'
// Second byte will be from 'address'+1
int bmp085ReadInt(byte address)
{
unsigned char msb, lsb;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.write(address);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 2);
while(Wire.available()<2)
;
msb = Wire.read();
lsb = Wire.read();
return (int) msb<<8 | lsb;
}
// Read the uncompensated temperature value
unsigned int bmp085ReadUT(){
unsigned int ut;
// Write 0x2E into Register 0xF4
// This requests a temperature reading
Wire.beginTransmission(BMP085_ADDRESS);
Wire.write((byte)0xF4);
Wire.write((byte)0x2E);
Wire.endTransmission();
// Wait at least 4.5 ms
delay(5);
// Read two bytes from registers 0xF6 and 0xF7
ut = bmp085ReadInt(0xF6);
return ut;
}
// Read the uncompensated pressure value
unsigned long bmp085ReadUP(){
unsigned char msb, lsb, xlsb;
unsigned long up = 0;
// Write 0x34+(OSS<<6) into register 0xF4
// Request a pressure reading w/ oversampling setting
Wire.beginTransmission(BMP085_ADDRESS);
Wire.write(0xF4);
Wire.write(0x34 + (OSS<<6));
Wire.endTransmission();
// Wait for conversion, delay time dependent on OSS
delay(2 + (3<<OSS));
// Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)
msb = bmp085Read(0xF6);
lsb = bmp085Read(0xF7);
xlsb = bmp085Read(0xF8);
up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);
return up;
}
void writeRegister(int deviceAddress, byte address, byte val) {
Wire.beginTransmission(deviceAddress); // Start transmission to device
Wire.write(address); // Send register address
Wire.write(val); // Send value to write
Wire.endTransmission(); // End transmission
}
int readRegister(int deviceAddress, byte address){
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address); // Register to read
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1); // Read a byte
while(!Wire.available()) {
// waiting
}
v = Wire.read();
return v;
}
float calcAltitude(float pressure){
float A = pressure/101325;
float B = 1/5.25588;
float C = pow(A,B);
C = 1 - C;
C = C /0.0000225577;
return C;
}
Granted, right now, it is merely a conglomeration of multiple example sketches, but they work. I get a sampled reading from the accelerometer, the GPS unit and the barometer once a second. However once I simply add the line #include <SD.h> to the sketch, it fails to run correctly. The serial monitor does not display anything. I have similar versions of the above sketch (omitted as they are much lengthier), but I get the same result: either jumbled text or nothing on the Serial monitor. If I comment out the line that include the SD.h library, everything works fine....
Are there known issues with the SD.h library or conflicts? And yes, I am NOT using the necessary pins for the SD access (digital pin #4) for my sensor connections....
UPDATE:
I at least figured out it has something to do with the SoftSerial (SoftSerial.h) library and the use of the SoftSerial object (which I called nss). I can load all libraries and get everything to work if I do not call nss.begin. Is there a reason why that would conflict?
Turns out I was out of memory. Having the Serial go unresponsive like that is a common symptom. This link ultimately is what I used to trace and conclude my memory issue.
First thing would be to check the Arduino site, on the SD documentation (here) there's a mention that the communication between the microcontroller and the SD card uses SPI (documentation here) which takes place on digital pins 11, 12 and 13. I wouldn't be surprised if this was the source of your problems with the Serial monitor.
Reading some comments in Sd2Card.h, it might be tricky to get your setup to work properly:
/**
* Define MEGA_SOFT_SPI non-zero to use software SPI on Mega Arduinos.
* Pins used are SS 10, MOSI 11, MISO 12, and SCK 13.
*
* MEGA_SOFT_SPI allows an unmodified Adafruit GPS Shield to be used
* on Mega Arduinos. Software SPI works well with GPS Shield V1.1
* but many SD cards will fail with GPS Shield V1.0.
*/
Even if you put MEGA_SOFT_SPI to a non 0 value, you'd probably still fail to pass the (defined(__AVR_ATmega1280__)||defined(__AVR_ATmega2560__)) check.
I would suggest trying your same sketch without the TinyGPS to try to pinpoint the issue.
Also, check out this sketch it seems to be doing something similar to what you're doing, maybe you can fix yours based on what's done here.
Use pin 4 for CS and change the MOSI, MISO and SCK pins in the library SD in Sd2card.h, hope you will get rid of the problem