dan
/
EM7180_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

de-Arduin-ify the drivers; now compiles, but won't do much

master
Daniel Peter Chokola 4 years ago
parent
4ac2f8860f
commit
eb4e55c6de
4 changed files with 292 additions and 204 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 308
      Drivers/EM7180/Src/em7180.c
  2. 55
      Drivers/EM7180/Src/lis2mdl.c
  3. 30
      Drivers/EM7180/Src/lps22hb.c
  4. 95
      Drivers/EM7180/Src/lsm6dsm.c

308
Drivers/EM7180/Src/em7180.c
Unescape View File

@ -15,6 +15,7 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <math.h>
#include "em7180.h" #include "em7180.h"
/* Private Global Variables */ /* Private Global Variables */
@ -49,7 +50,7 @@ static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
/* Function Definitions */ /* Function Definitions */
em7180_new(uint8_t pin, bool passthru) em7180_new(uint8_t pin, bool passthru)
{ {
pinMode(pin, INPUT); /* pinMode(pin, INPUT); */
_intPin = pin; _intPin = pin;
_passThru = passthru; _passThru = passthru;
} }
@ -59,23 +60,23 @@ void em7180_chip_id_get()
// Read SENtral device information // Read SENtral device information
uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1); uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2); uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2);
Serial.print("EM7180 ROM Version: 0x"); /* Serial.print("EM7180 ROM Version: 0x"); */
Serial.print(ROM1, HEX); /* Serial.print(ROM1, HEX); */
Serial.println(ROM2, HEX); /* Serial.println(ROM2, HEX); */
Serial.println("Should be: 0xE609"); /* Serial.println("Should be: 0xE609"); */
uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1); uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2); uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2);
Serial.print("EM7180 RAM Version: 0x"); /* Serial.print("EM7180 RAM Version: 0x"); */
Serial.print(RAM1); /* Serial.print(RAM1); */
Serial.println(RAM2); /* Serial.println(RAM2); */
uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID); uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID);
Serial.print("EM7180 ProductID: 0x"); /* Serial.print("EM7180 ProductID: 0x"); */
Serial.print(PID, HEX); /* Serial.print(PID, HEX); */
Serial.println(" Should be: 0x80"); /* Serial.println(" Should be: 0x80"); */
uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID); uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID);
Serial.print("EM7180 RevisionID: 0x"); /* Serial.print("EM7180 RevisionID: 0x"); */
Serial.print(RID, HEX); /* Serial.print(RID, HEX); */
Serial.println(" Should be: 0x02"); /* Serial.println(" Should be: 0x02"); */
} }
void em7180_load_fw_from_eeprom() void em7180_load_fw_from_eeprom()
@ -84,55 +85,92 @@ void em7180_load_fw_from_eeprom()
uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS, uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_FeatureFlags); EM7180_FeatureFlags);
if(featureflag & 0x01) if(featureflag & 0x01)
Serial.println("A barometer is installed"); {
/* Serial.println("A barometer is installed"); */
}
if(featureflag & 0x02) if(featureflag & 0x02)
Serial.println("A humidity sensor is installed"); {
/* Serial.println("A humidity sensor is installed"); */
}
if(featureflag & 0x04) if(featureflag & 0x04)
Serial.println("A temperature sensor is installed"); {
/* Serial.println("A temperature sensor is installed"); */
}
if(featureflag & 0x08) if(featureflag & 0x08)
Serial.println("A custom sensor is installed"); {
/* Serial.println("A custom sensor is installed"); */
}
if(featureflag & 0x10) if(featureflag & 0x10)
Serial.println("A second custom sensor is installed"); {
/* Serial.println("A second custom sensor is installed"); */
}
if(featureflag & 0x20) if(featureflag & 0x20)
Serial.println("A third custom sensor is installed"); {
/* Serial.println("A third custom sensor is installed"); */
}
delay(1000); // give some time to read the screen HAL_Delay(1000); // give some time to read the screen
// Check SENtral status, make sure EEPROM upload of firmware was accomplished // Check SENtral status, make sure EEPROM upload of firmware was accomplished
byte STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01); uint8_t STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus)
& 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
Serial.println("EEPROM detected on the sensor bus!"); {
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
Serial.println("EEPROM uploaded config file!"); {
/* Serial.println("EEPROM uploaded config file!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
Serial.println("EEPROM CRC incorrect!"); {
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
Serial.println("EM7180 in initialized state!"); {
/* Serial.println("EM7180 in initialized state!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
Serial.println("No EEPROM detected!"); {
/* Serial.println("No EEPROM detected!"); */
}
int count = 0; int count = 0;
while(!STAT) while(!STAT)
{ {
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01); lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
delay(500); HAL_Delay(500);
count++; count++;
STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01); STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
Serial.println("EEPROM detected on the sensor bus!"); {
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
Serial.println("EEPROM uploaded config file!"); {
/* Serial.println("EEPROM uploaded config file!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
Serial.println("EEPROM CRC incorrect!"); {
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
Serial.println("EM7180 in initialized state!"); {
/* Serial.println("EM7180 in initialized state!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
Serial.println("No EEPROM detected!"); {
/* Serial.println("No EEPROM detected!"); */
}
if(count > 10) if(count > 10)
{
break; break;
} }
}
if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)) if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))
Serial.println("EEPROM upload successful!"); {
/* Serial.println("EEPROM upload successful!"); */
}
} }
uint8_t em7180_status() uint8_t em7180_status()
@ -179,15 +217,15 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07); lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
// Enable EM7180 run mode // Enable EM7180 run mode
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
delay(100); HAL_Delay(100);
// EM7180 parameter adjustments // EM7180 parameter adjustments
Serial.println("Beginning Parameter Adjustments"); /* Serial.println("Beginning Parameter Adjustments"); */
// Read sensor default FS values from parameter space // Read sensor default FS values from parameter space
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74 lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
byte param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, uint8_t param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_ParamAcknowledge); EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4A)) while(!(param_xfer == 0x4A))
{ {
@ -199,12 +237,12 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3); param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]); EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]);
EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]); EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]);
Serial.print("Magnetometer Default Full Scale Range: +/-"); /* Serial.print("Magnetometer Default Full Scale Range: +/-"); */
Serial.print(EM7180_mag_fs); /* Serial.print(EM7180_mag_fs); */
Serial.println("uT"); /* Serial.println("uT"); */
Serial.print("Accelerometer Default Full Scale Range: +/-"); /* Serial.print("Accelerometer Default Full Scale Range: +/-"); */
Serial.print(EM7180_acc_fs); /* Serial.print(EM7180_acc_fs); */
Serial.println("g"); /* Serial.println("g"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75 lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4B)) while(!(param_xfer == 0x4B))
@ -216,9 +254,9 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2); param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3); param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]); EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]);
Serial.print("Gyroscope Default Full Scale Range: +/-"); /* Serial.print("Gyroscope Default Full Scale Range: +/-"); */
Serial.print(EM7180_gyro_fs); /* Serial.print(EM7180_gyro_fs); */
Serial.println("dps"); /* Serial.println("dps"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
@ -243,12 +281,12 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3); param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]); EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]);
EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]); EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]);
Serial.print("Magnetometer New Full Scale Range: +/-"); /* Serial.print("Magnetometer New Full Scale Range: +/-"); */
Serial.print(EM7180_mag_fs); /* Serial.print(EM7180_mag_fs); */
Serial.println("uT"); /* Serial.println("uT"); */
Serial.print("Accelerometer New Full Scale Range: +/-"); /* Serial.print("Accelerometer New Full Scale Range: +/-"); */
Serial.print(EM7180_acc_fs); /* Serial.print(EM7180_acc_fs); */
Serial.println("g"); /* Serial.println("g"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75 lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4B)) while(!(param_xfer == 0x4B))
@ -260,81 +298,120 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2); param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3); param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]); EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]);
Serial.print("Gyroscope New Full Scale Range: +/-"); /* Serial.print("Gyroscope New Full Scale Range: +/-"); */
Serial.print(EM7180_gyro_fs); /* Serial.print(EM7180_gyro_fs); */
Serial.println("dps"); /* Serial.println("dps"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
// Read EM7180 status // Read EM7180 status
uint8_t runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus); uint8_t runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01) if(runStatus & 0x01)
Serial.println(" EM7180 run status = normal mode"); {
/* Serial.println(" EM7180 run status = normal mode"); */
}
uint8_t algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS, uint8_t algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_AlgorithmStatus); EM7180_AlgorithmStatus);
if(algoStatus & 0x01) if(algoStatus & 0x01)
Serial.println(" EM7180 standby status"); {
/* Serial.println(" EM7180 standby status"); */
}
if(algoStatus & 0x02) if(algoStatus & 0x02)
Serial.println(" EM7180 algorithm slow"); {
/* Serial.println(" EM7180 algorithm slow"); */
}
if(algoStatus & 0x04) if(algoStatus & 0x04)
Serial.println(" EM7180 in stillness mode"); {
/* Serial.println(" EM7180 in stillness mode"); */
}
if(algoStatus & 0x08) if(algoStatus & 0x08)
Serial.println(" EM7180 mag calibration completed"); {
/* Serial.println(" EM7180 mag calibration completed"); */
}
if(algoStatus & 0x10) if(algoStatus & 0x10)
Serial.println(" EM7180 magnetic anomaly detected"); {
/* Serial.println(" EM7180 magnetic anomaly detected"); */
}
if(algoStatus & 0x20) if(algoStatus & 0x20)
Serial.println(" EM7180 unreliable sensor data"); {
/* Serial.println(" EM7180 unreliable sensor data"); */
}
uint8_t passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS, uint8_t passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_PassThruStatus); EM7180_PassThruStatus);
if(passthruStatus & 0x01) if(passthruStatus & 0x01)
Serial.print(" EM7180 in passthru mode!"); {
/* Serial.print(" EM7180 in passthru mode!"); */
}
uint8_t eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); uint8_t eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus);
if(eventStatus & 0x01) if(eventStatus & 0x01)
Serial.println(" EM7180 CPU reset"); {
/* Serial.println(" EM7180 CPU reset"); */
}
if(eventStatus & 0x02) if(eventStatus & 0x02)
Serial.println(" EM7180 Error"); {
/* Serial.println(" EM7180 Error"); */
}
if(eventStatus & 0x04) if(eventStatus & 0x04)
Serial.println(" EM7180 new quaternion result"); {
/* Serial.println(" EM7180 new quaternion result"); */
}
if(eventStatus & 0x08) if(eventStatus & 0x08)
Serial.println(" EM7180 new mag result"); {
/* Serial.println(" EM7180 new mag result"); */
}
if(eventStatus & 0x10) if(eventStatus & 0x10)
Serial.println(" EM7180 new accel result"); {
/* Serial.println(" EM7180 new accel result"); */
}
if(eventStatus & 0x20) if(eventStatus & 0x20)
Serial.println(" EM7180 new gyro result"); {
/* Serial.println(" EM7180 new gyro result"); */
}
delay(1000); // give some time to read the screen HAL_Delay(1000); // give some time to read the screen
// Check sensor status // Check sensor status
uint8_t sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS, uint8_t sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_SensorStatus); EM7180_SensorStatus);
Serial.print(" EM7180 sensor status = "); /* Serial.print(" EM7180 sensor status = "); */
Serial.println(sensorStatus); /* Serial.println(sensorStatus); */
if(sensorStatus & 0x01) if(sensorStatus & 0x01)
Serial.print("Magnetometer not acknowledging!"); {
/* Serial.print("Magnetometer not acknowledging!"); */
}
if(sensorStatus & 0x02) if(sensorStatus & 0x02)
Serial.print("Accelerometer not acknowledging!"); {
/* Serial.print("Accelerometer not acknowledging!"); */
}
if(sensorStatus & 0x04) if(sensorStatus & 0x04)
Serial.print("Gyro not acknowledging!"); {
/* Serial.print("Gyro not acknowledging!"); */
}
if(sensorStatus & 0x10) if(sensorStatus & 0x10)
Serial.print("Magnetometer ID not recognized!"); {
/* Serial.print("Magnetometer ID not recognized!"); */
}
if(sensorStatus & 0x20) if(sensorStatus & 0x20)
Serial.print("Accelerometer ID not recognized!"); {
/* Serial.print("Accelerometer ID not recognized!"); */
}
if(sensorStatus & 0x40) if(sensorStatus & 0x40)
Serial.print("Gyro ID not recognized!"); {
/* Serial.print("Gyro ID not recognized!"); */
Serial.print("Actual MagRate = "); }
Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualMagRate));
Serial.println(" Hz"); /* Serial.print("Actual MagRate = "); */
Serial.print("Actual AccelRate = "); /* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualMagRate)); */
Serial.print( /* Serial.println(" Hz"); */
10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualAccelRate)); /* Serial.print("Actual AccelRate = "); */
Serial.println(" Hz"); /* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualAccelRate)); */
Serial.print("Actual GyroRate = "); /* Serial.println(" Hz"); */
Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualGyroRate)); /* Serial.print("Actual GyroRate = "); */
Serial.println(" Hz"); /* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualGyroRate)); */
Serial.print("Actual BaroRate = "); /* Serial.println(" Hz"); */
Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualBaroRate)); /* Serial.print("Actual BaroRate = "); */
Serial.println(" Hz"); /* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualBaroRate)); */
/* Serial.println(" Hz"); */
} }
float em7180_uint32_reg_to_float(uint8_t *buf) float em7180_uint32_reg_to_float(uint8_t *buf)
@ -596,19 +673,19 @@ void em7180_passthrough()
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_AlgorithmControl); uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_AlgorithmControl);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01); lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01);
// c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus); // c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
// Serial.print("c = "); Serial.println(c); /* // Serial.print("c = "); Serial.println(c); */
// Verify standby status // Verify standby status
// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) { // if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
Serial.println("SENtral in standby mode"); /* Serial.println("SENtral in standby mode"); */
// Place SENtral in pass-through mode // Place SENtral in pass-through mode
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01); lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01) if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01)
{ {
Serial.println("SENtral in pass-through mode"); /* Serial.println("SENtral in pass-through mode"); */
} }
else else
{ {
Serial.println("ERROR! SENtral not in pass-through mode!"); /* Serial.println("ERROR! SENtral not in pass-through mode!"); */
} }
} }
@ -618,8 +695,8 @@ static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t data) uint8_t data_address2, uint8_t data)
{ {
uint8_t temp[2] = { data_address1, data_address2 }; uint8_t temp[2] = { data_address1, data_address2 };
Wire.transfer(device_address, &temp[0], 2, NULL, 0); /* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
Wire.transfer(device_address, &data, 1, NULL, 0); /* Wire.transfer(device_address, &data, 1, NULL, 0); */
} }
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1, static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
@ -628,23 +705,23 @@ static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
if(count > 128) if(count > 128)
{ {
count = 128; count = 128;
Serial.print("Page count cannot be more than 128 bytes!"); /* Serial.print("Page count cannot be more than 128 bytes!"); */
} }
uint8_t temp[2] = { data_address1, data_address2 }; uint8_t temp[2] = { data_address1, data_address2 };
Wire.transfer(device_address, &temp[0], 2, NULL, 0); /* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
Wire.transfer(device_address, &dest[0], count, NULL, 0); /* Wire.transfer(device_address, &dest[0], count, NULL, 0); */
} }
static uint8_t m24512dfm_read_byte(uint8_t device_address, static uint8_t m24512dfm_read_byte(uint8_t device_address,
uint8_t data_address1, uint8_t data_address2) uint8_t data_address1, uint8_t data_address2)
{ {
uint8_t data; // `data` will store the register data uint8_t data; // `data` will store the register data
Wire.beginTransmission(device_address); // Initialize the Tx buffer /* Wire.beginTransmission(device_address); // Initialize the Tx buffer */
Wire.write(data_address1); // Put slave register address in Tx buffer /* Wire.write(data_address1); // Put slave register address in Tx buffer */
Wire.write(data_address2); // Put slave register address in Tx buffer /* Wire.write(data_address2); // Put slave register address in Tx buffer */
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive /* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
Wire.requestFrom(device_address, 1); // Read one byte from slave register address /* Wire.requestFrom(device_address, 1); // Read one byte from slave register address */
data = Wire.read(); // Fill Rx buffer with result /* data = Wire.read(); // Fill Rx buffer with result */
return data; // Return data read from slave register return data; // Return data read from slave register
} }
@ -652,7 +729,7 @@ static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t count, uint8_t *dest) uint8_t data_address2, uint8_t count, uint8_t *dest)
{ {
uint8_t temp[2] = { data_address1, data_address2 }; uint8_t temp[2] = { data_address1, data_address2 };
Wire.transfer(device_address, &temp[0], 2, dest, count); /* Wire.transfer(device_address, &temp[0], 2, dest, count); */
} }
// I2C read/write functions for the EM7180 // I2C read/write functions for the EM7180
@ -661,20 +738,20 @@ void em7180_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
uint8_t temp[2]; uint8_t temp[2];
temp[0] = subAddress; temp[0] = subAddress;
temp[1] = data; temp[1] = data;
Wire.transfer(address, &temp[0], 2, NULL, 0); /* Wire.transfer(address, &temp[0], 2, NULL, 0); */
} }
static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress) static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress)
{ {
uint8_t temp[1]; uint8_t temp[1];
Wire.transfer(address, &subAddress, 1, &temp[0], 1); /* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
return temp[0]; return temp[0];
} }
static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count, static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest) uint8_t *dest)
{ {
Wire.transfer(address, &subAddress, 1, dest, count); /* Wire.transfer(address, &subAddress, 1, dest, count); */
} }
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
@ -726,7 +803,9 @@ __attribute__((optimize("O3"))) void em7180_update_quat_madgwick(float ax,
// Normalize accelerometer measurement // Normalize accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az); norm = sqrt(ax * ax + ay * ay + az * az);
if(norm == 0.0f) if(norm == 0.0f)
{
return; // handle NaN return; // handle NaN
}
norm = 1.0f / norm; norm = 1.0f / norm;
ax *= norm; ax *= norm;
ay *= norm; ay *= norm;
@ -735,7 +814,9 @@ __attribute__((optimize("O3"))) void em7180_update_quat_madgwick(float ax,
// Normalize magnetometer measurement // Normalize magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz); norm = sqrt(mx * mx + my * my + mz * mz);
if(norm == 0.0f) if(norm == 0.0f)
{
return; // handle NaN return; // handle NaN
}
norm = 1.0f / norm; norm = 1.0f / norm;
mx *= norm; mx *= norm;
my *= norm; my *= norm;
@ -854,7 +935,9 @@ void em7180_update_quat_mahony(float ax, float ay, float az, float gx, float gy,
// Normalize accelerometer measurement // Normalize accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az); norm = sqrt(ax * ax + ay * ay + az * az);
if(norm == 0.0f) if(norm == 0.0f)
{
return; // handle NaN return; // handle NaN
}
norm = 1.0f / norm; // use reciprocal for division norm = 1.0f / norm; // use reciprocal for division
ax *= norm; ax *= norm;
ay *= norm; ay *= norm;
@ -863,7 +946,9 @@ void em7180_update_quat_mahony(float ax, float ay, float az, float gx, float gy,
// Normalize magnetometer measurement // Normalize magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz); norm = sqrt(mx * mx + my * my + mz * mz);
if(norm == 0.0f) if(norm == 0.0f)
{
return; // handle NaN return; // handle NaN
}
norm = 1.0f / norm; // use reciprocal for division norm = 1.0f / norm; // use reciprocal for division
mx *= norm; mx *= norm;
my *= norm; my *= norm;
@ -924,5 +1009,4 @@ void em7180_update_quat_mahony(float ax, float ay, float az, float gx, float gy,
_q[1] = q2 * norm; _q[1] = q2 * norm;
_q[2] = q3 * norm; _q[2] = q3 * norm;
_q[3] = q4 * norm; _q[3] = q4 * norm;
} }

55
Drivers/EM7180/Src/lis2mdl.c
Unescape View File

@ -32,7 +32,7 @@ static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
/* Function Definitions */ /* Function Definitions */
lis2mdl_new(uint8_t pin) lis2mdl_new(uint8_t pin)
{ {
pinMode(pin, INPUT); /* pinMode(pin, INPUT); */
_intPin = pin; _intPin = pin;
} }
@ -47,9 +47,9 @@ void lis2mdl_reset()
// reset device // reset device
uint8_t temp = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A); uint8_t temp = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
delay(1); HAL_Delay(1);
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
delay(100); // Wait for all registers to reset HAL_Delay(100); // Wait for all registers to reset
} }
void lis2mdl_init(uint8_t MODR) void lis2mdl_init(uint8_t MODR)
@ -101,9 +101,8 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
{ 32767, 32767, 32767 }, mag_temp[3] = { 0, 0, 0 }; { 32767, 32767, 32767 }, mag_temp[3] = { 0, 0, 0 };
float _mRes = 0.0015f; float _mRes = 0.0015f;
Serial.println( /* Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!"); */
"Calculate mag offset bias: move all around to sample the complete response surface!"); HAL_Delay(4000);
delay(4000);
for(int ii = 0; ii < 4000; ii++) for(int ii = 0; ii < 4000; ii++)
{ {
@ -111,11 +110,15 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
for(int jj = 0; jj < 3; jj++) for(int jj = 0; jj < 3; jj++)
{ {
if(mag_temp[jj] > mag_max[jj]) if(mag_temp[jj] > mag_max[jj])
{
mag_max[jj] = mag_temp[jj]; mag_max[jj] = mag_temp[jj];
}
if(mag_temp[jj] < mag_min[jj]) if(mag_temp[jj] < mag_min[jj])
{
mag_min[jj] = mag_temp[jj]; mag_min[jj] = mag_temp[jj];
} }
delay(12); }
HAL_Delay(12);
} }
_mRes = 0.0015f; // fixed sensitivity _mRes = 0.0015f; // fixed sensitivity
@ -140,7 +143,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
dest2[1] = avg_rad / ((float) mag_scale[1]); dest2[1] = avg_rad / ((float) mag_scale[1]);
dest2[2] = avg_rad / ((float) mag_scale[2]); dest2[2] = avg_rad / ((float) mag_scale[2]);
Serial.println("Mag Calibration done!"); /* Serial.println("Mag Calibration done!"); */
} }
void lis2mdl_self_test() void lis2mdl_self_test()
@ -158,7 +161,7 @@ void lis2mdl_self_test()
sum[0] += temp[0]; sum[0] += temp[0];
sum[1] += temp[1]; sum[1] += temp[1];
sum[2] += temp[2]; sum[2] += temp[2];
delay(50); HAL_Delay(50);
} }
magNom[0] = (float) sum[0] / 50.0f; magNom[0] = (float) sum[0] / 50.0f;
@ -167,7 +170,7 @@ void lis2mdl_self_test()
uint8_t c = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C); uint8_t c = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
delay(100); // let mag respond HAL_Delay(100); // let mag respond
sum[0] = 0; sum[0] = 0;
sum[1] = 0; sum[1] = 0;
@ -178,7 +181,7 @@ void lis2mdl_self_test()
sum[0] += temp[0]; sum[0] += temp[0];
sum[1] += temp[1]; sum[1] += temp[1];
sum[2] += temp[2]; sum[2] += temp[2];
delay(50); HAL_Delay(50);
} }
magTest[0] = (float) sum[0] / 50.0f; magTest[0] = (float) sum[0] / 50.0f;
@ -186,18 +189,18 @@ void lis2mdl_self_test()
magTest[2] = (float) sum[2] / 50.0f; magTest[2] = (float) sum[2] / 50.0f;
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
delay(100); // let mag respond HAL_Delay(100); // let mag respond
Serial.println("Mag Self Test:"); /* Serial.println("Mag Self Test:"); */
Serial.print("Mx results:"); /* Serial.print("Mx results:"); */
Serial.print((magTest[0] - magNom[0]) * _mRes * 1000.0); /* Serial.print((magTest[0] - magNom[0]) * _mRes * 1000.0); */
Serial.println(" mG"); /* Serial.println(" mG"); */
Serial.print("My results:"); /* Serial.print("My results:"); */
Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0); /* Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0); */
Serial.print("Mz results:"); /* Serial.print("Mz results:"); */
Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0); /* Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0); */
Serial.println("Should be between 15 and 500 mG"); /* Serial.println("Should be between 15 and 500 mG"); */
delay(2000); // give some time to read the screen HAL_Delay(2000); // give some time to read the screen
} }
// I2C read/write functions for the LIS2MDL // I2C read/write functions for the LIS2MDL
@ -208,18 +211,18 @@ static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress,
uint8_t temp[2]; uint8_t temp[2];
temp[0] = subAddress; temp[0] = subAddress;
temp[1] = data; temp[1] = data;
Wire.transfer(address, &temp[0], 2, NULL, 0); /* Wire.transfer(address, &temp[0], 2, NULL, 0); */
} }
static uint8_t lis2mdl_read_byte(uint8_t address, uint8_t subAddress) static uint8_t lis2mdl_read_byte(uint8_t address, uint8_t subAddress)
{ {
uint8_t temp[1]; uint8_t temp[1];
Wire.transfer(address, &subAddress, 1, &temp[0], 1); /* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
return temp[0]; return temp[0];
} }
static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count, static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest) uint8_t *dest)
{ {
Wire.transfer(address, &subAddress, 1, dest, count); /* Wire.transfer(address, &subAddress, 1, dest, count); */
} }

