1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/modules/Actuator/actuator.c

722 lines
26 KiB
C
Raw Normal View History

/**
******************************************************************************
* @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 "accessorydesired.h"
#include "actuator.h"
#include "actuatorsettings.h"
#include "systemsettings.h"
#include "actuatordesired.h"
#include "actuatorcommand.h"
#include "flightstatus.h"
#include "mixersettings.h"
#include "mixerstatus.h"
#include "cameradesired.h"
#include "manualcontrolcommand.h"
#include "taskinfo.h"
// Private constants
#define MAX_QUEUE_SIZE 2
#if defined(PIOS_ACTUATOR_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1312
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define FAILSAFE_TIMEOUT_MS 100
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
#define CAMERA_BOOT_DELAY_MS 7000
// Private types
// Private variables
static xQueueHandle queue;
static xTaskHandle taskHandle;
static float lastResult[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
// used to inform the actuator thread that actuator update rate is changed
static volatile bool actuator_settings_updated;
// used to inform the actuator thread that mixer settings are changed
static volatile bool mixer_settings_updated;
// Private functions
static void actuatorTask(void *parameters);
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings);
static float MixerCurve(const float throttle, const float *curve, uint8_t elements);
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings);
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update);
static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
float ProcessMixer(const int index, const float curve1, const float curve2,
const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired,
const float period);
// this structure is equivalent to the UAVObjects for one mixer.
typedef struct {
uint8_t type;
int8_t matrix[5];
} __attribute__((packed)) Mixer_t;
/**
* @brief Module initialization
* @return 0
*/
int32_t ActuatorStart()
{
// Start main task
xTaskCreate(actuatorTask, (signed char *)"Actuator", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
#endif
return 0;
}
/**
* @brief Module initialization
* @return 0
*/
int32_t ActuatorInitialize()
{
// Register for notification of changes to ActuatorSettings
ActuatorSettingsInitialize();
ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb);
// Register for notification of changes to MixerSettings
MixerSettingsInitialize();
MixerSettingsConnectCallback(MixerSettingsUpdatedCb);
// Listen for ActuatorDesired updates (Primary input to this module)
ActuatorDesiredInitialize();
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
ActuatorDesiredConnectQueue(queue);
// Primary output of this module
ActuatorCommandInitialize();
Final step: lot of small fixes, last commit in this commit series This is the first cleanup pass through makefiles and pios. Probably it is difficult to track changes due to the nature of them. I would recommend to look at resulting files and compiled code instead. NOTE: original branch was rebased and lot of conflicts were fixed on the way. So do not expect that every commit in this series will be buildable (unlike original branch). Only final result was tested. The main goal was to remove as much duplication of code (and copy/paste errors) as possible, moving common parts out of Makefiles. It still is not perfect, and mostly no code changes made - Makefiles and #ifdefs only. But please while testing make sure that all code works as before, and no modules/options are missed by accident. Brief list of changes: - Moved common parts of Makefiles into the set of *.mk files. - Changed method of passing common vars from top Makefile to lower ones. - Some pios cleanup, mostly #ifdefs, and all pios_config.h files. - Many obsolete files removed (for instance, AHRS files, op_config.h). - Many obsolete or unused macros removed or fixed/renamed (ALL_DIGNOSTICS). - Unified pios_config.h template. Please don't remove lines for board configs, only comment/uncomment them. Adding new PIOS options, please propagate them to all board files keeping the same order. - Some formatting, spacing, indentation (no line endings change yet). - Some cosmetic fixes (no more C:\X\Y\filename.c printings on Windows). - Added some library.mk files to move libs into AR achives later. - EntireFlash target now uses cross-platform python script to generate bin files. So it works on all supported platforms: Linux, OSX, Windows. - Top level packaging is completely rewritten. Now it is a part of top Makefile. As such, all dependencies are checked and accounted, no more 'make -j' problems should occur. - Default GCS_BUILD_CONF is release now, may be changed if necessary using 'make GCS_BUILD_CONF=debug gcs'. - GCS build paths are separated into debug and release, so no more obj file clashes. Packaging system supports only release builds. - New target is introduced: 'clean_package'. Now 'make package' does not clean build directory. Use clean_package instead for distributable builds. - Targets like 'all', 'opfw_resource', etc now will print extra contex in parallel builds too. - If any of 'package', 'clean_package', 'opfw_resource' targets are given on command line, GCS build will depend on the resource, so all fw_*.opfw targets will be built and embedded into GCS. By default GCS does not depend on resource, and will be built w/o firmware (unless the resource files already exist and the Qt resource file is generated). - fw_simposix (ELF executable) is now packaged for linux. Run'n'play! - Make help is refined and is now up to date. Still broken: - UnitTests, should be fixed - SimPosix: buildable, but should be reworked. Next planned passes to do: - toolchain bootstrapping and packaging (including windows - WIP) - CMSIS/StdPeriph lib cleanup - more PIOS cleanup - move libs into AR archives to save build time - sim targets refactir and cleanup - move android-related directories under <top>/android - unit test targets fix - source code line ending changes (there are many different, were not changed) - coding style Merging this, please use --no-ff git option to make it the real commit point Conflicts: A lot of... :-)
2013-03-24 12:02:08 +01:00
#ifdef DIAG_MIXERSTATUS
// UAVO only used for inspecting the internal status of the mixer during debug
MixerStatusInitialize();
#endif
return 0;
}
MODULE_INITCALL(ActuatorInitialize, ActuatorStart)
/**
* @brief Main Actuator module task
*
* Universal matrix based mixer for VTOL, helis and fixed wing.
* Converts desired roll,pitch,yaw and throttle to servo/ESC outputs.
*
* Because of how the Throttle ranges from 0 to 1, the motors should too!
*
* Note this code depends on the UAVObjects for the mixers being all being the same
* and in sequence. If you change the object definition, make sure you check the code!
*
* @return -1 if error, 0 if success
*/
static void actuatorTask(__attribute__((unused)) void *parameters)
{
UAVObjEvent ev;
portTickType lastSysTime;
portTickType thisSysTime;
float dTSeconds;
uint32_t dTMilliseconds;
ActuatorCommandData command;
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
FlightStatusData flightStatus;
/* Read initial values of ActuatorSettings */
ActuatorSettingsData actuatorSettings;
actuator_settings_updated = false;
ActuatorSettingsGet(&actuatorSettings);
/* Read initial values of MixerSettings */
MixerSettingsData mixerSettings;
mixer_settings_updated = false;
MixerSettingsGet(&mixerSettings);
/* Force an initial configuration of the actuator update rates */
actuator_update_rate_if_changed(&actuatorSettings, true);
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
setFailsafe(&actuatorSettings, &mixerSettings);
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
#endif
// Wait until the ActuatorDesired object is updated
uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS);
/* Process settings updated events even in timeout case so we always act on the latest settings */
if (actuator_settings_updated) {
actuator_settings_updated = false;
ActuatorSettingsGet(&actuatorSettings);
actuator_update_rate_if_changed(&actuatorSettings, false);
}
if (mixer_settings_updated) {
mixer_settings_updated = false;
MixerSettingsGet(&mixerSettings);
}
if (rc != pdTRUE) {
/* Update of ActuatorDesired timed out. Go to failsafe */
setFailsafe(&actuatorSettings, &mixerSettings);
continue;
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
dTMilliseconds = (thisSysTime == lastSysTime) ? 1 : (thisSysTime - lastSysTime) * portTICK_RATE_MS;
lastSysTime = thisSysTime;
dTSeconds = dTMilliseconds * 0.001f;
FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
Final step: lot of small fixes, last commit in this commit series This is the first cleanup pass through makefiles and pios. Probably it is difficult to track changes due to the nature of them. I would recommend to look at resulting files and compiled code instead. NOTE: original branch was rebased and lot of conflicts were fixed on the way. So do not expect that every commit in this series will be buildable (unlike original branch). Only final result was tested. The main goal was to remove as much duplication of code (and copy/paste errors) as possible, moving common parts out of Makefiles. It still is not perfect, and mostly no code changes made - Makefiles and #ifdefs only. But please while testing make sure that all code works as before, and no modules/options are missed by accident. Brief list of changes: - Moved common parts of Makefiles into the set of *.mk files. - Changed method of passing common vars from top Makefile to lower ones. - Some pios cleanup, mostly #ifdefs, and all pios_config.h files. - Many obsolete files removed (for instance, AHRS files, op_config.h). - Many obsolete or unused macros removed or fixed/renamed (ALL_DIGNOSTICS). - Unified pios_config.h template. Please don't remove lines for board configs, only comment/uncomment them. Adding new PIOS options, please propagate them to all board files keeping the same order. - Some formatting, spacing, indentation (no line endings change yet). - Some cosmetic fixes (no more C:\X\Y\filename.c printings on Windows). - Added some library.mk files to move libs into AR achives later. - EntireFlash target now uses cross-platform python script to generate bin files. So it works on all supported platforms: Linux, OSX, Windows. - Top level packaging is completely rewritten. Now it is a part of top Makefile. As such, all dependencies are checked and accounted, no more 'make -j' problems should occur. - Default GCS_BUILD_CONF is release now, may be changed if necessary using 'make GCS_BUILD_CONF=debug gcs'. - GCS build paths are separated into debug and release, so no more obj file clashes. Packaging system supports only release builds. - New target is introduced: 'clean_package'. Now 'make package' does not clean build directory. Use clean_package instead for distributable builds. - Targets like 'all', 'opfw_resource', etc now will print extra contex in parallel builds too. - If any of 'package', 'clean_package', 'opfw_resource' targets are given on command line, GCS build will depend on the resource, so all fw_*.opfw targets will be built and embedded into GCS. By default GCS does not depend on resource, and will be built w/o firmware (unless the resource files already exist and the Qt resource file is generated). - fw_simposix (ELF executable) is now packaged for linux. Run'n'play! - Make help is refined and is now up to date. Still broken: - UnitTests, should be fixed - SimPosix: buildable, but should be reworked. Next planned passes to do: - toolchain bootstrapping and packaging (including windows - WIP) - CMSIS/StdPeriph lib cleanup - more PIOS cleanup - move libs into AR archives to save build time - sim targets refactir and cleanup - move android-related directories under <top>/android - unit test targets fix - source code line ending changes (there are many different, were not changed) - coding style Merging this, please use --no-ff git option to make it the real commit point Conflicts: A lot of... :-)
2013-03-24 12:02:08 +01:00
#ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus);
#endif
int nMixers = 0;
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
nMixers++;
}
}
if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working
continue;
}
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00f;
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
// The source for the secondary curve is selectable
float curve2 = 0;
AccessoryDesiredData accessory;
switch (mixerSettings.Curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_PITCH:
curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_YAW:
curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
ManualControlCommandCollectiveGet(&curve2);
curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY3:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) {
curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
} else {
curve2 = 0;
}
break;
}
float *status = (float *)&mixerStatus; // access status objects as an array of floats
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
// During boot all camera actuators should be completely disabled (PWM pulse = 0).
// command.Channel[i] is reused below as a channel PWM activity flag:
// 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later.
// Setting it to 1 by default means "Rescale this channel and enable PWM on its output".
command.Channel[ct] = 1;
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
// Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us
status[ct] = -1;
continue;
}
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
} else {
status[ct] = -1;
}
// Motors have additional protection for when to be on
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
// If not armed or motors aren't meant to spin all the time
if (!armed ||
(!spinWhileArmed && !positiveThrottle)) {
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
status[ct] = -1; // force min throttle
}
// If armed meant to keep spinning,
else if ((spinWhileArmed && !positiveThrottle) ||
(status[ct] < 0)) {
status[ct] = 0;
}
}
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
// these also will not be updated in failsafe mode. I'm not sure what
// the correct behavior is since it seems domain specific. I don't love
// this code
if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
status[ct] = accessory.AccessoryVal;
} else {
status[ct] = -1;
}
}
if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
CameraDesiredData cameraDesired;
if (CameraDesiredGet(&cameraDesired) == 0) {
switch (mixers[ct].type) {
case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
status[ct] = cameraDesired.RollOrServo1;
break;
case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2:
status[ct] = cameraDesired.PitchOrServo2;
break;
case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
status[ct] = cameraDesired.Yaw;
break;
default:
break;
}
} else {
status[ct] = -1;
}
// Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot
if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) {
command.Channel[ct] = 0;
}
}
}
// Set real actuator output values scaling them from mixers. All channels
// will be set except explicitly disabled (which will have PWM pulse = 0).
for (int i = 0; i < MAX_MIX_ACTUATORS; i++) {
if (command.Channel[i]) {
command.Channel[i] = scaleChannel(status[i],
actuatorSettings.ChannelMax[i],
actuatorSettings.ChannelMin[i],
actuatorSettings.ChannelNeutral[i]);
}
}
// Store update time
command.UpdateTime = dTMilliseconds;
if (command.UpdateTime > command.MaxUpdateTime) {
command.MaxUpdateTime = command.UpdateTime;
}
// Update output object
ActuatorCommandSet(&command);
// Update in case read only (eg. during servo configuration)
ActuatorCommandGet(&command);
Final step: lot of small fixes, last commit in this commit series This is the first cleanup pass through makefiles and pios. Probably it is difficult to track changes due to the nature of them. I would recommend to look at resulting files and compiled code instead. NOTE: original branch was rebased and lot of conflicts were fixed on the way. So do not expect that every commit in this series will be buildable (unlike original branch). Only final result was tested. The main goal was to remove as much duplication of code (and copy/paste errors) as possible, moving common parts out of Makefiles. It still is not perfect, and mostly no code changes made - Makefiles and #ifdefs only. But please while testing make sure that all code works as before, and no modules/options are missed by accident. Brief list of changes: - Moved common parts of Makefiles into the set of *.mk files. - Changed method of passing common vars from top Makefile to lower ones. - Some pios cleanup, mostly #ifdefs, and all pios_config.h files. - Many obsolete files removed (for instance, AHRS files, op_config.h). - Many obsolete or unused macros removed or fixed/renamed (ALL_DIGNOSTICS). - Unified pios_config.h template. Please don't remove lines for board configs, only comment/uncomment them. Adding new PIOS options, please propagate them to all board files keeping the same order. - Some formatting, spacing, indentation (no line endings change yet). - Some cosmetic fixes (no more C:\X\Y\filename.c printings on Windows). - Added some library.mk files to move libs into AR achives later. - EntireFlash target now uses cross-platform python script to generate bin files. So it works on all supported platforms: Linux, OSX, Windows. - Top level packaging is completely rewritten. Now it is a part of top Makefile. As such, all dependencies are checked and accounted, no more 'make -j' problems should occur. - Default GCS_BUILD_CONF is release now, may be changed if necessary using 'make GCS_BUILD_CONF=debug gcs'. - GCS build paths are separated into debug and release, so no more obj file clashes. Packaging system supports only release builds. - New target is introduced: 'clean_package'. Now 'make package' does not clean build directory. Use clean_package instead for distributable builds. - Targets like 'all', 'opfw_resource', etc now will print extra contex in parallel builds too. - If any of 'package', 'clean_package', 'opfw_resource' targets are given on command line, GCS build will depend on the resource, so all fw_*.opfw targets will be built and embedded into GCS. By default GCS does not depend on resource, and will be built w/o firmware (unless the resource files already exist and the Qt resource file is generated). - fw_simposix (ELF executable) is now packaged for linux. Run'n'play! - Make help is refined and is now up to date. Still broken: - UnitTests, should be fixed - SimPosix: buildable, but should be reworked. Next planned passes to do: - toolchain bootstrapping and packaging (including windows - WIP) - CMSIS/StdPeriph lib cleanup - more PIOS cleanup - move libs into AR archives to save build time - sim targets refactir and cleanup - move android-related directories under <top>/android - unit test targets fix - source code line ending changes (there are many different, were not changed) - coding style Merging this, please use --no-ff git option to make it the real commit point Conflicts: A lot of... :-)
2013-03-24 12:02:08 +01:00
#ifdef DIAG_MIXERSTATUS
MixerStatusSet(&mixerStatus);
#endif
// Update servo outputs
bool success = true;
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
success &= set_channel(n, command.Channel[n], &actuatorSettings);
}
if (!success) {
command.NumFailedUpdates++;
ActuatorCommandSet(&command);
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
}
}
}
/**
* Process mixing for one actuator
*/
float ProcessMixer(const int index, const float curve1, const float curve2,
const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period)
{
static float lastFilteredResult[MAX_MIX_ACTUATORS];
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
const Mixer_t *mixer = &mixers[index];
float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (result < 0.0f) { // idle throttle
result = 0.0f;
}
// feed forward
float accumulator = filterAccumulator[index];
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
lastResult[index] = result;
result += accumulator;
if (period > 0.0f) {
if (accumulator > 0.0f) {
float filter = mixerSettings->AccelTime / period;
if (filter < 1) {
filter = 1;
}
accumulator -= accumulator / filter;
} else {
float filter = mixerSettings->DecelTime / period;
if (filter < 1) {
filter = 1;
}
accumulator -= accumulator / filter;
}
}
filterAccumulator[index] = accumulator;
result += accumulator;
// acceleration limit
float dt = result - lastFilteredResult[index];
float maxDt = mixerSettings->MaxAccel * period;
if (dt > maxDt) { // we are accelerating too hard
result = lastFilteredResult[index] + maxDt;
}
lastFilteredResult[index] = result;
}
return result;
}
/**
* Interpolate a throttle curve. Throttle input should be in the range 0 to 1.
* Output is in the range 0 to 1.
*/
static float MixerCurve(const float throttle, const float *curve, uint8_t elements)
{
float scale = throttle * (float)(elements - 1);
int idx1 = scale;
scale -= (float)idx1; // remainder
if (curve[0] < -1) {
return throttle;
}
if (idx1 < 0) {
idx1 = 0; // clamp to lowest entry in table
scale = 0;
}
int idx2 = idx1 + 1;
if (idx2 >= elements) {
idx2 = elements - 1; // clamp to highest entry in table
if (idx1 >= elements) {
idx1 = elements - 1;
}
}
return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
}
/**
* 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.0f) {
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(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings)
{
/* grab only the parts that we are going to use */
int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM];
ActuatorCommandChannelGet(Channel);
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
// Reset ActuatorCommand to safe values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
Channel[n] = actuatorSettings->ChannelMin[n];
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
Channel[n] = actuatorSettings->ChannelNeutral[n];
} else {
Channel[n] = 0;
}
}
// Set alarm
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
// Update servo outputs
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
set_channel(n, Channel[n], actuatorSettings);
}
// Update output object's parts that we changed
ActuatorCommandChannelSet(Channel);
}
/**
* determine buzzer or blink sequence
**/
typedef enum { BUZZ_BUZZER = 0, BUZZ_ARMING = 1, BUZZ_INFO = 2, BUZZ_MAX = 3 } buzzertype;
static inline bool buzzerState(buzzertype type)
{
// This is for buzzers that take a PWM input
static uint32_t tune[BUZZ_MAX] = { 0 };
static uint32_t tunestate[BUZZ_MAX] = { 0 };
uint32_t newTune = 0;
if (type == BUZZ_BUZZER) {
// Decide what tune to play
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
newTune = 0b11110110110000; // pause, short, short, short, long
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
newTune = 0x80000000; // pause, short
} else {
newTune = 0;
}
} else { // BUZZ_ARMING || BUZZ_INFO
uint8_t arming;
FlightStatusArmedGet(&arming);
// base idle tune
newTune = 0x80000000; // 0b1000...
// Merge the error pattern for InfoLed
if (type == BUZZ_INFO) {
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
newTune |= 0b00000000001111111011111110000000;
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
newTune |= 0b00000000000000110110110000000000;
}
}
// fast double blink pattern if armed
if (arming == FLIGHTSTATUS_ARMED_ARMED) {
newTune |= 0xA0000000; // 0b101000...
}
}
// Do we need to change tune?
if (newTune != tune[type]) {
tune[type] = newTune;
// resynchronize all tunes on change, so they stay in sync
for (int i = 0; i < BUZZ_MAX; i++) {
tunestate[i] = tune[i];
}
}
// Play tune
bool buzzOn = false;
static portTickType lastSysTime = 0;
portTickType thisSysTime = xTaskGetTickCount();
portTickType dT = 0;
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
if (tune[type]) {
if (thisSysTime > lastSysTime) {
dT = thisSysTime - lastSysTime;
} else {
lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80
}
buzzOn = (tunestate[type] & 1);
if (dT > 80) {
// Go to next bit in alarm_seq_state
for (int i = 0; i < BUZZ_MAX; i++) {
tunestate[i] >>= 1;
if (tunestate[i] == 0) { // All done, re-start the tune
tunestate[i] = tune[i];
}
}
lastSysTime = thisSysTime;
}
}
return buzzOn;
}
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings)
{
return true;
}
#else
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings)
{
switch (actuatorSettings->ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
return true;
#if defined(PIOS_INCLUDE_I2C_ESC)
case ACTUATORSETTINGS_CHANNELTYPE_MK:
return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel], value);
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel], value);
#endif
default:
return false;
}
return false;
}
#endif /* if defined(ARCH_POSIX) || defined(ARCH_WIN32) */
/**
* @brief Update the servo update rate
*/
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update)
{
static uint16_t prevChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM];
// check if the any rate setting is changed
if (force_update ||
memcmp(prevChannelUpdateFreq,
actuatorSettings->ChannelUpdateFreq,
sizeof(prevChannelUpdateFreq)) != 0) {
/* Something has changed, apply the settings to HW */
memcpy(prevChannelUpdateFreq,
actuatorSettings->ChannelUpdateFreq,
sizeof(prevChannelUpdateFreq));
PIOS_Servo_SetHz(actuatorSettings->ChannelUpdateFreq, ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
}
}
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
actuator_settings_updated = true;
}
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
mixer_settings_updated = true;
}
/**
* @}
* @}
*/