void SensorReadMAG() { int16_t rawMAG[3]; #ifdef HMC5883 hmc5883lGetHeading(&rawMAG[0],&rawMAG[1], &rawMAG[2]); #endif #ifdef AK8975 AK8975_getHeading(&rawMAG[0],&rawMAG[1], &rawMAG[2]); #endif MAG_ORIENTATION(rawMAG[0],rawMAG[1],rawMAG[2]); //printf("Raw Mag:%d %d %d\n",Sensor.rawMAG[0], Sensor.rawMAG[1], Sensor.rawMAG[2]); }
void imu9Read(Axis3f* gyroOut, Axis3f* accOut, Axis3f* magOut) { imu6Read(gyroOut, accOut); if (isHmc5883lPresent) { hmc5883lGetHeading(&mag.x, &mag.y, &mag.z); magOut->x = (float)mag.x / MAG_GAUSS_PER_LSB; magOut->y = (float)mag.y / MAG_GAUSS_PER_LSB; magOut->z = (float)mag.z / MAG_GAUSS_PER_LSB; } else { magOut->x = 0.0; magOut->y = 0.0; magOut->z = 0.0; } }
void imu9Read(Axis3f* gyroOut, Axis3f* accOut, Axis3f* magOut) { imu6Read(gyroOut, accOut); if (isHmc5883lPresent) { hmc5883lGetHeading(&mag.x, &mag.y, &mag.z); magOut->x = (fix_t)mag.x * (1.0k / MAG_GAUSS_PER_LSB); magOut->y = (fix_t)mag.y * (1.0k / MAG_GAUSS_PER_LSB); magOut->z = (fix_t)mag.z * (1.0k / MAG_GAUSS_PER_LSB); } else { magOut->x = 0.0k; magOut->y = 0.0k; magOut->z = 0.0k; } }
static void stabilizerTask(void* param) { uint32_t attitudeCounter = 0; uint32_t altHoldCounter = 0; uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); while(1) { vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz imu6Read(&gyro, &acc); hmc5883lGetHeading(&heading.x, &heading.y, &heading.z); if (imu6IsCalibrated()) { commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired); commanderGetRPYType(&rollType, &pitchType, &yawType); // 250HZ if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z); // Estimate speed from acc (drifts) vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT; controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired, &pitchRateDesired, &yawRateDesired); attitudeCounter = 0; } // 100HZ if (imuHasBarometer() && (++altHoldCounter >= HOVER_UPDATE_RATE_DIVIDER)) { stabilizerAltHoldUpdate(); altHoldCounter = 0; } if (rollType == RATE) { rollRateDesired = eulerRollDesired; } if (pitchType == RATE) { pitchRateDesired = eulerPitchDesired; } if (yawType == RATE) { yawRateDesired = -eulerYawDesired; } // TODO: Investigate possibility to subtract gyro drift. controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired, yawRateDesired); controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); if (!altHold || !imuHasBarometer()) { // Use thrust from controller if not in altitude hold mode commanderGetThrust(&actuatorThrust); } else { // Added so thrust can be set to 0 while in altitude hold mode after disconnect commanderWatchdog(); } if (actuatorThrust > 0) { #if defined(TUNE_ROLL) distributePower(actuatorThrust, actuatorRoll, 0, 0); #elif defined(TUNE_PITCH) distributePower(actuatorThrust, 0, actuatorPitch, 0); #elif defined(TUNE_YAW) distributePower(actuatorThrust, 0, 0, -actuatorYaw); #else distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw); #endif } else { distributePower(0, 0, 0, 0); controllerResetAllPID(); } } } }
/** Do a self test. * @return True if self test passed, false otherwise */ bool hmc5883lSelfTest() { bool testStatus = true; int16_t mxp, myp, mzp; // positive magnetometer measurements int16_t mxn, myn, mzn; // negative magnetometer measurements struct { uint8_t configA; uint8_t configB; uint8_t mode; } regSave; // Save register values if (i2cdevRead(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)®Save) == false) { // TODO: error handling return false; } // Set gain (sensitivity) hmc5883lSetGain(HMC5883L_ST_GAIN); // Write CONFIG_A register and do positive test i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) | (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) | (HMC5883L_BIAS_POSITIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1))); /* Perform test measurement & check results */ hmc5883lSetMode(HMC5883L_MODE_SINGLE); vTaskDelay(M2T(HMC5883L_ST_DELAY_MS)); hmc5883lGetHeading(&mxp, &myp, &mzp); // Write CONFIG_A register and do negative test i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) | (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) | (HMC5883L_BIAS_NEGATIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1))); /* Perform test measurement & check results */ hmc5883lSetMode(HMC5883L_MODE_SINGLE); vTaskDelay(M2T(HMC5883L_ST_DELAY_MS)); hmc5883lGetHeading(&mxn, &myn, &mzn); if (hmc5883lEvaluateSelfTest(HMC5883L_ST_X_MIN, HMC5883L_ST_X_MAX, mxp, "pos X") && hmc5883lEvaluateSelfTest(HMC5883L_ST_Y_MIN, HMC5883L_ST_Y_MAX, myp, "pos Y") && hmc5883lEvaluateSelfTest(HMC5883L_ST_Z_MIN, HMC5883L_ST_Z_MAX, mzp, "pos Z") && hmc5883lEvaluateSelfTest(-HMC5883L_ST_X_MAX, -HMC5883L_ST_X_MIN, mxn, "neg X") && hmc5883lEvaluateSelfTest(-HMC5883L_ST_Y_MAX, -HMC5883L_ST_Y_MIN, myn, "neg Y") && hmc5883lEvaluateSelfTest(-HMC5883L_ST_Z_MAX, -HMC5883L_ST_Z_MIN, mzn, "neg Z")) { DEBUG_PRINT("Self test [OK].\n"); } else { testStatus = false; } // Restore registers if (i2cdevWrite(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)®Save) == false) { // TODO: error handling return false; } return true; //testStatus; }
static void stabilizerTask(void* param) { uint32_t attitudeCounter = 0; uint32_t lastWakeTime; vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR); //Wait for the system to be fully started to start stabilization loop systemWaitStart(); lastWakeTime = xTaskGetTickCount (); while(1) { vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); imu6Read(&gyro, &acc); hmc5883lGetHeading(&magHeadingX, &magHeadingY, &magHeadingZ); if(altMode == ALTIMETER_MODE_PRESSURE) { altimeterTmp = ms5611GetPressure(MS5611_OSR_4096); if(altimeterTmp > 0.0f) { pressure = altimeterTmp; altMode = ALTIMETER_MODE_TEMPERATURE; } } if(altMode == ALTIMETER_MODE_TEMPERATURE) { altimeterTmp = ms5611GetTemperature(MS5611_OSR_4096); if(altimeterTmp > 0.0f) { temperature = altimeterTmp; altMode = ALTIMETER_MODE_PRESSURE; } } if (imu6IsCalibrated()) { commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired); commanderGetRPYType(&rollType, &pitchType, &yawType); if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) { //sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); //sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); //controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual, // eulerRollDesired, eulerPitchDesired, -eulerYawDesired, // &rollRateDesired, &pitchRateDesired, &yawRateDesired); //controllerCorrectAttitudePID(0, 0, 0, 0, 0, 0, &rollRateDesired, &pitchRateDesired, &yawRateDesired); attitudeCounter = 0; } if (rollType == RATE) { rollRateDesired = eulerRollDesired; } if (pitchType == RATE) { pitchRateDesired = eulerPitchDesired; } if (yawType == RATE) { yawRateDesired = -eulerYawDesired; } /* // TODO: Investigate possibility to subtract gyro drift. controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired, yawRateDesired); controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw); */ commanderGetTrust(&actuatorThrust); distributePower(actuatorThrust, 0, 0, 0); if (actuatorThrust > 0) { #if defined(TUNE_ROLL) distributePower(actuatorThrust, actuatorRoll, 0, 0); #elif defined(TUNE_PITCH) distributePower(actuatorThrust, 0, actuatorPitch, 0); #elif defined(TUNE_YAW) distributePower(actuatorThrust, 0, 0, -actuatorYaw); #else distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw); #endif } else { distributePower(0, 0, 0, 0); controllerResetAllPID(); } #if 0 static int i = 0; if (++i > 19) { uartPrintf("%i, %i, %i\n", (int32_t)(eulerRollActual*100), (int32_t)(eulerPitchActual*100), (int32_t)(eulerYawActual*100)); i = 0; } #endif } } }