void sensorsAcquire(sensorData_t *sensors, const uint32_t tick) { if (RATE_DO_EXECUTE(IMU_RATE, tick)) { imu9Read(&sensors->gyro, &sensors->acc, &sensors->mag); } if (RATE_DO_EXECUTE(BARO_RATE, tick) && imuHasBarometer()) { #ifdef PLATFORM_CF1 ms5611GetData(&sensors->baro.pressure, &sensors->baro.temperature, &sensors->baro.asl); #else lps25hGetData(&sensors->baro.pressure, &sensors->baro.temperature, &sensors->baro.asl); #endif // Experimental: receive the position from parameters if (position.timestamp) { sensors->position = position; } } }
bool compassCalibration(const uint32_t tick) { // Begin 100Hz updates if (RATE_DO_EXECUTE(COMPASS_CAL_RATE, tick)) { if (calRequired == 1) { magcalOn = false; xmax = -10000; xmin = 10000; ymax = -10000; ymin = 10000; zmax = -10000; zmin = 10000; } else if (calRequired == 2) { magcalOn = true; if (f_magx > xmax) xmax = f_magx; if (f_magy > ymax) ymax = f_magy; if (f_magx < xmin) xmin = f_magx; if (f_magy < ymin) ymin = f_magy; } else if (calRequired == 3) { magcalOn = false; } else if (calRequired == 4) { magcalOn = true; if (f_magz > zmax) zmax = f_magz; if (f_magz < zmin) zmin = f_magz; } else if (calRequired == 5) { magcalOn = false; xoff = xmax+xmin; xoff = -(xoff)/2.0f; yoff = ymax+ymin; yoff = -(yoff)/2.0f; zoff = zmax+zmin; zoff = -(zoff)/2.0f; xsf = -1.0f; ysf = (xmax+xoff)/(ymax+yoff); zsf = (xmax+xoff)/(zmax+zoff); calRequired = 0; } else { magcalOn = false; calRequired = 0; } } return true; }
void estimatorComplementary(state_t *state, sensorData_t *sensorData, control_t *control, const uint32_t tick) { sensorsAcquire(sensorData, tick); // Read sensors at full rate (1000Hz) if (RATE_DO_EXECUTE(ATTITUDE_UPDATE_RATE, tick)) { sensfusion6UpdateQ(sensorData->gyro.x, sensorData->gyro.y, sensorData->gyro.z, sensorData->acc.x, sensorData->acc.y, sensorData->acc.z, ATTITUDE_UPDATE_DT); // Save attitude, adjusted for the legacy CF2 body coordinate system sensfusion6GetEulerRPY(&state->attitude.roll, &state->attitude.pitch, &state->attitude.yaw); // Save quaternion, hopefully one day this could be used in a better controller. // Note that this is not adjusted for the legacy coordinate system sensfusion6GetQuaternion( &state->attitudeQuaternion.x, &state->attitudeQuaternion.y, &state->attitudeQuaternion.z, &state->attitudeQuaternion.w); state->acc.z = sensfusion6GetAccZWithoutGravity(sensorData->acc.x, sensorData->acc.y, sensorData->acc.z); positionUpdateVelocity(state->acc.z, ATTITUDE_UPDATE_DT); } if (RATE_DO_EXECUTE(POS_UPDATE_RATE, tick)) { // If position sensor data is preset, pass it throught // FIXME: The position sensor shall be used as an input of the estimator if (sensorData->position.timestamp) { state->position = sensorData->position; } else { positionEstimate(state, sensorData, POS_UPDATE_DT, tick); } } }
void stateController(control_t *control, const sensorData_t *sensors, const state_t *state, const setpoint_t *setpoint, const uint32_t tick) { if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { // Rate-controled YAW is moving YAW angle setpoint if (setpoint->mode.yaw == modeVelocity) { attitudeDesired.yaw -= setpoint->attitudeRate.yaw/500.0; while (attitudeDesired.yaw > 180.0) attitudeDesired.yaw -= 360.0; while (attitudeDesired.yaw < -180.0) attitudeDesired.yaw += 360.0; } else { attitudeDesired.yaw = setpoint->attitude.yaw; } } if (RATE_DO_EXECUTE(POSITION_RATE, tick)) { positionController(&actuatorThrust, &attitudeDesired, state, setpoint); } if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) { // Switch between manual and automatic position control if (setpoint->mode.z == modeDisable) { actuatorThrust = setpoint->thrust; } if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) { attitudeDesired.roll = setpoint->attitude.roll; attitudeDesired.pitch = setpoint->attitude.pitch; } attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw, setpoint->attitude.roll, setpoint->attitude.pitch, attitudeDesired.yaw, &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw); if (setpoint->mode.roll == modeVelocity) { rateDesired.roll = setpoint->attitudeRate.roll; } if (setpoint->mode.pitch == modeVelocity) { rateDesired.pitch = setpoint->attitudeRate.pitch; } // TODO: Investigate possibility to subtract gyro drift. attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z, rateDesired.roll, rateDesired.pitch, rateDesired.yaw); attitudeControllerGetActuatorOutput(&control->roll, &control->pitch, &control->yaw); control->yaw = -control->yaw; } control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt(); if (control->thrust == 0) { control->thrust = 0; control->roll = 0; control->pitch = 0; control->yaw = 0; attitudeControllerResetAllPID(); // Reset the calculated YAW angle for rate control attitudeDesired.yaw = state->attitude.yaw; } }
void compassController(state_t *state, const sensorData_t *sensorData, const uint32_t tick) { #ifdef CAL_BUTTONS if (calSeqButton) { if ((calRequired % 2) == 0) calRequired++; } else if (calHorzButton && calRequired == 1) { calRequired++; } else if (calVertButton && calRequired == 3) { calRequired++; } #endif // Begin 100Hz updates if (RATE_DO_EXECUTE(COMPASS_CAL_RATE, tick)) { //Magnetometer axis data is used to calculate geodetic cf2 yaw angle // which is needed to apply gps lat && lon corrections to cf2 roll and pitch. //Manual compensation for magnetic inclination and attitude compensation has been implemented. //In addition the magnetometer requires calibration and repeated again // when changing cf2 platform hardware configurations or when there is a change // in the local magnetic disturbance nearby that affect the magnetometer. //Calibration data is gathered while // rotating the cf2 360 degrees or more in the horizontal plane during a ~30 second // period when the magcalOn parameter is true and // rotating the cf2 360 degrees or more in a 90 deg tilt angle during a second ~30 // second period when magcalOn is again true. //Read the x, y and z (min and max) values during each of the 30 second periods when // magcalOn is false //cfclient View Tabs Log Blocks utility can be used to capture min && max values while // watching the plotter tab of magcalOn switching between 1 (cal period) and 0 (read // cal values period) //Note: This assumes mag +X axis is aft, +y axis is larboard, +z axia is up magx = (sensorData->mag.x * RawMag); magy = (sensorData->mag.y * RawMag); magz = (sensorData->mag.z * RawMag); f_magx = (f_magx * 3 + magx) >> 2; f_magy = (f_magy * 3 + magy) >> 2; f_magz = (f_magz * 3 + magz) >> 2; calAdvance = false; if ((calRequired == 2) || (calRequired == 4)) { if (cal_Timeout(30000)) { //30s to collect horz/vert min & max x, y, z values calAdvance = true; lastOnOffTime = xTaskGetTickCount (); } } else if (calRequired < 5) { if (cal_Timeout(15000)) { //15 seconds between collection periods calAdvance = true; lastOnOffTime = xTaskGetTickCount (); } } #ifndef CAL_BUTTONS if (calAdvance) { calRequired++; calAdvance = false; } if (calRequired > 4) calRequired = 0; #endif compassCalibration(tick); } // end 100Hz updates // Begin 5HZ Updates if (RATE_DO_EXECUTE(COMPASS_RATE, tick)) { fmagx = (f_magx + xoff) * xsf; fmagy = (f_magy + yoff) * ysf; fmagz = (f_magz + zoff) * zsf; theta = -state->attitude.roll; //convert to magnetometer axis //Limit roll to +/- 90 degrees if (theta > 90.0f) { theta = 180.0f - theta; } else if (theta < -90.0f) { theta = -180.0f - theta; } theta *= D2R; phi = state->attitude.pitch * D2R; //convert to magnetometer axis sinr = sinf(theta); cosr = cosf(theta); sinp = sinf(phi); cosp = cosf(phi); //Limit pitch to +/- 90 degrees if (cosp < 0.0f) cosp = -cosp; //CF2 Yaw +/- 180 degrees, zero at power on, ccw positive rotation //yawangle +/- 180 degrees, cw positive rotation, zero at true north //Note roll from eulerActuals (gyro) is negated above xm = fmagx*cosp + fmagy*sinr*sinp + fmagz*cosr*sinp; ym = fmagy*cosr - fmagz*sinr; yawangle = atan2f(ym,xm) / D2R + magneticdeclination; AdjAngle(&yawangle); //+ angle cw state->attitude.yawgeo = -yawangle; //+ angle ccw // In converting position (desired-measured) to roll/pitch // D2R = (float) M_PI/180.0f // cos = cosf(yawgeo * D2R) // sin = sinf(yawgeo * D2R) // pitch = - position.x * cos - position.y * sin // roll = - position.y * cos + position.x * sin } //End 5 Hz updates }