void getValuesFromGyroscope(void)
{
writeFloatToBuffer(gyroscope_getX(), 0);
writeFloatToBuffer(gyroscope_getY(), 4);
writeFloatToBuffer(gyroscope_getZ(), 8);
sendAnswer(12);
}
void getValuesFromMagnetometer(void)
{
writeFloatToBuffer(compass_getX(), 0);
writeFloatToBuffer(compass_getY(), 4);
writeFloatToBuffer(compass_getZ(), 8);
sendAnswer(12);
}
void getValuesFromAccelerometer(void)
{
writeFloatToBuffer(accelerometer_getX(), 0);
writeFloatToBuffer(accelerometer_getY(), 4);
writeFloatToBuffer(accelerometer_getZ(), 8);
sendAnswer(12);
}
void sendBlobs(uint64_t address) {
uint8_t buffer[64];
uint8_t idx = 0;
buffer[idx++] = 'N';
buffer[idx++] = numberOfDetectedBlobs;
writeFloatToBuffer(buffer, kalmanStates.elevator.position, idx);
idx += 4;
writeFloatToBuffer(buffer, kalmanStates.aileron.position, idx);
idx += 4;
writeFloatToBuffer(buffer, mpcSetpoints.elevator, idx);
idx += 4;
writeFloatToBuffer(buffer, mpcSetpoints.aileron, idx);
idx += 4;
uint8_t i;
for (i = 0; i < numberOfDetectedBlobs; i++) {
writeFloatToBuffer(buffer, blobs[i].x, idx);
idx += 4;
writeFloatToBuffer(buffer, blobs[i].y, idx);
idx += 4;
}
xbeeSendMessageTo(&buffer, idx, address);
}
void getBatteryVoltage(void)
{
writeFloatToBuffer(charger_getBatteryVoltage(), 0);
sendAnswer(4);
}
