mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-01 17:52:12 +01:00
6d0df704a7
find ./flight/OpenPilot/Modules/ \! \( -name '*~' -a -prune \) -type f |
xargs -I{} bash -c 'echo {}; dos2unix {}; gnuindent -npro -kr -i8 -ts8 -sob -ss
-ncs -cp1 -il0 -hnl -l150 {};'
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1776 ebee16cc-31ac-478f-84a7-5cbb03baadba
639 lines
25 KiB
C
639 lines
25 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup ActuatorModule Actuator Module
|
|
* @brief Compute servo/motor settings based on @ref ActuatorDesired "desired actuator positions" and aircraft type.
|
|
* This is where all the mixing of channels is computed.
|
|
* @{
|
|
*
|
|
* @file actuator.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief Actuator module. Drives the actuators (servos, motors etc).
|
|
*
|
|
* @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
|
|
*/
|
|
|
|
#include "openpilot.h"
|
|
#include "actuator.h"
|
|
#include "actuatorsettings.h"
|
|
#include "vtolsettings.h"
|
|
#include "vtolstatus.h"
|
|
#include "systemsettings.h"
|
|
#include "actuatordesired.h"
|
|
#include "actuatorcommand.h"
|
|
#include "manualcontrolcommand.h"
|
|
|
|
//yaw stabilisation for helis...
|
|
#include "stabilizationsettings.h"
|
|
#include "attitudeactual.h"
|
|
//yaw stabilisation for helis...
|
|
|
|
// Private constants
|
|
#define MAX_QUEUE_SIZE 2
|
|
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
|
#define FAILSAFE_TIMEOUT_MS 100
|
|
|
|
// Private types
|
|
//yaw stabilisation for helis...
|
|
//currently just coppied from simple stabilisation
|
|
#define YAW_INTEGRAL_LIMIT 0.5
|
|
AttitudeActualData attitudeActual;
|
|
StabilizationSettingsData stabSettings;
|
|
float ccpmYawError, ccpmYawErrorLast;
|
|
float ccpmYawDerivative;
|
|
float ccpmYawIntegral, ccpmYawIntegralLimit;
|
|
float desiredYaw;
|
|
static float bound(float val, float min, float max);
|
|
//yaw stabilisation for helis...
|
|
|
|
// Private variables
|
|
static xQueueHandle queue;
|
|
static xTaskHandle taskHandle;
|
|
|
|
// Private functions
|
|
static void actuatorTask(void *parameters);
|
|
static int32_t mixerFixedWing(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd);
|
|
static int32_t mixerFixedWingElevon(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd);
|
|
static int32_t mixerVTOL(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd);
|
|
static int32_t InterpolateCCPMCurve(float *Output, float input, const float *Curve);
|
|
static int32_t mixerCCPM(ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd);
|
|
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
|
|
static void setFailsafe();
|
|
|
|
static float bound(float val, float min, float max);
|
|
|
|
/**
|
|
* @brief Module initialization
|
|
* @return 0
|
|
*/
|
|
int32_t ActuatorInitialize()
|
|
{
|
|
// Create object queue
|
|
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
|
|
|
// Listen for ExampleObject1 updates
|
|
ActuatorDesiredConnectQueue(queue);
|
|
|
|
ccpmYawErrorLast = 0.0;
|
|
|
|
// Start main task
|
|
xTaskCreate(actuatorTask, (signed char *)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @brief Main module task
|
|
*/
|
|
static void actuatorTask(void *parameters)
|
|
{
|
|
// UAVObjEvent ev;
|
|
ActuatorSettingsData settings;
|
|
SystemSettingsData sysSettings;
|
|
ActuatorDesiredData desired;
|
|
ActuatorCommandData cmd;
|
|
portTickType lastSysTime;
|
|
|
|
// Set servo update frequency (done only on start-up)
|
|
ActuatorSettingsGet(&settings);
|
|
PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
|
|
|
|
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
|
|
setFailsafe();
|
|
|
|
// Main task loop
|
|
lastSysTime = xTaskGetTickCount();
|
|
while (1) {
|
|
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
|
|
/*if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
|
|
{
|
|
setFailsafe();
|
|
continue;
|
|
} */
|
|
|
|
// Read settings
|
|
ActuatorSettingsGet(&settings);
|
|
SystemSettingsGet(&sysSettings);
|
|
|
|
// Reset ActuatorCommand to neutral values
|
|
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
|
|
cmd.Channel[n] = settings.ChannelNeutral[n];
|
|
}
|
|
|
|
// Read input object
|
|
ActuatorDesiredGet(&desired);
|
|
|
|
// Call appropriate mixer depending on the airframe configuration
|
|
if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) {
|
|
if (mixerFixedWing(&settings, &desired, &cmd) == -1) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
|
}
|
|
} else if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) {
|
|
if (mixerFixedWingElevon(&settings, &desired, &cmd) == -1) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
|
}
|
|
} else if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) {
|
|
if (mixerVTOL(&settings, &desired, &cmd) == -1) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
|
}
|
|
} else if (sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) {
|
|
if (mixerCCPM(&settings, &desired, &cmd) == -1) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
} else {
|
|
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
|
}
|
|
}
|
|
// Update output object
|
|
ActuatorCommandSet(&cmd);
|
|
// Update in case read only (eg. during servo configuration)
|
|
ActuatorCommandGet(&cmd);
|
|
|
|
// Update servo outputs
|
|
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
|
|
PIOS_Servo_Set(n, cmd.Channel[n]);
|
|
}
|
|
|
|
// Wait until next update
|
|
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS);
|
|
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Mixer for Fixed Wing airframes. Converts desired roll,pitch,yaw and throttle to servo outputs.
|
|
* @return -1 if error, 0 if success
|
|
*/
|
|
static int32_t mixerFixedWing(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd)
|
|
{
|
|
|
|
// Check settings
|
|
if (settings->FixedWingPitch1 == ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE ||
|
|
settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE ||
|
|
settings->FixedWingThrottle == ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE) {
|
|
return -1;
|
|
}
|
|
|
|
ManualControlCommandData manualControl;
|
|
ManualControlCommandGet(&manualControl);
|
|
|
|
// Set pitch servo command
|
|
cmd->Channel[settings->FixedWingPitch1] = scaleChannel(desired->Pitch, settings->ChannelMax[settings->FixedWingPitch1],
|
|
settings->ChannelMin[settings->FixedWingPitch1],
|
|
settings->ChannelNeutral[settings->FixedWingPitch1]);
|
|
if (settings->FixedWingPitch2 != ACTUATORSETTINGS_FIXEDWINGPITCH2_NONE) {
|
|
cmd->Channel[settings->FixedWingPitch2] = scaleChannel(desired->Pitch, settings->ChannelMax[settings->FixedWingPitch2],
|
|
settings->ChannelMin[settings->FixedWingPitch2],
|
|
settings->ChannelNeutral[settings->FixedWingPitch2]);
|
|
}
|
|
// Set roll servo command
|
|
cmd->Channel[settings->FixedWingRoll1] = scaleChannel(desired->Roll, settings->ChannelMax[settings->FixedWingRoll1],
|
|
settings->ChannelMin[settings->FixedWingRoll1],
|
|
settings->ChannelNeutral[settings->FixedWingRoll1]);
|
|
if (settings->FixedWingRoll2 != ACTUATORSETTINGS_FIXEDWINGROLL2_NONE) {
|
|
cmd->Channel[settings->FixedWingRoll2] = scaleChannel(desired->Roll, settings->ChannelMax[settings->FixedWingRoll2],
|
|
settings->ChannelMin[settings->FixedWingRoll2],
|
|
settings->ChannelNeutral[settings->FixedWingRoll2]);
|
|
}
|
|
// Set yaw servo command
|
|
if (settings->FixedWingYaw != ACTUATORSETTINGS_FIXEDWINGYAW_NONE) {
|
|
cmd->Channel[settings->FixedWingYaw] = scaleChannel(desired->Yaw, settings->ChannelMax[settings->FixedWingYaw],
|
|
settings->ChannelMin[settings->FixedWingYaw],
|
|
settings->ChannelNeutral[settings->FixedWingYaw]);
|
|
}
|
|
// Set throttle servo command
|
|
if (manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE)
|
|
cmd->Channel[settings->FixedWingThrottle] = scaleChannel(desired->Throttle, settings->ChannelMax[settings->FixedWingThrottle],
|
|
settings->ChannelMin[settings->FixedWingThrottle],
|
|
settings->ChannelNeutral[settings->FixedWingThrottle]);
|
|
else
|
|
cmd->Channel[settings->FixedWingThrottle] = -1;
|
|
|
|
// Done
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Mixer for Fixed Wing airframes with elevons. Converts desired roll,pitch,yaw and throttle to servo outputs.
|
|
* @return -1 if error, 0 if success
|
|
*/
|
|
static int32_t mixerFixedWingElevon(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd)
|
|
{
|
|
// Check settings
|
|
if (settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE ||
|
|
settings->FixedWingRoll2 == ACTUATORSETTINGS_FIXEDWINGROLL2_NONE ||
|
|
settings->FixedWingThrottle == ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE) {
|
|
return -1;
|
|
}
|
|
|
|
ManualControlCommandData manualControl;
|
|
ManualControlCommandGet(&manualControl);
|
|
|
|
// Set first elevon servo command
|
|
cmd->Channel[settings->FixedWingRoll1] = scaleChannel(desired->Pitch + desired->Roll, settings->ChannelMax[settings->FixedWingRoll1],
|
|
settings->ChannelMin[settings->FixedWingRoll1],
|
|
settings->ChannelNeutral[settings->FixedWingRoll1]);
|
|
|
|
// Set second elevon servo command
|
|
cmd->Channel[settings->FixedWingRoll2] = scaleChannel(desired->Pitch - desired->Roll, settings->ChannelMax[settings->FixedWingRoll2],
|
|
settings->ChannelMin[settings->FixedWingRoll2],
|
|
settings->ChannelNeutral[settings->FixedWingRoll2]);
|
|
|
|
// Set throttle servo command
|
|
if (manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE)
|
|
cmd->Channel[settings->FixedWingThrottle] = scaleChannel(desired->Throttle, settings->ChannelMax[settings->FixedWingThrottle],
|
|
settings->ChannelMin[settings->FixedWingThrottle],
|
|
settings->ChannelNeutral[settings->FixedWingThrottle]);
|
|
else
|
|
cmd->Channel[settings->FixedWingThrottle] = -1;
|
|
|
|
// Done
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Mixer for VTOL (quads and octo copters). Converts desired roll,pitch,yaw and throttle to servo outputs.
|
|
*
|
|
* I will probably add settings to change the weighting for each control to each motor. This will allow more
|
|
* flexible support for various motor topologies. For now hard coding in simple versions and lettings the
|
|
* scaling capabilities fix the subtlties.
|
|
*
|
|
* Also, because of how the Throttle ranges from 0 to 1, the motors should too!
|
|
*
|
|
* @return -1 if error, 0 if success
|
|
*/
|
|
static int32_t mixerVTOL(const ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd)
|
|
{
|
|
VTOLSettingsData vtolSettings;
|
|
VTOLStatusData vtolStatus;
|
|
VTOLSettingsGet(&vtolSettings);
|
|
VTOLStatusGet(&vtolStatus);
|
|
|
|
ManualControlCommandData manualControl;
|
|
ManualControlCommandGet(&manualControl);
|
|
|
|
const int vtolMin = -1;
|
|
int vtolMax = -1;
|
|
|
|
if ((manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) && (desired->Throttle >= 0))
|
|
vtolMax = 1;
|
|
else
|
|
vtolMax = -1;
|
|
|
|
if (((settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) +
|
|
(settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
|
|
(settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) +
|
|
(settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) +
|
|
(settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) +
|
|
(settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) +
|
|
(settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) + (settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE)) <= 2) {
|
|
return -1; // can't fly with 2 or less engines (i believe)
|
|
}
|
|
|
|
if (settings->VTOLMotorN != ACTUATORSETTINGS_VTOLMOTORN_NONE) {
|
|
vtolStatus.MotorN = desired->Throttle * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_PITCH] +
|
|
desired->Roll * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_ROLL] + desired->Yaw * vtolSettings.MotorN[VTOLSETTINGS_MOTORN_YAW];
|
|
cmd->Channel[settings->VTOLMotorN] = scaleChannel(bound(vtolStatus.MotorN, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorN],
|
|
settings->ChannelMin[settings->VTOLMotorN],
|
|
settings->ChannelNeutral[settings->VTOLMotorN]);
|
|
}
|
|
if (settings->VTOLMotorNE != ACTUATORSETTINGS_VTOLMOTORNE_NONE) {
|
|
vtolStatus.MotorNE = desired->Throttle * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_PITCH] +
|
|
desired->Roll * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_ROLL] + desired->Yaw * vtolSettings.MotorNE[VTOLSETTINGS_MOTORNE_YAW];
|
|
cmd->Channel[settings->VTOLMotorNE] = scaleChannel(bound(vtolStatus.MotorNE, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorNE],
|
|
settings->ChannelMin[settings->VTOLMotorNE],
|
|
settings->ChannelNeutral[settings->VTOLMotorNE]);
|
|
}
|
|
if (settings->VTOLMotorE != ACTUATORSETTINGS_VTOLMOTORE_NONE) {
|
|
vtolStatus.MotorE = desired->Throttle * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_PITCH] +
|
|
desired->Roll * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_ROLL] + desired->Yaw * vtolSettings.MotorE[VTOLSETTINGS_MOTORE_YAW];
|
|
cmd->Channel[settings->VTOLMotorE] = scaleChannel(bound(vtolStatus.MotorE, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorE],
|
|
settings->ChannelMin[settings->VTOLMotorE],
|
|
settings->ChannelNeutral[settings->VTOLMotorE]);
|
|
}
|
|
if (settings->VTOLMotorSE != ACTUATORSETTINGS_VTOLMOTORSE_NONE) {
|
|
vtolStatus.MotorSE = desired->Throttle * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_PITCH] +
|
|
desired->Roll * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_ROLL] + desired->Yaw * vtolSettings.MotorSE[VTOLSETTINGS_MOTORSE_YAW];
|
|
cmd->Channel[settings->VTOLMotorSE] = scaleChannel(bound(vtolStatus.MotorSE, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorSE],
|
|
settings->ChannelMin[settings->VTOLMotorSE],
|
|
settings->ChannelNeutral[settings->VTOLMotorSE]);
|
|
}
|
|
if (settings->VTOLMotorS != ACTUATORSETTINGS_VTOLMOTORS_NONE) {
|
|
vtolStatus.MotorS = desired->Throttle * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_PITCH] +
|
|
desired->Roll * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_ROLL] + desired->Yaw * vtolSettings.MotorS[VTOLSETTINGS_MOTORS_YAW];
|
|
cmd->Channel[settings->VTOLMotorS] = scaleChannel(bound(vtolStatus.MotorS, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorS],
|
|
settings->ChannelMin[settings->VTOLMotorS],
|
|
settings->ChannelNeutral[settings->VTOLMotorS]);
|
|
}
|
|
if (settings->VTOLMotorSW != ACTUATORSETTINGS_VTOLMOTORSW_NONE) {
|
|
vtolStatus.MotorSW = bound(desired->Throttle * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_PITCH] +
|
|
desired->Roll * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_ROLL] +
|
|
desired->Yaw * vtolSettings.MotorSW[VTOLSETTINGS_MOTORSW_YAW], vtolMin, vtolMax);
|
|
cmd->Channel[settings->VTOLMotorSW] = scaleChannel(vtolStatus.MotorSW,
|
|
settings->ChannelMax[settings->VTOLMotorSW],
|
|
settings->ChannelMin[settings->VTOLMotorSW],
|
|
settings->ChannelNeutral[settings->VTOLMotorSW]);
|
|
}
|
|
if (settings->VTOLMotorW != ACTUATORSETTINGS_VTOLMOTORW_NONE) {
|
|
vtolStatus.MotorW = desired->Throttle * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_PITCH] +
|
|
desired->Roll * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_ROLL] + desired->Yaw * vtolSettings.MotorW[VTOLSETTINGS_MOTORW_YAW];
|
|
cmd->Channel[settings->VTOLMotorW] = scaleChannel(bound(vtolStatus.MotorW, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorW],
|
|
settings->ChannelMin[settings->VTOLMotorW],
|
|
settings->ChannelNeutral[settings->VTOLMotorW]);
|
|
}
|
|
if (settings->VTOLMotorNW != ACTUATORSETTINGS_VTOLMOTORNW_NONE) {
|
|
vtolStatus.MotorNW = desired->Throttle * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_THROTTLE] +
|
|
desired->Pitch * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_PITCH] +
|
|
desired->Roll * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_ROLL] + desired->Yaw * vtolSettings.MotorNW[VTOLSETTINGS_MOTORNW_YAW];
|
|
cmd->Channel[settings->VTOLMotorNW] = scaleChannel(bound(vtolStatus.MotorNW, vtolMin, vtolMax),
|
|
settings->ChannelMax[settings->VTOLMotorNW],
|
|
settings->ChannelMin[settings->VTOLMotorNW],
|
|
settings->ChannelNeutral[settings->VTOLMotorNW]);
|
|
}
|
|
VTOLStatusSet(&vtolStatus);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int32_t InterpolateCCPMCurve(float *output, float input, const float *curve)
|
|
{
|
|
int8_t curvIndex;
|
|
float slope;
|
|
float offset;
|
|
float value;
|
|
|
|
//determine which section of the 5 point curve we are on
|
|
if (input <= .25) {
|
|
curvIndex = 0;
|
|
offset = 0;
|
|
} else if (input <= .50) {
|
|
curvIndex = 1;
|
|
offset = 0.25;
|
|
} else if (input <= .75) {
|
|
curvIndex = 2;
|
|
offset = 0.50;
|
|
} else {
|
|
curvIndex = 3;
|
|
offset = 0.75;
|
|
}
|
|
|
|
//calculate the linear interpolation for the selected segment
|
|
slope = (curve[curvIndex + 1] - curve[curvIndex]) / 0.25;
|
|
value = curve[curvIndex] + ((input - offset) * slope);
|
|
|
|
//bound the output to valid percentage values
|
|
if (value > 100.0)
|
|
value = 100.0;
|
|
if (value < 0.0)
|
|
value = 0.0;
|
|
|
|
*output = value;
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
/**
|
|
* Mixer for CCPM (single rotor helicopters). Converts desired roll,pitch,yaw and throttle to servo outputs.
|
|
* @return -1 if error, 0 if success
|
|
*/
|
|
static int32_t mixerCCPM(ActuatorSettingsData * settings, const ActuatorDesiredData * desired, ActuatorCommandData * cmd)
|
|
{
|
|
#define degtorad 0.0174532925 //pi/180
|
|
// TODO: Implement CCPM mixers
|
|
int configOK;
|
|
float curveValue;
|
|
float throttle;
|
|
float bladePitch;
|
|
float CCPMCyclicConstant;
|
|
float servoW;
|
|
float servoX;
|
|
float servoY;
|
|
float servoZ;
|
|
|
|
configOK = 0;
|
|
|
|
//calculate the throttle value based on the curve - scale to 0.0 -> +1.0
|
|
InterpolateCCPMCurve(&curveValue, desired->Throttle, settings->CCPMThrottleCurve);
|
|
throttle = curveValue / 100.0;
|
|
|
|
//calculate the Blade Pitch value based on the curve - scale to -1.0 -> +1.0
|
|
InterpolateCCPMCurve(&curveValue, desired->Throttle, settings->CCPMPitchCurve);
|
|
bladePitch = (curveValue / 50.0) - 1.0;
|
|
|
|
//calculate how much Cyclic to apply to the mixing
|
|
CCPMCyclicConstant = 1 - settings->CCPMCollectiveConstant;
|
|
|
|
// Set ServoW servo command
|
|
if (settings->CCPMServoW != ACTUATORSETTINGS_CCPMSERVOW_NONE) {
|
|
servoW = (CCPMCyclicConstant * ((sin((settings->CCPMAngleW + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll)
|
|
+ (cos((settings->CCPMAngleW + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch)))
|
|
+ (settings->CCPMCollectiveConstant * bladePitch);
|
|
|
|
cmd->Channel[settings->CCPMServoW] = scaleChannel(servoW, settings->ChannelMax[settings->CCPMServoW],
|
|
settings->ChannelMin[settings->CCPMServoW],
|
|
settings->ChannelNeutral[settings->CCPMServoW]);
|
|
configOK++;
|
|
}
|
|
// Set ServoX servo command
|
|
if (settings->CCPMServoX != ACTUATORSETTINGS_CCPMSERVOX_NONE) {
|
|
servoX = (CCPMCyclicConstant * ((sin((settings->CCPMAngleX + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll)
|
|
+ (cos((settings->CCPMAngleX + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch)))
|
|
+ (settings->CCPMCollectiveConstant * bladePitch);
|
|
|
|
cmd->Channel[settings->CCPMServoX] = scaleChannel(servoX, settings->ChannelMax[settings->CCPMServoX],
|
|
settings->ChannelMin[settings->CCPMServoX],
|
|
settings->ChannelNeutral[settings->CCPMServoX]);
|
|
configOK++;
|
|
}
|
|
// Set ServoY servo command
|
|
if (settings->CCPMServoY != ACTUATORSETTINGS_CCPMSERVOY_NONE) {
|
|
servoY = (CCPMCyclicConstant * ((sin((settings->CCPMAngleY + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll)
|
|
+ (cos((settings->CCPMAngleY + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch)))
|
|
+ (settings->CCPMCollectiveConstant * bladePitch);
|
|
|
|
cmd->Channel[settings->CCPMServoY] = scaleChannel(servoY, settings->ChannelMax[settings->CCPMServoY],
|
|
settings->ChannelMin[settings->CCPMServoY],
|
|
settings->ChannelNeutral[settings->CCPMServoY]);
|
|
configOK++;
|
|
}
|
|
// Set ServoZ servo command
|
|
if (settings->CCPMServoZ != ACTUATORSETTINGS_CCPMSERVOZ_NONE) {
|
|
servoZ = (CCPMCyclicConstant * ((sin((settings->CCPMAngleZ + settings->CCPMCorrectionAngle) * degtorad) * desired->Roll)
|
|
+ (cos((settings->CCPMAngleZ + settings->CCPMCorrectionAngle) * degtorad) * desired->Pitch)))
|
|
+ (settings->CCPMCollectiveConstant * bladePitch);
|
|
|
|
cmd->Channel[settings->CCPMServoZ] = scaleChannel(servoZ, settings->ChannelMax[settings->CCPMServoZ],
|
|
settings->ChannelMin[settings->CCPMServoZ],
|
|
settings->ChannelNeutral[settings->CCPMServoZ]);
|
|
configOK++;
|
|
}
|
|
|
|
// Set throttle servo command
|
|
if (settings->CCPMThrottle != ACTUATORSETTINGS_CCPMTHROTTLE_NONE) {
|
|
cmd->Channel[settings->CCPMThrottle] = scaleChannel(throttle, settings->ChannelMax[settings->CCPMThrottle],
|
|
settings->ChannelMin[settings->CCPMThrottle],
|
|
settings->ChannelNeutral[settings->CCPMThrottle]);
|
|
} else {
|
|
configOK = 0;
|
|
}
|
|
|
|
// Set TailRotor servo command
|
|
if (settings->CCPMYawStabilizationInManualMode == ACTUATORSETTINGS_CCPMYAWSTABILIZATIONINMANUALMODE_FALSE) { //update the servo as per the desired control mode (manual or stabalized)
|
|
if (settings->CCPMTailRotor != ACTUATORSETTINGS_CCPMTAILROTOR_NONE) {
|
|
cmd->Channel[settings->CCPMTailRotor] = scaleChannel(desired->Yaw, settings->ChannelMax[settings->CCPMTailRotor],
|
|
settings->ChannelMin[settings->CCPMTailRotor],
|
|
settings->ChannelNeutral[settings->CCPMTailRotor]);
|
|
} else {
|
|
configOK = 0;
|
|
}
|
|
ccpmYawErrorLast = 0.0;
|
|
|
|
} else { //TODO need to implement the stabilization PID loop to provide heading hold gyro functionality in manual control mode
|
|
//currently just coppied from simple stabilisation
|
|
|
|
AttitudeActualGet(&attitudeActual);
|
|
StabilizationSettingsGet(&stabSettings);
|
|
|
|
if (desired->Yaw < 0) {
|
|
desiredYaw = 360 + (desired->Yaw * 180.0);
|
|
} else {
|
|
desiredYaw = (desired->Yaw * 180.0);
|
|
}
|
|
|
|
// Yaw stabilization control loop
|
|
ccpmYawError = desiredYaw - attitudeActual.Yaw;
|
|
//this should make it take the quickest path to reach the desired yaw
|
|
if (ccpmYawError > 180.0)
|
|
ccpmYawError -= 360;
|
|
if (ccpmYawError < -180.0)
|
|
ccpmYawError += 360;
|
|
ccpmYawDerivative = ccpmYawError - ccpmYawErrorLast;
|
|
ccpmYawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
|
|
ccpmYawIntegral = bound(ccpmYawIntegral + ccpmYawError * stabSettings.UpdatePeriod, -ccpmYawIntegralLimit, ccpmYawIntegralLimit);
|
|
desiredYaw = stabSettings.YawKp * ccpmYawError + stabSettings.YawKi * ccpmYawIntegral + stabSettings.YawKd * ccpmYawDerivative;;
|
|
desiredYaw = bound(desiredYaw, -1.0, 1.0);
|
|
ccpmYawErrorLast = ccpmYawError;
|
|
if (settings->CCPMTailRotor != ACTUATORSETTINGS_CCPMTAILROTOR_NONE) {
|
|
cmd->Channel[settings->CCPMTailRotor] = scaleChannel(desiredYaw, settings->ChannelMax[settings->CCPMTailRotor],
|
|
settings->ChannelMin[settings->CCPMTailRotor],
|
|
settings->ChannelNeutral[settings->CCPMTailRotor]);
|
|
} else {
|
|
configOK = 0;
|
|
}
|
|
}
|
|
if (configOK < 2)
|
|
return -1;
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Convert channel from -1/+1 to servo pulse duration in microseconds
|
|
*/
|
|
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral)
|
|
{
|
|
int16_t valueScaled;
|
|
// Scale
|
|
if (value >= 0.0) {
|
|
valueScaled = (int16_t) (value * ((float)(max - neutral))) + neutral;
|
|
} else {
|
|
valueScaled = (int16_t) (value * ((float)(neutral - min))) + neutral;
|
|
}
|
|
|
|
if (max > min) {
|
|
if (valueScaled > max)
|
|
valueScaled = max;
|
|
if (valueScaled < min)
|
|
valueScaled = min;
|
|
} else {
|
|
if (valueScaled < max)
|
|
valueScaled = max;
|
|
if (valueScaled > min)
|
|
valueScaled = min;
|
|
}
|
|
|
|
return valueScaled;
|
|
}
|
|
|
|
/**
|
|
* Set actuator output to the neutral values (failsafe)
|
|
*/
|
|
static void setFailsafe()
|
|
{
|
|
ActuatorSettingsData settings;
|
|
ActuatorCommandData cmd;
|
|
|
|
// Read settings
|
|
ActuatorSettingsGet(&settings);
|
|
|
|
// Reset ActuatorCommand to neutral values
|
|
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
|
|
cmd.Channel[n] = settings.ChannelNeutral[n];
|
|
}
|
|
|
|
// Set alarm
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
|
|
// Update servo outputs
|
|
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
|
|
PIOS_Servo_Set(n, cmd.Channel[n]);
|
|
}
|
|
|
|
// Update output object
|
|
ActuatorCommandSet(&cmd);
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|