From 6806e895cc611153873326bf6924c9258e2c1efe Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 30 Jan 2013 18:57:32 -0500 Subject: [PATCH] First version. Passes simple tests. --- flight/Modules/Actuator/actuator.c | 16 ++++--- flight/Modules/CameraStab/camerastab.c | 45 +++++++++++++++++-- .../src/plugins/config/camerastabilization.ui | 4 +- .../configcamerastabilizationwidget.cpp | 8 ++-- shared/uavobjectdefinition/cameradesired.xml | 4 +- .../camerastabsettings.xml | 4 +- shared/uavobjectdefinition/mixersettings.xml | 4 +- 7 files changed, 65 insertions(+), 20 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 06d217770..f63c829b4 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -206,7 +206,11 @@ static void actuatorTask(void* parameters) // Check how long since last update thisSysTime = xTaskGetTickCount(); - if(thisSysTime > lastSysTime) // reuse dt in case of wraparound + // reuse dt in case of wraparound + // todo (theothercliff thinks this might be needed): + // if dT actually matters... + // fix it to know max value and subtract for currently correct dT on wrap + if(thisSysTime > lastSysTime) dT = (thisSysTime - lastSysTime) * (portTICK_RATE_MS * 0.001f); lastSysTime = thisSysTime; @@ -328,17 +332,17 @@ static void actuatorTask(void* parameters) status[ct] = -1; } - if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) && + 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_CAMERAROLL: - status[ct] = cameraDesired.Roll; + case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1: + status[ct] = cameraDesired.RollOrServo1; break; - case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCH: - status[ct] = cameraDesired.Pitch; + case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCHORSERVO2: + status[ct] = cameraDesired.PitchOrServo2; break; case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: status[ct] = cameraDesired.Yaw; diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index 23d283e9c..c2eeea647 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -161,6 +161,12 @@ static void attitudeUpdated(UAVObjEvent* ev) (float)SAMPLE_PERIOD_MS; csd->lastSysTime = thisSysTime; + // storage for elevon roll component before the pitch component has been generated + // we are guaranteed that the iteration order of i is roll pitch yaw + // that guarnteees this won't be used uninited, but the compiler doesn't know that + // so we init it or turn the warning/error off for each compiler + float elevon_roll = 0.0; + // process axes for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { @@ -212,14 +218,47 @@ static void attitudeUpdated(UAVObjEvent* ev) applyFeedForward(i, dT_millis, &attitude, &cameraStab); #endif - // set output channels + // bounding for elevon mixing occurs on the unmixed output + // to limit the range of the mixed output you must limit the range + // of both the unmixed pitch and unmixed roll float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); + + // set output channels switch (i) { case CAMERASTABSETTINGS_INPUT_ROLL: - CameraDesiredRollSet(&output); + // we are guaranteed that the iteration order of i is roll pitch yaw + // for elevon mixing we simply grab the value for later use + if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ELEVONSSGYAWPITCHROLL) { + elevon_roll = output; + } + else { + CameraDesiredRollOrServo1Set(&output); + } break; case CAMERASTABSETTINGS_INPUT_PITCH: - CameraDesiredPitchSet(&output); + // we are guaranteed that the iteration order of i is roll pitch yaw + // for elevon mixing we use the value we previously grabbed and set both s1 and s2 + if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ELEVONSSGYAWPITCHROLL) { + float elevon_pitch; + elevon_pitch = output; + output = (elevon_pitch + elevon_roll) / 2; + CameraDesiredRollOrServo1Set(&output); + // elevon reversing works like this: + // first use the normal reversing facilities to get servo 1 working in the correct direction + // for both roll and pitch + // then use the new reversing switches to reverse servo 2 roll and/or pitch as needed + if (cameraStab.ElevonSSGServo2RollReverse == CAMERASTABSETTINGS_ELEVONSSGSERVO2ROLLREVERSE_TRUE) { + elevon_roll = 1.0 - elevon_roll; + } + if (cameraStab.ElevonSSGServo2PitchReverse == CAMERASTABSETTINGS_ELEVONSSGSERVO2PITCHREVERSE_TRUE) { + elevon_pitch = 1.0 - elevon_pitch; + } + output = (elevon_pitch - elevon_roll) / 2; + CameraDesiredPitchOrServo2Set(&output); + } + else { + CameraDesiredPitchOrServo2Set(&output); + } break; case CAMERASTABSETTINGS_INPUT_YAW: CameraDesiredYawSet(&output); diff --git a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui index 91d6e15b7..3b6226c85 100755 --- a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui @@ -340,7 +340,7 @@ font: bold 12px; margin:1px; - Pitch + Pitch Or Servo2 Qt::AlignCenter @@ -369,7 +369,7 @@ font: bold 12px; margin:1px; - Roll + Roll Or Servo1 Qt::AlignCenter diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index 06d742d28..621c5b032 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -155,7 +155,7 @@ void ConfigCameraStabilizationWidget::refreshWidgetsValues(UAVObject *obj) // Then search for any mixer channels set to this outputs[i]->setCurrentIndex(0); for (int j = 0; j < NUM_MIXERS; j++) - if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i) && + if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i) && outputs[i]->currentIndex() != (j + 1)) outputs[i]->setCurrentIndex(j + 1); } @@ -219,7 +219,7 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() if ((mixerNum >= 0) && // Short circuit in case of none (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED) && - (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLL + i) ) { + (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i) ) { // If the mixer channel already mapped to something, it should not be // used for camera output, we reset it to none outputs[i]->setCurrentIndex(0); @@ -230,13 +230,13 @@ void ConfigCameraStabilizationWidget::updateObjectsFromWidgets() } else { // Make sure no other channels have this output set for (int j = 0; j < NUM_MIXERS; j++) - if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i)) + if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i)) *mixerTypes[j] = MixerSettings::MIXER1TYPE_DISABLED; // If this channel is assigned to one of the outputs that is not disabled // set it if ((mixerNum >= 0) && (mixerNum < NUM_MIXERS)) - *mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLL + i; + *mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLLORSERVO1 + i; } } } while(widgetUpdated); diff --git a/shared/uavobjectdefinition/cameradesired.xml b/shared/uavobjectdefinition/cameradesired.xml index 7d9cdbe64..c842edf2a 100644 --- a/shared/uavobjectdefinition/cameradesired.xml +++ b/shared/uavobjectdefinition/cameradesired.xml @@ -1,8 +1,8 @@ Desired camera outputs. Comes from @ref CameraStabilization module. - - + + diff --git a/shared/uavobjectdefinition/camerastabsettings.xml b/shared/uavobjectdefinition/camerastabsettings.xml index 478c74269..1fd8889c7 100644 --- a/shared/uavobjectdefinition/camerastabsettings.xml +++ b/shared/uavobjectdefinition/camerastabsettings.xml @@ -8,11 +8,13 @@ - + + + diff --git a/shared/uavobjectdefinition/mixersettings.xml b/shared/uavobjectdefinition/mixersettings.xml index ffbc067f9..f5a114f95 100644 --- a/shared/uavobjectdefinition/mixersettings.xml +++ b/shared/uavobjectdefinition/mixersettings.xml @@ -27,8 +27,8 @@ - - + +