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

Merge branch 'abeck/OP-1769-Ground' into next

Browse Source
This commit is contained in:
abeck70 2015-03-14 12:07:46 +11:00
commit 50cbc0cdb3
8 changed files with 392 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,6 +45,11 @@
#include "cameradesired.h" #include "cameradesired.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "taskinfo.h" #include "taskinfo.h"
#include <systemsettings.h>
#include <sanitycheck.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <vtolpathfollowersettings.h>
#endif
#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 +81,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.
@ -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,13 @@ int32_t ActuatorInitialize()
MixerStatusInitialize(); MixerStatusInitialize();
#endif #endif
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
#endif
SystemSettingsInitialize();
SystemSettingsConnectCallback(&SettingsUpdatedCb);
return 0; return 0;
} }
MODULE_INITCALL(ActuatorInitialize, ActuatorStart); MODULE_INITCALL(ActuatorInitialize, ActuatorStart);
@ -178,7 +200,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 +207,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 +231,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 +247,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) {
@ -258,12 +264,15 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
} }
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool activeThrottle = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
bool positiveThrottle = (throttleDesired > 0.00f);
// 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,42 @@ 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 unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements);
int idx1 = 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 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 +598,9 @@ 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;
return unsigned_value;
} }
@ -593,21 +640,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 +666,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 +761,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 +818,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 +832,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 +850,74 @@ 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)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
SystemSettingsThrustControlGet(&thrustType);
} }
/** /**

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

@ -38,6 +38,7 @@
// Private constants // Private constants
#define ARMED_THRESHOLD 0.50f #define ARMED_THRESHOLD 0.50f
#define GROUND_LOW_THROTTLE 0.01f
// Private types // Private types
typedef enum { typedef enum {
@ -62,7 +63,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 +83,12 @@ void armHandler(bool newinit)
bool lowThrottle = cmd.Throttle < 0; bool lowThrottle = cmd.Throttle < 0;
bool armSwitch = false; if (frameType == FRAME_TYPE_GROUND) {
// Deadbanding applied in receiver.c typically at 2% but we don't assume its enabled.
lowThrottle = fabsf(cmd.Throttle) < GROUND_LOW_THROTTLE;
}
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

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

@ -108,6 +108,7 @@ static float thrustHi = 0.0f;
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// Private variables // Private variables
static DelayedCallbackInfo *callbackHandle; static DelayedCallbackInfo *callbackHandle;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
// Private functions // Private functions
static void configurationUpdatedCb(UAVObjEvent *ev); static void configurationUpdatedCb(UAVObjEvent *ev);
@ -116,6 +117,7 @@ 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);
#endif #endif
static void SettingsUpdatedCb(UAVObjEvent *ev);
#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,39 @@ int32_t ManualControlInitialize()
} }
MODULE_INITCALL(ManualControlInitialize, ManualControlStart); MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
}
/** /**
* 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

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

@ -41,10 +41,12 @@
#include <flighttelemetrystats.h> #include <flighttelemetrystats.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <stabilizationsettings.h> #include <stabilizationsettings.h>
#include <vtolpathfollowersettings.h>
#endif #endif
#include <flightmodesettings.h> #include <flightmodesettings.h>
#include <systemsettings.h> #include <systemsettings.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;
} }
@ -141,13 +146,45 @@ int32_t ReceiverInitialize()
ManualControlSettingsInitialize(); ManualControlSettingsInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
StabilizationSettingsInitialize(); StabilizationSettingsInitialize();
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
#endif #endif
SystemSettingsInitialize();
SystemSettingsConnectCallback(&SettingsUpdatedCb);
return 0; return 0;
} }
MODULE_INITCALL(ReceiverInitialize, ReceiverStart); MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
}
/** /**
* Module task * Module task
*/ */
@ -243,22 +280,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 +313,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 +376,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 +460,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

54
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp
View File

