diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 465957138..147a590a4 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD USE_THUMB_MODE = YES # List of modules to include -MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude +MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization #MODULES = Telemetry Example #MODULES = Telemetry MK/MKSerial diff --git a/flight/OpenPilot/Modules/Stabilization/inc/stabilization.h b/flight/OpenPilot/Modules/Stabilization/inc/stabilization.h new file mode 100644 index 000000000..705f8a4d8 --- /dev/null +++ b/flight/OpenPilot/Modules/Stabilization/inc/stabilization.h @@ -0,0 +1,31 @@ +/** + ****************************************************************************** + * + * @file stabilization.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Attitude stabilization module. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef STABILIZATION_H +#define STABILIZATION_H + +int32_t StabilizationInitialize(); + +#endif // STABILIZATION_H diff --git a/flight/OpenPilot/Modules/Stabilization/stabilization.c b/flight/OpenPilot/Modules/Stabilization/stabilization.c new file mode 100644 index 000000000..0bf83448c --- /dev/null +++ b/flight/OpenPilot/Modules/Stabilization/stabilization.c @@ -0,0 +1,180 @@ +/** + ****************************************************************************** + * + * @file stabilization.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Attitude stabilization module. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "stabilization.h" +#include "stabilizationsettings.h" +#include "actuatordesired.h" +#include "attitudedesired.h" +#include "attitudeactual.h" +#include "manualcontrolcommand.h" +#include "systemsettings.h" + +// Private constants +#define STACK_SIZE configMINIMAL_STACK_SIZE +#define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define PITCH_INTEGRAL_LIMIT 0.5 +#define ROLL_INTEGRAL_LIMIT 0.5 +#define YAW_INTEGRAL_LIMIT 0.5 + +// Private types + +// Private variables +static xTaskHandle taskHandle; + +// Private functions +static void stabilizationTask(void* parameters); +static float bound(float val, float min, float max); + +/** + * Module initialization + */ +int32_t StabilizationInitialize() +{ + // Initialize variables + + // Start main task + xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); + + return 0; +} + +/** + * Module task + */ +static void stabilizationTask(void* parameters) +{ + StabilizationSettingsData stabSettings; + ActuatorDesiredData actuatorDesired; + AttitudeDesiredData attitudeDesired; + AttitudeActualData attitudeActual; + ManualControlCommandData manualControl; + SystemSettingsData systemSettings; + portTickType lastSysTime; + float pitchError, pitchErrorLast; + float rollError, rollErrorLast; + float yawError, yawErrorLast; + float pitchDerivative; + float rollDerivative; + float yawDerivative; + float pitchIntegral, pitchIntegralLimit; + float rollIntegral, rollIntegralLimit; + float yawIntegral, yawIntegralLimit; + + // Initialize + pitchIntegral = 0.0; + rollIntegral = 0.0; + yawIntegral = 0.0; + pitchErrorLast = 0.0; + rollErrorLast = 0.0; + yawErrorLast = 0.0; + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) + { + // Read settings and other objects + StabilizationSettingsGet(&stabSettings); + SystemSettingsGet(&systemSettings); + ManualControlCommandGet(&manualControl); + AttitudeDesiredGet(&attitudeDesired); + AttitudeActualGet(&attitudeActual); + + // Pitch stabilization control loop + pitchError = bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch; + pitchDerivative = pitchError - pitchErrorLast; + pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi; + pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit); + actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative; + actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0); + pitchErrorLast = pitchError; + + // Roll stabilization control loop + rollError = bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll; + rollDerivative = rollError - rollErrorLast; + rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi; + rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit); + actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative; + actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0); + rollErrorLast = rollError; + + // Yaw stabilization control loop (only enabled on VTOL airframes) + if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL ) + { + yawError = attitudeDesired.Yaw - attitudeActual.Yaw; + yawDerivative = yawError - yawErrorLast; + yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi; + yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit); + actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;; + actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0); + yawErrorLast = yawError; + } + else + { + actuatorDesired.Yaw = 0.0; + } + + // Setup throttle + actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax); + + // Write actuator desired (if not in manual mode) + if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL ) + { + ActuatorDesiredSet(&actuatorDesired); + } + else + { + pitchIntegral = 0.0; + rollIntegral = 0.0; + yawIntegral = 0.0; + pitchErrorLast = 0.0; + rollErrorLast = 0.0; + yawErrorLast = 0.0; + } + + // Clear alarms + AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); + + // Wait until next update + vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS ); + } +} + +/** + * Bound input value between limits + */ +static float bound(float val, float min, float max) +{ + if ( val < min ) + { + val = min; + } + else if ( val > max ) + { + val = max; + } + return val; +} diff --git a/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h b/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h index 5c943a2d0..354194ff6 100644 --- a/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h @@ -81,46 +81,46 @@ typedef struct { // Field information // Field FixedWingRoll1 information /* Enumeration options for field FixedWingRoll1 */ -typedef enum { ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL1=0, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL2=1, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL3=2, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL4=3, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL5=4, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL6=5, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL7=6, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL8=7, ACTUATORSETTINGS_FIXEDWINGROLL1_NONE=8, } ActuatorSettingsFixedWingRoll1Options; +typedef enum { ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGROLL1_NONE=8, } ActuatorSettingsFixedWingRoll1Options; // Field FixedWingRoll2 information /* Enumeration options for field FixedWingRoll2 */ -typedef enum { ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL1=0, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL2=1, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL3=2, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL4=3, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL5=4, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL6=5, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL7=6, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL8=7, ACTUATORSETTINGS_FIXEDWINGROLL2_NONE=8, } ActuatorSettingsFixedWingRoll2Options; +typedef enum { ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGROLL2_NONE=8, } ActuatorSettingsFixedWingRoll2Options; // Field FixedWingPitch1 information /* Enumeration options for field FixedWingPitch1 */ -typedef enum { ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL1=0, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL2=1, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL3=2, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL4=3, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL5=4, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL6=5, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL7=6, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL8=7, ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE=8, } ActuatorSettingsFixedWingPitch1Options; +typedef enum { ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE=8, } ActuatorSettingsFixedWingPitch1Options; // Field FixedWingPitch2 information /* Enumeration options for field FixedWingPitch2 */ -typedef enum { ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL1=0, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL2=1, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL3=2, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL4=3, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL5=4, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL6=5, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL7=6, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL8=7, ACTUATORSETTINGS_FIXEDWINGPITCH2_NONE=8, } ActuatorSettingsFixedWingPitch2Options; +typedef enum { ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGPITCH2_NONE=8, } ActuatorSettingsFixedWingPitch2Options; // Field FixedWingYaw information /* Enumeration options for field FixedWingYaw */ -typedef enum { ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL1=0, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL2=1, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL3=2, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL4=3, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL5=4, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL6=5, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL7=6, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL8=7, ACTUATORSETTINGS_FIXEDWINGYAW_NONE=8, } ActuatorSettingsFixedWingYawOptions; +typedef enum { ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGYAW_NONE=8, } ActuatorSettingsFixedWingYawOptions; // Field FixedWingThrottle information /* Enumeration options for field FixedWingThrottle */ -typedef enum { ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL1=0, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL2=1, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL3=2, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL4=3, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL5=4, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL6=5, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL7=6, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL8=7, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE=8, } ActuatorSettingsFixedWingThrottleOptions; +typedef enum { ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE=8, } ActuatorSettingsFixedWingThrottleOptions; // Field VTOLMotorN information /* Enumeration options for field VTOLMotorN */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORN_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORN_NONE=8, } ActuatorSettingsVTOLMotorNOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORN_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORN_NONE=8, } ActuatorSettingsVTOLMotorNOptions; // Field VTOLMotorNE information /* Enumeration options for field VTOLMotorNE */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORNE_NONE=8, } ActuatorSettingsVTOLMotorNEOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORNE_NONE=8, } ActuatorSettingsVTOLMotorNEOptions; // Field VTOLMotorE information /* Enumeration options for field VTOLMotorE */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORE_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORE_NONE=8, } ActuatorSettingsVTOLMotorEOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORE_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORE_NONE=8, } ActuatorSettingsVTOLMotorEOptions; // Field VTOLMotorSE information /* Enumeration options for field VTOLMotorSE */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORSE_NONE=8, } ActuatorSettingsVTOLMotorSEOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORSE_NONE=8, } ActuatorSettingsVTOLMotorSEOptions; // Field VTOLMotorS information /* Enumeration options for field VTOLMotorS */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORS_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORS_NONE=8, } ActuatorSettingsVTOLMotorSOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORS_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORS_NONE=8, } ActuatorSettingsVTOLMotorSOptions; // Field VTOLMotorSW information /* Enumeration options for field VTOLMotorSW */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORSW_NONE=8, } ActuatorSettingsVTOLMotorSWOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORSW_NONE=8, } ActuatorSettingsVTOLMotorSWOptions; // Field VTOLMotorW information /* Enumeration options for field VTOLMotorW */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORW_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORW_NONE=8, } ActuatorSettingsVTOLMotorWOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORW_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORW_NONE=8, } ActuatorSettingsVTOLMotorWOptions; // Field VTOLMotorNW information /* Enumeration options for field VTOLMotorNW */ -typedef enum { ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL1=0, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL2=1, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL3=2, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL4=3, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL5=4, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL6=5, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL7=6, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL8=7, ACTUATORSETTINGS_VTOLMOTORNW_NONE=8, } ActuatorSettingsVTOLMotorNWOptions; +typedef enum { ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORNW_NONE=8, } ActuatorSettingsVTOLMotorNWOptions; // Field ChannelUpdateFreq information /* Number of elements for field ChannelUpdateFreq */ #define ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM 2 diff --git a/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h b/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h index e83ef2567..7089124f5 100644 --- a/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/manualcontrolsettings.h @@ -75,19 +75,19 @@ typedef struct { typedef enum { MANUALCONTROLSETTINGS_INPUTMODE_PWM=0, MANUALCONTROLSETTINGS_INPUTMODE_PPM=1, MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM=2, } ManualControlSettingsInputModeOptions; // Field Roll information /* Enumeration options for field Roll */ -typedef enum { MANUALCONTROLSETTINGS_ROLL_CHANNEL1=0, MANUALCONTROLSETTINGS_ROLL_CHANNEL2=1, MANUALCONTROLSETTINGS_ROLL_CHANNEL3=2, MANUALCONTROLSETTINGS_ROLL_CHANNEL4=3, MANUALCONTROLSETTINGS_ROLL_CHANNEL5=4, MANUALCONTROLSETTINGS_ROLL_CHANNEL6=5, MANUALCONTROLSETTINGS_ROLL_CHANNEL7=6, MANUALCONTROLSETTINGS_ROLL_CHANNEL8=7, MANUALCONTROLSETTINGS_ROLL_NONE=8, } ManualControlSettingsRollOptions; +typedef enum { MANUALCONTROLSETTINGS_ROLL_CHANNEL0=0, MANUALCONTROLSETTINGS_ROLL_CHANNEL1=1, MANUALCONTROLSETTINGS_ROLL_CHANNEL2=2, MANUALCONTROLSETTINGS_ROLL_CHANNEL3=3, MANUALCONTROLSETTINGS_ROLL_CHANNEL4=4, MANUALCONTROLSETTINGS_ROLL_CHANNEL5=5, MANUALCONTROLSETTINGS_ROLL_CHANNEL6=6, MANUALCONTROLSETTINGS_ROLL_CHANNEL7=7, MANUALCONTROLSETTINGS_ROLL_NONE=8, } ManualControlSettingsRollOptions; // Field Pitch information /* Enumeration options for field Pitch */ -typedef enum { MANUALCONTROLSETTINGS_PITCH_CHANNEL1=0, MANUALCONTROLSETTINGS_PITCH_CHANNEL2=1, MANUALCONTROLSETTINGS_PITCH_CHANNEL3=2, MANUALCONTROLSETTINGS_PITCH_CHANNEL4=3, MANUALCONTROLSETTINGS_PITCH_CHANNEL5=4, MANUALCONTROLSETTINGS_PITCH_CHANNEL6=5, MANUALCONTROLSETTINGS_PITCH_CHANNEL7=6, MANUALCONTROLSETTINGS_PITCH_CHANNEL8=7, MANUALCONTROLSETTINGS_PITCH_NONE=8, } ManualControlSettingsPitchOptions; +typedef enum { MANUALCONTROLSETTINGS_PITCH_CHANNEL0=0, MANUALCONTROLSETTINGS_PITCH_CHANNEL1=1, MANUALCONTROLSETTINGS_PITCH_CHANNEL2=2, MANUALCONTROLSETTINGS_PITCH_CHANNEL3=3, MANUALCONTROLSETTINGS_PITCH_CHANNEL4=4, MANUALCONTROLSETTINGS_PITCH_CHANNEL5=5, MANUALCONTROLSETTINGS_PITCH_CHANNEL6=6, MANUALCONTROLSETTINGS_PITCH_CHANNEL7=7, MANUALCONTROLSETTINGS_PITCH_NONE=8, } ManualControlSettingsPitchOptions; // Field Yaw information /* Enumeration options for field Yaw */ -typedef enum { MANUALCONTROLSETTINGS_YAW_CHANNEL1=0, MANUALCONTROLSETTINGS_YAW_CHANNEL2=1, MANUALCONTROLSETTINGS_YAW_CHANNEL3=2, MANUALCONTROLSETTINGS_YAW_CHANNEL4=3, MANUALCONTROLSETTINGS_YAW_CHANNEL5=4, MANUALCONTROLSETTINGS_YAW_CHANNEL6=5, MANUALCONTROLSETTINGS_YAW_CHANNEL7=6, MANUALCONTROLSETTINGS_YAW_CHANNEL8=7, MANUALCONTROLSETTINGS_YAW_NONE=8, } ManualControlSettingsYawOptions; +typedef enum { MANUALCONTROLSETTINGS_YAW_CHANNEL0=0, MANUALCONTROLSETTINGS_YAW_CHANNEL1=1, MANUALCONTROLSETTINGS_YAW_CHANNEL2=2, MANUALCONTROLSETTINGS_YAW_CHANNEL3=3, MANUALCONTROLSETTINGS_YAW_CHANNEL4=4, MANUALCONTROLSETTINGS_YAW_CHANNEL5=5, MANUALCONTROLSETTINGS_YAW_CHANNEL6=6, MANUALCONTROLSETTINGS_YAW_CHANNEL7=7, MANUALCONTROLSETTINGS_YAW_NONE=8, } ManualControlSettingsYawOptions; // Field Throttle information /* Enumeration options for field Throttle */ -typedef enum { MANUALCONTROLSETTINGS_THROTTLE_CHANNEL1=0, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL2=1, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL3=2, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL4=3, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL5=4, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL6=5, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL7=6, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL8=7, MANUALCONTROLSETTINGS_THROTTLE_NONE=8, } ManualControlSettingsThrottleOptions; +typedef enum { MANUALCONTROLSETTINGS_THROTTLE_CHANNEL0=0, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL1=1, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL2=2, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL3=3, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL4=4, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL5=5, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL6=6, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL7=7, MANUALCONTROLSETTINGS_THROTTLE_NONE=8, } ManualControlSettingsThrottleOptions; // Field FlightMode information /* Enumeration options for field FlightMode */ -typedef enum { MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL1=0, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL2=1, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL3=2, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL4=3, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL5=4, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL6=5, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL7=6, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL8=7, MANUALCONTROLSETTINGS_FLIGHTMODE_NONE=8, } ManualControlSettingsFlightModeOptions; +typedef enum { MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL0=0, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL1=1, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL2=2, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL3=3, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL4=4, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL5=5, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL6=6, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL7=7, MANUALCONTROLSETTINGS_FLIGHTMODE_NONE=8, } ManualControlSettingsFlightModeOptions; // Field ChannelMax information /* Number of elements for field ChannelMax */ #define MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM 8 diff --git a/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h b/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h index 80fdc8552..cc5a57409 100644 --- a/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h @@ -33,7 +33,7 @@ #define STABILIZATIONSETTINGS_H // Object constants -#define STABILIZATIONSETTINGS_OBJID 1855169608U +#define STABILIZATIONSETTINGS_OBJID 2185681924U #define STABILIZATIONSETTINGS_NAME "StabilizationSettings" #define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta" #define STABILIZATIONSETTINGS_ISSINGLEINST 1 @@ -57,16 +57,36 @@ // Object data typedef struct { + uint16_t UpdatePeriod; float RollMax; float PitchMax; float ThrottleMax; + float PitchKp; + float PitchKi; + float PitchKd; + float RollKp; + float RollKi; + float RollKd; + float YawKp; + float YawKi; + float YawKd; } __attribute__((packed)) StabilizationSettingsData; // Field information +// Field UpdatePeriod information // Field RollMax information // Field PitchMax information // Field ThrottleMax information +// Field PitchKp information +// Field PitchKi information +// Field PitchKd information +// Field RollKp information +// Field RollKi information +// Field RollKd information +// Field YawKp information +// Field YawKi information +// Field YawKd information // Generic interface functions diff --git a/flight/OpenPilot/UAVObjects/inc/systemalarms.h b/flight/OpenPilot/UAVObjects/inc/systemalarms.h index ba22c2882..0b600c477 100644 --- a/flight/OpenPilot/UAVObjects/inc/systemalarms.h +++ b/flight/OpenPilot/UAVObjects/inc/systemalarms.h @@ -57,7 +57,7 @@ // Object data typedef struct { - uint8_t Alarm[8]; + uint8_t Alarm[9]; } __attribute__((packed)) SystemAlarmsData; @@ -66,9 +66,9 @@ typedef struct { /* Enumeration options for field Alarm */ typedef enum { SYSTEMALARMS_ALARM_OK=0, SYSTEMALARMS_ALARM_WARNING=1, SYSTEMALARMS_ALARM_ERROR=2, SYSTEMALARMS_ALARM_CRITICAL=3, } SystemAlarmsAlarmOptions; /* Array element names for field Alarm */ -typedef enum { SYSTEMALARMS_ALARM_OUTOFMEMORY=0, SYSTEMALARMS_ALARM_STACKOVERFLOW=1, SYSTEMALARMS_ALARM_CPUOVERLOAD=2, SYSTEMALARMS_ALARM_EVENTSYSTEM=3, SYSTEMALARMS_ALARM_SDCARD=4, SYSTEMALARMS_ALARM_TELEMETRY=5, SYSTEMALARMS_ALARM_MANUALCONTROL=6, SYSTEMALARMS_ALARM_ACTUATOR=7, } SystemAlarmsAlarmElem; +typedef enum { SYSTEMALARMS_ALARM_OUTOFMEMORY=0, SYSTEMALARMS_ALARM_STACKOVERFLOW=1, SYSTEMALARMS_ALARM_CPUOVERLOAD=2, SYSTEMALARMS_ALARM_EVENTSYSTEM=3, SYSTEMALARMS_ALARM_SDCARD=4, SYSTEMALARMS_ALARM_TELEMETRY=5, SYSTEMALARMS_ALARM_MANUALCONTROL=6, SYSTEMALARMS_ALARM_ACTUATOR=7, SYSTEMALARMS_ALARM_STABILIZATION=8, } SystemAlarmsAlarmElem; /* Number of elements for field Alarm */ -#define SYSTEMALARMS_ALARM_NUMELEM 8 +#define SYSTEMALARMS_ALARM_NUMELEM 9 // Generic interface functions diff --git a/flight/OpenPilot/UAVObjects/stabilizationsettings.c b/flight/OpenPilot/UAVObjects/stabilizationsettings.c index 502915567..2bfaf82dc 100644 --- a/flight/OpenPilot/UAVObjects/stabilizationsettings.c +++ b/flight/OpenPilot/UAVObjects/stabilizationsettings.c @@ -73,9 +73,19 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) // Initialize object fields to their default values UAVObjGetInstanceData(obj, instId, &data); memset(&data, 0, sizeof(StabilizationSettingsData)); + data.UpdatePeriod = 10; data.RollMax = 35; data.PitchMax = 35; - data.ThrottleMax = 100; + data.ThrottleMax = 1; + data.PitchKp = 0.04; + data.PitchKi = 4e-06; + data.PitchKd = 0.01; + data.RollKp = 0.02; + data.RollKi = 4e-06; + data.RollKd = 0.01; + data.YawKp = 1; + data.YawKi = 0; + data.YawKd = 0; UAVObjSetInstanceData(obj, instId, &data);