rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

OP-1769 Ground Support

Browse Source 
1. Receiver.c for ground allows disabled channels for all but yaw and throttle
2. Reciever.c applies a deadband on throttle 0 position
3. Arming understands ground have a 0 throttle position
4. Actuator optimisations in general and fixes for mixing
5. Actuator mixer support for reversible motors
This commit is contained in:
abeck70 2015-03-10 22:54:51 +11:00
parent 8c31f96dd7
commit 52147333ed
5 changed files with 367 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

394
flight/modules/Actuator/actuator.c
View File

@ -45,6 +45,9 @@
#include "cameradesired.h" #include "cameradesired.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "taskinfo.h" #include "taskinfo.h"
#include <systemsettings.h>
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#undef PIOS_INCLUDE_INSTRUMENTATION #undef PIOS_INCLUDE_INSTRUMENTATION
#ifdef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h> #include <pios_instrumentation.h>
@ -76,26 +79,33 @@ static int8_t counter;
// Private variables // Private variables
static xQueueHandle queue; static xQueueHandle queue;
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
static float lastResult[MAX_MIX_ACTUATORS] = { 0 }; static float lastResult[MAX_MIX_ACTUATORS] = { 0 };
static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 }; static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 };
static uint8_t pinsMode[MAX_MIX_ACTUATORS]; static uint8_t pinsMode[MAX_MIX_ACTUATORS];
// used to inform the actuator thread that actuator update rate is changed // used to inform the actuator thread that actuator update rate is changed
static volatile bool actuator_settings_updated; static ActuatorSettingsData actuatorSettings;
static bool spinWhileArmed;
// used to inform the actuator thread that mixer settings are changed // used to inform the actuator thread that mixer settings are changed
static volatile bool mixer_settings_updated; static MixerSettingsData mixerSettings;
static int mixer_settings_count = 2;
// Private functions // Private functions
static void actuatorTask(void *parameters); static void actuatorTask(void *parameters);
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings); static void setFailsafe();
static float MixerCurve(const float throttle, const float *curve, uint8_t elements); static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements);
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings); static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements);
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update); 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 MixerSettingsUpdatedCb(UAVObjEvent *ev);
static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
static void SettingsUpdatedCb(UAVObjEvent *ev);
float ProcessMixer(const int index, const float curve1, const float curve2, float ProcessMixer(const int index, const float curve1, const float curve2,
const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, ActuatorDesiredData *desired,
const float period); const float period);
// this structure is equivalent to the UAVObjects for one mixer. // this structure is equivalent to the UAVObjects for one mixer.
@ -104,6 +114,8 @@ typedef struct {
int8_t matrix[5]; int8_t matrix[5];
} __attribute__((packed)) Mixer_t; } __attribute__((packed)) Mixer_t;
#define DIAG_MIXERSTATUS
/** /**
* @brief Module initialization * @brief Module initialization
* @return 0 * @return 0
@ -116,6 +128,9 @@ int32_t ActuatorStart()
#ifdef PIOS_INCLUDE_WDG #ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
#endif #endif
SettingsUpdatedCb(NULL);
MixerSettingsUpdatedCb(NULL);
ActuatorSettingsUpdatedCb(NULL);
return 0; return 0;
} }
@ -149,6 +164,11 @@ int32_t ActuatorInitialize()
MixerStatusInitialize(); MixerStatusInitialize();
#endif #endif
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsInitialize();
SystemSettingsConnectCallback(&SettingsUpdatedCb);
return 0; return 0;
} }
MODULE_INITCALL(ActuatorInitialize, ActuatorStart); MODULE_INITCALL(ActuatorInitialize, ActuatorStart);
@ -178,7 +198,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ActuatorDesiredData desired; ActuatorDesiredData desired;
MixerStatusData mixerStatus; MixerStatusData mixerStatus;
FlightStatusData flightStatus; FlightStatusData flightStatus;
SystemSettingsThrustControlOptions thrustType;
float throttleDesired; float throttleDesired;
float collectiveDesired; float collectiveDesired;
@ -186,21 +205,17 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
counter = PIOS_Instrumentation_CreateCounter(0xAC700001); counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
#endif #endif
/* Read initial values of ActuatorSettings */ /* Read initial values of ActuatorSettings */
ActuatorSettingsData actuatorSettings;
actuator_settings_updated = false;
ActuatorSettingsGet(&actuatorSettings); ActuatorSettingsGet(&actuatorSettings);
/* Read initial values of MixerSettings */ /* Read initial values of MixerSettings */
MixerSettingsData mixerSettings;
mixer_settings_updated = false;
MixerSettingsGet(&mixerSettings); MixerSettingsGet(&mixerSettings);
/* Force an initial configuration of the actuator update rates */ /* Force an initial configuration of the actuator update rates */
actuator_update_rate_if_changed(&actuatorSettings, true); actuator_update_rate_if_changed(true);
// Go to the neutral (failsafe) values until an ActuatorDesired update is received // Go to the neutral (failsafe) values until an ActuatorDesired update is received
setFailsafe(&actuatorSettings, &mixerSettings); setFailsafe();
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
@ -214,20 +229,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
#ifdef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeStart(counter); PIOS_Instrumentation_TimeStart(counter);
#endif #endif
/* Process settings updated events even in timeout case so we always act on the latest settings */
if (actuator_settings_updated) {
actuator_settings_updated = false;
ActuatorSettingsGet(&actuatorSettings);
actuator_update_rate_if_changed(&actuatorSettings, false);
}
if (mixer_settings_updated) {
mixer_settings_updated = false;
MixerSettingsGet(&mixerSettings);
}
if (rc != pdTRUE) { if (rc != pdTRUE) {
/* Update of ActuatorDesired timed out. Go to failsafe */ /* Update of ActuatorDesired timed out. Go to failsafe */
setFailsafe(&actuatorSettings, &mixerSettings); setFailsafe();
continue; continue;
} }
@ -240,7 +245,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
ActuatorDesiredGet(&desired); ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command); ActuatorCommandGet(&command);
SystemSettingsThrustControlGet(&thrustType);
// read in throttle and collective -demultiplex thrust // read in throttle and collective -demultiplex thrust
switch (thrustType) { switch (thrustType) {
@ -259,11 +263,16 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
// ground needs to cature for reverse which is negative throttle
bool activeThrottle = (throttleDesired < 0.00f || throttleDesired > 0.00f);
bool positiveThrottle = (throttleDesired > 0.00f);
// safety settings // safety settings
if (!armed) { if (!armed) {
throttleDesired = 0; throttleDesired = 0;
} }
if (throttleDesired <= 0.00f || !armed) {
if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
// force set all other controls to zero if throttle is cut (previously set in Stabilization) // force set all other controls to zero if throttle is cut (previously set in Stabilization)
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Roll = 0; desired.Roll = 0;
@ -279,46 +288,47 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
#ifdef DIAG_MIXERSTATUS #ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus); MixerStatusGet(&mixerStatus);
#endif #endif
int nMixers = 0;
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { setFailsafe();
if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
nMixers++;
}
}
if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working
continue; continue;
} }
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool activeThrottle = (throttleDesired < 0.00f || throttleDesired > 0.00f); float curve1 = 0.0f;
bool positiveThrottle = (throttleDesired > 0.00f); float curve2 = 0.0f;
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); // 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);
// The source for the secondary curve is selectable // The source for the secondary curve is selectable
float curve2 = 0;
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
switch (mixerSettings.Curve2Source) { uint8_t curve2Source = mixerSettings.Curve2Source;
switch (curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); // assume reversible motor/mixer initially
curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL: case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); // Throttle curve contribution the same for +ve vs -ve roll
curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_PITCH: case MIXERSETTINGS_CURVE2SOURCE_PITCH:
curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2, // Throttle curve contribution the same for +ve vs -ve pitch
MIXERSETTINGS_THROTTLECURVE2_NUMELEM); curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_YAW: case MIXERSETTINGS_CURVE2SOURCE_YAW:
curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); // Throttle curve contribution the same for +ve vs -ve yaw
curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2, // assume reversible motor/mixer initially
MIXERSETTINGS_THROTTLECURVE2_NUMELEM); curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
@ -327,14 +337,19 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) { if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) {
curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); // Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
} else { } else {
curve2 = 0; curve2 = 0.0f;
} }
break; break;
default:
curve2 = 0.0f;
break;
} }
float *status = (float *)&mixerStatus; // access status objects as an array of floats float *status = (float *)&mixerStatus; // access status objects as an array of floats
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) { for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
// During boot all camera actuators should be completely disabled (PWM pulse = 0). // During boot all camera actuators should be completely disabled (PWM pulse = 0).
@ -343,20 +358,22 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
// Setting it to 1 by default means "Rescale this channel and enable PWM on its output". // Setting it to 1 by default means "Rescale this channel and enable PWM on its output".
command.Channel[ct] = 1; command.Channel[ct] = 1;
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { 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 // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us
status[ct] = -1; status[ct] = -1;
continue; continue;
} }
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); if (curve1 < 0.0f) {
} else { curve1 = 0.0f;
status[ct] = -1; }
} if (curve2 < 0.0f) {
curve2 = 0.0f;
// Motors have additional protection for when to be on }
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
// If not armed or motors aren't meant to spin all the time // If not armed or motors aren't meant to spin all the time
if (!armed || if (!armed ||
(!spinWhileArmed && !positiveThrottle)) { (!spinWhileArmed && !positiveThrottle)) {
@ -369,57 +386,60 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
(status[ct] < 0)) { (status[ct] < 0)) {
status[ct] = 0; status[ct] = 0;
} }
} } 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 // Reversable Motors are like Motors but go to neutral instead of minimum
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type // If not armed or motor is inactive - no "spinwhilearmed" for this engine type
if (!armed || !activeThrottle) { if (!armed || !activeThrottle) {
filterAccumulator[ct] = 0; filterAccumulator[ct] = 0;
lastResult[ct] = 0; lastResult[ct] = 0;
status[ct] = 0; // force neutral throttle status[ct] = 0; // force neutral throttle
} }
} } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
} else {
status[ct] = -1;
// If an accessory channel is selected for direct bypass mode // If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped // In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING // 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 // 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 // the correct behavior is since it seems domain specific. I don't love
// this code // this code
if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { (mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) { if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
status[ct] = accessory.AccessoryVal; status[ct] = accessory.AccessoryVal;
} else { } else {
status[ct] = -1; status[ct] = -1;
}
}
if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
CameraDesiredData cameraDesired;
if (CameraDesiredGet(&cameraDesired) == 0) {
switch (mixers[ct].type) {
case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
status[ct] = cameraDesired.RollOrServo1;
break;
case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2:
status[ct] = cameraDesired.PitchOrServo2;
break;
case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
status[ct] = cameraDesired.Yaw;
break;
default:
break;
} }
} else {
status[ct] = -1;
} }
// Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) { (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
command.Channel[ct] = 0; 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;
}
// Disable camera actuators for CAMERA_BOOT_DELAY_MS after boot
if (thisSysTime < (CAMERA_BOOT_DELAY_MS / portTICK_RATE_MS)) {
command.Channel[ct] = 0;
}
} }
} }
} }
@ -454,7 +474,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
bool success = true; bool success = true;
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
success &= set_channel(n, command.Channel[n], &actuatorSettings); success &= set_channel(n, command.Channel[n]);
} }
PIOS_Servo_Update(); PIOS_Servo_Update();
@ -475,10 +495,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
* Process mixing for one actuator * Process mixing for one actuator
*/ */
float ProcessMixer(const int index, const float curve1, const float curve2, float ProcessMixer(const int index, const float curve1, const float curve2,
const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period) ActuatorDesiredData *desired, const float period)
{ {
static float lastFilteredResult[MAX_MIX_ACTUATORS]; static float lastFilteredResult[MAX_MIX_ACTUATORS];
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
const Mixer_t *mixer = &mixers[index]; const Mixer_t *mixer = &mixers[index];
float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) + float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
@ -495,18 +515,18 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
// feed forward // feed forward
float accumulator = filterAccumulator[index]; float accumulator = filterAccumulator[index];
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; accumulator += (result - lastResult[index]) * mixerSettings.FeedForward;
lastResult[index] = result; lastResult[index] = result;
result += accumulator; result += accumulator;
if (period > 0.0f) { if (period > 0.0f) {
if (accumulator > 0.0f) { if (accumulator > 0.0f) {
float invFilter = period / mixerSettings->AccelTime; float invFilter = period / mixerSettings.AccelTime;
if (invFilter > 1) { if (invFilter > 1) {
invFilter = 1; invFilter = 1;
} }
accumulator -= accumulator * invFilter; accumulator -= accumulator * invFilter;
} else { } else {
float invFilter = period / mixerSettings->DecelTime; float invFilter = period / mixerSettings.DecelTime;
if (invFilter > 1) { if (invFilter > 1) {
invFilter = 1; invFilter = 1;
} }
@ -518,7 +538,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
// acceleration limit // acceleration limit
float dt = result - lastFilteredResult[index]; float dt = result - lastFilteredResult[index];
float maxDt = mixerSettings->MaxAccel * period; float maxDt = mixerSettings.MaxAccel * period;
if (dt > maxDt) { // we are accelerating too hard if (dt > maxDt) { // we are accelerating too hard
result = lastFilteredResult[index] + maxDt; result = lastFilteredResult[index] + maxDt;
} }
@ -530,17 +550,23 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
/** /**
* Interpolate a throttle curve. Throttle input should be in the range 0 to 1. * Interpolate a throttle curve
* Output is in the range 0 to 1. * 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 MixerCurve(const float throttle, const float *curve, uint8_t elements) static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements)
{ {
float scale = throttle * (float)(elements - 1); float abs_input = fabsf(input);
int idx1 = scale; float scale = abs_input * (float)(elements - 1);
int idx1 = scale;
scale -= (float)idx1; // remainder scale -= (float)idx1; // remainder
if (curve[0] < -1) { if (curve[0] < -1) {
return throttle; return input;
} }
if (idx1 < 0) { if (idx1 < 0) {
idx1 = 0; // clamp to lowest entry in table idx1 = 0; // clamp to lowest entry in table
@ -553,7 +579,48 @@ static float MixerCurve(const float throttle, const float *curve, uint8_t elemen
idx1 = elements - 1; idx1 = elements - 1;
} }
} }
return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
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;
} }
@ -593,21 +660,22 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
/** /**
* Set actuator output to the neutral values (failsafe) * Set actuator output to the neutral values (failsafe)
*/ */
static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings) static void setFailsafe()
{ {
/* grab only the parts that we are going to use */ /* grab only the parts that we are going to use */
int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM];
ActuatorCommandChannelGet(Channel); ActuatorCommandChannelGet(Channel);
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
// Reset ActuatorCommand to safe values // Reset ActuatorCommand to safe values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
Channel[n] = actuatorSettings->ChannelMin[n]; Channel[n] = actuatorSettings.ChannelMin[n];
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
Channel[n] = actuatorSettings->ChannelNeutral[n]; // reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point
Channel[n] = actuatorSettings.ChannelNeutral[n];
} else { } else {
Channel[n] = 0; Channel[n] = 0;
} }
@ -618,7 +686,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
// Update servo outputs // Update servo outputs
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
set_channel(n, Channel[n], actuatorSettings); set_channel(n, Channel[n]);
} }
// Send the updated command // Send the updated command
PIOS_Servo_Update(); PIOS_Servo_Update();
@ -713,39 +781,39 @@ static inline bool buzzerState(buzzertype type)
#if defined(ARCH_POSIX) || defined(ARCH_WIN32) #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) static bool set_channel(uint8_t mixer_channel, uint16_t value)
{ {
return true; return true;
} }
#else #else
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings) static bool set_channel(uint8_t mixer_channel, uint16_t value)
{ {
switch (actuatorSettings->ChannelType[mixer_channel]) { switch (actuatorSettings.ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
return true; return true;
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED: case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
return true; return true;
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED: case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]); buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
return true; return true;
case ACTUATORSETTINGS_CHANNELTYPE_PWM: case ACTUATORSETTINGS_CHANNELTYPE_PWM:
{ {
uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]]; uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]];
switch (mode) { switch (mode) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125: case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
// Remap 1000-2000 range to 125-250 // Remap 1000-2000 range to 125-250
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE); PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
break; break;
default: default:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
break; break;
} }
return true; return true;
@ -770,12 +838,12 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
/** /**
* @brief Update the servo update rate * @brief Update the servo update rate
*/ */
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update) static void actuator_update_rate_if_changed(bool force_update)
{ {
static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM]; static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings->BankMode, sizeof(prevBankMode)) != 0); bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings->BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0); bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
// check if any setting is changed // check if any setting is changed
if (updateMode || updateFreq) { if (updateMode || updateFreq) {
@ -784,15 +852,15 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM]; uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 }; uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
if (force_update || (actuatorSettings->BankMode[i] != prevBankMode[i])) { if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
PIOS_Servo_SetBankMode(i, PIOS_Servo_SetBankMode(i,
actuatorSettings->BankMode[i] == actuatorSettings.BankMode[i] ==
ACTUATORSETTINGS_BANKMODE_PWM ? ACTUATORSETTINGS_BANKMODE_PWM ?
PIOS_SERVO_BANK_MODE_PWM : PIOS_SERVO_BANK_MODE_PWM :
PIOS_SERVO_BANK_MODE_SINGLE_PULSE PIOS_SERVO_BANK_MODE_SINGLE_PULSE
); );
} }
switch (actuatorSettings->BankMode[i]) { switch (actuatorSettings.BankMode[i]) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125: case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
@ -802,37 +870,73 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
clock[i] = ACTUATOR_PWM_CLOCK; clock[i] = ACTUATOR_PWM_CLOCK;
break; break;
default: // PWM default: // PWM
freq[i] = actuatorSettings->BankUpdateFreq[i]; freq[i] = actuatorSettings.BankUpdateFreq[i];
clock[i] = ACTUATOR_PWM_CLOCK; clock[i] = ACTUATOR_PWM_CLOCK;
break; break;
} }
} }
memcpy(prevBankMode, memcpy(prevBankMode,
actuatorSettings->BankMode, actuatorSettings.BankMode,
sizeof(prevBankMode)); sizeof(prevBankMode));
PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM); PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
memcpy(prevBankUpdateFreq, memcpy(prevBankUpdateFreq,
actuatorSettings->BankUpdateFreq, actuatorSettings.BankUpdateFreq,
sizeof(prevBankUpdateFreq)); sizeof(prevBankUpdateFreq));
// retrieve mode from related bank // retrieve mode from related bank
for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) { for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
uint8_t bank = PIOS_Servo_GetPinBank(i); uint8_t bank = PIOS_Servo_GetPinBank(i);
pinsMode[i] = actuatorSettings->BankMode[bank]; pinsMode[i] = actuatorSettings.BankMode[bank];
} }
} }
} }
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
actuator_settings_updated = true; 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) static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
mixer_settings_updated = true; 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)
{
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
frameType = GetCurrentFrameType();
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;
}
}
SystemSettingsThrustControlGet(&thrustType);
} }
/** /**

9
flight/modules/ManualControl/armhandler.c
View File

@ -62,7 +62,7 @@ static bool forcedDisArm(void);
* @input: ManualControlCommand, AccessoryDesired * @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming * @output: FlightStatus.Arming
*/ */
void armHandler(bool newinit) void armHandler(bool newinit, FrameType_t frameType)
{ {
static ArmState_t armState; static ArmState_t armState;
@ -82,7 +82,12 @@ void armHandler(bool newinit)
bool lowThrottle = cmd.Throttle < 0; bool lowThrottle = cmd.Throttle < 0;
bool armSwitch = false; if (frameType == FRAME_TYPE_GROUND) {
// Deadbanding appled in receiver.c at 2%
lowThrottle = fabsf(cmd.Throttle) < 0.01f;
}
bool armSwitch = false;
switch (settings.Arming) { switch (settings.Arming) {
case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:

3
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -32,6 +32,7 @@
#include <openpilot.h> #include <openpilot.h>
#include <flightstatus.h> #include <flightstatus.h>
#include <sanitycheck.h>
typedef void (*handlerFunc)(bool newinit); typedef void (*handlerFunc)(bool newinit);
@ -45,7 +46,7 @@ typedef struct controlHandlerStruct {
* @input: ManualControlCommand, AccessoryDesired * @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming * @output: FlightStatus.Arming
*/ */
void armHandler(bool newinit); void armHandler(bool newinit, FrameType_t frameType);
/** /**
* @brief Handler to control Manual flightmode - input directly steers actuators * @brief Handler to control Manual flightmode - input directly steers actuators

34
flight/modules/ManualControl/manualcontrol.c
View File

@ -104,6 +104,7 @@ static const controlHandler handler_PATHPLANNER = {
static float thrustAtBrakeStart = 0.0f; static float thrustAtBrakeStart = 0.0f;
static float thrustLo = 0.0f; static float thrustLo = 0.0f;
static float thrustHi = 0.0f; static float thrustHi = 0.0f;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// Private variables // Private variables
@ -115,6 +116,7 @@ static void commandUpdatedCb(UAVObjEvent *ev);
static void manualControlTask(void); static void manualControlTask(void);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings); static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
static void SettingsUpdatedCb(UAVObjEvent *ev);
#endif #endif
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
@ -135,11 +137,14 @@ int32_t ManualControlStart()
// clear alarms // clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
SettingsUpdatedCb(NULL);
// Make sure unarmed on power up // Make sure unarmed on power up
armHandler(true); armHandler(true, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandlerInit(); takeOffLocationHandlerInit();
#endif #endif
// Start main task // Start main task
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
@ -164,6 +169,8 @@ int32_t ManualControlInitialize()
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolSelfTuningStatsInitialize(); VtolSelfTuningStatsInitialize();
VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
#endif #endif
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES); callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
@ -171,13 +178,36 @@ int32_t ManualControlInitialize()
} }
MODULE_INITCALL(ManualControlInitialize, ManualControlStart); MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
frameType = GetCurrentFrameType();
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;
}
}
}
/** /**
* Module task * Module task
*/ */
static void manualControlTask(void) static void manualControlTask(void)
{ {
// Process Arming // Process Arming
armHandler(false); armHandler(false, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandler(); takeOffLocationHandler();
#endif #endif

96
flight/modules/Receiver/receiver.c
View File

@ -44,7 +44,9 @@
#endif #endif
#include <flightmodesettings.h> #include <flightmodesettings.h>
#include <systemsettings.h> #include <systemsettings.h>
#include <vtolpathfollowersettings.h>
#include <taskinfo.h> #include <taskinfo.h>
#include <sanitycheck.h>
#if defined(PIOS_INCLUDE_USB_RCTX) #if defined(PIOS_INCLUDE_USB_RCTX)
@ -72,6 +74,7 @@
// Private variables // Private variables
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static portTickType lastSysTime; static portTickType lastSysTime;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF; static portTickType lastSysTimeLPF;
@ -84,6 +87,7 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool validInputRange(int16_t min, int16_t max, uint16_t value); static bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband); static void applyDeadband(float *value, float deadband);
static void SettingsUpdatedCb(UAVObjEvent *ev);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position); static uint8_t isAssistedFlightMode(uint8_t position);
@ -124,6 +128,7 @@ int32_t ReceiverStart()
#ifdef PIOS_INCLUDE_WDG #ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif #endif
SettingsUpdatedCb(NULL);
return 0; return 0;
} }
@ -142,12 +147,40 @@ int32_t ReceiverInitialize()
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
StabilizationSettingsInitialize(); StabilizationSettingsInitialize();
#endif #endif
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsInitialize();
SystemSettingsConnectCallback(&SettingsUpdatedCb);
return 0; return 0;
} }
MODULE_INITCALL(ReceiverInitialize, ReceiverStart); MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
frameType = GetCurrentFrameType();
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;
}
}
}
/** /**
* Module task * Module task
*/ */
@ -243,22 +276,16 @@ static void receiverTask(__attribute__((unused)) void *parameters)
} }
} }
// Check settings, if error raise alarm // Sanity Check Throttle and Yaw
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| ||
// Check all channel mappings are valid // Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
|| ||
// Check the driver exists // Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER
|| ||
// Check collective if required // Check collective if required
@ -282,15 +309,39 @@ static void receiverTask(__attribute__((unused)) void *parameters)
continue; continue;
} }
if (frameType != FRAME_TYPE_GROUND) {
// Sanity Check Pitch and Roll
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
}
// decide if we have valid manual input or not // decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw, && validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]);
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); if (frameType != FRAME_TYPE_GROUND) {
valid_input_detected &= validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
}
if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Collective, valid_input_detected &= validInputRange(settings.ChannelMin.Collective,
@ -321,10 +372,14 @@ static void receiverTask(__attribute__((unused)) void *parameters)
} }
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = settings.FailsafeChannel.Throttle; if (frameType != FRAME_TYPE_GROUND) {
cmd.Roll = settings.FailsafeChannel.Roll; cmd.Throttle = settings.FailsafeChannel.Throttle;
cmd.Pitch = settings.FailsafeChannel.Pitch; } else {
cmd.Yaw = settings.FailsafeChannel.Yaw; cmd.Throttle = 0.0f;
}
cmd.Roll = settings.FailsafeChannel.Roll;
cmd.Pitch = settings.FailsafeChannel.Pitch;
cmd.Yaw = settings.FailsafeChannel.Yaw;
cmd.Collective = settings.FailsafeChannel.Collective; cmd.Collective = settings.FailsafeChannel.Collective;
switch (thrustType) { switch (thrustType) {
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
@ -401,6 +456,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
applyDeadband(&cmd.Roll, deadband_checked); applyDeadband(&cmd.Roll, deadband_checked);
applyDeadband(&cmd.Pitch, deadband_checked); applyDeadband(&cmd.Pitch, deadband_checked);
applyDeadband(&cmd.Yaw, deadband_checked); applyDeadband(&cmd.Yaw, deadband_checked);
if (frameType == FRAME_TYPE_GROUND) { // assumes reversible motors
applyDeadband(&cmd.Throttle, deadband_checked);
}
} }
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms // Apply Low Pass Filter to input channels, time delta between calls in ms