@ -116,7 +116,6 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true); m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true);
m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true); m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true);
// Default Curve2 range -1 -> +1, allow forward/reverse (Car and Tank)
m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE);
m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH);
@ -145,8 +144,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
m_aircraft->differentialSteeringSlider1->setEnabled(true); m_aircraft->differentialSteeringSlider1->setEnabled(true);
m_aircraft->differentialSteeringSlider2->setEnabled(true); m_aircraft->differentialSteeringSlider2->setEnabled(true);
m_aircraft->gvThrottleCurve1GroupBox->setTitle(""); m_aircraft->gvThrottleCurve1GroupBox->setTitle("Throttle curve1");
m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve"); m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve2 ");
m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH);
m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE);
@ -154,13 +153,11 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
initMixerCurves(frameType); initMixerCurves(frameType);
// If new setup, set sliders to defaults and set curves values // If new setup, set sliders to defaults and set curves values
// Allow forward/reverse 0.8 / -0.8 for Throttle, keep some room
// to allow rotate at full throttle and heading stabilization
if (frameTypeSaved->getValue().toString() != "GroundVehicleDifferential") { if (frameTypeSaved->getValue().toString() != "GroundVehicleDifferential") {
m_aircraft->differentialSteeringSlider1->setValue(100); m_aircraft->differentialSteeringSlider1->setValue(100);
m_aircraft->differentialSteeringSlider2->setValue(100); m_aircraft->differentialSteeringSlider2->setValue(100);
m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0); m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0);
m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 0.8, -0.8); m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0);
} }
} else if (frameType == "GroundVehicleMotorcycle" || frameType == "Motorcycle") { } else if (frameType == "GroundVehicleMotorcycle" || frameType == "Motorcycle") {
// Motorcycle // Motorcycle
@ -179,12 +176,11 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
m_aircraft->gvSteering2Label->setText("Balancing"); m_aircraft->gvSteering2Label->setText("Balancing");
// Curve1 for Motorcyle // Curve1 for Motorcyle
m_aircraft->gvThrottleCurve1GroupBox->setTitle("Rear throttle curve"); m_aircraft->gvThrottleCurve1GroupBox->setTitle("Throttle curve1");
m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true); m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true);
m_aircraft->gvThrottleCurve2GroupBox->setTitle(""); m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve2");
m_aircraft->gvThrottleCurve2GroupBox->setEnabled(false); m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true);
// Curve range 0 -> +1 (no reverse)
m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_THROTTLE);
m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE);
@ -192,8 +188,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
// If new setup, set curves values // If new setup, set curves values
if (frameTypeSaved->getValue().toString() != "GroundVehicleMotorCycle") { if (frameTypeSaved->getValue().toString() != "GroundVehicleMotorCycle") {
m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0); m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0);
m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0); m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0);
} }
} else { } else {
// Car // Car
@ -212,11 +208,10 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
m_aircraft->gvSteering1Label->setText("Front steering"); m_aircraft->gvSteering1Label->setText("Front steering");
m_aircraft->gvSteering2Label->setText("Rear steering"); m_aircraft->gvSteering2Label->setText("Rear steering");
// Curve2 for Car m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve2");
m_aircraft->gvThrottleCurve2GroupBox->setTitle("Throttle curve");
m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true); m_aircraft->gvThrottleCurve2GroupBox->setEnabled(true);
m_aircraft->gvThrottleCurve1GroupBox->setTitle(""); m_aircraft->gvThrottleCurve1GroupBox->setTitle("Throttle curve1");
m_aircraft->gvThrottleCurve1GroupBox->setEnabled(false); m_aircraft->gvThrottleCurve1GroupBox->setEnabled(true);
m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH); m_aircraft->groundVehicleThrottle2->setMixerType(MixerCurve::MIXERCURVE_PITCH);
m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE); m_aircraft->groundVehicleThrottle1->setMixerType(MixerCurve::MIXERCURVE_THROTTLE);
@ -225,11 +220,8 @@ void ConfigGroundVehicleWidget::setupUI(QString frameType)
// If new setup, set curves values // If new setup, set curves values
if (frameTypeSaved->getValue().toString() != "GroundVehicleCar") { if (frameTypeSaved->getValue().toString() != "GroundVehicleCar") {
m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0); m_aircraft->groundVehicleThrottle1->initLinearCurve(5, 1.0, 0.0);
// Set curve2 range from -0.926 to 1 (forward / reverse) m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, 0.0);
// Take in account 4% offset in Throttle input after calibration
// 0.5 / 0.54 = 0.926
m_aircraft->groundVehicleThrottle2->initLinearCurve(5, 1.0, -0.926);
} }
} }
@ -325,7 +317,13 @@ void ConfigGroundVehicleWidget::initMixerCurves(QString frameType)
m_aircraft->groundVehicleThrottle1->initCurve(&curveValues); m_aircraft->groundVehicleThrottle1->initCurve(&curveValues);
} else { } else {
// no, init a straight curve // no, init a straight curve
m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 1.0); if (frameType == "GroundVehicleDifferential") {
m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 0.8, 0.0);
} else if (frameType == "GroundVehicleCar") {
m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 1.0, 0.0);
} else {
m_aircraft->groundVehicleThrottle1->initLinearCurve(curveValues.count(), 1.0, 0.0);
}
} }
// Setup all Throttle2 curves for all types of airframes // Setup all Throttle2 curves for all types of airframes
@ -336,11 +334,11 @@ void ConfigGroundVehicleWidget::initMixerCurves(QString frameType)
} else { } else {
// no, init a straight curve // no, init a straight curve
if (frameType == "GroundVehicleDifferential") { if (frameType == "GroundVehicleDifferential") {
m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 0.8, -0.8); m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 0.8, 0.0);
} else if (frameType == "GroundVehicleCar") { } else if (frameType == "GroundVehicleCar") {
m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0, -1.0); m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0, 0.0);
} else { } else {
m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0); m_aircraft->groundVehicleThrottle2->initLinearCurve(curveValues.count(), 1.0, 0.0);
} }
} }
} }
@ -513,11 +511,11 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType)
channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1; channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1; channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1;
setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR);
setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127);
// Output success message // Output success message
m_aircraft->gvStatusLabel->setText("Mixer generated"); m_aircraft->gvStatusLabel->setText("Mixer generated");

