int main() {
char buf[17] = " ";
Accel accel;
beginMyOled();
initI2CAccel(&accel, I2C2);
// show ID
sprintf(buf, "ID: %x", accelReadReg((const)&(accel), 0x0));
displayOnOLED(buf,0);
int data[3];
unsigned int i, dec;
while (TRUE){
if (! ( accelReadReg((const)&(accel), 0x30) & BIT_7) )
continue; // only update when data is ready.
readAccel(&accel);
for (i=0; i<3; i++) // convert to g * 100
data[i] = map(0,0xff,0,100,accel.axis[i]);
for (i=0; i<3; i++){
if (data[i] < 0){
data[i] *= -1; // show as decimal
sprintf(buf, "%c: -%d.%02d g", i+0x58, data[i]/100, (data[i] - data[i]/100));
}else // (ascii) 0x58 = X
sprintf(buf, "%c: %02d.%02d g", i+0x58, data[i]/100, (data[i] - data[i]/100));
displayOnOLED(buf,i+1);
}
}
return (EXIT_SUCCESS);
}
int main(){
init();
setup();
while(1){
csock.run();
#ifdef ACCEL
if(millis() - timer > 10){
readAccel();
timer = millis();
}
#endif
#ifdef DEMO
if(millis() - demotimer > 50){
if(digitalRead(switchpin) == LOW){
Serial.println(F("switch low"));
demosend();
}
demotimer = millis();
}
#endif
if(canSend){
flush();
}
checkRecv();
}
return 0;
}
/**
*\brief Meant to decide the setpoint of where the quadcopter should be. It does so by setting the target accelerometer reading for both x and y
*/
void ForegroundMotorDriver()
{
readAccel();
//determineNextTargetAccel();
//approachNextTargetAccel(goalTargetAccel_X, 'x');
//approachNextTargetAccel(goalTargetAccel_Y, 'y');
}
/*
* send acc. data to uart 3
*/
void sendAcclData(void) {
readAccel();
int aMagnitude = abs(Ax) + abs(Ay) + abs(Az);
short b;
unsigned char accl[20];
accl[19] = '\0';
b = usprintf((char*) accl, "%i,%i,%i\n", Ax, Ay, Az);
#ifndef wlan
nrf24l01p_send(accl);
#else
for (int i = 0; i < b; i++) {
ROM_UARTCharPut(UART3_BASE, accl[i]);
}
//ROM_UARTCharPut(UART3_BASE, '\n');
#endif
b = usprintf((char*) accl, "%i\n", aMagnitude);
#ifndef wlan
nrf24l01p_send(accl);
#else
for (int i = 0; i < b; i++) {
ROM_UARTCharPut(UART3_BASE, accl[i]);
}
//ROM_UARTCharPut(UART3_BASE, '\n');
#endif
}
void TR_ADXL345::get_xyz(float* xyz) {
int readings[3];
readAccel(readings);
for(int i = 0; i < 3; i++) {
xyz[i] = readings[i] * gains[i];
}
}
/***************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/
void Adafruit_LSM9DS0::read()
{
/* Read all the sensors. */
readAccel();
readMag();
readGyro();
readTemp();
}
void TR_ADXL345::calibrate() {
int i, x, y, z, count = 100;
float xSum = 0.0, ySum = 0.0, zSum = 0.0;
for(i = 0; i < count; i++) {
readAccel(&x, &y, &z);
xSum += x;
ySum += y;
zSum += z;
delay(10);
}
offset[0] = xSum / count;
offset[1] = ySum / count;
offset[2] = zSum / count;
}
void MPU6000::init() {
// Reset device.
txbuf[0] = mpu6000::REG_PWR_MGMT_1;
txbuf[1] = 0x80;
exchange(2);
chThdSleepMilliseconds(100); // TODO(yoos): Check whether or not datasheet specifies this holdoff.
// Set sample rate to 1 kHz.
txbuf[0] = mpu6000::REG_SMPLRT_DIV;
txbuf[1] = 0x00;
exchange(2);
// Set DLPF to 4 (20 Hz gyro bandwidth1 Hz accelerometer bandwidth)
txbuf[0] = mpu6000::REG_CONFIG;
txbuf[1] = 0x04;
exchange(2);
// Wake up device and set clock source to Gyro Z.
txbuf[0] = mpu6000::REG_PWR_MGMT_1;
txbuf[1] = 0x03;
exchange(2);
// Set gyro full range to 2000 dps. We first read in the current register
// value so we can change what we need and leave everything else alone.
// See DS p. 12.
txbuf[0] = mpu6000::REG_GYRO_CONFIG | (1<<7);
exchange(2);
chThdSleepMicroseconds(0); // TODO(yoos): Without this, the GYRO_CONFIG register does not get set. This was not the case in the old C firmware. Why?
txbuf[0] = mpu6000::REG_GYRO_CONFIG;
txbuf[1] = (rxbuf[1] & ~0x18) | 0x18;
exchange(2);
// Set accelerometer full range to 16 g. See DS p. 13.
txbuf[0] = mpu6000::REG_ACCEL_CONFIG | (1<<7);
exchange(2);
txbuf[0] = mpu6000::REG_ACCEL_CONFIG;
txbuf[1] = (rxbuf[1] & ~0x18) | 0x18;
exchange(2);
// Read once to clear out bad data?
readGyro();
readAccel();
}
/*
* read and calculate heading with basic tilt compensation
* @returns float containing heading value -> -180 to +180
*
*/
float getHeading(void) {
readMagneto();
readAccel();
double pitch, roll;
if ((Az > 9500 && Az < 10050) || (Az < -9500 && Az > -10050)) {
pitch = roll = 0;
} else {
pitch = getPitch();
roll = getRoll();
}
int Xh = Mx * cos(pitch) + Mz * sin(pitch); // Virtual X and Virtual Y (corrected)
int Yh = Mx * sin(pitch) * sin(roll) + My * cos(roll)
- Mz * sin(roll) * cos(pitch);
float heading = (atan2f(Yh, Xh) * 180 / M_PI);
if (heading < 0) {
heading = 360 + heading;
}
return heading;
}
void Pheeno::sensorFusionPositionUpdate(float timeStep, float northOffset){
if (millis() - positionUpdateTimeStart >= timeStep){
timeStep = (millis() - positionUpdateTimeStart)/1000; //Convert ms to s
positionUpdateTimeStart = millis();
readEncoders();
int countL = encoderCountL;
int countR = encoderCountR;
float Dr = pi * wheelDiameter * (countR - oldSFPUEncoderCountR) / (encoderCountsPerRotation * motorGearRatio); // Linear distance right wheel has rotated.
float Dl = pi * wheelDiameter * (countL - oldSFPUEncoderCountL) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
//Check integer roll over!
if (countL < 0 && oldSFPUEncoderCountL > 0){
Dl = pi*wheelDiameter * ((countL - (-32768)) + (32767 - oldSFPUEncoderCountL)) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
}
if (countR < 0 && oldEPUEncoderCountR > 0){
Dr = pi*wheelDiameter * ((countR - (-32768)) + (32767 - oldSFPUEncoderCountR)) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
}
float Dc = (Dr + Dl)/2; //Distance center of bot has moved read by encoders.
oldSFPUEncoderCountR = countR;
oldSFPUEncoderCountL = countL;
float botD = 0.8 * Dc + 0.2 * botVel * timeStep;
readCompass(northOffset);
if (botA0 > -pi/2 && botA0 < pi/2){
botA = 0.9 * wrapToPi(botA + (Dr - Dl)/axelLength) + 0.1 * wrapToPi(deg2rad(IMUOrientation));
}
else{
botA = 0.9 * wrapTo2Pi(botA + (Dr - Dl)/axelLength) + 0.1 * wrapTo2Pi(deg2rad(IMUOrientation));
}
botXPos += botD * cos(botA0 + (botA-botA0)/2);
botYPos += botD * sin(botA0 + (botA-botA0)/2);
readAccel();
botVel = 0.95 * Dc/timeStep + 0.05 * (botVel + IMUACCY * timeStep);
botA0 = botA;
}
}
static msg_t Thread1(void *arg) {
(void)arg;
// Next deadline.
systime_t time;
chRegSetThreadName("accelreader");
// Reader thread loop.
time = chTimeNow();
while (TRUE) {
readAccel();
chMtxLock(&accelMtx);
// Reprogramming the four PWM channels using the accelerometer data.
if (accel_y[4] < 0) {
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)-accel_y[4]);
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)accel_y[4]);
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)0);
}
if (accel_x[4] < 0) {
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)-accel_x[4]);
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)accel_x[4]);
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)0);
}
chMtxUnlock();
// Waiting until the next 250 milliseconds time interval.
chThdSleepUntil(time += MS2ST(100));
}
return (msg_t)0;
}
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
// them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch
// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
// subtract the biases ourselves. This results in a more accurate measurement in general and can
// remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
// is good practice.
void LSM9DS1::calibrate(bool autoCalc)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
uint8_t samples = 0;
int ii;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
// Turn on FIFO and set threshold to 32 samples
enableFIFO(true);
setFIFO(FIFO_THS, 0x1F);
while (samples < 0x1F)
{
samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
}
for(ii = 0; ii < samples ; ii++)
{ // Read the gyro data stored in the FIFO
readGyro();
gBiasRawTemp[0] += gx;
gBiasRawTemp[1] += gy;
gBiasRawTemp[2] += gz;
readAccel();
aBiasRawTemp[0] += ax;
aBiasRawTemp[1] += ay;
aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
}
for (ii = 0; ii < 3; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
gBias[ii] = calcGyro(gBiasRaw[ii]);
aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
aBias[ii] = calcAccel(aBiasRaw[ii]);
}
enableFIFO(false);
setFIFO(FIFO_OFF, 0x00);
if (autoCalc) _autoCalc = true;
}
void computeAccelRTBias(void)
{
uint16_t samples;
float accelSum[3] = { 0, 0, 0 };
accelCalibrating = true;
for (samples = 0; samples < 2000; samples++) {
readAccel();
accelSum[XAXIS] += rawAccel[XAXIS].value;
accelSum[YAXIS] += rawAccel[YAXIS].value;
accelSum[ZAXIS] += rawAccel[ZAXIS].value;
delayMicroseconds(1000);
}
accelRTBias[XAXIS] = accelSum[XAXIS] / 2000.0f;
accelRTBias[YAXIS] = accelSum[YAXIS] / 2000.0f;
accelRTBias[ZAXIS] = (accelSum[ZAXIS] / 2000.0f) - (9.8056f / fabs(sensorConfig.accelScaleFactor[ZAXIS]));
accelCalibrating = false;
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
#endif
#if defined(USE_CHR6DM_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false) &&
(ahrsCalibrating == false))
#endif
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
#endif
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL] += rawGyro[ROLL].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW] += rawGyro[YAW].value;
gyroSum100Hz[ROLL] += rawGyro[ROLL].value;
gyroSum100Hz[PITCH] += rawGyro[PITCH].value;
gyroSum100Hz[YAW] += rawGyro[YAW].value;
gyroSum500Hz[ROLL] += rawGyro[ROLL].value;
gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
gyroSum500Hz[YAW] += rawGyro[YAW].value;
///////////////////////////////
if ((frameCounter % COUNT_500HZ) == 0) {
frame_500Hz = true;
for (index = 0; index < 3; index++) {
gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
gyroSum500Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0) {
frame_200Hz = true;
for (index = 0; index < 3; index++) {
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
#endif
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0) {
frame_100Hz = true;
for (index = 0; index < 3; index++) {
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
gyroSummedSamples100Hz[index] = gyroSum100Hz[index];
gyroSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ) {
readTemperatureRequestPressure();
} else if (frameCounter == FRAME_COUNT) {
readPressureRequestTemperature();
} else {
readPressureRequestPressure();
}
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_50HZ) == 0) {
readMag();
magSum[XAXIS] += rawMag[XAXIS].value;
magSum[YAXIS] += rawMag[YAXIS].value;
magSum[ZAXIS] += rawMag[ZAXIS].value;
}
if ((frameCounter % COUNT_50HZ) == 0) {
frame_50Hz = true;
}
///////////////////////////////
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
}
void TR_ADXL345::readAccel(int* xyz) {
readAccel(xyz, xyz + 1, xyz + 2);
}
void BMA180::readAccel(int * buff) {
readAccel(&buff[0], &buff[1], &buff[2]);
}
void readSensors()
{
readGyro();
readAccel();
readMag();
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
if ((systemReady == true ) &&
(cliBusy == false) &&
(accelCalibrating == false) &&
(escCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL ] += rawGyro[ROLL ].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW ] += rawGyro[YAW ].value;
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0)
{
frame_200Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0)
{
frame_100Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ)
readTemperatureRequestPressure();
else if (frameCounter == FRAME_COUNT)
readPressureRequestTemperature();
else
readPressureRequestPressure();
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if ((frameCounter % COUNT_50HZ) == 0)
{
frame_50Hz = true;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_10HZ) == 0)
newMagData = readMag();
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
}
int main()
{
int32_t bemf_vals[4] = {0,0,0,0};
int32_t bemf_vals_filt[4] = {0,0,0,0};
init();
// set up DMA/SPI buffers
init_rx_buffer();
init_tx_buffer();
// set up pid structs
pid_struct pid_structs[4];
{ uint8_t i;
for (i = 0; i < 4; ++i) init_pid_struct(&(pid_structs[i]), i);
}
update_dig_pin_configs();
config_adc_in_from_regs();
//debug_printf("starting\n");
int low_volt_alarmed = 0;
// Loop until button is pressed
uint32_t count = 0;
while (1)
{
count += 1;
{
// only sample motor backemf 1/4 of the time
const uint8_t bemf_update_time = (count % 4 == 1);
if (bemf_update_time)
{
// idle breaking
MOT0_DIR1_PORT->BSRRH |= MOT0_DIR1;
MOT0_DIR2_PORT->BSRRH |= MOT0_DIR2;
MOT1_DIR1_PORT->BSRRH |= MOT1_DIR1;
MOT1_DIR2_PORT->BSRRH |= MOT1_DIR2;
MOT2_DIR1_PORT->BSRRH |= MOT2_DIR1;
MOT2_DIR2_PORT->BSRRH |= MOT2_DIR2;
MOT3_DIR1_PORT->BSRRH |= MOT3_DIR1;
MOT3_DIR2_PORT->BSRRH |= MOT3_DIR2;
}
// let the motor coast
// delay_us(700);
// now I squeeze in most sensor updates instead of just sleeping
uint32_t before = usCount;
update_dig_pins();
int16_t batt = adc_update();
readAccel();
readMag();
readGyro();
if (batt < 636) // about 5.75 volts
{
configDigitalOutPin(LED1_PIN, LED1_PORT);
low_volt_alarmed = 1;
if (count % 50 < 10) // low duty cycle to save battery
{
LED1_PORT->BSRRL |= LED1_PIN; // ON
}
else
{
LED1_PORT->BSRRH |= LED1_PIN; // OFF
}
}
else if (low_volt_alarmed)
{
// make sure led is off coming out of the low voltage alarm
configDigitalOutPin(LED1_PIN, LED1_PORT);
LED1_PORT->BSRRH |= LED1_PIN; // OFF
low_volt_alarmed = 0;
}
if (adc_dirty)
{
adc_dirty = 0;
config_adc_in_from_regs();
}
if (dig_dirty)
{
dig_dirty = 0;
update_dig_pin_configs();
}
uint32_t sensor_update_time = usCount - before;
const uint16_t us_delay_needed = 700;
const uint8_t got_time_to_burn = sensor_update_time < us_delay_needed;
if (got_time_to_burn)
{
// sleep the remainder of the coasting period before sampling bemf
delay_us(us_delay_needed-sensor_update_time);
}
if (bemf_update_time)
{
update_bemfs(bemf_vals, bemf_vals_filt);
// set the motor directions back up
update_motor_modes();
uint8_t channel;
for (channel = 0; channel < 4; ++channel)
{
uint8_t shift = 2*channel;
uint8_t motor_mode = (aTxBuffer[REG_RW_MOT_MODES] & (0b11 << shift)) >> shift;
motor_update(bemf_vals[channel], bemf_vals_filt[channel], &pid_structs[channel], channel, motor_mode);
}
}
else
{
// updating sensors doesnt' take as long as all of the adc for backemf updates
// sleep a bit so that our time through the loop is consistent for PID control
delay_us(222);
}
}
}
