2010-09-27 09:28:34 +02:00
|
|
|
/**
|
|
|
|
******************************************************************************
|
|
|
|
* @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
|
|
|
|
*/
|
|
|
|
|
2010-12-20 17:38:17 +01:00
|
|
|
|
2013-05-02 23:31:14 +02:00
|
|
|
#include <openpilot.h>
|
|
|
|
|
2011-06-04 22:31:06 +02:00
|
|
|
#include "accessorydesired.h"
|
2010-09-27 09:28:34 +02:00
|
|
|
#include "actuator.h"
|
|
|
|
#include "actuatorsettings.h"
|
|
|
|
#include "systemsettings.h"
|
|
|
|
#include "actuatordesired.h"
|
|
|
|
#include "actuatorcommand.h"
|
2011-05-03 18:04:44 +02:00
|
|
|
#include "flightstatus.h"
|
2010-09-27 09:28:34 +02:00
|
|
|
#include "mixersettings.h"
|
|
|
|
#include "mixerstatus.h"
|
2011-08-10 04:24:27 +02:00
|
|
|
#include "cameradesired.h"
|
2011-09-07 06:16:34 +02:00
|
|
|
#include "manualcontrolcommand.h"
|
2013-05-02 23:31:14 +02:00
|
|
|
#include "taskinfo.h"
|
2015-03-10 12:54:51 +01:00
|
|
|
#include <systemsettings.h>
|
|
|
|
#include <sanitycheck.h>
|
2015-03-10 13:07:14 +01:00
|
|
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
2015-03-10 12:54:51 +01:00
|
|
|
#include <vtolpathfollowersettings.h>
|
2015-03-10 13:07:14 +01:00
|
|
|
#endif
|
2014-06-08 22:39:16 +02:00
|
|
|
#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
|
2010-09-27 09:28:34 +02:00
|
|
|
|
|
|
|
// Private constants
|
2015-01-31 14:40:40 +01:00
|
|
|
#define MAX_QUEUE_SIZE 2
|
2011-02-01 03:18:35 +01:00
|
|
|
|
|
|
|
#if defined(PIOS_ACTUATOR_STACK_SIZE)
|
2015-01-31 14:40:40 +01:00
|
|
|
#define STACK_SIZE_BYTES PIOS_ACTUATOR_STACK_SIZE
|
2011-02-01 03:18:35 +01:00
|
|
|
#else
|
2015-01-31 14:40:40 +01:00
|
|
|
#define STACK_SIZE_BYTES 1312
|
2011-02-01 03:18:35 +01:00
|
|
|
#endif
|
|
|
|
|
2015-01-31 14:40:40 +01:00
|
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver
|
|
|
|
#define FAILSAFE_TIMEOUT_MS 100
|
|
|
|
#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
|
2010-09-27 09:28:34 +02:00
|
|
|
|
2015-01-31 14:40:40 +01:00
|
|
|
#define CAMERA_BOOT_DELAY_MS 7000
|
2012-11-25 12:36:34 +01:00
|
|
|
|
2015-01-31 14:40:40 +01:00
|
|
|
#define ACTUATOR_ONESHOT125_CLOCK 2000000
|
|
|
|
#define ACTUATOR_ONESHOT125_PULSE_SCALE 4
|
|
|
|
#define ACTUATOR_PWM_CLOCK 1000000
|
2010-09-27 09:28:34 +02:00
|
|
|
// Private types
|
|
|
|
|
|
|
|
|
|
|
|
// Private variables
|
2010-10-01 14:33:20 +02:00
|
|
|
static xQueueHandle queue;
|
2010-09-27 09:28:34 +02:00
|
|
|
static xTaskHandle taskHandle;
|
2015-03-10 12:54:51 +01:00
|
|
|
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
|
|
|
|
static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
|
2010-09-27 09:28:34 +02:00
|
|
|
|
2015-01-23 23:51:53 +01:00
|
|
|
static float lastResult[MAX_MIX_ACTUATORS] = { 0 };
|
|
|
|
static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 };
|
|
|
|
static uint8_t pinsMode[MAX_MIX_ACTUATORS];
|
2012-06-23 22:55:57 +02:00
|
|
|
// used to inform the actuator thread that actuator update rate is changed
|
2015-03-10 12:54:51 +01:00
|
|
|
static ActuatorSettingsData actuatorSettings;
|
|
|
|
static bool spinWhileArmed;
|
|
|
|
|
2012-08-12 01:46:00 +02:00
|
|
|
// used to inform the actuator thread that mixer settings are changed
|
2015-03-10 12:54:51 +01:00
|
|
|
static MixerSettingsData mixerSettings;
|
|
|
|
static int mixer_settings_count = 2;
|
2010-10-03 20:37:07 +02:00
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
// Private functions
|
2013-05-19 16:37:30 +02:00
|
|
|
static void actuatorTask(void *parameters);
|
2010-09-27 09:28:34 +02:00
|
|
|
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
|
2015-03-10 12:54:51 +01:00
|
|
|
static void setFailsafe();
|
|
|
|
static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements);
|
|
|
|
static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements);
|
|
|
|
static bool set_channel(uint8_t mixer_channel, uint16_t value);
|
|
|
|
static void actuator_update_rate_if_changed(bool force_update);
|
2013-05-19 16:37:30 +02:00
|
|
|
static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
|
|
|
|
static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
|
2015-03-10 12:54:51 +01:00
|
|
|
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
2010-09-27 09:28:34 +02:00
|
|
|
float ProcessMixer(const int index, const float curve1, const float curve2,
|
2015-03-10 12:54:51 +01:00
|
|
|
ActuatorDesiredData *desired,
|
2013-05-19 16:37:30 +02:00
|
|
|
const float period);
|
2010-09-27 09:28:34 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// this structure is equivalent to the UAVObjects for one mixer.
|
2010-10-01 14:33:33 +02:00
|
|
|
typedef struct {
|
2013-05-19 16:37:30 +02:00
|
|
|
uint8_t type;
|
|
|
|
int8_t matrix[5];
|
2010-10-01 14:33:33 +02:00
|
|
|
} __attribute__((packed)) Mixer_t;
|
|
|
|
|
2011-06-20 07:35:40 +02:00
|
|
|
/**
|
|
|
|
* @brief Module initialization
|
|
|
|
* @return 0
|
|
|
|
*/
|
|
|
|
int32_t ActuatorStart()
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
// Start main task
|
2014-06-18 01:47:43 +02:00
|
|
|
xTaskCreate(actuatorTask, "Actuator", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
2013-05-19 16:37:30 +02:00
|
|
|
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ACTUATOR, taskHandle);
|
2013-05-15 05:20:52 +02:00
|
|
|
#ifdef PIOS_INCLUDE_WDG
|
2013-05-19 16:37:30 +02:00
|
|
|
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
|
2013-05-15 05:20:52 +02:00
|
|
|
#endif
|
2015-03-10 12:54:51 +01:00
|
|
|
SettingsUpdatedCb(NULL);
|
|
|
|
MixerSettingsUpdatedCb(NULL);
|
|
|
|
ActuatorSettingsUpdatedCb(NULL);
|
2013-05-19 16:37:30 +02:00
|
|
|
return 0;
|
2011-06-20 07:35:40 +02:00
|
|
|
}
|
2010-10-01 14:33:33 +02:00
|
|
|
|
2010-09-27 09:28:34 +02:00
|
|
|
/**
|
|
|
|
* @brief Module initialization
|
|
|
|
* @return 0
|
|
|
|
*/
|
|
|
|
int32_t ActuatorInitialize()
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
// Register for notification of changes to ActuatorSettings
|
|
|
|
ActuatorSettingsInitialize();
|
|
|
|
ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb);
|
2012-08-12 01:46:00 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// Register for notification of changes to MixerSettings
|
|
|
|
MixerSettingsInitialize();
|
|
|
|
MixerSettingsConnectCallback(MixerSettingsUpdatedCb);
|
2012-08-12 01:46:00 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// Listen for ActuatorDesired updates (Primary input to this module)
|
|
|
|
ActuatorDesiredInitialize();
|
|
|
|
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
|
|
|
ActuatorDesiredConnectQueue(queue);
|
2012-08-12 01:46:00 +02:00
|
|
|
|
2014-03-03 18:58:09 +01:00
|
|
|
// Register AccessoryDesired (Secondary input to this module)
|
|
|
|
AccessoryDesiredInitialize();
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// Primary output of this module
|
|
|
|
ActuatorCommandInitialize();
|
2012-08-12 01:46:00 +02:00
|
|
|
|
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
|
2013-05-19 16:37:30 +02:00
|
|
|
// UAVO only used for inspecting the internal status of the mixer during debug
|
|
|
|
MixerStatusInitialize();
|
2011-06-18 18:59:02 +02:00
|
|
|
#endif
|
|
|
|
|
2015-03-10 13:07:14 +01:00
|
|
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
2015-03-10 12:54:51 +01:00
|
|
|
VtolPathFollowerSettingsInitialize();
|
|
|
|
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
2015-03-10 13:07:14 +01:00
|
|
|
#endif
|
2015-03-10 12:54:51 +01:00
|
|
|
SystemSettingsInitialize();
|
|
|
|
SystemSettingsConnectCallback(&SettingsUpdatedCb);
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
return 0;
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
2013-06-04 05:37:40 +02:00
|
|
|
MODULE_INITCALL(ActuatorInitialize, ActuatorStart);
|
2010-09-27 09:28:34 +02:00
|
|
|
|
|
|
|
/**
|
2011-02-01 03:17:55 +01:00
|
|
|
* @brief Main Actuator module task
|
2010-10-01 14:33:33 +02:00
|
|
|
*
|
|
|
|
* 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
|
2010-09-27 09:28:34 +02:00
|
|
|
*/
|
2013-05-19 16:37:30 +02:00
|
|
|
static void actuatorTask(__attribute__((unused)) void *parameters)
|
2010-09-27 09:28:34 +02:00
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
UAVObjEvent ev;
|
|
|
|
portTickType lastSysTime;
|
|
|
|
portTickType thisSysTime;
|
|
|
|
float dTSeconds;
|
|
|
|
uint32_t dTMilliseconds;
|
|
|
|
|
|
|
|
ActuatorCommandData command;
|
|
|
|
ActuatorDesiredData desired;
|
|
|
|
MixerStatusData mixerStatus;
|
|
|
|
FlightStatusData flightStatus;
|
2014-02-09 19:33:29 +01:00
|
|
|
float throttleDesired;
|
|
|
|
float collectiveDesired;
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2014-06-08 22:39:16 +02:00
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
|
|
counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
|
|
|
|
#endif
|
2013-05-19 16:37:30 +02:00
|
|
|
/* Read initial values of ActuatorSettings */
|
|
|
|
|
|
|
|
ActuatorSettingsGet(&actuatorSettings);
|
|
|
|
|
|
|
|
/* Read initial values of MixerSettings */
|
|
|
|
MixerSettingsGet(&mixerSettings);
|
|
|
|
|
|
|
|
/* Force an initial configuration of the actuator update rates */
|
2015-03-10 12:54:51 +01:00
|
|
|
actuator_update_rate_if_changed(true);
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
|
2015-03-10 12:54:51 +01:00
|
|
|
setFailsafe();
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
// Main task loop
|
|
|
|
lastSysTime = xTaskGetTickCount();
|
|
|
|
while (1) {
|
2013-05-15 05:20:52 +02:00
|
|
|
#ifdef PIOS_INCLUDE_WDG
|
2013-05-19 16:37:30 +02:00
|
|
|
PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
|
2013-05-15 05:20:52 +02:00
|
|
|
#endif
|
2010-11-23 18:13:42 +01:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// Wait until the ActuatorDesired object is updated
|
|
|
|
uint8_t rc = xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS);
|
2014-06-08 22:39:16 +02:00
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
|
|
PIOS_Instrumentation_TimeStart(counter);
|
|
|
|
#endif
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
if (rc != pdTRUE) {
|
|
|
|
/* Update of ActuatorDesired timed out. Go to failsafe */
|
2015-03-10 12:54:51 +01:00
|
|
|
setFailsafe();
|
2013-05-19 16:37:30 +02:00
|
|
|
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);
|
2014-02-09 19:33:29 +01:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|
2011-06-03 06:18:34 +02:00
|
|
|
|
2014-03-02 15:25:49 +01:00
|
|
|
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
|
2015-03-11 03:49:21 +01:00
|
|
|
bool activeThrottle = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
|
2015-03-10 12:54:51 +01:00
|
|
|
bool positiveThrottle = (throttleDesired > 0.00f);
|
|
|
|
|
2014-03-02 15:25:49 +01:00
|
|
|
// safety settings
|
2014-03-09 10:00:58 +01:00
|
|
|
if (!armed) {
|
2014-03-02 15:25:49 +01:00
|
|
|
throttleDesired = 0;
|
|
|
|
}
|
2015-03-10 12:54:51 +01:00
|
|
|
|
|
|
|
if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
|
2014-03-02 15:25:49 +01:00
|
|
|
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
|
|
|
|
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
|
|
|
|
desired.Roll = 0;
|
|
|
|
}
|
|
|
|
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
|
|
|
|
desired.Pitch = 0;
|
|
|
|
}
|
|
|
|
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
|
|
|
|
desired.Yaw = 0;
|
|
|
|
}
|
2014-02-16 11:30:28 +01:00
|
|
|
}
|
|
|
|
|
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
|
2013-05-19 16:37:30 +02:00
|
|
|
MixerStatusGet(&mixerStatus);
|
2011-06-17 16:50:10 +02:00
|
|
|
#endif
|
2015-03-10 12:54:51 +01:00
|
|
|
|
|
|
|
if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
|
|
|
|
setFailsafe();
|
2013-05-19 16:37:30 +02:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
float curve1 = 0.0f;
|
|
|
|
float curve2 = 0.0f;
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
// 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);
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
// The source for the secondary curve is selectable
|
|
|
|
AccessoryDesiredData accessory;
|
2015-03-10 12:54:51 +01:00
|
|
|
uint8_t curve2Source = mixerSettings.Curve2Source;
|
|
|
|
switch (curve2Source) {
|
2013-05-19 16:37:30 +02:00
|
|
|
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
|
2015-03-10 12:54:51 +01:00
|
|
|
// assume reversible motor/mixer initially
|
|
|
|
curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
|
2013-05-19 16:37:30 +02:00
|
|
|
break;
|
|
|
|
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
|
2015-03-10 12:54:51 +01:00
|
|
|
// Throttle curve contribution the same for +ve vs -ve roll
|
|
|
|
curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
|
2013-05-19 16:37:30 +02:00
|
|
|
break;
|
|
|
|
case MIXERSETTINGS_CURVE2SOURCE_PITCH:
|
2015-03-10 12:54:51 +01:00
|
|
|
// Throttle curve contribution the same for +ve vs -ve pitch
|
|
|
|
curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
|
|
|
|
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
|
2013-05-19 16:37:30 +02:00
|
|
|
break;
|
|
|
|
case MIXERSETTINGS_CURVE2SOURCE_YAW:
|
2015-03-10 12:54:51 +01:00
|
|
|
// Throttle curve contribution the same for +ve vs -ve yaw
|
|
|
|
curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
|
2013-05-19 16:37:30 +02:00
|
|
|
break;
|
|
|
|
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
|
2015-03-10 12:54:51 +01:00
|
|
|
// assume reversible motor/mixer initially
|
|
|
|
curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
|
|
|
|
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
|
2013-05-19 16:37:30 +02:00
|
|
|
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) {
|
2015-03-10 12:54:51 +01:00
|
|
|
// Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
|
|
|
|
curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
|
2013-05-19 16:37:30 +02:00
|
|
|
} else {
|
2015-03-10 12:54:51 +01:00
|
|
|
curve2 = 0.0f;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
break;
|
2015-03-10 12:54:51 +01:00
|
|
|
default:
|
|
|
|
curve2 = 0.0f;
|
|
|
|
break;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
float *status = (float *)&mixerStatus; // access status objects as an array of floats
|
|
|
|
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
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;
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
uint8_t mixer_type = mixers[ct].type;
|
|
|
|
|
|
|
|
if (mixer_type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
|
2013-05-19 16:37:30 +02:00
|
|
|
// Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us
|
|
|
|
status[ct] = -1;
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
|
|
|
|
if (curve1 < 0.0f) {
|
|
|
|
curve1 = 0.0f;
|
|
|
|
}
|
|
|
|
if (curve2 < 0.0f) {
|
|
|
|
curve2 = 0.0f;
|
|
|
|
}
|
|
|
|
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
|
2013-05-19 16:37:30 +02:00
|
|
|
// If not armed or motors aren't meant to spin all the time
|
|
|
|
if (!armed ||
|
2014-03-09 10:00:58 +01:00
|
|
|
(!spinWhileArmed && !positiveThrottle)) {
|
2013-05-19 16:37:30 +02:00
|
|
|
filterAccumulator[ct] = 0;
|
|
|
|
lastResult[ct] = 0;
|
|
|
|
status[ct] = -1; // force min throttle
|
|
|
|
}
|
|
|
|
// If armed meant to keep spinning,
|
2014-03-09 10:00:58 +01:00
|
|
|
else if ((spinWhileArmed && !positiveThrottle) ||
|
2013-05-19 16:37:30 +02:00
|
|
|
(status[ct] < 0)) {
|
|
|
|
status[ct] = 0;
|
|
|
|
}
|
2015-03-10 12:54:51 +01:00
|
|
|
} else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
|
|
|
|
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
|
|
|
|
// Reversable Motors are like Motors but go to neutral instead of minimum
|
2014-03-09 01:37:22 +01:00
|
|
|
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type
|
|
|
|
if (!armed || !activeThrottle) {
|
|
|
|
filterAccumulator[ct] = 0;
|
|
|
|
lastResult[ct] = 0;
|
|
|
|
status[ct] = 0; // force neutral throttle
|
|
|
|
}
|
2015-03-10 12:54:51 +01:00
|
|
|
} else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
|
|
|
|
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
|
|
|
|
} else {
|
|
|
|
status[ct] = -1;
|
2014-03-09 01:37:22 +01:00
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
// 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;
|
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
|
|
|
|
(mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
|
|
|
|
CameraDesiredData cameraDesired;
|
|
|
|
if (CameraDesiredGet(&cameraDesired) == 0) {
|
|
|
|
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;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
// Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot
|
|
|
|
if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) {
|
|
|
|
command.Channel[ct] = 0;
|
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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]) {
|
2015-01-24 13:37:00 +01:00
|
|
|
command.Channel[i] = scaleChannel(status[i],
|
|
|
|
actuatorSettings.ChannelMax[i],
|
|
|
|
actuatorSettings.ChannelMin[i],
|
|
|
|
actuatorSettings.ChannelNeutral[i]);
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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);
|
2010-10-03 20:37:07 +02:00
|
|
|
|
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
|
2013-05-19 16:37:30 +02:00
|
|
|
MixerStatusSet(&mixerStatus);
|
2011-11-26 23:12:32 +01:00
|
|
|
#endif
|
|
|
|
|
2011-06-23 08:47:55 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
// Update servo outputs
|
|
|
|
bool success = true;
|
2011-01-10 02:11:44 +01:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
|
2015-03-10 12:54:51 +01:00
|
|
|
success &= set_channel(n, command.Channel[n]);
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2011-01-10 02:11:44 +01:00
|
|
|
|
2015-01-23 23:51:53 +01:00
|
|
|
PIOS_Servo_Update();
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
if (!success) {
|
|
|
|
command.NumFailedUpdates++;
|
|
|
|
ActuatorCommandSet(&command);
|
|
|
|
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
|
|
|
}
|
2014-06-08 22:39:16 +02:00
|
|
|
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
|
|
|
PIOS_Instrumentation_TimeEnd(counter);
|
|
|
|
#endif
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
2013-05-19 16:37:30 +02:00
|
|
|
* Process mixing for one actuator
|
2010-09-27 19:30:42 +02:00
|
|
|
*/
|
2010-09-27 09:28:34 +02:00
|
|
|
float ProcessMixer(const int index, const float curve1, const float curve2,
|
2015-03-10 12:54:51 +01:00
|
|
|
ActuatorDesiredData *desired, const float period)
|
2010-09-27 09:28:34 +02:00
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
static float lastFilteredResult[MAX_MIX_ACTUATORS];
|
2015-03-10 12:54:51 +01:00
|
|
|
const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
|
2013-05-19 16:37:30 +02:00
|
|
|
const Mixer_t *mixer = &mixers[index];
|
|
|
|
|
2015-01-23 23:51:53 +01:00
|
|
|
float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
|
|
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2]) * curve2) +
|
|
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL]) * desired->Roll) +
|
|
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH]) * desired->Pitch) +
|
|
|
|
(((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW]) * desired->Yaw)) / 128.0f;
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2014-03-09 01:37:22 +01:00
|
|
|
// note: no feedforward for reversable motors yet for safety reasons
|
2013-05-19 16:37:30 +02:00
|
|
|
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
|
|
|
|
if (result < 0.0f) { // idle throttle
|
|
|
|
result = 0.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
// feed forward
|
|
|
|
float accumulator = filterAccumulator[index];
|
2015-03-10 12:54:51 +01:00
|
|
|
accumulator += (result - lastResult[index]) * mixerSettings.FeedForward;
|
2013-05-19 16:37:30 +02:00
|
|
|
lastResult[index] = result;
|
|
|
|
result += accumulator;
|
|
|
|
if (period > 0.0f) {
|
|
|
|
if (accumulator > 0.0f) {
|
2015-03-10 12:54:51 +01:00
|
|
|
float invFilter = period / mixerSettings.AccelTime;
|
2014-06-08 22:39:16 +02:00
|
|
|
if (invFilter > 1) {
|
|
|
|
invFilter = 1;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2014-06-08 22:39:16 +02:00
|
|
|
accumulator -= accumulator * invFilter;
|
2013-05-19 16:37:30 +02:00
|
|
|
} else {
|
2015-03-10 12:54:51 +01:00
|
|
|
float invFilter = period / mixerSettings.DecelTime;
|
2014-06-08 22:39:16 +02:00
|
|
|
if (invFilter > 1) {
|
|
|
|
invFilter = 1;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2014-06-08 22:39:16 +02:00
|
|
|
accumulator -= accumulator * invFilter;
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
filterAccumulator[index] = accumulator;
|
|
|
|
result += accumulator;
|
|
|
|
|
|
|
|
// acceleration limit
|
|
|
|
float dt = result - lastFilteredResult[index];
|
2015-03-10 12:54:51 +01:00
|
|
|
float maxDt = mixerSettings.MaxAccel * period;
|
2013-05-19 16:37:30 +02:00
|
|
|
if (dt > maxDt) { // we are accelerating too hard
|
|
|
|
result = lastFilteredResult[index] + maxDt;
|
|
|
|
}
|
|
|
|
lastFilteredResult[index] = result;
|
|
|
|
}
|
|
|
|
|
|
|
|
return result;
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
2015-03-10 12:54:51 +01:00
|
|
|
* 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)
|
2010-09-27 19:30:42 +02:00
|
|
|
*/
|
2015-03-10 12:54:51 +01:00
|
|
|
static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements)
|
2010-09-27 09:28:34 +02:00
|
|
|
{
|
2015-03-14 02:01:30 +01:00
|
|
|
float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements);
|
2015-03-14 02:15:59 +01:00
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
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)
|
|
|
|
{
|
|
|
|
float abs_input = fabsf(input);
|
|
|
|
float scale = abs_input * (float)(elements - 1);
|
|
|
|
int idx1 = scale;
|
|
|
|
|
|
|
|
scale -= (float)idx1; // remainder
|
|
|
|
if (curve[0] < -1) {
|
|
|
|
return input;
|
|
|
|
}
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
|
|
|
|
return unsigned_value;
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
* 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)
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
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;
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Set actuator output to the neutral values (failsafe)
|
|
|
|
*/
|
2015-03-10 12:54:51 +01:00
|
|
|
static void setFailsafe()
|
2010-09-27 09:28:34 +02:00
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
/* grab only the parts that we are going to use */
|
|
|
|
int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM];
|
|
|
|
|
|
|
|
ActuatorCommandChannelGet(Channel);
|
|
|
|
|
2015-03-10 12:54:51 +01:00
|
|
|
const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
// Reset ActuatorCommand to safe values
|
|
|
|
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
|
|
|
|
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
|
2015-03-10 12:54:51 +01:00
|
|
|
Channel[n] = actuatorSettings.ChannelMin[n];
|
2014-03-09 01:37:22 +01:00
|
|
|
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
|
2015-03-10 12:54:51 +01:00
|
|
|
// reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point
|
|
|
|
Channel[n] = actuatorSettings.ChannelNeutral[n];
|
2013-05-19 16:37:30 +02:00
|
|
|
} 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) {
|
2015-03-10 12:54:51 +01:00
|
|
|
set_channel(n, Channel[n]);
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2015-01-25 21:25:14 +01:00
|
|
|
// Send the updated command
|
|
|
|
PIOS_Servo_Update();
|
2013-05-19 16:37:30 +02:00
|
|
|
|
|
|
|
// Update output object's parts that we changed
|
|
|
|
ActuatorCommandChannelSet(Channel);
|
2010-09-27 09:28:34 +02:00
|
|
|
}
|
|
|
|
|
2012-11-05 10:09:43 +01:00
|
|
|
/**
|
|
|
|
* determine buzzer or blink sequence
|
|
|
|
**/
|
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
typedef enum { BUZZ_BUZZER = 0, BUZZ_ARMING = 1, BUZZ_INFO = 2, BUZZ_MAX = 3 } buzzertype;
|
2012-11-05 10:09:43 +01:00
|
|
|
|
|
|
|
static inline bool buzzerState(buzzertype type)
|
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
// 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;
|
2012-11-05 10:09:43 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2011-01-12 01:39:18 +01:00
|
|
|
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
|
2015-03-10 12:54:51 +01:00
|
|
|
static bool set_channel(uint8_t mixer_channel, uint16_t value)
|
2012-08-12 01:46:00 +02:00
|
|
|
{
|
2013-05-19 16:37:30 +02:00
|
|
|
return true;
|
2011-01-12 01:39:18 +01:00
|
|
|
}
|
|
|
|
#else
|
2015-03-10 12:54:51 +01:00
|
|
|
static bool set_channel(uint8_t mixer_channel, uint16_t value)
|
2012-08-12 01:46:00 +02:00
|
|
|
{
|
2015-03-10 12:54:51 +01:00
|
|
|
switch (actuatorSettings.ChannelType[mixer_channel]) {
|
2013-05-19 16:37:30 +02:00
|
|
|
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
|
2015-03-10 12:54:51 +01:00
|
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
|
|
|
|
buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
|
2013-05-19 16:37:30 +02:00
|
|
|
return true;
|
|
|
|
|
|
|
|
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
|
2015-03-10 12:54:51 +01:00
|
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
|
|
|
|
buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
|
2013-05-19 16:37:30 +02:00
|
|
|
return true;
|
|
|
|
|
|
|
|
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
|
2015-03-10 12:54:51 +01:00
|
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
|
|
|
|
buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
|
2013-05-19 16:37:30 +02:00
|
|
|
return true;
|
|
|
|
|
|
|
|
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
|
2015-01-24 13:37:00 +01:00
|
|
|
{
|
2015-03-10 12:54:51 +01:00
|
|
|
uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]];
|
2015-01-31 14:40:40 +01:00
|
|
|
switch (mode) {
|
|
|
|
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
|
|
|
|
// Remap 1000-2000 range to 125-250
|
2015-03-10 12:54:51 +01:00
|
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
|
2015-01-31 14:40:40 +01:00
|
|
|
break;
|
|
|
|
default:
|
2015-03-10 12:54:51 +01:00
|
|
|
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
|
2015-01-31 14:40:40 +01:00
|
|
|
break;
|
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
return true;
|
2015-01-31 14:40:40 +01:00
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2011-02-20 08:14:02 +01:00
|
|
|
#if defined(PIOS_INCLUDE_I2C_ESC)
|
2013-05-19 16:37:30 +02:00
|
|
|
case ACTUATORSETTINGS_CHANNELTYPE_MK:
|
|
|
|
return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel], value);
|
2011-06-23 08:47:55 +02:00
|
|
|
|
2013-05-19 16:37:30 +02:00
|
|
|
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
|
|
|
|
return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel], value);
|
2011-06-23 08:47:55 +02:00
|
|
|
|
2011-01-12 01:39:18 +01:00
|
|
|
#endif
|
2013-05-19 16:37:30 +02:00
|
|
|
default:
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
#endif /* if defined(ARCH_POSIX) || defined(ARCH_WIN32) */
|
2011-01-10 02:11:44 +01:00
|
|
|
|
2012-08-12 01:46:00 +02:00
|
|
|
/**
|
|
|
|
* @brief Update the servo update rate
|
|
|
|
*/
|
2015-03-10 12:54:51 +01:00
|
|
|
static void actuator_update_rate_if_changed(bool force_update)
|
2012-08-12 01:46:00 +02:00
|
|
|
{
|
2015-01-23 23:51:53 +01:00
|
|
|
static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
|
|
|
|
static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
|
2015-03-10 12:54:51 +01:00
|
|
|
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
|
|
|
|
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
|
2013-05-19 16:37:30 +02:00
|
|
|
|
2015-02-22 14:21:23 +01:00
|
|
|
// check if any setting is changed
|
2015-02-22 13:12:00 +01:00
|
|
|
if (updateMode || updateFreq) {
|
2013-05-19 16:37:30 +02:00
|
|
|
/* Something has changed, apply the settings to HW */
|
2015-01-23 23:51:53 +01:00
|
|
|
|
|
|
|
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
|
2015-01-29 21:07:21 +01:00
|
|
|
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
|
2015-01-23 23:51:53 +01:00
|
|
|
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
|
2015-03-10 12:54:51 +01:00
|
|
|
if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
|
2015-02-22 13:12:00 +01:00
|
|
|
PIOS_Servo_SetBankMode(i,
|
2015-03-10 12:54:51 +01:00
|
|
|
actuatorSettings.BankMode[i] ==
|
2015-02-22 13:12:00 +01:00
|
|
|
ACTUATORSETTINGS_BANKMODE_PWM ?
|
|
|
|
PIOS_SERVO_BANK_MODE_PWM :
|
|
|
|
PIOS_SERVO_BANK_MODE_SINGLE_PULSE
|
|
|
|
);
|
|
|
|
}
|
2015-03-10 12:54:51 +01:00
|
|
|
switch (actuatorSettings.BankMode[i]) {
|
2015-01-29 21:07:21 +01:00
|
|
|
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
|
2015-01-31 14:40:40 +01:00
|
|
|
freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
|
2015-02-22 13:12:00 +01:00
|
|
|
clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
|
2015-01-29 21:07:21 +01:00
|
|
|
break;
|
2015-02-17 23:20:07 +01:00
|
|
|
case ACTUATORSETTINGS_BANKMODE_PWMSYNC:
|
2015-02-02 02:58:22 +01:00
|
|
|
freq[i] = 100;
|
2015-01-29 21:07:21 +01:00
|
|
|
clock[i] = ACTUATOR_PWM_CLOCK;
|
|
|
|
break;
|
|
|
|
default: // PWM
|
2015-03-10 12:54:51 +01:00
|
|
|
freq[i] = actuatorSettings.BankUpdateFreq[i];
|
2015-01-29 21:07:21 +01:00
|
|
|
clock[i] = ACTUATOR_PWM_CLOCK;
|
|
|
|
break;
|
2015-01-23 23:51:53 +01:00
|
|
|
}
|
|
|
|
}
|
2015-02-22 13:12:00 +01:00
|
|
|
|
|
|
|
memcpy(prevBankMode,
|
2015-03-10 12:54:51 +01:00
|
|
|
actuatorSettings.BankMode,
|
2015-02-22 13:12:00 +01:00
|
|
|
sizeof(prevBankMode));
|
|
|
|
|
2015-01-29 21:07:21 +01:00
|
|
|
PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
|
2015-01-23 23:51:53 +01:00
|
|
|
|
2015-02-22 13:12:00 +01:00
|
|
|
memcpy(prevBankUpdateFreq,
|
2015-03-10 12:54:51 +01:00
|
|
|
actuatorSettings.BankUpdateFreq,
|
2015-02-22 13:12:00 +01:00
|
|
|
sizeof(prevBankUpdateFreq));
|
2015-01-23 23:51:53 +01:00
|
|
|
// retrieve mode from related bank
|
|
|
|
for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
|
|
|
|
uint8_t bank = PIOS_Servo_GetPinBank(i);
|
2015-03-10 12:54:51 +01:00
|
|
|
pinsMode[i] = actuatorSettings.BankMode[bank];
|
2015-01-23 23:51:53 +01:00
|
|
|
}
|
2013-05-19 16:37:30 +02:00
|
|
|
}
|
2012-08-12 01:46:00 +02:00
|
|
|
}
|
|
|
|
|
2013-05-05 09:02:24 +02:00
|
|
|
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
2012-08-12 01:46:00 +02:00
|
|
|
{
|
2015-03-10 12:54:51 +01:00
|
|
|
ActuatorSettingsGet(&actuatorSettings);
|
|
|
|
spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
|
|
|
|
if (frameType == FRAME_TYPE_GROUND) {
|
|
|
|
spinWhileArmed = false;
|
|
|
|
}
|
|
|
|
actuator_update_rate_if_changed(false);
|
2012-08-12 01:46:00 +02:00
|
|
|
}
|
|
|
|
|
2013-05-05 09:02:24 +02:00
|
|
|
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
2012-08-12 01:46:00 +02:00
|
|
|
{
|
2015-03-10 12:54:51 +01:00
|
|
|
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();
|
2015-03-10 13:07:14 +01:00
|
|
|
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
|
|
|
uint8_t TreatCustomCraftAs;
|
|
|
|
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
|
2015-03-10 12:54:51 +01:00
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2015-03-10 13:07:14 +01:00
|
|
|
#endif
|
2015-03-10 12:54:51 +01:00
|
|
|
|
|
|
|
SystemSettingsThrustControlGet(&thrustType);
|
2012-08-12 01:46:00 +02:00
|
|
|
}
|
2010-09-27 09:28:34 +02:00
|
|
|
|
|
|
|
/**
|
|
|
|
* @}
|
2010-09-27 19:30:42 +02:00
|
|
|
* @}
|
|
|
|
*/
|