mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Enable guidance in revo. Update manual control to set the position desired
when the switch is flipped.
This commit is contained in:
James Cotton
parent
0eedaa1250
commit
337d5b7079
@ -45,13 +45,13 @@
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "guidance.h"
|
||||
#include "guidancesettings.h"
|
||||
#include "attituderaw.h"
|
||||
#include "accels.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "positiondesired.h" // object that will be updated by the module
|
||||
#include "positionactual.h"
|
||||
#include "manualcontrol.h"
|
||||
#include "flightstatus.h"
|
||||
#include "guidancesettings.h"
|
||||
#include "nedaccel.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "stabilizationsettings.h"
|
||||
@ -61,7 +61,7 @@
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 1
|
||||
#define MAX_QUEUE_SIZE 4
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
|
||||
// Private types
|
||||
@ -97,7 +97,6 @@ int32_t GuidanceStart()
|
||||
*/
|
||||
int32_t GuidanceInitialize()
|
||||
{
|
||||
|
||||
GuidanceSettingsInitialize();
|
||||
PositionDesiredInitialize();
|
||||
NedAccelInitialize();
|
||||
@ -107,7 +106,7 @@ int32_t GuidanceInitialize()
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for updates.
|
||||
AttitudeRawConnectQueue(queue);
|
||||
AccelsConnectQueue(queue);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -157,11 +156,11 @@ static void guidanceTask(void *parameters)
|
||||
}
|
||||
|
||||
// Collect downsampled attitude data
|
||||
AttitudeRawData attitudeRaw;
|
||||
AttitudeRawGet(&attitudeRaw);
|
||||
accel[0] += attitudeRaw.accels[0];
|
||||
accel[1] += attitudeRaw.accels[1];
|
||||
accel[2] += attitudeRaw.accels[2];
|
||||
AccelsData accels;
|
||||
AccelsGet(&accels);
|
||||
accel[0] += accels.x;
|
||||
accel[1] += accels.y;
|
||||
accel[2] += accels.z;
|
||||
accel_accum++;
|
||||
|
||||
// Continue collecting data if not enough time
|
||||
@ -173,6 +172,8 @@ static void guidanceTask(void *parameters)
|
||||
accel[0] /= accel_accum;
|
||||
accel[1] /= accel_accum;
|
||||
accel[2] /= accel_accum;
|
||||
accel[0] = accel[1] = accel[2] = 0;
|
||||
accel_accum = 0;
|
||||
|
||||
//rotate avg accels into earth frame and store it
|
||||
AttitudeActualData attitudeActual;
|
||||
@ -187,45 +188,30 @@ static void guidanceTask(void *parameters)
|
||||
for (uint8_t j=0; j<3; j++)
|
||||
accel_ned[i] += Rbe[j][i]*accel[j];
|
||||
}
|
||||
accel_ned[2] += 9.81;
|
||||
accel_ned[2] += 9.81f;
|
||||
|
||||
NedAccelData accelData;
|
||||
NedAccelGet(&accelData);
|
||||
// Convert from m/s to cm/s
|
||||
accelData.North = accel_ned[0];
|
||||
accelData.East = accel_ned[1];
|
||||
accelData.Down = accel_ned[2];
|
||||
NedAccelSet(&accelData);
|
||||
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
SystemSettingsGet(&systemSettings);
|
||||
GuidanceSettingsGet(&guidanceSettings);
|
||||
|
||||
if ((PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_GUIDANCE) &&
|
||||
if ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) &&
|
||||
((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( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD )
|
||||
updateVtolDesiredVelocity();
|
||||
else
|
||||
manualSetDesiredVelocity();
|
||||
updateVtolDesiredAttitude();
|
||||
|
||||
} else {
|
||||
// Be cleaner and get rid of global variables
|
||||
northVelIntegral = 0;
|
||||
@ -236,9 +222,6 @@ static void guidanceTask(void *parameters)
|
||||
downPosIntegral = 0;
|
||||
positionHoldLast = 0;
|
||||
}
|
||||
|
||||
accel[0] = accel[1] = accel[2] = 0;
|
||||
accel_accum = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -34,19 +34,20 @@
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "manualcontrol.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "accessorydesired.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "altitudeholddesired.h"
|
||||
#include "baroaltitude.h"
|
||||
#include "flighttelemetrystats.h"
|
||||
#include "flightstatus.h"
|
||||
#include "accessorydesired.h"
|
||||
#include "receiveractivity.h"
|
||||
#include "altitudeholddesired.h"
|
||||
#include "manualcontrol.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "positionactual.h"
|
||||
#include "baroaltitude.h"
|
||||
#include "positiondesired.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "receiveractivity.h"
|
||||
|
||||
// Private constants
|
||||
#if defined(PIOS_MANUAL_STACK_SIZE)
|
||||
@ -83,6 +84,7 @@ static portTickType lastSysTime;
|
||||
static void updateActuatorDesired(ManualControlCommandData * cmd);
|
||||
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd);
|
||||
static void positionDesired(ManualControlCommandData * cmd);
|
||||
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
|
||||
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
||||
static void setArmedIfChanged(uint8_t val);
|
||||
@ -383,6 +385,9 @@ static void manualControlTask(void *parameters)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
|
||||
altitudeHoldDesired(&cmd);
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
positionDesired(&cmd);
|
||||
break;
|
||||
default:
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
@ -600,6 +605,41 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
|
||||
|
||||
#if defined(REVOLUTION)
|
||||
// TODO: Need compile flag to exclude this from copter control
|
||||
/**
|
||||
* @brief Update the position desired to current location when
|
||||
* enabled and allow the waypoint to be moved by transmitter
|
||||
*/
|
||||
static void positionDesired(ManualControlCommandData * cmd)
|
||||
{
|
||||
static portTickType lastSysTime;
|
||||
portTickType thisSysTime;
|
||||
float dT;
|
||||
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
if(dT > 1) {
|
||||
// After not being in this mode for a while init at current height
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
PositionDesiredData positionDesired;
|
||||
PositionDesiredGet(&positionDesired);
|
||||
positionDesired.North = positionActual.North;
|
||||
positionDesired.East = positionActual.East;
|
||||
positionDesired.Down = positionActual.Down;
|
||||
PositionDesiredSet(&positionDesired);
|
||||
} else {
|
||||
// TODO: Implement moving magic waypoint with remote
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Update the altitude desired to current altitude when
|
||||
* enabled and enable altitude mode for stabilization
|
||||
* @todo: Need compile flag to exclude this from copter control
|
||||
*/
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd)
|
||||
{
|
||||
const float DEADBAND_HIGH = 0.55;
|
||||
|
||||
@ -49,11 +49,10 @@ endif
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# List of modules to include
|
||||
MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP
|
||||
MODULES += AltitudeHold
|
||||
MODULES = Sensors Attitude/revolution ManualControl Stabilization Altitude/revolution Actuator GPS FirmwareIAP AltitudeHold Guidance
|
||||
MODULES += CameraStab
|
||||
MODULES += OveroSync
|
||||
MODULES += Telemetry
|
||||
#MODULES += OveroSync
|
||||
PYMODULES =
|
||||
#FlightPlan
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user