30
Drivers/EM7180/Src/lps22hb.c
Unescape View File

@ -30,7 +30,7 @@ static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count,
/* Function Definitions */ /* Function Definitions */
lps22h_new(uint8_t pin) lps22h_new(uint8_t pin)
{ {
pinMode(pin, INPUT); /* pinMode(pin, INPUT); */
_intPin = pin; _intPin = pin;
} }
@ -76,33 +76,35 @@ void lps22h_init(uint8_t PODR)
static void lps22h_write_byte(uint8_t address, uint8_t subAddress, uint8_t data) static void lps22h_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
{ {
Wire.beginTransmission(address); // Initialize the Tx buffer /* Wire.beginTransmission(address); // Initialize the Tx buffer */
Wire.write(subAddress); // Put slave register address in Tx buffer /* Wire.write(subAddress); // Put slave register address in Tx buffer */
Wire.write(data); // Put data in Tx buffer /* Wire.write(data); // Put data in Tx buffer */
Wire.endTransmission(); // Send the Tx buffer /* Wire.endTransmission(); // Send the Tx buffer */
} }
static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress) static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress)
{ {
uint8_t data; // `data` will store the register data uint8_t data; // `data` will store the register data
Wire.beginTransmission(address); // Initialize the Tx buffer /* Wire.beginTransmission(address); // Initialize the Tx buffer */
Wire.write(subAddress); // Put slave register address in Tx buffer /* Wire.write(subAddress); // Put slave register address in Tx buffer */
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive /* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
Wire.requestFrom(address, (size_t) 1); // Read one uint8_t from slave register address /* Wire.requestFrom(address, (size_t) 1); // Read one uint8_t from slave register address */
data = Wire.read(); // Fill Rx buffer with result /* data = Wire.read(); // Fill Rx buffer with result */
return data; // Return data lps22h_read from slave register return data; // Return data lps22h_read from slave register
} }
static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count, static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest) uint8_t *dest)
{ {
Wire.beginTransmission(address); // Initialize the Tx buffer /* Wire.beginTransmission(address); // Initialize the Tx buffer */
Wire.write(subAddress); // Put slave register address in Tx buffer /* Wire.write(subAddress); // Put slave register address in Tx buffer */
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive /* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
uint8_t i = 0; uint8_t i = 0;
Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address /* Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address */
/*
while(Wire.available()) while(Wire.available())
{ {
dest[i++] = Wire.lps22h_read(); dest[i++] = Wire.lps22h_read();
} // Put lps22h_read results in the Rx buffer } // Put lps22h_read results in the Rx buffer
*/
} }

