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
}
