diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 6bd471666..7f929b3fa 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -57,6 +57,8 @@ #define FAILSAFE_TIMEOUT_MS 100 #define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM +#define CAMERA_BOOT_DELAY_MS 7000 + // Private types @@ -205,7 +207,7 @@ static void actuatorTask(void* parameters) // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound - dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; + dT = (thisSysTime - lastSysTime) * (portTICK_RATE_MS * 0.001f); lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); @@ -277,10 +279,15 @@ static void actuatorTask(void* parameters) for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) { + // During boot all camera actuators should be completely disabled (PWM pulse = 0). + // command.Channel[i] is reused below as a channel PWM activity flag: + // 0 - PWM disabled, >0 - PWM set to real mixer value using scaleChannel() later. + // Setting it to 1 by default means "Rescale this channel and enable PWM on its output". + command.Channel[ct] = 1; + if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { // Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us status[ct] = -1; - command.Channel[ct] = 0; continue; } @@ -289,8 +296,6 @@ static void actuatorTask(void* parameters) else status[ct] = -1; - - // Motors have additional protection for when to be on if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { @@ -322,6 +327,7 @@ static void actuatorTask(void* parameters) else status[ct] = -1; } + if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) && (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) { @@ -343,19 +349,26 @@ static void actuatorTask(void* parameters) } 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; } } - - for(int i = 0; i < MAX_MIX_ACTUATORS; i++) - command.Channel[i] = scaleChannel(status[i], + + // Set real actuator output values scaling them from mixers. All channels + // will be set except explicitly disabled (which will have PWM pulse = 0). + for (int i = 0; i < MAX_MIX_ACTUATORS; i++) + if (command.Channel[i]) + command.Channel[i] = scaleChannel(status[i], actuatorSettings.ChannelMax[i], actuatorSettings.ChannelMin[i], actuatorSettings.ChannelNeutral[i]); - + // Store update time - command.UpdateTime = 1000.0f*dT; - if(1000.0f*dT > command.MaxUpdateTime) - command.MaxUpdateTime = 1000.0f*dT; + command.UpdateTime = dT * 1000.0f; + if (command.UpdateTime > command.MaxUpdateTime) + command.MaxUpdateTime = command.UpdateTime; // Update output object ActuatorCommandSet(&command); diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index 784b1d481..8e4d2e828 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -57,7 +57,6 @@ // Configuration // #define SAMPLE_PERIOD_MS 10 -#define BOOT_DELAY 7000 // Private types @@ -213,23 +212,20 @@ static void attitudeUpdated(UAVObjEvent* ev) applyFeedForward(i, dT, &attitude, &cameraStab); #endif - // set output channels (but wait BOOT_DELAY after board reset) + // set output channels float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); - if (thisSysTime > (portTICK_RATE_MS * BOOT_DELAY )) { - - switch (i) { - case CAMERASTABSETTINGS_INPUT_ROLL: - CameraDesiredRollSet(&output); - break; - case CAMERASTABSETTINGS_INPUT_PITCH: - CameraDesiredPitchSet(&output); - break; - case CAMERASTABSETTINGS_INPUT_YAW: - CameraDesiredYawSet(&output); - break; - default: - PIOS_Assert(0); - } + switch (i) { + case CAMERASTABSETTINGS_INPUT_ROLL: + CameraDesiredRollSet(&output); + break; + case CAMERASTABSETTINGS_INPUT_PITCH: + CameraDesiredPitchSet(&output); + break; + case CAMERASTABSETTINGS_INPUT_YAW: + CameraDesiredYawSet(&output); + break; + default: + PIOS_Assert(0); } } } diff --git a/shared/uavobjectdefinition/actuatorcommand.xml b/shared/uavobjectdefinition/actuatorcommand.xml index a28b70df4..17a7b7af5 100644 --- a/shared/uavobjectdefinition/actuatorcommand.xml +++ b/shared/uavobjectdefinition/actuatorcommand.xml @@ -2,7 +2,7 @@ Contains the pulse duration sent to each of the channels. Set by @ref ActuatorModule - +