95
Drivers/EM7180/Src/lsm6dsm.c
Unescape View File

@ -34,9 +34,9 @@ static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count,
/* Function Definitions */ /* Function Definitions */
lsm6dsm_new(uint8_t pin1, uint8_t pin2) lsm6dsm_new(uint8_t pin1, uint8_t pin2)
{ {
pinMode(pin1, INPUT); /* pinMode(pin1, INPUT); */
_intPin1 = pin1; _intPin1 = pin1;
pinMode(pin2, INPUT); /* pinMode(pin2, INPUT); */
_intPin2 = pin2; _intPin2 = pin2;
} }
@ -102,7 +102,7 @@ void lsm6dsm_reset()
// reset device // reset device
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
delay(100); // Wait for all registers to reset HAL_Delay(100); // Wait for all registers to reset
} }
void lsm6dsm_init(uint8_t ascale, uint8_t gscale, uint8_t AODR, uint8_t GODR) void lsm6dsm_init(uint8_t ascale, uint8_t gscale, uint8_t AODR, uint8_t GODR)
@ -144,68 +144,68 @@ void lsm6dsm_selfTest()
gyroNom[2] = temp[3]; gyroNom[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
delay(100); // let accel respond HAL_Delay(100); // let accel respond
readData(temp); readData(temp);
accelPTest[0] = temp[4]; accelPTest[0] = temp[4];
accelPTest[1] = temp[5]; accelPTest[1] = temp[5];
accelPTest[2] = temp[6]; accelPTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
delay(100); // let accel respond HAL_Delay(100); // let accel respond
readData(temp); readData(temp);
accelNTest[0] = temp[4]; accelNTest[0] = temp[4];
accelNTest[1] = temp[5]; accelNTest[1] = temp[5];
accelNTest[2] = temp[6]; accelNTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
delay(100); // let gyro respond HAL_Delay(100); // let gyro respond
readData(temp); readData(temp);
gyroPTest[0] = temp[1]; gyroPTest[0] = temp[1];
gyroPTest[1] = temp[2]; gyroPTest[1] = temp[2];
gyroPTest[2] = temp[3]; gyroPTest[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
delay(100); // let gyro respond HAL_Delay(100); // let gyro respond
readData(temp); readData(temp);
gyroNTest[0] = temp[1]; gyroNTest[0] = temp[1];
gyroNTest[1] = temp[2]; gyroNTest[1] = temp[2];
gyroNTest[2] = temp[3]; gyroNTest[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
delay(100); // let accel and gyro respond HAL_Delay(100); // let accel and gyro respond
Serial.println("Accel Self Test:"); /* Serial.println("Accel Self Test:"); */
Serial.print("+Ax results:"); /* Serial.print("+Ax results:"); */
Serial.print((accelPTest[0] - accelNom[0]) * _aRes * 1000.0); /* Serial.print((accelPTest[0] - accelNom[0]) * _aRes * 1000.0); */
Serial.println(" mg"); /* Serial.println(" mg"); */
Serial.print("-Ax results:"); /* Serial.print("-Ax results:"); */
Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0); /* Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0); */
Serial.print("+Ay results:"); /* Serial.print("+Ay results:"); */
Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0); /* Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0); */
Serial.print("-Ay results:"); /* Serial.print("-Ay results:"); */
Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0); /* Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0); */
Serial.print("+Az results:"); /* Serial.print("+Az results:"); */
Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0); /* Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0); */
Serial.print("-Az results:"); /* Serial.print("-Az results:"); */
Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0); /* Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0); */
Serial.println("Should be between 90 and 1700 mg"); /* Serial.println("Should be between 90 and 1700 mg"); */
Serial.println("Gyro Self Test:"); /* Serial.println("Gyro Self Test:"); */
Serial.print("+Gx results:"); /* Serial.print("+Gx results:"); */
Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); /* Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); */
Serial.println(" dps"); /* Serial.println(" dps"); */
Serial.print("-Gx results:"); /* Serial.print("-Gx results:"); */
Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes); /* Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes); */
Serial.print("+Gy results:"); /* Serial.print("+Gy results:"); */
Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes); /* Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes); */
Serial.print("-Gy results:"); /* Serial.print("-Gy results:"); */
Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes); /* Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes); */
Serial.print("+Gz results:"); /* Serial.print("+Gz results:"); */
Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes); /* Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes); */
Serial.print("-Gz results:"); /* Serial.print("-Gz results:"); */
Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes); /* Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes); */
Serial.println("Should be between 20 and 80 dps"); /* Serial.println("Should be between 20 and 80 dps"); */
delay(2000); HAL_Delay(2000);
} }
@ -214,9 +214,8 @@ void lsm6dsm_offsetBias(float *dest1, float *dest2)
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 }; int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
int32_t sum[7] = { 0, 0, 0, 0, 0, 0, 0 }; int32_t sum[7] = { 0, 0, 0, 0, 0, 0, 0 };
Serial.println( /* Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!"); */
"Calculate accel and gyro offset biases: keep sensor flat and motionless!"); HAL_Delay(4000);
delay(4000);
for(int ii = 0; ii < 128; ii++) for(int ii = 0; ii < 128; ii++)
{ {
@ -227,7 +226,7 @@ void lsm6dsm_offsetBias(float *dest1, float *dest2)
sum[4] += temp[4]; sum[4] += temp[4];
sum[5] += temp[5]; sum[5] += temp[5];
sum[6] += temp[6]; sum[6] += temp[6];
delay(50); HAL_Delay(50);
} }
dest1[0] = sum[1] * _gRes / 128.0f; dest1[0] = sum[1] * _gRes / 128.0f;
@ -284,18 +283,18 @@ static void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress,
uint8_t temp[2]; uint8_t temp[2];
temp[0] = subAddress; temp[0] = subAddress;
temp[1] = data; temp[1] = data;
Wire.transfer(address, &temp[0], 2, NULL, 0); /* Wire.transfer(address, &temp[0], 2, NULL, 0); */
} }
static uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress) static uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress)
{ {
uint8_t temp[1]; uint8_t temp[1];
Wire.transfer(address, &subAddress, 1, &temp[0], 1); /* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
return temp[0]; return temp[0];
} }
static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count, static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest) uint8_t *dest)
{ {
Wire.transfer(address, &subAddress, 1, dest, count); /* Wire.transfer(address, &subAddress, 1, dest, count); */
} }

Write Preview
Loading…
Cancel
Save