2010-09-27 09:28:34 +02:00
|
|
|
/**
|
|
|
|
******************************************************************************
|
|
|
|
*
|
|
|
|
* @file guidance.c
|
|
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
|
|
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
|
|
|
|
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
|
|
|
|
* of @ref ManualControlCommand is Auto.
|
|
|
|
*
|
|
|
|
* @see The GNU Public License (GPL) Version 3
|
|
|
|
*
|
|
|
|
*****************************************************************************/
|
|
|
|
/*
|
|
|
|
* This program is free software; you can redistribute it and/or modify
|
|
|
|
* it under the terms of the GNU General Public License as published by
|
|
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
|
|
* (at your option) any later version.
|
|
|
|
*
|
|
|
|
* This program is distributed in the hope that it will be useful, but
|
|
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
|
|
* for more details.
|
|
|
|
*
|
|
|
|
* You should have received a copy of the GNU General Public License along
|
|
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
|
*/
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Input object: ActiveWaypoint
|
|
|
|
* Input object: PositionActual
|
|
|
|
* Input object: ManualControlCommand
|
|
|
|
* Output object: AttitudeDesired
|
|
|
|
*
|
|
|
|
* This module will periodically update the value of the AttitudeDesired object.
|
|
|
|
*
|
|
|
|
* The module executes in its own thread in this example.
|
|
|
|
*
|
|
|
|
* Modules have no API, all communication to other modules is done through UAVObjects.
|
|
|
|
* However modules may use the API exposed by shared libraries.
|
|
|
|
* See the OpenPilot wiki for more details.
|
|
|
|
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "openpilot.h"
|
|
|
|
#include "guidance.h"
|
|
|
|
#include "guidancesettings.h"
|
2011-01-14 01:51:54 +01:00
|
|
|
#include "attituderaw.h"
|
2010-09-27 09:28:34 +02:00
|
|
|
#include "attitudeactual.h"
|
|
|
|
#include "positiondesired.h" // object that will be updated by the module
|
|
|
|
#include "positionactual.h"
|
2011-03-02 02:25:44 +01:00
|
|
|
#include "manualcontrol.h"
|
2010-09-27 09:28:34 +02:00
|
|
|
#include "manualcontrolcommand.h"
|
2011-01-14 01:51:54 +01:00
|
|
|
#include "nedaccel.h"
|
2011-03-02 02:25:27 +01:00
|
|
|
#include "stabilizationdesired.h"
|
2010-09-27 09:28:34 +02:00
|
|
|
#include "stabilizationsettings.h"
|
|
|
|
#include "systemsettings.h"
|
|
|
|
#include "velocitydesired.h"
|
|
|
|
#include "velocityactual.h"
|
2011-01-14 01:51:54 +01:00
|
|
|
#include "CoordinateConversions.h"
|
2010-09-27 09:28:34 +02:00
|
|
|
|
|
|
|
// Private constants
|
2011-01-14 01:51:54 +01:00
|
|
|
#define MAX_QUEUE_SIZE 1
|
2011-02-03 21:00:00 +01:00
|
|
|
#define STACK_SIZE_BYTES 1024
|
2011-01-09 21:49:33 +01:00
|
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
|
2010-09-27 09:28:34 +02:00
|
|
|
// Private types
|
|
|
|
|
|
|
|
// Private variables
|
|
|
|
static xTaskHandle guidanceTaskHandle;
|
2011-01-14 01:51:54 +01:00
|
|
|
static xQueueHandle queue;
|
2010-09-27 09:28:34 +02:00
|
|
|
|
|
|
|
// Private functions
|
|
|
|
static void guidanceTask(void *parameters);
|
|
|
|
static float bound(float val, float min, float max);
|
|
|
|
|
2010-10-04 04:01:34 +02:00
|
|
|
static void updateVtolDesiredVelocity();
|
2010-11-12 17:57:13 +01:00
|
|
|
static void manualSetDesiredVelocity();
|
2010-10-04 04:01:34 +02:00
|
|
|
static void updateVtolDesiredAttitude();
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
/**
|
|
|
|
* Initialise the module, called on startup
|
|
|
|
* \returns 0 on success or -1 if initialisation failed
|
|
|
|
*/
|
|
|
|
int32_t GuidanceInitialize()
|
|
|
|
{
|
2011-01-14 01:51:54 +01:00
|
|
|
// Create object queue
|
|
|
|
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
|
|
|
|
|
|
|
// Listen for updates.
|
|
|
|
AttitudeRawConnectQueue(queue);
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
// Start main task
|
2011-01-09 01:03:19 +01:00
|
|
|
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
|
2011-01-09 00:43:34 +01:00
|
|
|
TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
|
2010-09-27 09:28:34 +02:00
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2010-10-04 04:01:34 +02:00
|
|
|
static float northIntegral = 0;
|
|
|
|
static float eastIntegral = 0;
|
|
|
|
static float downIntegral = 0;
|
2010-11-12 17:57:16 +01:00
|
|
|
static uint8_t positionHoldLast = 0;
|
2010-10-04 04:01:34 +02:00
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
/**
|
|
|
|
* Module thread, should not return.
|
|
|
|
*/
|
|
|
|
static void guidanceTask(void *parameters)
|
|
|
|
{
|
|
|
|
SystemSettingsData systemSettings;
|
|
|
|
GuidanceSettingsData guidanceSettings;
|
|
|
|
ManualControlCommandData manualControl;
|
|
|
|
|
2011-01-14 01:51:54 +01:00
|
|
|
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];
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
// Main task loop
|
2011-01-14 01:51:54 +01:00
|
|
|
lastUpdateTime = xTaskGetTickCount();
|
2010-09-27 09:28:34 +02:00
|
|
|
while (1) {
|
2011-02-03 21:00:00 +01:00
|
|
|
GuidanceSettingsGet(&guidanceSettings);
|
2011-01-14 01:51:54 +01:00
|
|
|
|
|
|
|
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
|
2011-01-14 01:52:00 +01:00
|
|
|
if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE )
|
2011-01-14 01:51:54 +01:00
|
|
|
{
|
2011-01-14 01:52:00 +01:00
|
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
|
|
|
} else {
|
|
|
|
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
|
|
|
|
}
|
2011-01-14 01:51:54 +01:00
|
|
|
|
|
|
|
// Collect downsampled attitude data
|
|
|
|
AttitudeRawData attitudeRaw;
|
|
|
|
AttitudeRawGet(&attitudeRaw);
|
2011-02-03 03:42:39 +01:00
|
|
|
accel[0] += attitudeRaw.accels[0];
|
|
|
|
accel[1] += attitudeRaw.accels[1];
|
|
|
|
accel[2] += attitudeRaw.accels[2];
|
2011-01-14 01:51:54 +01:00
|
|
|
accel_accum++;
|
|
|
|
|
|
|
|
// Continue collecting data if not enough time
|
|
|
|
thisTime = xTaskGetTickCount();
|
2011-01-14 01:52:00 +01:00
|
|
|
if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )
|
2011-01-14 01:51:54 +01:00
|
|
|
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
|
2011-02-05 02:14:55 +01:00
|
|
|
accelData.North = accel_ned[0] * 100;
|
|
|
|
accelData.East = accel_ned[1] * 100;
|
|
|
|
accelData.Down = accel_ned[2] * 100;
|
2011-01-14 01:51:54 +01:00
|
|
|
NedAccelSet(&accelData);
|
|
|
|
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
ManualControlCommandGet(&manualControl);
|
|
|
|
SystemSettingsGet(&systemSettings);
|
|
|
|
GuidanceSettingsGet(&guidanceSettings);
|
2010-11-12 17:57:13 +01:00
|
|
|
|
2011-03-02 02:25:44 +01:00
|
|
|
if ((PARSE_FLIGHT_MODE(manualControl.FlightMode) == FLIGHTMODE_GUIDANCE) &&
|
2010-10-10 02:46:04 +02:00
|
|
|
((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
|
|
|
|
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
|
2010-12-28 19:37:36 +01:00
|
|
|
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
|
|
|
|
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
|
2010-11-12 17:57:13 +01:00
|
|
|
{
|
2010-11-12 17:57:16 +01:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2011-03-25 21:17:31 +01:00
|
|
|
if( manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD )
|
2011-01-14 01:51:57 +01:00
|
|
|
updateVtolDesiredVelocity();
|
|
|
|
else
|
|
|
|
manualSetDesiredVelocity();
|
|
|
|
updateVtolDesiredAttitude();
|
|
|
|
|
2010-10-04 04:01:34 +02:00
|
|
|
} else {
|
|
|
|
// Be cleaner and get rid of global variables
|
|
|
|
northIntegral = 0;
|
|
|
|
eastIntegral = 0;
|
|
|
|
downIntegral = 0;
|
2010-11-12 17:57:16 +01:00
|
|
|
positionHoldLast = 0;
|
2011-01-14 01:51:54 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
accel[0] = accel[1] = accel[2] = 0;
|
|
|
|
accel_accum = 0;
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2011-01-14 01:51:57 +01:00
|
|
|
/**
|
|
|
|
* Compute desired velocity from the current position
|
|
|
|
*
|
|
|
|
* Takes in @ref PositionActual and compares it to @ref PositionDesired
|
|
|
|
* and computes @ref VelocityDesired
|
|
|
|
*/
|
2010-10-04 04:01:34 +02:00
|
|
|
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);
|
2011-01-14 01:52:00 +01:00
|
|
|
float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP],
|
|
|
|
0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]);
|
2010-10-04 04:01:34 +02:00
|
|
|
|
|
|
|
velocityDesired.North = groundspeed * cosf(heading);
|
|
|
|
velocityDesired.East = groundspeed * sinf(heading);
|
|
|
|
|
|
|
|
float dDown = positionDesired.Down - positionActual.Down;
|
2011-01-14 01:52:00 +01:00
|
|
|
velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP],
|
|
|
|
-guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX],
|
|
|
|
guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]);
|
2010-10-04 04:01:34 +02:00
|
|
|
|
|
|
|
VelocityDesiredSet(&velocityDesired);
|
|
|
|
}
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
/**
|
2011-01-14 01:51:57 +01:00
|
|
|
* Compute desired attitude from the desired velocity
|
|
|
|
*
|
|
|
|
* Takes in @ref NedActual which has the acceleration in the
|
|
|
|
* NED frame as the feedback term and then compares the
|
|
|
|
* @ref VelocityActual against the @ref VelocityDesired
|
2010-09-27 09:28:34 +02:00
|
|
|
*/
|
2010-10-04 04:01:34 +02:00
|
|
|
static void updateVtolDesiredAttitude()
|
2010-09-27 09:28:34 +02:00
|
|
|
{
|
2010-10-04 04:01:34 +02:00
|
|
|
static portTickType lastSysTime;
|
|
|
|
portTickType thisSysTime = xTaskGetTickCount();;
|
|
|
|
float dT;
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
VelocityDesiredData velocityDesired;
|
|
|
|
VelocityActualData velocityActual;
|
2011-03-02 02:25:27 +01:00
|
|
|
StabilizationDesiredData stabDesired;
|
2010-09-27 09:28:34 +02:00
|
|
|
AttitudeActualData attitudeActual;
|
2011-01-14 01:51:54 +01:00
|
|
|
NedAccelData nedAccel;
|
2010-09-27 09:28:34 +02:00
|
|
|
GuidanceSettingsData guidanceSettings;
|
|
|
|
StabilizationSettingsData stabSettings;
|
|
|
|
SystemSettingsData systemSettings;
|
|
|
|
|
|
|
|
float northError;
|
|
|
|
float northCommand;
|
2010-11-12 17:57:13 +01:00
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
float eastError;
|
|
|
|
float eastCommand;
|
2010-11-12 17:57:13 +01:00
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
float downError;
|
2011-01-14 01:51:54 +01:00
|
|
|
float downCommand;
|
2010-10-04 04:01:34 +02:00
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
|
|
|
SystemSettingsGet(&systemSettings);
|
|
|
|
GuidanceSettingsGet(&guidanceSettings);
|
|
|
|
|
|
|
|
VelocityActualGet(&velocityActual);
|
|
|
|
VelocityDesiredGet(&velocityDesired);
|
2011-03-02 02:25:27 +01:00
|
|
|
StabilizationDesiredGet(&stabDesired);
|
2010-10-04 04:01:34 +02:00
|
|
|
VelocityDesiredGet(&velocityDesired);
|
|
|
|
AttitudeActualGet(&attitudeActual);
|
|
|
|
StabilizationSettingsGet(&stabSettings);
|
2011-01-14 01:51:54 +01:00
|
|
|
NedAccelGet(&nedAccel);
|
2010-10-04 04:01:34 +02:00
|
|
|
|
2011-03-25 21:17:31 +01:00
|
|
|
// Testing code - refactor into manual control command
|
|
|
|
ManualControlCommandData manualControlData;
|
|
|
|
ManualControlCommandGet(&manualControlData);
|
|
|
|
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
|
2010-10-04 04:01:34 +02:00
|
|
|
|
2011-01-14 01:51:54 +01:00
|
|
|
// Compute desired north command
|
2010-10-04 04:01:34 +02:00
|
|
|
northError = velocityDesired.North - velocityActual.North;
|
2011-01-14 01:51:54 +01:00
|
|
|
northIntegral = bound(northIntegral + northError * dT,
|
2011-01-14 01:52:00 +01:00
|
|
|
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
|
|
|
|
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
|
|
|
|
northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
|
|
|
|
northIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
|
|
|
|
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
|
2010-10-04 04:01:34 +02:00
|
|
|
|
2011-01-14 01:51:54 +01:00
|
|
|
// Compute desired east command
|
2010-10-04 04:01:34 +02:00
|
|
|
eastError = velocityDesired.East - velocityActual.East;
|
|
|
|
eastIntegral = bound(eastIntegral + eastError * dT,
|
2011-01-14 01:52:00 +01:00
|
|
|
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
|
|
|
|
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
|
|
|
|
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
|
|
|
|
eastIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
|
|
|
|
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
|
2011-01-14 01:51:54 +01:00
|
|
|
|
|
|
|
// Compute desired down command
|
|
|
|
downError = velocityDesired.Down - velocityActual.Down;
|
2011-01-14 01:52:00 +01:00
|
|
|
downIntegral = bound(downIntegral + downError * dT,
|
|
|
|
-guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
|
|
|
|
guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
|
|
|
|
downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
|
|
|
|
downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] -
|
|
|
|
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
|
2011-01-14 01:51:54 +01:00
|
|
|
|
2011-03-02 02:25:27 +01:00
|
|
|
stabDesired.Throttle = bound(downCommand, 0, 1);
|
2010-10-04 04:01:34 +02:00
|
|
|
|
|
|
|
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
|
|
|
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
2011-03-02 02:25:27 +01:00
|
|
|
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
|
2010-12-10 18:14:35 +01:00
|
|
|
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
|
2011-01-14 01:52:00 +01:00
|
|
|
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
|
2011-03-02 02:25:27 +01:00
|
|
|
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
|
2010-10-04 04:01:34 +02:00
|
|
|
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
|
2011-01-14 01:52:00 +01:00
|
|
|
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
|
2010-10-04 04:01:34 +02:00
|
|
|
|
2011-01-14 01:51:54 +01:00
|
|
|
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
|
|
|
|
// For now override throttle with manual control. Disable at your risk, quad goes to China.
|
|
|
|
ManualControlCommandData manualControl;
|
|
|
|
ManualControlCommandGet(&manualControl);
|
2011-03-02 02:25:27 +01:00
|
|
|
stabDesired.Throttle = manualControl.Throttle;
|
2011-01-14 01:51:54 +01:00
|
|
|
}
|
2010-10-04 04:01:34 +02:00
|
|
|
|
2011-03-02 02:25:27 +01:00
|
|
|
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
|
|
|
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
2011-03-25 21:17:31 +01:00
|
|
|
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
2011-03-02 02:25:27 +01:00
|
|
|
|
|
|
|
StabilizationDesiredSet(&stabDesired);
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
|
2011-01-14 01:51:57 +01:00
|
|
|
/**
|
|
|
|
* Set the desired velocity from the input sticks
|
|
|
|
*/
|
2010-11-12 17:57:13 +01:00
|
|
|
static void manualSetDesiredVelocity()
|
|
|
|
{
|
|
|
|
ManualControlCommandData cmd;
|
|
|
|
VelocityDesiredData velocityDesired;
|
|
|
|
|
|
|
|
ManualControlCommandGet(&cmd);
|
|
|
|
VelocityDesiredGet(&velocityDesired);
|
2010-12-13 08:03:04 +01:00
|
|
|
|
|
|
|
GuidanceSettingsData guidanceSettings;
|
|
|
|
GuidanceSettingsGet(&guidanceSettings);
|
2010-11-12 17:57:13 +01:00
|
|
|
|
2011-01-14 01:52:00 +01:00
|
|
|
velocityDesired.North = -guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Pitch;
|
|
|
|
velocityDesired.East = guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Roll;
|
2010-11-12 17:57:13 +01:00
|
|
|
velocityDesired.Down = 0;
|
|
|
|
|
|
|
|
VelocityDesiredSet(&velocityDesired);
|
|
|
|
}
|
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
/**
|
|
|
|
* Bound input value between limits
|
|
|
|
*/
|
|
|
|
static float bound(float val, float min, float max)
|
|
|
|
{
|
|
|
|
if (val < min) {
|
|
|
|
val = min;
|
|
|
|
} else if (val > max) {
|
|
|
|
val = max;
|
|
|
|
}
|
|
|
|
return val;
|
|
|
|
}
|