20
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -779,17 +779,15 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch
mSettings->setMixerValueRoll(100); mSettings->setMixerValueRoll(100);
mSettings->setMixerValuePitch(100); mSettings->setMixerValuePitch(100);
mSettings->setMixerValueYaw(100); mSettings->setMixerValueYaw(100);
// Set curve2 range from -0.926 to 1 : take in account 4% offset in Throttle input
// 0.5 / 0.54 = 0.926
maxThrottle = 1; maxThrottle = 1;
minThrottle = -0.926; minThrottle = 0;
break; break;
case VehicleConfigurationSource::GROUNDVEHICLE_DIFFERENTIAL: case VehicleConfigurationSource::GROUNDVEHICLE_DIFFERENTIAL:
mSettings->setMixerValueRoll(100); mSettings->setMixerValueRoll(100);
mSettings->setMixerValuePitch(100); mSettings->setMixerValuePitch(100);
mSettings->setMixerValueYaw(100); mSettings->setMixerValueYaw(100);
maxThrottle = 0.8; maxThrottle = 0.8;
minThrottle = -0.8; minThrottle = 0;
break; break;
default: default:
break; break;
@ -2076,8 +2074,8 @@ void VehicleConfigurationHelper::setupCar()
// Motor (Chan 2) // Motor (Chan 2)
channels[1].type = MIXER_TYPE_REVERSABLEMOTOR; channels[1].type = MIXER_TYPE_REVERSABLEMOTOR;
channels[1].throttle1 = 0; channels[1].throttle1 = 100;
channels[1].throttle2 = 100; channels[1].throttle2 = 0;
channels[1].roll = 0; channels[1].roll = 0;
channels[1].pitch = 0; channels[1].pitch = 0;
channels[1].yaw = 0; channels[1].yaw = 0;
@ -2101,16 +2099,16 @@ void VehicleConfigurationHelper::setupTank()
// Left Motor (Chan 1) // Left Motor (Chan 1)
channels[0].type = MIXER_TYPE_REVERSABLEMOTOR; channels[0].type = MIXER_TYPE_REVERSABLEMOTOR;
channels[0].throttle1 = 0; channels[0].throttle1 = 100;
channels[0].throttle2 = 100; channels[0].throttle2 = 0;
channels[0].roll = 0; channels[0].roll = 0;
channels[0].pitch = 0; channels[0].pitch = 0;
channels[0].yaw = 100; channels[0].yaw = 100;
// Right Motor (Chan 2) // Right Motor (Chan 2)
channels[1].type = MIXER_TYPE_REVERSABLEMOTOR; channels[1].type = MIXER_TYPE_REVERSABLEMOTOR;
channels[1].throttle1 = 0; channels[1].throttle1 = 100;
channels[1].throttle2 = 100; channels[1].throttle2 = 0;
channels[1].roll = 0; channels[1].roll = 0;
channels[1].pitch = 0; channels[1].pitch = 0;
channels[1].yaw = -100; channels[1].yaw = -100;
@ -2140,7 +2138,7 @@ void VehicleConfigurationHelper::setupMotorcycle()
channels[0].pitch = 0; channels[0].pitch = 0;
channels[0].yaw = 100; channels[0].yaw = 100;
// Motor (Chan 2) : Curve1, no reverse // Motor (Chan 2)
channels[1].type = MIXER_TYPE_MOTOR; channels[1].type = MIXER_TYPE_MOTOR;
channels[1].throttle1 = 100; channels[1].throttle1 = 100;
channels[1].throttle2 = 0; channels[1].throttle2 = 0;

2
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -1,7 +1,7 @@
<xml> <xml>
<object name="VtolPathFollowerSettings" singleinstance="true" settings="true" category="Control"> <object name="VtolPathFollowerSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref VtolPathFollowerModule</description> <description>Settings for the @ref VtolPathFollowerModule</description>
<field name="TreatCustomCraftAs" units="switch" type="enum" elements="1" options="FixedWing,VTOL" defaultvalue="FixedWing"/> <field name="TreatCustomCraftAs" units="switch" type="enum" elements="1" options="FixedWing,VTOL,Ground" defaultvalue="FixedWing"/>
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="10.0" description="maximum allowed horizontal movement velocity"/> <field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="10.0" description="maximum allowed horizontal movement velocity"/>
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="4.0" description="maximum allowed climb/dive velocity"/> <field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="4.0" description="maximum allowed climb/dive velocity"/>
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/> <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/>