char *my_ltoa(
long val,
char *buf,
unsigned int base)
{
unsigned long uval;
if (val < 0)
uval = -val, *buf++ = '-';
else
uval = val;
return my_ultoa(uval, buf, base);
}
unsigned short MakeDataPacket(sampleData_t* sample , unsigned char mode, void** start)
{
// Get sample
static char output[100]; // Ensure you dont overrun this buffer
char *p = output;
unsigned short num = 0;
// Ascii mode
if ((mode == 0)||(mode == 128))
{
// ascii mode
// Print string
p += my_nitoa(p, sample->sampleCount); *p++ = ',';
p += my_nitoa(p, sample->sensor.accel.x); *p++ = ',';
p += my_nitoa(p, sample->sensor.accel.y); *p++ = ',';
p += my_nitoa(p, sample->sensor.accel.z); *p++ = ',';
p += my_nitoa(p, sample->sensor.gyro.x); *p++ = ',';
p += my_nitoa(p, sample->sensor.gyro.y); *p++ = ',';
p += my_nitoa(p, sample->sensor.gyro.z); *p++ = ',';
p += my_nitoa(p, sample->sensor.mag.x); *p++ = ',';
p += my_nitoa(p, sample->sensor.mag.y); *p++ = ',';
p += my_nitoa(p, sample->sensor.mag.z);
// If top bit set then append extra data
if ((mode & 0x80)||(status.newXdata==1))
{
*p++ = ',';
p += my_nitoa(p, status.battmv); *p++ = ',';
p += my_nitoa(p, status.temperature); *p++ = ',';
{
char * ptr = (char*)my_ultoa(status.pressure);
unsigned char len = strlen(ptr);
memcpy(p,ptr,len);
p+=len;
*p++ = ',';
}
p += my_nitoa(p, status.inactivity);
}
*p++ = '\r';*p++ = '\n'; *p = '\0';
// Count bytes
num = p - output;
}
// Binary slip mode
if (mode == 1)
{
// binary mode 1
*p++ = SLIP_END;
*p++ = NINE_AXIS_PACKET;
if (status.newXdata==1) *p++ = PACKET_V2;
else *p++ = PACKET_V1;
// KL: to keep compatibility with original format
// (TODO, migrate to better format with sample at end aswell for better framing)
p += slip_encode(p, &sample->sampleCount, sizeof(unsigned short));
p += slip_encode(p, &sample->sampleTicks, sizeof(unsigned long));
p += slip_encode(p, &sample->sensor, sizeof(sensor_t));
if (status.newXdata == 1)
{
p += slip_encode(p, &status.battmv, sizeof(unsigned short));
p += slip_encode(p, &status.temperature, sizeof(short));
p += slip_encode(p, &status.pressure, sizeof(unsigned long));
}
*p++ = SLIP_END;
// Count bytes
num = p - output;
}
// 20 byte special mode for ble attribute notifications
if (mode == 2)
{
// 20 Byte payload for BLE applications
// Copy sample to static
memcpy(&output[0],sample,(sizeof(sampleData_t)-4)); // 20
// Set number of bytes
num = 20;
}
// Set pointer to data
*start = output;
return num;
}
