mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-05 13:24:11 +01:00
1087 lines
43 KiB
C
1087 lines
43 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 LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
|
* 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 <flightmodesettings.h>
|
|
#include "mixersettings.h"
|
|
#include "mixerstatus.h"
|
|
#include "cameradesired.h"
|
|
#include "hwsettings.h"
|
|
#include "manualcontrolcommand.h"
|
|
#include "taskinfo.h"
|
|
#include <systemsettings.h>
|
|
#include <sanitycheck.h>
|
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
|
#include <vtolpathfollowersettings.h>
|
|
#endif
|
|
#undef PIOS_INCLUDE_INSTRUMENTATION
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
#include <pios_instrumentation.h>
|
|
static int8_t counter;
|
|
// Counter 0xAC700001 total Actuator body execution time(excluding queue waits etc).
|
|
#endif
|
|
|
|
// 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) // device driver
|
|
#define FAILSAFE_TIMEOUT_MS 100
|
|
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
|
|
|
|
#define CAMERA_BOOT_DELAY_MS 7000
|
|
|
|
#define ACTUATOR_ONESHOT_CLOCK 12000000
|
|
#define ACTUATOR_ONESHOT125_PULSE_FACTOR 1.5f
|
|
#define ACTUATOR_ONESHOT42_PULSE_FACTOR 0.5f
|
|
#define ACTUATOR_MULTISHOT_PULSE_FACTOR 0.24f
|
|
#define ACTUATOR_PWM_CLOCK 1000000
|
|
// Private types
|
|
|
|
|
|
// Private variables
|
|
static xQueueHandle queue;
|
|
static xTaskHandle taskHandle;
|
|
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
|
|
static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
|
|
static bool camStabEnabled;
|
|
|
|
static uint8_t pinsMode[MAX_MIX_ACTUATORS];
|
|
// used to inform the actuator thread that actuator update rate is changed
|
|
static ActuatorSettingsData actuatorSettings;
|
|
static bool spinWhileArmed;
|
|
|
|
// used to inform the actuator thread that mixer settings are changed
|
|
static MixerSettingsData mixerSettings;
|
|
static int mixer_settings_count = 2;
|
|
|
|
// Private functions
|
|
static void actuatorTask(void *parameters);
|
|
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
|
|
static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool alwaysStabilizeWhenArmed, float throttleDesired);
|
|
static void setFailsafe();
|
|
static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor);
|
|
static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor);
|
|
static bool set_channel(uint8_t mixer_channel, uint16_t value);
|
|
static void actuator_update_rate_if_changed(bool force_update);
|
|
static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
|
|
static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
|
|
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
|
float ProcessMixer(const int index, const float curve1, const float curve2,
|
|
ActuatorDesiredData *desired,
|
|
bool multirotor, bool fixedwing);
|
|
|
|
// 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, "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
|
|
SettingsUpdatedCb(NULL);
|
|
MixerSettingsUpdatedCb(NULL);
|
|
ActuatorSettingsUpdatedCb(NULL);
|
|
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);
|
|
|
|
// Register AccessoryDesired (Secondary input to this module)
|
|
AccessoryDesiredInitialize();
|
|
|
|
// Check if CameraStab module is enabled
|
|
HwSettingsOptionalModulesData optionalModules;
|
|
HwSettingsInitialize();
|
|
HwSettingsOptionalModulesGet(&optionalModules);
|
|
camStabEnabled = (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED);
|
|
|
|
// Primary output of this module
|
|
ActuatorCommandInitialize();
|
|
|
|
#ifdef DIAG_MIXERSTATUS
|
|
// UAVO only used for inspecting the internal status of the mixer during debug
|
|
MixerStatusInitialize();
|
|
#endif
|
|
|
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
|
VtolPathFollowerSettingsInitialize();
|
|
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
|
#endif
|
|
SystemSettingsInitialize();
|
|
SystemSettingsConnectCallback(&SettingsUpdatedCb);
|
|
|
|
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;
|
|
uint32_t dTMilliseconds;
|
|
|
|
ActuatorCommandData command;
|
|
ActuatorDesiredData desired;
|
|
MixerStatusData mixerStatus;
|
|
FlightModeSettingsData settings;
|
|
FlightStatusData flightStatus;
|
|
float throttleDesired;
|
|
float collectiveDesired;
|
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
|
|
#endif
|
|
/* Read initial values of ActuatorSettings */
|
|
|
|
ActuatorSettingsGet(&actuatorSettings);
|
|
|
|
/* Read initial values of MixerSettings */
|
|
MixerSettingsGet(&mixerSettings);
|
|
|
|
/* Force an initial configuration of the actuator update rates */
|
|
actuator_update_rate_if_changed(true);
|
|
|
|
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
|
|
setFailsafe();
|
|
|
|
// 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);
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
PIOS_Instrumentation_TimeStart(counter);
|
|
#endif
|
|
|
|
if (rc != pdTRUE) {
|
|
/* Update of ActuatorDesired timed out. Go to failsafe */
|
|
setFailsafe();
|
|
continue;
|
|
}
|
|
|
|
// Check how long since last update
|
|
thisSysTime = xTaskGetTickCount();
|
|
dTMilliseconds = (thisSysTime == lastSysTime) ? 1 : (thisSysTime - lastSysTime) * portTICK_RATE_MS;
|
|
lastSysTime = thisSysTime;
|
|
|
|
FlightStatusGet(&flightStatus);
|
|
FlightModeSettingsGet(&settings);
|
|
ActuatorDesiredGet(&desired);
|
|
ActuatorCommandGet(&command);
|
|
|
|
// read in throttle and collective -demultiplex thrust
|
|
switch (thrustType) {
|
|
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
|
|
throttleDesired = desired.Thrust;
|
|
ManualControlCommandCollectiveGet(&collectiveDesired);
|
|
break;
|
|
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
|
|
ManualControlCommandThrottleGet(&throttleDesired);
|
|
collectiveDesired = desired.Thrust;
|
|
break;
|
|
default:
|
|
ManualControlCommandThrottleGet(&throttleDesired);
|
|
ManualControlCommandCollectiveGet(&collectiveDesired);
|
|
}
|
|
|
|
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
|
|
bool activeThrottle = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
|
|
bool positiveThrottle = (throttleDesired > 0.00f);
|
|
bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor.
|
|
bool fixedwing = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING); // check if frame is a fixedwing.
|
|
bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED;
|
|
bool alwaysStabilizeWhenArmed = flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE;
|
|
|
|
if (alwaysArmed) {
|
|
alwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous.
|
|
}
|
|
// safety settings
|
|
if (!armed) {
|
|
throttleDesired = 0.00f; // this also happens in scaleMotors as a per axis check
|
|
}
|
|
|
|
if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
|
|
// throttleDesired should never be 0 or go below 0.
|
|
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
|
|
// todo: can probably remove this
|
|
if (!(multirotor && alwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed
|
|
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
|
|
desired.Roll = 0.00f;
|
|
}
|
|
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
|
|
desired.Pitch = 0.00f;
|
|
}
|
|
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
|
|
desired.Yaw = 0.00f;
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef DIAG_MIXERSTATUS
|
|
MixerStatusGet(&mixerStatus);
|
|
#endif
|
|
|
|
if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
|
|
setFailsafe();
|
|
continue;
|
|
}
|
|
|
|
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
|
|
|
float curve1 = 0.0f; // curve 1 is the throttle curve applied to all motors.
|
|
float curve2 = 0.0f;
|
|
|
|
// Interpolate curve 1 from throttleDesired as input.
|
|
// assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values
|
|
// map differently
|
|
curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor);
|
|
|
|
// The source for the secondary curve is selectable
|
|
AccessoryDesiredData accessory;
|
|
uint8_t curve2Source = mixerSettings.Curve2Source;
|
|
switch (curve2Source) {
|
|
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
|
|
// assume reversible motor/mixer initially
|
|
curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
break;
|
|
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
|
|
// Throttle curve contribution the same for +ve vs -ve roll
|
|
if (multirotor) {
|
|
curve2 = MixerCurveFullRangeProportional(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
} else {
|
|
curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
}
|
|
break;
|
|
case MIXERSETTINGS_CURVE2SOURCE_PITCH:
|
|
// Throttle curve contribution the same for +ve vs -ve pitch
|
|
if (multirotor) {
|
|
curve2 = MixerCurveFullRangeProportional(desired.Pitch, mixerSettings.ThrottleCurve2,
|
|
MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
} else {
|
|
curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
|
|
MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
}
|
|
break;
|
|
case MIXERSETTINGS_CURVE2SOURCE_YAW:
|
|
// Throttle curve contribution the same for +ve vs -ve yaw
|
|
if (multirotor) {
|
|
curve2 = MixerCurveFullRangeProportional(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
} else {
|
|
curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
}
|
|
break;
|
|
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
|
|
// assume reversible motor/mixer initially
|
|
curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
|
|
MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
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) {
|
|
// Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
|
|
curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
|
|
} else {
|
|
curve2 = 0.0f;
|
|
}
|
|
break;
|
|
default:
|
|
curve2 = 0.0f;
|
|
break;
|
|
}
|
|
|
|
float *status = (float *)&mixerStatus; // access status objects as an array of floats
|
|
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
|
|
float maxMotor = -1.0f; // highest motor value. Addition method needs this to be -1.0f, division method needs this to be 1.0f
|
|
float minMotor = 1.0f; // lowest motor value Addition method needs this to be 1.0f, division method needs this to be -1.0f
|
|
|
|
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;
|
|
|
|
uint8_t mixer_type = mixers[ct].type;
|
|
|
|
if (mixer_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 ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
|
|
float nonreversible_curve1 = curve1;
|
|
float nonreversible_curve2 = curve2;
|
|
if (nonreversible_curve1 < 0.0f) {
|
|
nonreversible_curve1 = 0.0f;
|
|
}
|
|
if (nonreversible_curve2 < 0.0f) {
|
|
if (!multirotor) { // allow negative throttle if multirotor. function scaleMotors handles the sanity checks.
|
|
nonreversible_curve2 = 0.0f;
|
|
}
|
|
}
|
|
status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, multirotor, fixedwing);
|
|
// If not armed or motors aren't meant to spin all the time
|
|
if (!armed ||
|
|
(!spinWhileArmed && !positiveThrottle)) {
|
|
status[ct] = -1; // force min throttle
|
|
}
|
|
// If armed meant to keep spinning,
|
|
else if ((spinWhileArmed && !positiveThrottle) ||
|
|
(status[ct] < 0)) {
|
|
if (!multirotor) {
|
|
status[ct] = 0;
|
|
// allow throttle values lower than 0 if multirotor.
|
|
// Values will be scaled to 0 if they need to be in the scaleMotor function
|
|
}
|
|
}
|
|
} else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
|
|
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, multirotor, fixedwing);
|
|
// Reversable Motors are like Motors but go to neutral instead of minimum
|
|
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type
|
|
if (!armed || !activeThrottle) {
|
|
status[ct] = 0; // force neutral throttle
|
|
}
|
|
} else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
|
|
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, multirotor, fixedwing);
|
|
} else {
|
|
status[ct] = -1;
|
|
|
|
// 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 ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
|
|
(mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
|
|
if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
|
|
status[ct] = accessory.AccessoryVal;
|
|
} else {
|
|
status[ct] = -1;
|
|
}
|
|
}
|
|
|
|
if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
|
|
(mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
|
|
if (camStabEnabled) {
|
|
CameraDesiredData cameraDesired;
|
|
CameraDesiredGet(&cameraDesired);
|
|
switch (mixer_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;
|
|
}
|
|
}
|
|
}
|
|
|
|
// If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value.
|
|
// For use in function scaleMotor
|
|
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
|
|
if (maxMotor < status[ct]) {
|
|
maxMotor = status[ct];
|
|
}
|
|
if (minMotor > status[ct]) {
|
|
minMotor = status[ct];
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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]) {
|
|
if (mixers[i].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If mixer is for a motor we need to find the highest value of all motors
|
|
command.Channel[i] = scaleMotor(status[i],
|
|
actuatorSettings.ChannelMax[i],
|
|
actuatorSettings.ChannelMin[i],
|
|
actuatorSettings.ChannelNeutral[i],
|
|
maxMotor,
|
|
minMotor,
|
|
armed,
|
|
alwaysStabilizeWhenArmed,
|
|
throttleDesired);
|
|
} else { // else we scale the channel
|
|
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);
|
|
|
|
#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]);
|
|
}
|
|
|
|
PIOS_Servo_Update();
|
|
|
|
if (!success) {
|
|
command.NumFailedUpdates++;
|
|
ActuatorCommandSet(&command);
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
}
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
PIOS_Instrumentation_TimeEnd(counter);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Process mixing for one actuator
|
|
*/
|
|
float ProcessMixer(const int index, const float curve1, const float curve2,
|
|
ActuatorDesiredData *desired, bool multirotor, bool fixedwing)
|
|
{
|
|
const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
|
|
const Mixer_t *mixer = &mixers[index];
|
|
float differential = 1.0f;
|
|
|
|
// Apply differential only for fixedwing and Roll servos
|
|
if (fixedwing && (mixerSettings.FirstRollServo > 0) &&
|
|
(mixer->type == MIXERSETTINGS_MIXER1TYPE_SERVO) &&
|
|
(mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] != 0)) {
|
|
// Positive differential
|
|
if (mixerSettings.RollDifferential > 0) {
|
|
// Check for first Roll servo (should be left aileron or elevon) and Roll desired (positive/negative)
|
|
if (((index == mixerSettings.FirstRollServo - 1) && (desired->Roll > 0.0f))
|
|
|| ((index != mixerSettings.FirstRollServo - 1) && (desired->Roll < 0.0f))) {
|
|
differential -= (mixerSettings.RollDifferential * 0.01f);
|
|
}
|
|
} else if (mixerSettings.RollDifferential < 0) {
|
|
if (((index == mixerSettings.FirstRollServo - 1) && (desired->Roll < 0.0f))
|
|
|| ((index != mixerSettings.FirstRollServo - 1) && (desired->Roll > 0.0f))) {
|
|
differential -= (-mixerSettings.RollDifferential * 0.01f);
|
|
}
|
|
}
|
|
}
|
|
|
|
float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2]) * curve2) +
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL]) * desired->Roll * differential) +
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH]) * desired->Pitch) +
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW]) * desired->Yaw)) / 128.0f;
|
|
|
|
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
|
|
if (!multirotor) { // we allow negative throttle with a multirotor
|
|
if (result < 0.0f) { // zero throttle
|
|
result = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
/**
|
|
* Interpolate a throttle curve
|
|
* Full range input (-1 to 1) for yaw, roll, pitch
|
|
* Output range (-1 to 1) reversible motor/throttle curve
|
|
*
|
|
* Input of -1 -> -lookup(1)
|
|
* Input of 0 -> lookup(0)
|
|
* Input of 1 -> lookup(1)
|
|
*/
|
|
static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor)
|
|
{
|
|
float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor);
|
|
|
|
if (input < 0.0f) {
|
|
return -unsigned_value;
|
|
} else {
|
|
return unsigned_value;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Interpolate a throttle curve
|
|
* Full range input (-1 to 1) for yaw, roll, pitch
|
|
* Output range (0 to 1) non-reversible motor/throttle curve
|
|
*
|
|
* Input of -1 -> lookup(1)
|
|
* Input of 0 -> lookup(0)
|
|
* Input of 1 -> lookup(1)
|
|
*/
|
|
static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor)
|
|
{
|
|
float abs_input = fabsf(input);
|
|
float scale = abs_input * (float)(elements - 1);
|
|
int idx1 = scale;
|
|
|
|
scale -= (float)idx1; // remainder
|
|
if (curve[0] < -1) {
|
|
return abs_input;
|
|
}
|
|
int idx2 = idx1 + 1;
|
|
if (idx2 >= elements) {
|
|
idx2 = elements - 1; // clamp to highest entry in table
|
|
if (idx1 >= elements) {
|
|
if (multirotor) {
|
|
// if multirotor frame we can return throttle values higher than 100%.
|
|
// Since the we don't have elements in the curve higher than 100% we return
|
|
// the last element multiplied by the throttle float
|
|
if (input < 2.0f) { // this limits positive throttle to 200% of max value in table (Maybe this is too much allowance)
|
|
return curve[idx2] * input;
|
|
} else {
|
|
return curve[idx2] * 2.0f; // return 200% of max value in table
|
|
}
|
|
}
|
|
idx1 = elements - 1;
|
|
}
|
|
}
|
|
|
|
float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
|
|
return unsigned_value;
|
|
}
|
|
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
/**
|
|
* Move and compress all motor outputs so that none goes below neutral,
|
|
* and all motors are below or equal to max.
|
|
*/
|
|
static inline int16_t scaleMotorMoveAndCompress(float valueMotor, int16_t max, int16_t neutral, float maxMotor, float minMotor)
|
|
{
|
|
// The valueMotor parameter is the desired motor value somewhere in the
|
|
// [minMotor, maxMotor] range, which is [< -1.00, > 1.00].
|
|
//
|
|
// Before converting valueMotor to the [neutral, max] range, we scale
|
|
// valueMotor to a value in the [0.0f, 1.0f] range.
|
|
//
|
|
// This is done by, first, conceptually moving all three values valueMotor,
|
|
// minMotor, and maxMotor, equally so that the [minMotor, maxMotor] range,
|
|
// are contained or overlaps with the [0.0f, 1.0f] range.
|
|
//
|
|
// Then if the [minMotor, maxMotor] range is larger than 1.0f, the values
|
|
// are compressed enough to shrink the [minMotor + move, maxMotor + move]
|
|
// range to fit within the [0.0f, 1.0f] range.
|
|
|
|
// First move the values so that the source range [minMotor, maxMotor]
|
|
// covers the target range [0.0f, 1.0f] as much as possible.
|
|
float moveValue = 0.0f;
|
|
|
|
if (minMotor <= 0.0f) {
|
|
// Negative minMotor always adjust to 0.
|
|
moveValue = -minMotor;
|
|
} else if (maxMotor > 1.0f) {
|
|
// A too large maxMotor value adjust the range down towards, but not past, the minMotor value.
|
|
float beyondMax = maxMotor - 1.0f;
|
|
moveValue = -(beyondMax < minMotor ? beyondMax : minMotor);
|
|
}
|
|
|
|
// Then calculate the compress value, if the source range is greater than 1.0f.
|
|
float compressValue = 1.0f;
|
|
|
|
float rangeMotor = maxMotor - minMotor;
|
|
if (rangeMotor > 1.0f) {
|
|
compressValue = rangeMotor;
|
|
}
|
|
|
|
// Combine the movement and compression, to get the value within [0.0f, 1.0f]
|
|
float movedAndCompressedValue = (valueMotor + moveValue) / compressValue;
|
|
|
|
// And last, convert the value into the [neutral, max] range.
|
|
int16_t valueScaled = movedAndCompressedValue * ((float)(max - neutral)) + neutral;
|
|
|
|
if (valueScaled > max) {
|
|
valueScaled = max; // clamp to max value only after scaling is done.
|
|
}
|
|
|
|
PIOS_Assert(valueScaled >= neutral);
|
|
|
|
return valueScaled;
|
|
}
|
|
|
|
/**
|
|
* Constrain motor values to keep any one motor value from going too far out of range of another motor
|
|
*/
|
|
static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool alwaysStabilizeWhenArmed, float throttleDesired)
|
|
{
|
|
int16_t valueScaled;
|
|
|
|
if (max > min) {
|
|
valueScaled = scaleMotorMoveAndCompress(value, max, neutral, maxMotor, minMotor);
|
|
} else {
|
|
// not sure what to do about reversed polarity right now. Why would anyone do this?
|
|
valueScaled = scaleChannel(value, max, min, neutral);
|
|
}
|
|
|
|
// I've added the bool alwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max.
|
|
// NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option
|
|
// This prevents motors startup sync issues causing possible ESC failures.
|
|
|
|
// safety checks
|
|
if (!armed) {
|
|
// if not armed return min EVERYTIME!
|
|
valueScaled = min;
|
|
} else if (!alwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) {
|
|
// all motors idle is alwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed
|
|
// stabilize when armed?
|
|
valueScaled = neutral;
|
|
} else if (!spinWhileArmed && (throttleDesired <= 0.0f)) {
|
|
// soft disarm
|
|
valueScaled = min;
|
|
}
|
|
|
|
return valueScaled;
|
|
}
|
|
|
|
/**
|
|
* Set actuator output to the neutral values (failsafe)
|
|
*/
|
|
static void setFailsafe()
|
|
{
|
|
/* 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 || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
|
|
// reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point
|
|
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]);
|
|
}
|
|
// Send the updated command
|
|
PIOS_Servo_Update();
|
|
|
|
// 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)
|
|
{
|
|
return true;
|
|
}
|
|
#else
|
|
static bool set_channel(uint8_t mixer_channel, uint16_t value)
|
|
{
|
|
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:
|
|
{
|
|
uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]];
|
|
switch (mode) {
|
|
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
|
|
// Remap 1000-2000 range to 125-250µs
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value * ACTUATOR_ONESHOT125_PULSE_FACTOR);
|
|
break;
|
|
case ACTUATORSETTINGS_BANKMODE_ONESHOT42:
|
|
// Remap 1000-2000 range to 41,666-83,333µs
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value * ACTUATOR_ONESHOT42_PULSE_FACTOR);
|
|
break;
|
|
case ACTUATORSETTINGS_BANKMODE_MULTISHOT:
|
|
// Remap 1000-2000 range to 5-25µs
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], (value * ACTUATOR_MULTISHOT_PULSE_FACTOR) - 180);
|
|
break;
|
|
default:
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
|
|
break;
|
|
}
|
|
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(bool force_update)
|
|
{
|
|
static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
|
|
static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
|
|
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
|
|
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
|
|
|
|
// check if any setting is changed
|
|
if (updateMode || updateFreq) {
|
|
/* Something has changed, apply the settings to HW */
|
|
|
|
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
|
|
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
|
|
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
|
|
if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
|
|
PIOS_Servo_SetBankMode(i,
|
|
actuatorSettings.BankMode[i] ==
|
|
ACTUATORSETTINGS_BANKMODE_PWM ?
|
|
PIOS_SERVO_BANK_MODE_PWM :
|
|
PIOS_SERVO_BANK_MODE_SINGLE_PULSE
|
|
);
|
|
}
|
|
switch (actuatorSettings.BankMode[i]) {
|
|
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
|
|
case ACTUATORSETTINGS_BANKMODE_ONESHOT42:
|
|
case ACTUATORSETTINGS_BANKMODE_MULTISHOT:
|
|
freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
|
|
clock[i] = ACTUATOR_ONESHOT_CLOCK; // Setup an 12MHz timer clock
|
|
break;
|
|
case ACTUATORSETTINGS_BANKMODE_PWMSYNC:
|
|
freq[i] = 100;
|
|
clock[i] = ACTUATOR_PWM_CLOCK;
|
|
break;
|
|
default: // PWM
|
|
freq[i] = actuatorSettings.BankUpdateFreq[i];
|
|
clock[i] = ACTUATOR_PWM_CLOCK;
|
|
break;
|
|
}
|
|
}
|
|
|
|
memcpy(prevBankMode,
|
|
actuatorSettings.BankMode,
|
|
sizeof(prevBankMode));
|
|
|
|
PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
|
|
|
|
memcpy(prevBankUpdateFreq,
|
|
actuatorSettings.BankUpdateFreq,
|
|
sizeof(prevBankUpdateFreq));
|
|
// retrieve mode from related bank
|
|
for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
|
|
uint8_t bank = PIOS_Servo_GetPinBank(i);
|
|
pinsMode[i] = actuatorSettings.BankMode[bank];
|
|
}
|
|
}
|
|
}
|
|
|
|
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
ActuatorSettingsGet(&actuatorSettings);
|
|
spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
|
|
if (frameType == FRAME_TYPE_GROUND) {
|
|
spinWhileArmed = false;
|
|
}
|
|
actuator_update_rate_if_changed(false);
|
|
}
|
|
|
|
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
MixerSettingsGet(&mixerSettings);
|
|
mixer_settings_count = 0;
|
|
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
|
|
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
|
|
if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
|
|
mixer_settings_count++;
|
|
}
|
|
}
|
|
}
|
|
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
frameType = GetCurrentFrameType();
|
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
|
uint8_t TreatCustomCraftAs;
|
|
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
|
|
|
|
if (frameType == FRAME_TYPE_CUSTOM) {
|
|
switch (TreatCustomCraftAs) {
|
|
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
|
|
frameType = FRAME_TYPE_FIXED_WING;
|
|
break;
|
|
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
|
|
frameType = FRAME_TYPE_MULTIROTOR;
|
|
break;
|
|
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
|
|
frameType = FRAME_TYPE_GROUND;
|
|
break;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
SystemSettingsThrustControlGet(&thrustType);
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|