/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateVtolDesiredVelocity()
{
GuidanceSettingsData guidanceSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
VelocityDesiredData velocityDesired;
GuidanceSettingsGet(&guidanceSettings);
PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired);
VelocityDesiredGet(&velocityDesired);
// Note all distances in cm
float dNorth = positionDesired.North - positionActual.North;
float dEast = positionDesired.East - positionActual.East;
float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2));
float heading = atan2f(dEast, dNorth);
float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP],
0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]);
velocityDesired.North = groundspeed * cosf(heading);
velocityDesired.East = groundspeed * sinf(heading);
float dDown = positionDesired.Down - positionActual.Down;
velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP],
-guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX],
guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]);
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateVtolDesiredVelocity()
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT = 0;
GuidanceSettingsData guidanceSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
VelocityDesiredData velocityDesired;
GuidanceSettingsGet(&guidanceSettings);
PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
float northCommand;
float eastCommand;
float downCommand;
// Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Note all distances in cm
// Compute desired north command
northError = positionDesired.North - positionActual.North;
northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
northPosIntegral);
eastError = positionDesired.East - positionActual.East;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral);
velocityDesired.North = bound(northCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
velocityDesired.East = bound(eastCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
downError = positionDesired.Down - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param2, float param3, float param4, float param5_lat_x,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
{
if (frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)
{
GlobalPositionDesiredData pos;
GlobalPositionDesiredGet(&pos);
pos.Latitude = param5_lat_x*1E7;
pos.Longitude = param6_lon_y*1E7;
pos.Altitude = param7_alt_z;
pos.AltitudeIsAbsoluteMSL = (frame == MAV_FRAME_GLOBAL) ? 1 : 0;
GlobalPositionDesiredSet(&pos);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
PositionDesiredData pos;
PositionDesiredGet(&pos);
pos.North = param5_lat_x*1E2; // Convert from meter to cm
pos.East = param6_lon_y*1E2; // Convert from meter to cm
pos.Down = param7_alt_z*1E2; // Convert from meter to cm
PositionDesiredSet(&pos);
}
}
/**
* Module thread, should not return.
*/
static void guidanceTask(void *parameters)
{
SystemSettingsData systemSettings;
GuidanceSettingsData guidanceSettings;
ManualControlCommandData manualControl;
portTickType thisTime;
portTickType lastUpdateTime;
UAVObjEvent ev;
float accel[3] = {0,0,0};
uint32_t accel_accum = 0;
float q[4];
float Rbe[3][3];
float accel_ned[3];
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
GuidanceSettingsGet(&guidanceSettings);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
// Collect downsampled attitude data
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
accel[0] += attitudeRaw.accels[0];
accel[1] += attitudeRaw.accels[1];
accel[2] += attitudeRaw.accels[2];
accel_accum++;
// Continue collecting data if not enough time
thisTime = xTaskGetTickCount();
if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )
continue;
lastUpdateTime = xTaskGetTickCount();
accel[0] /= accel_accum;
accel[1] /= accel_accum;
accel[2] /= accel_accum;
//rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0]=attitudeActual.q1;
q[1]=attitudeActual.q2;
q[2]=attitudeActual.q3;
q[3]=attitudeActual.q4;
Quaternion2R(q, Rbe);
for (uint8_t i=0; i<3; i++){
accel_ned[i]=0;
for (uint8_t j=0; j<3; j++)
accel_ned[i] += Rbe[j][i]*accel[j];
}
accel_ned[2] += 9.81;
NedAccelData accelData;
NedAccelGet(&accelData);
// Convert from m/s to cm/s
accelData.North = accel_ned[0] * 100;
accelData.East = accel_ned[1] * 100;
accelData.Down = accel_ned[2] * 100;
NedAccelSet(&accelData);
ManualControlCommandGet(&manualControl);
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) &&
((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
{
if(positionHoldLast == 0) {
/* When enter position hold mode save current position */
PositionDesiredData positionDesired;
PositionActualData positionActual;
PositionDesiredGet(&positionDesired);
PositionActualGet(&positionActual);
positionDesired.North = positionActual.North;
positionDesired.East = positionActual.East;
PositionDesiredSet(&positionDesired);
positionHoldLast = 1;
}
if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP)
updateVtolDesiredVelocity();
else
manualSetDesiredVelocity();
updateVtolDesiredAttitude();
} else {
// Be cleaner and get rid of global variables
northIntegral = 0;
eastIntegral = 0;
downIntegral = 0;
positionHoldLast = 0;
}
accel[0] = accel[1] = accel[2] = 0;
accel_accum = 0;
}
}
