Skip to content

Instantly share code, notes, and snippets.

View reefwing's full-sized avatar

David Such reefwing

View GitHub Profile
struct ForceLookup {
uint16_t voltage; // millivolts (mV)
uint16_t force; // grams (g)
};
ForceLookup table[FLT_SIZE] = { {0, 0}, {900, 20}, {1400, 50}, {1750, 100}, {2250, 200}, {2500, 310},
{2800, 450}, {3000, 580}, {3200, 710}, {3400, 900}, {3450, 1000} };
float interpolate(uint16_t adc_mV) {
// Based on ADC voltage (mV), lookup the force (g) and interpolate between adjacent values if required.
void sendResponse(char *key, char *value) {
Serial.print("{\"");
Serial.print(key);
Serial.print("\":\"");
Serial.print(value);
Serial.print("\"}\r\n");
}
void sendResponse(char *key, int value) {
Serial.print("{\"");
void parseCommand() {
char *cmdPtr = rx.getCommand();
switch(rx.hash(cmdPtr) % HASH_SIZE) {
case xIMU3_ping:
rx.sendPing(pingPacket);
break;
case xIMU3_deviceName:
rx.sendResponse("deviceName", "Arduino");
break;
#define BAD_ID -1
typedef struct {
char *key;
int val;
} msp_commands_t;
static msp_commands_t lookupTable[] = {
{ "MSP_API_VERSION", MSP_API_VERSION }, { "MSP_FC_VARIANT", MSP_FC_VARIANT }, { "MSP_FC_VERSION", MSP_FC_VERSION }, { "MSP_BOARD_INFO", MSP_BOARD_INFO },
{ "MSP_BUILD_INFO", MSP_BUILD_INFO }, { "MSP_NAME", MSP_NAME }, { "MSP_SET_NAME", MSP_SET_NAME }, { "MSP_IDENT", MSP_IDENT }, { "MSP_STATUS", MSP_STATUS },
unsigned long hash(unsigned char *str) {
unsigned long hash = 5381;
int c;
while (c = *str++)
hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
return hash;
}
void checkForCommand() {
char buffer[BUFFER_SIZE];
// Check for xIMU3 Command Messages
if (Serial.available() > 0) {
// read the incoming bytes:
int blen = Serial.readBytesUntil(STOP_BYTE, buffer, BUFFER_SIZE);
bool cmdFound = false;
// Command character count
MagTestResults ReefwingLSM9DS1::selfTestMag() {
MagTestResults results;
BiasOffsets mag_noST, mag_ST;
// Write 0x1C = 0b0001 1100 to CTRL_REG1_M
// ODR = 80 Hz, OM = Low Performance, FAST_ODR disabled, Self Test disabled
writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x1C);
// Write 0x40 = 0b0100 0000 to CTRL_REG2_M
// FS = ± 12 gauss
ScaledData ReefwingLSM9DS1::readGyro() {
RawData gyr;
ScaledData result;
// Read the signed 16-bit RAW values
gyr = readGyroRaw();
// Subtract the bias offsets
gyr.rx -= _config.gyro.bias.x;
gyr.ry -= _config.gyro.bias.y;
float ReefwingLSM9DS1::readTemp(TempScale scale) {
uint8_t OUT_TEMP_L = readByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_OUT_TEMP_L);
uint8_t OUT_TEMP_H = readByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_OUT_TEMP_H);
uint16_t count = (OUT_TEMP_H << 8) | (OUT_TEMP_L & 0xff);
int16_t val = (int16_t)count;
float result = ((float)val)/16.0f + _config.temp.offset; // In Celsius
switch(scale) {
bool ReefwingLSM9DS1::gyroAvailable() {
uint8_t STATUS_REG = readByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_STATUS_REG);
return (STATUS_REG & 0x02);
}
bool ReefwingLSM9DS1::accelAvailable() {
uint8_t STATUS_REG = readByte(LSM9DS1AG_ADDRESS, LSM9DS1AG_STATUS_REG);
return (STATUS_REG & 0x01);