From 4e6f100ae56e1942b0b30cc7a3a173e66b332d03 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Fri, 8 Oct 2010 15:38:23 +0000 Subject: [PATCH] Flight: Swap to Les' code for main line git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1920 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/OpenPilot/Makefile | 3 +- flight/OpenPilot/Makefile.posix | 4 +- .../Modules/ManualControl/manualcontrol.c | 29 +-- .../Stabilization => }/inc/stabilization.h | 0 .../simple/Stabilization/inc/stabilization.h | 43 ---- .../simple/Stabilization/stabilization.c | 231 ------------------ .../{les/Stabilization => }/stabilization.c | 36 +-- .../UAVObjects/inc/lesstabilizationsettings.h | 145 ----------- .../UAVObjects/inc/stabilizationsettings.h | 82 ++++--- .../UAVObjects/lesstabilizationsettings.c | 139 ----------- .../UAVObjects/stabilizationsettings.c | 39 +-- flight/OpenPilot/UAVObjects/uavobjectsinit.c | 2 - .../uavobjects/lesstabilizationsettings.cpp | 205 ---------------- .../uavobjects/lesstabilizationsettings.h | 132 ---------- .../uavobjects/lesstabilizationsettings.py | 212 ---------------- .../uavobjects/stabilizationsettings.cpp | 133 +++++----- .../uavobjects/stabilizationsettings.h | 82 ++++--- .../uavobjects/stabilizationsettings.py | 138 ++++------- ground/src/plugins/uavobjects/uavobjects.pro | 2 - .../src/plugins/uavobjects/uavobjectsinit.cpp | 2 - .../lesstabilizationsettings.xml | 23 -- .../stabilizationsettings.xml | 43 ++-- 22 files changed, 274 insertions(+), 1451 deletions(-) rename flight/OpenPilot/Modules/Stabilization/{les/Stabilization => }/inc/stabilization.h (100%) delete mode 100644 flight/OpenPilot/Modules/Stabilization/simple/Stabilization/inc/stabilization.h delete mode 100644 flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c rename flight/OpenPilot/Modules/Stabilization/{les/Stabilization => }/stabilization.c (91%) delete mode 100644 flight/OpenPilot/UAVObjects/inc/lesstabilizationsettings.h delete mode 100644 flight/OpenPilot/UAVObjects/lesstabilizationsettings.c delete mode 100644 ground/src/plugins/uavobjects/lesstabilizationsettings.cpp delete mode 100644 ground/src/plugins/uavobjects/lesstabilizationsettings.h delete mode 100644 ground/src/plugins/uavobjects/lesstabilizationsettings.py delete mode 100644 ground/src/shared/uavobjectdefinition/lesstabilizationsettings.xml diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 69ae5ca37..5ad77f11f 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -57,7 +57,7 @@ FLASH_TOOL = OPENOCD USE_THUMB_MODE = YES # List of modules to include -MODULES = Actuator Telemetry GPS ManualControl Altitude AHRSComms Stabilization/simple/Stabilization Watchdog FirmwareIAP +MODULES = Actuator Telemetry GPS ManualControl Altitude AHRSComms Stabilization Watchdog FirmwareIAP #MODULES = Telemetry Example #MODULES = Telemetry MK/MKSerial @@ -174,7 +174,6 @@ SRC += $(OPUAVOBJ)/attituderaw.c SRC += $(OPUAVOBJ)/homelocation.c SRC += $(OPUAVOBJ)/mixersettings.c SRC += $(OPUAVOBJ)/mixerstatus.c -SRC += $(OPUAVOBJ)/lesstabilizationsettings.c SRC += $(OPUAVOBJ)/positiondesired.c SRC += $(OPUAVOBJ)/velocitydesired.c SRC += $(OPUAVOBJ)/velocityactual.c diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix index 125dbbea1..fccc54e47 100644 --- a/flight/OpenPilot/Makefile.posix +++ b/flight/OpenPilot/Makefile.posix @@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD USE_THUMB_MODE = YES # List of modules to include -MODULES = Telemetry Stabilization/simple/Stabilization Guidance ManualControl +MODULES = Telemetry Stabilization Guidance ManualControl #MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization #MODULES = Telemetry Example #MODULES = Telemetry MK/MKSerial @@ -155,11 +155,11 @@ SRC += $(OPUAVOBJ)/gpstime.c SRC += $(OPUAVOBJ)/gpssatellites.c SRC += $(OPUAVOBJ)/mixersettings.c SRC += $(OPUAVOBJ)/mixerstatus.c -SRC += $(OPUAVOBJ)/lesstabilizationsettings.c SRC += $(OPUAVOBJ)/positiondesired.c SRC += $(OPUAVOBJ)/velocitydesired.c SRC += $(OPUAVOBJ)/velocityactual.c SRC += $(OPUAVOBJ)/guidancesettings.c +SRC += $(OPUAVOBJ)/firmwareiapobj.c endif ## PIOS Hardware (posix) diff --git a/flight/OpenPilot/Modules/ManualControl/manualcontrol.c b/flight/OpenPilot/Modules/ManualControl/manualcontrol.c index ca57c9a17..dd297f063 100644 --- a/flight/OpenPilot/Modules/ManualControl/manualcontrol.c +++ b/flight/OpenPilot/Modules/ManualControl/manualcontrol.c @@ -283,21 +283,8 @@ static void manualControlTask(void *parameters) } else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) { attitude.Roll = cmd.Roll * stabSettings.RollMax; attitude.Pitch = cmd.Pitch * stabSettings.PitchMax; - if (stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { - /* rate stabilization */ - attitude.Yaw = cmd.Yaw * stabSettings.YawMax; - } else { - /* heading stabilization */ - if (cmd.Yaw < 0) { - attitude.Yaw = 360 + (cmd.Yaw * 180.0); - } else { - attitude.Yaw = (cmd.Yaw * 180.0); - } - } - if (cmd.Throttle < 0) - attitude.Throttle = -1; - else - attitude.Throttle = cmd.Throttle * stabSettings.ThrottleMax; + attitude.Yaw = fmod(cmd.Yaw * 180.0, 360); + attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle; AttitudeDesiredSet(&attitude); } @@ -308,17 +295,7 @@ static void manualControlTask(void *parameters) attitudeSettings.PitchBias = cmd.Accessory1 * 15; attitudeSettings.RollBias = cmd.Accessory2 * 15; AHRSSettingsSet(&attitudeSettings); - } else if (cmd.Accessory3 > .9) { - // REALLY don't want to end up here accidentally. I've also saved by meta for Stabilization setting to be - // flight read only by default - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); - if (cmd.Accessory1 > 0) - stabSettings.PitchKp = cmd.Accessory1 * 0.05; - if (cmd.Accessory2 > 0) - stabSettings.RollKp = cmd.Accessory2 * 0.05; - StabilizationSettingsSet(&stabSettings); - } + } } } diff --git a/flight/OpenPilot/Modules/Stabilization/les/Stabilization/inc/stabilization.h b/flight/OpenPilot/Modules/Stabilization/inc/stabilization.h similarity index 100% rename from flight/OpenPilot/Modules/Stabilization/les/Stabilization/inc/stabilization.h rename to flight/OpenPilot/Modules/Stabilization/inc/stabilization.h diff --git a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/inc/stabilization.h b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/inc/stabilization.h deleted file mode 100644 index ab9d08aac..000000000 --- a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/inc/stabilization.h +++ /dev/null @@ -1,43 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup StabilizationModule Stabilization Module - * @brief Stabilization PID loops in an airframe type independent manner - * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the - * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" - * @{ - * - * @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 - -/** - * @} - * @} - */ \ No newline at end of file diff --git a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c deleted file mode 100644 index 7da7bf24b..000000000 --- a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c +++ /dev/null @@ -1,231 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup StabilizationModule Stabilization Module - * @brief Stabilization PID loops in an airframe type independent manner - * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the - * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual" - * @{ - * - * @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 MAX_QUEUE_SIZE 2 -#define STACK_SIZE configMINIMAL_STACK_SIZE -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define FAILSAFE_TIMEOUT_MS 100 - -// Private types - -// Private variables -static xQueueHandle queue; -static xTaskHandle taskHandle; - -// Private functions -static void stabilizationTask(void* parameters); -static float bound(float val, float min, float max); - -/** - * Module initialization - */ -int32_t StabilizationInitialize() -{ - // Create object queue - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - - // Listen for AttitudeActual updates. - AttitudeActualConnectQueue(queue); - - // Start main task - xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); - - return 0; -} -float dT = 1; - -/** - * Module task - */ -static void stabilizationTask(void* parameters) -{ - UAVObjEvent ev; - - StabilizationSettingsData stabSettings; - ActuatorDesiredData actuatorDesired; - AttitudeDesiredData attitudeDesired; - AttitudeActualData attitudeActual; - ManualControlCommandData manualControl; - SystemSettingsData systemSettings; - portTickType lastSysTime; - portTickType thisSysTime; - float pitchError, pitchErrorLast; - float rollError, rollErrorLast; - float yawError, yawErrorLast; - float pitchDerivative; - float rollDerivative; - float yawDerivative; - float pitchIntegral; - float rollIntegral; - float yawIntegral; - float yawPrevious; - float yawChange; - - // Initialize - pitchIntegral = 0.0; - rollIntegral = 0.0; - yawIntegral = 0.0; - pitchErrorLast = 0.0; - rollErrorLast = 0.0; - yawErrorLast = 0.0; - yawPrevious = 0.0; - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) - { - // Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe - if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) - { - AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); - } - - // 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; - lastSysTime = thisSysTime; - - // Read settings and other objects - StabilizationSettingsGet(&stabSettings); - SystemSettingsGet(&systemSettings); - ManualControlCommandGet(&manualControl); - AttitudeDesiredGet(&attitudeDesired); - AttitudeActualGet(&attitudeActual); - - // Pitch stabilization control loop - pitchError = attitudeDesired.Pitch - attitudeActual.Pitch; - pitchDerivative = (pitchError - pitchErrorLast) / dT; - pitchIntegral = bound(pitchIntegral + pitchError * dT, -stabSettings.PitchIntegralLimit, stabSettings.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 = attitudeDesired.Roll - attitudeActual.Roll; - rollDerivative = (rollError - rollErrorLast) / dT; - rollIntegral = bound(rollIntegral + rollError * dT, -stabSettings.RollIntegralLimit, stabSettings.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 )|| - ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)|| - ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)|| - ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) || - ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) || - ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP)) - { - if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw - // Avoid a huge (wrong) change if we crossed 0/360 - yawChange = attitudeActual.Yaw - yawPrevious; - if (yawChange>180.0) yawChange -= 360; - if (yawChange<-180.0) yawChange += 360; - yawChange = yawChange / dT; - yawPrevious = attitudeActual.Yaw; - yawError = bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange; - } else { // heading stabilization - yawError = attitudeDesired.Yaw - attitudeActual.Yaw; - } - - //this should make it take the quickest path to reach the desired yaw - if (yawError>180.0)yawError -= 360; - if (yawError<-180.0)yawError += 360; - yawDerivative = (yawError - yawErrorLast) / dT; - yawIntegral = bound(yawIntegral + yawError * dT, -stabSettings.YawIntegralLimit, stabSettings.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, stabSettings.ThrottleMin, stabSettings.ThrottleMax); - - // Save dT - actuatorDesired.UpdateTime = dT * 1000; - - // 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); - } -} - -/** - * 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/Modules/Stabilization/les/Stabilization/stabilization.c b/flight/OpenPilot/Modules/Stabilization/stabilization.c similarity index 91% rename from flight/OpenPilot/Modules/Stabilization/les/Stabilization/stabilization.c rename to flight/OpenPilot/Modules/Stabilization/stabilization.c index b45199104..a146ac5b7 100644 --- a/flight/OpenPilot/Modules/Stabilization/les/Stabilization/stabilization.c +++ b/flight/OpenPilot/Modules/Stabilization/stabilization.c @@ -33,7 +33,7 @@ #include "openpilot.h" #include "stabilization.h" -#include "lesstabilizationsettings.h" +#include "stabilizationsettings.h" #include "actuatordesired.h" #include "attitudedesired.h" #include "attitudeactual.h" @@ -66,7 +66,7 @@ typedef struct { // Private variables static xTaskHandle taskHandle; -static LesStabilizationSettingsData settings; +static StabilizationSettingsData settings; static xQueueHandle queue; float dT = 1; pid_type pids[PID_MAX]; @@ -75,7 +75,7 @@ pid_type pids[PID_MAX]; // Private functions static void stabilizationTask(void* parameters); -static float ApplyPid(pid_type * pid, const float desired, const float actual, const bool angular); +static float ApplyPid(pid_type * pid, const float desired, const float actual, const uint8_t angular); static float bound(float val); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent * ev); @@ -94,8 +94,8 @@ int32_t StabilizationInitialize() AttitudeActualConnectQueue(queue); AttitudeRawConnectQueue(queue); - LesStabilizationSettingsConnectCallback(SettingsUpdatedCb); - SettingsUpdatedCb(LesStabilizationSettingsHandle()); + StabilizationSettingsConnectCallback(SettingsUpdatedCb); + SettingsUpdatedCb(StabilizationSettingsHandle()); // Start main task xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); @@ -119,6 +119,8 @@ static void stabilizationTask(void* parameters) SystemSettingsData systemSettings; ManualControlCommandData manualControl; + SettingsUpdatedCb((UAVObjEvent *) NULL); + // Main task loop lastSysTime = xTaskGetTickCount(); ZeroPids(); @@ -140,7 +142,7 @@ static void stabilizationTask(void* parameters) AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); SystemSettingsGet(&systemSettings); - + float *manualAxis = &manualControl.Roll; float *attitudeDesiredAxis = &attitudeDesired.Roll; @@ -149,7 +151,7 @@ static void stabilizationTask(void* parameters) //Calculate desired rate float rates[MAX_AXES]= {0,0,0}; - for(int ct=0; ct< MAX_AXES; ct++) + for(int8_t ct=0; ct< MAX_AXES; ct++) { switch(manualControl.StabilizationSettings[ct]) { @@ -158,15 +160,15 @@ static void stabilizationTask(void* parameters) break; case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_POSITION: - rates[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], true); + rates[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1); break; } } - bool shouldUpdate = false; + uint8_t shouldUpdate = 0; ActuatorDesiredGet(&actuatorDesired); //Calculate desired command - for(int ct=0; ct< MAX_AXES; ct++) + for(int8_t ct=0; ct< MAX_AXES; ct++) { if(fabs(rates[ct]) > settings.MaximumRate[ct]) { @@ -184,16 +186,16 @@ static void stabilizationTask(void* parameters) case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE: case MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_POSITION: { - float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rates[ct], attitudeRaw.gyros_filtered[ct], false); + float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rates[ct], attitudeRaw.gyros_filtered[ct], 0); actuatorDesiredAxis[ct] = bound(command); - shouldUpdate = true; + shouldUpdate = 1; break; } } } if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) { - shouldUpdate = false; + shouldUpdate = 0; } @@ -216,7 +218,7 @@ static void stabilizationTask(void* parameters) } } -float ApplyPid(pid_type * pid, const float desired, const float actual, const bool angular) +float ApplyPid(pid_type * pid, const float desired, const float actual, const uint8_t angular) { float err = desired - actual; if(angular) //take shortest route to desired position @@ -247,7 +249,7 @@ float ApplyPid(pid_type * pid, const float desired, const float actual, const bo static void ZeroPids(void) { - for(int ct = 0; ct < PID_MAX; ct++) { + for(int8_t ct = 0; ct < PID_MAX; ct++) { pids[ct].iAccumulator = 0; pids[ct].lastErr = 0; } @@ -271,10 +273,10 @@ static float bound(float val) static void SettingsUpdatedCb(UAVObjEvent * ev) { memset(pids,0,sizeof (pid_type) * PID_MAX); - LesStabilizationSettingsGet(&settings); + StabilizationSettingsGet(&settings); float * data = settings.RollRatePI; - for(int pid=0; pid < PID_MAX; pid++) + for(int8_t pid=0; pid < PID_MAX; pid++) { pids[pid].p = *data++; pids[pid].i = *data++; diff --git a/flight/OpenPilot/UAVObjects/inc/lesstabilizationsettings.h b/flight/OpenPilot/UAVObjects/inc/lesstabilizationsettings.h deleted file mode 100644 index 81f844258..000000000 --- a/flight/OpenPilot/UAVObjects/inc/lesstabilizationsettings.h +++ /dev/null @@ -1,145 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup UAVObjects OpenPilot UAVObjects - * @{ - * @addtogroup LesStabilizationSettings LesStabilizationSettings - * @brief PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - * - * Autogenerated files and functions for LesStabilizationSettings Object - - * @{ - * - * @file lesstabilizationsettings.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Implementation of the LesStabilizationSettings object. This file has been - * automatically generated by the UAVObjectGenerator. - * - * @note Object definition file: lesstabilizationsettings.xml. - * This is an automatically generated file. - * DO NOT modify manually. - * - * @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 LESSTABILIZATIONSETTINGS_H -#define LESSTABILIZATIONSETTINGS_H - -// Object constants -#define LESSTABILIZATIONSETTINGS_OBJID 3247121950U -#define LESSTABILIZATIONSETTINGS_NAME "LesStabilizationSettings" -#define LESSTABILIZATIONSETTINGS_METANAME "LesStabilizationSettingsMeta" -#define LESSTABILIZATIONSETTINGS_ISSINGLEINST 1 -#define LESSTABILIZATIONSETTINGS_ISSETTINGS 1 -#define LESSTABILIZATIONSETTINGS_NUMBYTES sizeof(LesStabilizationSettingsData) - -// Object access macros -/** - * @function LesStabilizationSettingsGet(dataOut) - * @brief Populate a LesStabilizationSettingsData object - * @param[out] dataOut - */ -#define LesStabilizationSettingsGet(dataOut) UAVObjGetData(LesStabilizationSettingsHandle(), dataOut) -#define LesStabilizationSettingsSet(dataIn) UAVObjSetData(LesStabilizationSettingsHandle(), dataIn) -#define LesStabilizationSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(LesStabilizationSettingsHandle(), instId, dataOut) -#define LesStabilizationSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(LesStabilizationSettingsHandle(), instId, dataIn) -#define LesStabilizationSettingsConnectQueue(queue) UAVObjConnectQueue(LesStabilizationSettingsHandle(), queue, EV_MASK_ALL_UPDATES) -#define LesStabilizationSettingsConnectCallback(cb) UAVObjConnectCallback(LesStabilizationSettingsHandle(), cb, EV_MASK_ALL_UPDATES) -#define LesStabilizationSettingsCreateInstance() UAVObjCreateInstance(LesStabilizationSettingsHandle()) -#define LesStabilizationSettingsRequestUpdate() UAVObjRequestUpdate(LesStabilizationSettingsHandle()) -#define LesStabilizationSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(LesStabilizationSettingsHandle(), instId) -#define LesStabilizationSettingsUpdated() UAVObjUpdated(LesStabilizationSettingsHandle()) -#define LesStabilizationSettingsInstUpdated(instId) UAVObjUpdated(LesStabilizationSettingsHandle(), instId) -#define LesStabilizationSettingsGetMetadata(dataOut) UAVObjGetMetadata(LesStabilizationSettingsHandle(), dataOut) -#define LesStabilizationSettingsSetMetadata(dataIn) UAVObjSetMetadata(LesStabilizationSettingsHandle(), dataIn) -#define LesStabilizationSettingsReadOnly(dataIn) UAVObjReadOnly(LesStabilizationSettingsHandle()) - -// Object data -typedef struct { - uint8_t UpdatePeriod; - uint8_t RollMax; - uint8_t PitchMax; - uint8_t YawMax; - float ManualRate[3]; - float MaximumRate[3]; - float RollRatePI[3]; - float PitchRatePI[3]; - float YawRatePI[3]; - float RollPI[3]; - float PitchPI[3]; - float YawPI[3]; - -} __attribute__((packed)) LesStabilizationSettingsData; - -// Field information -// Field UpdatePeriod information -// Field RollMax information -// Field PitchMax information -// Field YawMax information -// Field ManualRate information -/* Array element names for field ManualRate */ -typedef enum { LESSTABILIZATIONSETTINGS_MANUALRATE_ROLL=0, LESSTABILIZATIONSETTINGS_MANUALRATE_PITCH=1, LESSTABILIZATIONSETTINGS_MANUALRATE_YAW=2 } LesStabilizationSettingsManualRateElem; -/* Number of elements for field ManualRate */ -#define LESSTABILIZATIONSETTINGS_MANUALRATE_NUMELEM 3 -// Field MaximumRate information -/* Array element names for field MaximumRate */ -typedef enum { LESSTABILIZATIONSETTINGS_MAXIMUMRATE_ROLL=0, LESSTABILIZATIONSETTINGS_MAXIMUMRATE_PITCH=1, LESSTABILIZATIONSETTINGS_MAXIMUMRATE_YAW=2 } LesStabilizationSettingsMaximumRateElem; -/* Number of elements for field MaximumRate */ -#define LESSTABILIZATIONSETTINGS_MAXIMUMRATE_NUMELEM 3 -// Field RollRatePI information -/* Array element names for field RollRatePI */ -typedef enum { LESSTABILIZATIONSETTINGS_ROLLRATEPI_KP=0, LESSTABILIZATIONSETTINGS_ROLLRATEPI_KI=1, LESSTABILIZATIONSETTINGS_ROLLRATEPI_ILIMIT=2 } LesStabilizationSettingsRollRatePIElem; -/* Number of elements for field RollRatePI */ -#define LESSTABILIZATIONSETTINGS_ROLLRATEPI_NUMELEM 3 -// Field PitchRatePI information -/* Array element names for field PitchRatePI */ -typedef enum { LESSTABILIZATIONSETTINGS_PITCHRATEPI_KP=0, LESSTABILIZATIONSETTINGS_PITCHRATEPI_KI=1, LESSTABILIZATIONSETTINGS_PITCHRATEPI_ILIMIT=2 } LesStabilizationSettingsPitchRatePIElem; -/* Number of elements for field PitchRatePI */ -#define LESSTABILIZATIONSETTINGS_PITCHRATEPI_NUMELEM 3 -// Field YawRatePI information -/* Array element names for field YawRatePI */ -typedef enum { LESSTABILIZATIONSETTINGS_YAWRATEPI_KP=0, LESSTABILIZATIONSETTINGS_YAWRATEPI_KI=1, LESSTABILIZATIONSETTINGS_YAWRATEPI_ILIMIT=2 } LesStabilizationSettingsYawRatePIElem; -/* Number of elements for field YawRatePI */ -#define LESSTABILIZATIONSETTINGS_YAWRATEPI_NUMELEM 3 -// Field RollPI information -/* Array element names for field RollPI */ -typedef enum { LESSTABILIZATIONSETTINGS_ROLLPI_KP=0, LESSTABILIZATIONSETTINGS_ROLLPI_KI=1, LESSTABILIZATIONSETTINGS_ROLLPI_ILIMIT=2 } LesStabilizationSettingsRollPIElem; -/* Number of elements for field RollPI */ -#define LESSTABILIZATIONSETTINGS_ROLLPI_NUMELEM 3 -// Field PitchPI information -/* Array element names for field PitchPI */ -typedef enum { LESSTABILIZATIONSETTINGS_PITCHPI_KP=0, LESSTABILIZATIONSETTINGS_PITCHPI_KI=1, LESSTABILIZATIONSETTINGS_PITCHPI_ILIMIT=2 } LesStabilizationSettingsPitchPIElem; -/* Number of elements for field PitchPI */ -#define LESSTABILIZATIONSETTINGS_PITCHPI_NUMELEM 3 -// Field YawPI information -/* Array element names for field YawPI */ -typedef enum { LESSTABILIZATIONSETTINGS_YAWPI_KP=0, LESSTABILIZATIONSETTINGS_YAWPI_KI=1, LESSTABILIZATIONSETTINGS_YAWPI_ILIMIT=2 } LesStabilizationSettingsYawPIElem; -/* Number of elements for field YawPI */ -#define LESSTABILIZATIONSETTINGS_YAWPI_NUMELEM 3 - - -// Generic interface functions -int32_t LesStabilizationSettingsInitialize(); -UAVObjHandle LesStabilizationSettingsHandle(); - -#endif // LESSTABILIZATIONSETTINGS_H - -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h b/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h index 61d2b32a7..3482385d8 100644 --- a/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h @@ -41,7 +41,7 @@ #define STABILIZATIONSETTINGS_H // Object constants -#define STABILIZATIONSETTINGS_OBJID 1346414844U +#define STABILIZATIONSETTINGS_OBJID 3792991236U #define STABILIZATIONSETTINGS_NAME "StabilizationSettings" #define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta" #define STABILIZATIONSETTINGS_ISSINGLEINST 1 @@ -74,21 +74,14 @@ typedef struct { uint8_t RollMax; uint8_t PitchMax; uint8_t YawMax; - uint8_t YawMode; - float ThrottleMax; - float ThrottleMin; - float RollIntegralLimit; - float PitchIntegralLimit; - float YawIntegralLimit; - float PitchKp; - float PitchKi; - float PitchKd; - float RollKp; - float RollKi; - float RollKd; - float YawKp; - float YawKi; - float YawKd; + float ManualRate[3]; + float MaximumRate[3]; + float RollRatePI[3]; + float PitchRatePI[3]; + float YawRatePI[3]; + float RollPI[3]; + float PitchPI[3]; + float YawPI[3]; } __attribute__((packed)) StabilizationSettingsData; @@ -96,23 +89,46 @@ typedef struct { // Field RollMax information // Field PitchMax information // Field YawMax information -// Field YawMode information -/* Enumeration options for field YawMode */ -typedef enum { STABILIZATIONSETTINGS_YAWMODE_RATE=0, STABILIZATIONSETTINGS_YAWMODE_HEADING=1 } StabilizationSettingsYawModeOptions; -// Field ThrottleMax information -// Field ThrottleMin information -// Field RollIntegralLimit information -// Field PitchIntegralLimit information -// Field YawIntegralLimit 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 +// Field ManualRate information +/* Array element names for field ManualRate */ +typedef enum { STABILIZATIONSETTINGS_MANUALRATE_ROLL=0, STABILIZATIONSETTINGS_MANUALRATE_PITCH=1, STABILIZATIONSETTINGS_MANUALRATE_YAW=2 } StabilizationSettingsManualRateElem; +/* Number of elements for field ManualRate */ +#define STABILIZATIONSETTINGS_MANUALRATE_NUMELEM 3 +// Field MaximumRate information +/* Array element names for field MaximumRate */ +typedef enum { STABILIZATIONSETTINGS_MAXIMUMRATE_ROLL=0, STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH=1, STABILIZATIONSETTINGS_MAXIMUMRATE_YAW=2 } StabilizationSettingsMaximumRateElem; +/* Number of elements for field MaximumRate */ +#define STABILIZATIONSETTINGS_MAXIMUMRATE_NUMELEM 3 +// Field RollRatePI information +/* Array element names for field RollRatePI */ +typedef enum { STABILIZATIONSETTINGS_ROLLRATEPI_KP=0, STABILIZATIONSETTINGS_ROLLRATEPI_KI=1, STABILIZATIONSETTINGS_ROLLRATEPI_ILIMIT=2 } StabilizationSettingsRollRatePIElem; +/* Number of elements for field RollRatePI */ +#define STABILIZATIONSETTINGS_ROLLRATEPI_NUMELEM 3 +// Field PitchRatePI information +/* Array element names for field PitchRatePI */ +typedef enum { STABILIZATIONSETTINGS_PITCHRATEPI_KP=0, STABILIZATIONSETTINGS_PITCHRATEPI_KI=1, STABILIZATIONSETTINGS_PITCHRATEPI_ILIMIT=2 } StabilizationSettingsPitchRatePIElem; +/* Number of elements for field PitchRatePI */ +#define STABILIZATIONSETTINGS_PITCHRATEPI_NUMELEM 3 +// Field YawRatePI information +/* Array element names for field YawRatePI */ +typedef enum { STABILIZATIONSETTINGS_YAWRATEPI_KP=0, STABILIZATIONSETTINGS_YAWRATEPI_KI=1, STABILIZATIONSETTINGS_YAWRATEPI_ILIMIT=2 } StabilizationSettingsYawRatePIElem; +/* Number of elements for field YawRatePI */ +#define STABILIZATIONSETTINGS_YAWRATEPI_NUMELEM 3 +// Field RollPI information +/* Array element names for field RollPI */ +typedef enum { STABILIZATIONSETTINGS_ROLLPI_KP=0, STABILIZATIONSETTINGS_ROLLPI_KI=1, STABILIZATIONSETTINGS_ROLLPI_ILIMIT=2 } StabilizationSettingsRollPIElem; +/* Number of elements for field RollPI */ +#define STABILIZATIONSETTINGS_ROLLPI_NUMELEM 3 +// Field PitchPI information +/* Array element names for field PitchPI */ +typedef enum { STABILIZATIONSETTINGS_PITCHPI_KP=0, STABILIZATIONSETTINGS_PITCHPI_KI=1, STABILIZATIONSETTINGS_PITCHPI_ILIMIT=2 } StabilizationSettingsPitchPIElem; +/* Number of elements for field PitchPI */ +#define STABILIZATIONSETTINGS_PITCHPI_NUMELEM 3 +// Field YawPI information +/* Array element names for field YawPI */ +typedef enum { STABILIZATIONSETTINGS_YAWPI_KP=0, STABILIZATIONSETTINGS_YAWPI_KI=1, STABILIZATIONSETTINGS_YAWPI_ILIMIT=2 } StabilizationSettingsYawPIElem; +/* Number of elements for field YawPI */ +#define STABILIZATIONSETTINGS_YAWPI_NUMELEM 3 // Generic interface functions diff --git a/flight/OpenPilot/UAVObjects/lesstabilizationsettings.c b/flight/OpenPilot/UAVObjects/lesstabilizationsettings.c deleted file mode 100644 index 16f417ab3..000000000 --- a/flight/OpenPilot/UAVObjects/lesstabilizationsettings.c +++ /dev/null @@ -1,139 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup UAVObjects OpenPilot UAVObjects - * @{ - * @addtogroup LesStabilizationSettings LesStabilizationSettings - * @brief PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - * - * Autogenerated files and functions for LesStabilizationSettings Object - * @{ - * - * @file lesstabilizationsettings.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Implementation of the LesStabilizationSettings object. This file has been - * automatically generated by the UAVObjectGenerator. - * - * @note Object definition file: lesstabilizationsettings.xml. - * This is an automatically generated file. - * DO NOT modify manually. - * - * @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 "lesstabilizationsettings.h" - -// Private variables -static UAVObjHandle handle; - -// Private functions -static void setDefaults(UAVObjHandle obj, uint16_t instId); - -/** - * Initialize object. - * \return 0 Success - * \return -1 Failure - */ -int32_t LesStabilizationSettingsInitialize() -{ - // Register object with the object manager - handle = UAVObjRegister(LESSTABILIZATIONSETTINGS_OBJID, LESSTABILIZATIONSETTINGS_NAME, LESSTABILIZATIONSETTINGS_METANAME, 0, - LESSTABILIZATIONSETTINGS_ISSINGLEINST, LESSTABILIZATIONSETTINGS_ISSETTINGS, LESSTABILIZATIONSETTINGS_NUMBYTES, &setDefaults); - - // Done - if (handle != 0) - { - return 0; - } - else - { - return -1; - } -} - -/** - * Initialize object fields and metadata with the default values. - * If a default value is not specified the object fields - * will be initialized to zero. - */ -static void setDefaults(UAVObjHandle obj, uint16_t instId) -{ - LesStabilizationSettingsData data; - UAVObjMetadata metadata; - - // Initialize object fields to their default values - UAVObjGetInstanceData(obj, instId, &data); - memset(&data, 0, sizeof(LesStabilizationSettingsData)); - data.UpdatePeriod = 10; - data.RollMax = 35; - data.PitchMax = 35; - data.YawMax = 35; - data.ManualRate[0] = 150; - data.ManualRate[1] = 150; - data.ManualRate[2] = 150; - data.MaximumRate[0] = 300; - data.MaximumRate[1] = 300; - data.MaximumRate[2] = 300; - data.RollRatePI[0] = 0.0015; - data.RollRatePI[1] = 0; - data.RollRatePI[2] = 0.5; - data.PitchRatePI[0] = 0.0015; - data.PitchRatePI[1] = 0; - data.PitchRatePI[2] = 0.5; - data.YawRatePI[0] = 0.003; - data.YawRatePI[1] = 0; - data.YawRatePI[2] = 0.5; - data.RollPI[0] = 2; - data.RollPI[1] = 0; - data.RollPI[2] = 0.5; - data.PitchPI[0] = 2; - data.PitchPI[1] = 0; - data.PitchPI[2] = 0.5; - data.YawPI[0] = 2; - data.YawPI[1] = 0; - data.YawPI[2] = 0.5; - - UAVObjSetInstanceData(obj, instId, &data); - - // Initialize object metadata to their default values - metadata.access = ACCESS_READWRITE; - metadata.gcsAccess = ACCESS_READWRITE; - metadata.telemetryAcked = 1; - metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE; - metadata.telemetryUpdatePeriod = 0; - metadata.gcsTelemetryAcked = 1; - metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; - metadata.gcsTelemetryUpdatePeriod = 0; - metadata.loggingUpdateMode = UPDATEMODE_NEVER; - metadata.loggingUpdatePeriod = 0; - UAVObjSetMetadata(obj, &metadata); -} - -/** - * Get object handle - */ -UAVObjHandle LesStabilizationSettingsHandle() -{ - return handle; -} - -/** - * @} - */ - diff --git a/flight/OpenPilot/UAVObjects/stabilizationsettings.c b/flight/OpenPilot/UAVObjects/stabilizationsettings.c index bc95eae14..91468d9a2 100644 --- a/flight/OpenPilot/UAVObjects/stabilizationsettings.c +++ b/flight/OpenPilot/UAVObjects/stabilizationsettings.c @@ -83,21 +83,30 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) data.RollMax = 35; data.PitchMax = 35; data.YawMax = 35; - data.YawMode = 0; - data.ThrottleMax = 1; - data.ThrottleMin = -1; - data.RollIntegralLimit = 0.5; - data.PitchIntegralLimit = 0.5; - data.YawIntegralLimit = 0.5; - 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 = 0.04; - data.YawKi = 4e-06; - data.YawKd = 0.01; + data.ManualRate[0] = 150; + data.ManualRate[1] = 150; + data.ManualRate[2] = 150; + data.MaximumRate[0] = 300; + data.MaximumRate[1] = 300; + data.MaximumRate[2] = 300; + data.RollRatePI[0] = 0.0015; + data.RollRatePI[1] = 0; + data.RollRatePI[2] = 0.5; + data.PitchRatePI[0] = 0.0015; + data.PitchRatePI[1] = 0; + data.PitchRatePI[2] = 0.5; + data.YawRatePI[0] = 0.003; + data.YawRatePI[1] = 0; + data.YawRatePI[2] = 0.5; + data.RollPI[0] = 2; + data.RollPI[1] = 0; + data.RollPI[2] = 0.5; + data.PitchPI[0] = 2; + data.PitchPI[1] = 0; + data.PitchPI[2] = 0.5; + data.YawPI[0] = 2; + data.YawPI[1] = 0; + data.YawPI[2] = 0.5; UAVObjSetInstanceData(obj, instId, &data); diff --git a/flight/OpenPilot/UAVObjects/uavobjectsinit.c b/flight/OpenPilot/UAVObjects/uavobjectsinit.c index 147f4d90a..4220ba8b9 100644 --- a/flight/OpenPilot/UAVObjects/uavobjectsinit.c +++ b/flight/OpenPilot/UAVObjects/uavobjectsinit.c @@ -47,7 +47,6 @@ #include "gpstime.h" #include "guidancesettings.h" #include "homelocation.h" -#include "lesstabilizationsettings.h" #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" #include "mixersettings.h" @@ -89,7 +88,6 @@ void UAVObjectsInitializeAll() GPSTimeInitialize(); GuidanceSettingsInitialize(); HomeLocationInitialize(); - LesStabilizationSettingsInitialize(); ManualControlCommandInitialize(); ManualControlSettingsInitialize(); MixerSettingsInitialize(); diff --git a/ground/src/plugins/uavobjects/lesstabilizationsettings.cpp b/ground/src/plugins/uavobjects/lesstabilizationsettings.cpp deleted file mode 100644 index bd17af09e..000000000 --- a/ground/src/plugins/uavobjects/lesstabilizationsettings.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/** - ****************************************************************************** - * - * @file lesstabilizationsettings.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @see The GNU Public License (GPL) Version 3 - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup UAVObjectsPlugin UAVObjects Plugin - * @{ - * - * @note Object definition file: lesstabilizationsettings.xml. - * This is an automatically generated file. - * DO NOT modify manually. - * - * @brief The UAVUObjects GCS plugin - *****************************************************************************/ -/* - * 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 "lesstabilizationsettings.h" -#include "uavobjectfield.h" - -const QString LesStabilizationSettings::NAME = QString("LesStabilizationSettings"); - -/** - * Constructor - */ -LesStabilizationSettings::LesStabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME) -{ - // Create fields - QList fields; - QStringList UpdatePeriodElemNames; - UpdatePeriodElemNames.append("0"); - fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT8, UpdatePeriodElemNames, QStringList()) ); - QStringList RollMaxElemNames; - RollMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("RollMax"), QString("Degrees"), UAVObjectField::UINT8, RollMaxElemNames, QStringList()) ); - QStringList PitchMaxElemNames; - PitchMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("PitchMax"), QString("Degrees"), UAVObjectField::UINT8, PitchMaxElemNames, QStringList()) ); - QStringList YawMaxElemNames; - YawMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("YawMax"), QString("Degrees"), UAVObjectField::UINT8, YawMaxElemNames, QStringList()) ); - QStringList ManualRateElemNames; - ManualRateElemNames.append("Roll"); - ManualRateElemNames.append("Pitch"); - ManualRateElemNames.append("Yaw"); - fields.append( new UAVObjectField(QString("ManualRate"), QString("Degrees/sec"), UAVObjectField::FLOAT32, ManualRateElemNames, QStringList()) ); - QStringList MaximumRateElemNames; - MaximumRateElemNames.append("Roll"); - MaximumRateElemNames.append("Pitch"); - MaximumRateElemNames.append("Yaw"); - fields.append( new UAVObjectField(QString("MaximumRate"), QString("Degrees/sec"), UAVObjectField::FLOAT32, MaximumRateElemNames, QStringList()) ); - QStringList RollRatePIElemNames; - RollRatePIElemNames.append("Kp"); - RollRatePIElemNames.append("Ki"); - RollRatePIElemNames.append("ILimit"); - fields.append( new UAVObjectField(QString("RollRatePI"), QString(""), UAVObjectField::FLOAT32, RollRatePIElemNames, QStringList()) ); - QStringList PitchRatePIElemNames; - PitchRatePIElemNames.append("Kp"); - PitchRatePIElemNames.append("Ki"); - PitchRatePIElemNames.append("ILimit"); - fields.append( new UAVObjectField(QString("PitchRatePI"), QString(""), UAVObjectField::FLOAT32, PitchRatePIElemNames, QStringList()) ); - QStringList YawRatePIElemNames; - YawRatePIElemNames.append("Kp"); - YawRatePIElemNames.append("Ki"); - YawRatePIElemNames.append("ILimit"); - fields.append( new UAVObjectField(QString("YawRatePI"), QString(""), UAVObjectField::FLOAT32, YawRatePIElemNames, QStringList()) ); - QStringList RollPIElemNames; - RollPIElemNames.append("Kp"); - RollPIElemNames.append("Ki"); - RollPIElemNames.append("ILimit"); - fields.append( new UAVObjectField(QString("RollPI"), QString(""), UAVObjectField::FLOAT32, RollPIElemNames, QStringList()) ); - QStringList PitchPIElemNames; - PitchPIElemNames.append("Kp"); - PitchPIElemNames.append("Ki"); - PitchPIElemNames.append("ILimit"); - fields.append( new UAVObjectField(QString("PitchPI"), QString(""), UAVObjectField::FLOAT32, PitchPIElemNames, QStringList()) ); - QStringList YawPIElemNames; - YawPIElemNames.append("Kp"); - YawPIElemNames.append("Ki"); - YawPIElemNames.append("ILimit"); - fields.append( new UAVObjectField(QString("YawPI"), QString(""), UAVObjectField::FLOAT32, YawPIElemNames, QStringList()) ); - - // Initialize object - initializeFields(fields, (quint8*)&data, NUMBYTES); - // Set the default field values - setDefaultFieldValues(); -} - -/** - * Get the default metadata for this object - */ -UAVObject::Metadata LesStabilizationSettings::getDefaultMetadata() -{ - UAVObject::Metadata metadata; - metadata.flightAccess = ACCESS_READWRITE; - metadata.gcsAccess = ACCESS_READWRITE; - metadata.gcsTelemetryAcked = 1; - metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; - metadata.gcsTelemetryUpdatePeriod = 0; - metadata.flightTelemetryAcked = 1; - metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; - metadata.flightTelemetryUpdatePeriod = 0; - metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER; - metadata.loggingUpdatePeriod = 0; - return metadata; -} - -/** - * Initialize object fields with the default values. - * If a default value is not specified the object fields - * will be initialized to zero. - */ -void LesStabilizationSettings::setDefaultFieldValues() -{ - data.UpdatePeriod = 10; - data.RollMax = 35; - data.PitchMax = 35; - data.YawMax = 35; - data.ManualRate[0] = 150; - data.ManualRate[1] = 150; - data.ManualRate[2] = 150; - data.MaximumRate[0] = 300; - data.MaximumRate[1] = 300; - data.MaximumRate[2] = 300; - data.RollRatePI[0] = 0.0015; - data.RollRatePI[1] = 0; - data.RollRatePI[2] = 0.5; - data.PitchRatePI[0] = 0.0015; - data.PitchRatePI[1] = 0; - data.PitchRatePI[2] = 0.5; - data.YawRatePI[0] = 0.003; - data.YawRatePI[1] = 0; - data.YawRatePI[2] = 0.5; - data.RollPI[0] = 2; - data.RollPI[1] = 0; - data.RollPI[2] = 0.5; - data.PitchPI[0] = 2; - data.PitchPI[1] = 0; - data.PitchPI[2] = 0.5; - data.YawPI[0] = 2; - data.YawPI[1] = 0; - data.YawPI[2] = 0.5; - -} - -/** - * Get the object data fields - */ -LesStabilizationSettings::DataFields LesStabilizationSettings::getData() -{ - QMutexLocker locker(mutex); - return data; -} - -/** - * Set the object data fields - */ -void LesStabilizationSettings::setData(const DataFields& data) -{ - QMutexLocker locker(mutex); - // Get metadata - Metadata mdata = getMetadata(); - // Update object if the access mode permits - if ( mdata.gcsAccess == ACCESS_READWRITE ) - { - this->data = data; - emit objectUpdatedAuto(this); // trigger object updated event - emit objectUpdated(this); - } -} - -/** - * Create a clone of this object, a new instance ID must be specified. - * Do not use this function directly to create new instances, the - * UAVObjectManager should be used instead. - */ -UAVDataObject* LesStabilizationSettings::clone(quint32 instID) -{ - LesStabilizationSettings* obj = new LesStabilizationSettings(); - obj->initialize(instID, this->getMetaObject()); - return obj; -} - -/** - * Static function to retrieve an instance of the object. - */ -LesStabilizationSettings* LesStabilizationSettings::GetInstance(UAVObjectManager* objMngr, quint32 instID) -{ - return dynamic_cast(objMngr->getObject(LesStabilizationSettings::OBJID, instID)); -} diff --git a/ground/src/plugins/uavobjects/lesstabilizationsettings.h b/ground/src/plugins/uavobjects/lesstabilizationsettings.h deleted file mode 100644 index a7b05f60e..000000000 --- a/ground/src/plugins/uavobjects/lesstabilizationsettings.h +++ /dev/null @@ -1,132 +0,0 @@ -/** - ****************************************************************************** - * - * @file lesstabilizationsettings.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @see The GNU Public License (GPL) Version 3 - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup UAVObjectsPlugin UAVObjects Plugin - * @{ - * - * @note Object definition file: lesstabilizationsettings.xml. - * This is an automatically generated file. - * DO NOT modify manually. - * - * @brief The UAVUObjects GCS plugin - *****************************************************************************/ -/* - * 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 LESSTABILIZATIONSETTINGS_H -#define LESSTABILIZATIONSETTINGS_H - -#include "uavdataobject.h" -#include "uavobjectmanager.h" - -class UAVOBJECTS_EXPORT LesStabilizationSettings: public UAVDataObject -{ - Q_OBJECT - -public: - // Field structure - typedef struct { - quint8 UpdatePeriod; - quint8 RollMax; - quint8 PitchMax; - quint8 YawMax; - float ManualRate[3]; - float MaximumRate[3]; - float RollRatePI[3]; - float PitchRatePI[3]; - float YawRatePI[3]; - float RollPI[3]; - float PitchPI[3]; - float YawPI[3]; - - } __attribute__((packed)) DataFields; - - // Field information - // Field UpdatePeriod information - // Field RollMax information - // Field PitchMax information - // Field YawMax information - // Field ManualRate information - /* Array element names for field ManualRate */ - typedef enum { MANUALRATE_ROLL=0, MANUALRATE_PITCH=1, MANUALRATE_YAW=2 } ManualRateElem; - /* Number of elements for field ManualRate */ - static const quint32 MANUALRATE_NUMELEM = 3; - // Field MaximumRate information - /* Array element names for field MaximumRate */ - typedef enum { MAXIMUMRATE_ROLL=0, MAXIMUMRATE_PITCH=1, MAXIMUMRATE_YAW=2 } MaximumRateElem; - /* Number of elements for field MaximumRate */ - static const quint32 MAXIMUMRATE_NUMELEM = 3; - // Field RollRatePI information - /* Array element names for field RollRatePI */ - typedef enum { ROLLRATEPI_KP=0, ROLLRATEPI_KI=1, ROLLRATEPI_ILIMIT=2 } RollRatePIElem; - /* Number of elements for field RollRatePI */ - static const quint32 ROLLRATEPI_NUMELEM = 3; - // Field PitchRatePI information - /* Array element names for field PitchRatePI */ - typedef enum { PITCHRATEPI_KP=0, PITCHRATEPI_KI=1, PITCHRATEPI_ILIMIT=2 } PitchRatePIElem; - /* Number of elements for field PitchRatePI */ - static const quint32 PITCHRATEPI_NUMELEM = 3; - // Field YawRatePI information - /* Array element names for field YawRatePI */ - typedef enum { YAWRATEPI_KP=0, YAWRATEPI_KI=1, YAWRATEPI_ILIMIT=2 } YawRatePIElem; - /* Number of elements for field YawRatePI */ - static const quint32 YAWRATEPI_NUMELEM = 3; - // Field RollPI information - /* Array element names for field RollPI */ - typedef enum { ROLLPI_KP=0, ROLLPI_KI=1, ROLLPI_ILIMIT=2 } RollPIElem; - /* Number of elements for field RollPI */ - static const quint32 ROLLPI_NUMELEM = 3; - // Field PitchPI information - /* Array element names for field PitchPI */ - typedef enum { PITCHPI_KP=0, PITCHPI_KI=1, PITCHPI_ILIMIT=2 } PitchPIElem; - /* Number of elements for field PitchPI */ - static const quint32 PITCHPI_NUMELEM = 3; - // Field YawPI information - /* Array element names for field YawPI */ - typedef enum { YAWPI_KP=0, YAWPI_KI=1, YAWPI_ILIMIT=2 } YawPIElem; - /* Number of elements for field YawPI */ - static const quint32 YAWPI_NUMELEM = 3; - - - // Constants - static const quint32 OBJID = 3247121950U; - static const QString NAME; - static const bool ISSINGLEINST = 1; - static const bool ISSETTINGS = 1; - static const quint32 NUMBYTES = sizeof(DataFields); - - // Functions - LesStabilizationSettings(); - - DataFields getData(); - void setData(const DataFields& data); - Metadata getDefaultMetadata(); - UAVDataObject* clone(quint32 instID); - - static LesStabilizationSettings* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0); - -private: - DataFields data; - - void setDefaultFieldValues(); - -}; - -#endif // LESSTABILIZATIONSETTINGS_H diff --git a/ground/src/plugins/uavobjects/lesstabilizationsettings.py b/ground/src/plugins/uavobjects/lesstabilizationsettings.py deleted file mode 100644 index 22d0d818f..000000000 --- a/ground/src/plugins/uavobjects/lesstabilizationsettings.py +++ /dev/null @@ -1,212 +0,0 @@ -## -############################################################################## -# -# @file lesstabilizationsettings.py -# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. -# @brief Implementation of the LesStabilizationSettings object. This file has been -# automatically generated by the UAVObjectGenerator. -# -# @note Object definition file: lesstabilizationsettings.xml. -# This is an automatically generated file. -# DO NOT modify manually. -# -# @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 -# - - -import uavobject - -import struct -from collections import namedtuple - -# This is a list of instances of the data fields contained in this object -_fields = [ \ - uavobject.UAVObjectField( - 'UpdatePeriod', - 'B', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'RollMax', - 'B', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'PitchMax', - 'B', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'YawMax', - 'B', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'ManualRate', - 'f', - 3, - [ - 'Roll', - 'Pitch', - 'Yaw', - ], - { - } - ), - uavobject.UAVObjectField( - 'MaximumRate', - 'f', - 3, - [ - 'Roll', - 'Pitch', - 'Yaw', - ], - { - } - ), - uavobject.UAVObjectField( - 'RollRatePI', - 'f', - 3, - [ - 'Kp', - 'Ki', - 'ILimit', - ], - { - } - ), - uavobject.UAVObjectField( - 'PitchRatePI', - 'f', - 3, - [ - 'Kp', - 'Ki', - 'ILimit', - ], - { - } - ), - uavobject.UAVObjectField( - 'YawRatePI', - 'f', - 3, - [ - 'Kp', - 'Ki', - 'ILimit', - ], - { - } - ), - uavobject.UAVObjectField( - 'RollPI', - 'f', - 3, - [ - 'Kp', - 'Ki', - 'ILimit', - ], - { - } - ), - uavobject.UAVObjectField( - 'PitchPI', - 'f', - 3, - [ - 'Kp', - 'Ki', - 'ILimit', - ], - { - } - ), - uavobject.UAVObjectField( - 'YawPI', - 'f', - 3, - [ - 'Kp', - 'Ki', - 'ILimit', - ], - { - } - ), -] - - -class LesStabilizationSettings(uavobject.UAVObject): - ## Object constants - OBJID = 3247121950 - NAME = "LesStabilizationSettings" - METANAME = "LesStabilizationSettingsMeta" - ISSINGLEINST = 1 - ISSETTINGS = 1 - - def __init__(self): - uavobject.UAVObject.__init__(self, - self.OBJID, - self.NAME, - self.METANAME, - 0, - self.ISSINGLEINST) - - for f in _fields: - self.add_field(f) - - def __str__(self): - s = ("0x%08X (%10u) %-30s %3u bytes format '%s'\n" - % (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format)) - for f in self.get_tuple()._fields: - s += ("\t%s\n" % f) - return (s) - -def main(): - # Instantiate the object and dump out some interesting info - x = LesStabilizationSettings() - print (x) - -if __name__ == "__main__": - #import pdb ; pdb.run('main()') - main() diff --git a/ground/src/plugins/uavobjects/stabilizationsettings.cpp b/ground/src/plugins/uavobjects/stabilizationsettings.cpp index b24e78737..3d521aeac 100644 --- a/ground/src/plugins/uavobjects/stabilizationsettings.cpp +++ b/ground/src/plugins/uavobjects/stabilizationsettings.cpp @@ -44,61 +44,53 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS QList fields; QStringList RollMaxElemNames; RollMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("RollMax"), QString("degrees"), UAVObjectField::UINT8, RollMaxElemNames, QStringList()) ); + fields.append( new UAVObjectField(QString("RollMax"), QString("Degrees"), UAVObjectField::UINT8, RollMaxElemNames, QStringList()) ); QStringList PitchMaxElemNames; PitchMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("PitchMax"), QString("degrees"), UAVObjectField::UINT8, PitchMaxElemNames, QStringList()) ); + fields.append( new UAVObjectField(QString("PitchMax"), QString("Degrees"), UAVObjectField::UINT8, PitchMaxElemNames, QStringList()) ); QStringList YawMaxElemNames; YawMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("YawMax"), QString("degrees"), UAVObjectField::UINT8, YawMaxElemNames, QStringList()) ); - QStringList YawModeElemNames; - YawModeElemNames.append("0"); - QStringList YawModeEnumOptions; - YawModeEnumOptions.append("rate"); - YawModeEnumOptions.append("heading"); - fields.append( new UAVObjectField(QString("YawMode"), QString("degrees"), UAVObjectField::ENUM, YawModeElemNames, YawModeEnumOptions) ); - QStringList ThrottleMaxElemNames; - ThrottleMaxElemNames.append("0"); - fields.append( new UAVObjectField(QString("ThrottleMax"), QString("frac"), UAVObjectField::FLOAT32, ThrottleMaxElemNames, QStringList()) ); - QStringList ThrottleMinElemNames; - ThrottleMinElemNames.append("0"); - fields.append( new UAVObjectField(QString("ThrottleMin"), QString("frac"), UAVObjectField::FLOAT32, ThrottleMinElemNames, QStringList()) ); - QStringList RollIntegralLimitElemNames; - RollIntegralLimitElemNames.append("0"); - fields.append( new UAVObjectField(QString("RollIntegralLimit"), QString(""), UAVObjectField::FLOAT32, RollIntegralLimitElemNames, QStringList()) ); - QStringList PitchIntegralLimitElemNames; - PitchIntegralLimitElemNames.append("0"); - fields.append( new UAVObjectField(QString("PitchIntegralLimit"), QString(""), UAVObjectField::FLOAT32, PitchIntegralLimitElemNames, QStringList()) ); - QStringList YawIntegralLimitElemNames; - YawIntegralLimitElemNames.append("0"); - fields.append( new UAVObjectField(QString("YawIntegralLimit"), QString(""), UAVObjectField::FLOAT32, YawIntegralLimitElemNames, QStringList()) ); - QStringList PitchKpElemNames; - PitchKpElemNames.append("0"); - fields.append( new UAVObjectField(QString("PitchKp"), QString(""), UAVObjectField::FLOAT32, PitchKpElemNames, QStringList()) ); - QStringList PitchKiElemNames; - PitchKiElemNames.append("0"); - fields.append( new UAVObjectField(QString("PitchKi"), QString(""), UAVObjectField::FLOAT32, PitchKiElemNames, QStringList()) ); - QStringList PitchKdElemNames; - PitchKdElemNames.append("0"); - fields.append( new UAVObjectField(QString("PitchKd"), QString(""), UAVObjectField::FLOAT32, PitchKdElemNames, QStringList()) ); - QStringList RollKpElemNames; - RollKpElemNames.append("0"); - fields.append( new UAVObjectField(QString("RollKp"), QString(""), UAVObjectField::FLOAT32, RollKpElemNames, QStringList()) ); - QStringList RollKiElemNames; - RollKiElemNames.append("0"); - fields.append( new UAVObjectField(QString("RollKi"), QString(""), UAVObjectField::FLOAT32, RollKiElemNames, QStringList()) ); - QStringList RollKdElemNames; - RollKdElemNames.append("0"); - fields.append( new UAVObjectField(QString("RollKd"), QString(""), UAVObjectField::FLOAT32, RollKdElemNames, QStringList()) ); - QStringList YawKpElemNames; - YawKpElemNames.append("0"); - fields.append( new UAVObjectField(QString("YawKp"), QString(""), UAVObjectField::FLOAT32, YawKpElemNames, QStringList()) ); - QStringList YawKiElemNames; - YawKiElemNames.append("0"); - fields.append( new UAVObjectField(QString("YawKi"), QString(""), UAVObjectField::FLOAT32, YawKiElemNames, QStringList()) ); - QStringList YawKdElemNames; - YawKdElemNames.append("0"); - fields.append( new UAVObjectField(QString("YawKd"), QString(""), UAVObjectField::FLOAT32, YawKdElemNames, QStringList()) ); + fields.append( new UAVObjectField(QString("YawMax"), QString("Degrees"), UAVObjectField::UINT8, YawMaxElemNames, QStringList()) ); + QStringList ManualRateElemNames; + ManualRateElemNames.append("Roll"); + ManualRateElemNames.append("Pitch"); + ManualRateElemNames.append("Yaw"); + fields.append( new UAVObjectField(QString("ManualRate"), QString("Degrees/sec"), UAVObjectField::FLOAT32, ManualRateElemNames, QStringList()) ); + QStringList MaximumRateElemNames; + MaximumRateElemNames.append("Roll"); + MaximumRateElemNames.append("Pitch"); + MaximumRateElemNames.append("Yaw"); + fields.append( new UAVObjectField(QString("MaximumRate"), QString("Degrees/sec"), UAVObjectField::FLOAT32, MaximumRateElemNames, QStringList()) ); + QStringList RollRatePIElemNames; + RollRatePIElemNames.append("Kp"); + RollRatePIElemNames.append("Ki"); + RollRatePIElemNames.append("ILimit"); + fields.append( new UAVObjectField(QString("RollRatePI"), QString(""), UAVObjectField::FLOAT32, RollRatePIElemNames, QStringList()) ); + QStringList PitchRatePIElemNames; + PitchRatePIElemNames.append("Kp"); + PitchRatePIElemNames.append("Ki"); + PitchRatePIElemNames.append("ILimit"); + fields.append( new UAVObjectField(QString("PitchRatePI"), QString(""), UAVObjectField::FLOAT32, PitchRatePIElemNames, QStringList()) ); + QStringList YawRatePIElemNames; + YawRatePIElemNames.append("Kp"); + YawRatePIElemNames.append("Ki"); + YawRatePIElemNames.append("ILimit"); + fields.append( new UAVObjectField(QString("YawRatePI"), QString(""), UAVObjectField::FLOAT32, YawRatePIElemNames, QStringList()) ); + QStringList RollPIElemNames; + RollPIElemNames.append("Kp"); + RollPIElemNames.append("Ki"); + RollPIElemNames.append("ILimit"); + fields.append( new UAVObjectField(QString("RollPI"), QString(""), UAVObjectField::FLOAT32, RollPIElemNames, QStringList()) ); + QStringList PitchPIElemNames; + PitchPIElemNames.append("Kp"); + PitchPIElemNames.append("Ki"); + PitchPIElemNames.append("ILimit"); + fields.append( new UAVObjectField(QString("PitchPI"), QString(""), UAVObjectField::FLOAT32, PitchPIElemNames, QStringList()) ); + QStringList YawPIElemNames; + YawPIElemNames.append("Kp"); + YawPIElemNames.append("Ki"); + YawPIElemNames.append("ILimit"); + fields.append( new UAVObjectField(QString("YawPI"), QString(""), UAVObjectField::FLOAT32, YawPIElemNames, QStringList()) ); // Initialize object initializeFields(fields, (quint8*)&data, NUMBYTES); @@ -135,21 +127,30 @@ void StabilizationSettings::setDefaultFieldValues() data.RollMax = 35; data.PitchMax = 35; data.YawMax = 35; - data.YawMode = 0; - data.ThrottleMax = 1; - data.ThrottleMin = -1; - data.RollIntegralLimit = 0.5; - data.PitchIntegralLimit = 0.5; - data.YawIntegralLimit = 0.5; - 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 = 0.04; - data.YawKi = 4e-06; - data.YawKd = 0.01; + data.ManualRate[0] = 150; + data.ManualRate[1] = 150; + data.ManualRate[2] = 150; + data.MaximumRate[0] = 300; + data.MaximumRate[1] = 300; + data.MaximumRate[2] = 300; + data.RollRatePI[0] = 0.0015; + data.RollRatePI[1] = 0; + data.RollRatePI[2] = 0.5; + data.PitchRatePI[0] = 0.0015; + data.PitchRatePI[1] = 0; + data.PitchRatePI[2] = 0.5; + data.YawRatePI[0] = 0.003; + data.YawRatePI[1] = 0; + data.YawRatePI[2] = 0.5; + data.RollPI[0] = 2; + data.RollPI[1] = 0; + data.RollPI[2] = 0.5; + data.PitchPI[0] = 2; + data.PitchPI[1] = 0; + data.PitchPI[2] = 0.5; + data.YawPI[0] = 2; + data.YawPI[1] = 0; + data.YawPI[2] = 0.5; } diff --git a/ground/src/plugins/uavobjects/stabilizationsettings.h b/ground/src/plugins/uavobjects/stabilizationsettings.h index 097189065..6f81e82f0 100644 --- a/ground/src/plugins/uavobjects/stabilizationsettings.h +++ b/ground/src/plugins/uavobjects/stabilizationsettings.h @@ -46,21 +46,14 @@ public: quint8 RollMax; quint8 PitchMax; quint8 YawMax; - quint8 YawMode; - float ThrottleMax; - float ThrottleMin; - float RollIntegralLimit; - float PitchIntegralLimit; - float YawIntegralLimit; - float PitchKp; - float PitchKi; - float PitchKd; - float RollKp; - float RollKi; - float RollKd; - float YawKp; - float YawKi; - float YawKd; + float ManualRate[3]; + float MaximumRate[3]; + float RollRatePI[3]; + float PitchRatePI[3]; + float YawRatePI[3]; + float RollPI[3]; + float PitchPI[3]; + float YawPI[3]; } __attribute__((packed)) DataFields; @@ -68,27 +61,50 @@ public: // Field RollMax information // Field PitchMax information // Field YawMax information - // Field YawMode information - /* Enumeration options for field YawMode */ - typedef enum { YAWMODE_RATE=0, YAWMODE_HEADING=1 } YawModeOptions; - // Field ThrottleMax information - // Field ThrottleMin information - // Field RollIntegralLimit information - // Field PitchIntegralLimit information - // Field YawIntegralLimit 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 + // Field ManualRate information + /* Array element names for field ManualRate */ + typedef enum { MANUALRATE_ROLL=0, MANUALRATE_PITCH=1, MANUALRATE_YAW=2 } ManualRateElem; + /* Number of elements for field ManualRate */ + static const quint32 MANUALRATE_NUMELEM = 3; + // Field MaximumRate information + /* Array element names for field MaximumRate */ + typedef enum { MAXIMUMRATE_ROLL=0, MAXIMUMRATE_PITCH=1, MAXIMUMRATE_YAW=2 } MaximumRateElem; + /* Number of elements for field MaximumRate */ + static const quint32 MAXIMUMRATE_NUMELEM = 3; + // Field RollRatePI information + /* Array element names for field RollRatePI */ + typedef enum { ROLLRATEPI_KP=0, ROLLRATEPI_KI=1, ROLLRATEPI_ILIMIT=2 } RollRatePIElem; + /* Number of elements for field RollRatePI */ + static const quint32 ROLLRATEPI_NUMELEM = 3; + // Field PitchRatePI information + /* Array element names for field PitchRatePI */ + typedef enum { PITCHRATEPI_KP=0, PITCHRATEPI_KI=1, PITCHRATEPI_ILIMIT=2 } PitchRatePIElem; + /* Number of elements for field PitchRatePI */ + static const quint32 PITCHRATEPI_NUMELEM = 3; + // Field YawRatePI information + /* Array element names for field YawRatePI */ + typedef enum { YAWRATEPI_KP=0, YAWRATEPI_KI=1, YAWRATEPI_ILIMIT=2 } YawRatePIElem; + /* Number of elements for field YawRatePI */ + static const quint32 YAWRATEPI_NUMELEM = 3; + // Field RollPI information + /* Array element names for field RollPI */ + typedef enum { ROLLPI_KP=0, ROLLPI_KI=1, ROLLPI_ILIMIT=2 } RollPIElem; + /* Number of elements for field RollPI */ + static const quint32 ROLLPI_NUMELEM = 3; + // Field PitchPI information + /* Array element names for field PitchPI */ + typedef enum { PITCHPI_KP=0, PITCHPI_KI=1, PITCHPI_ILIMIT=2 } PitchPIElem; + /* Number of elements for field PitchPI */ + static const quint32 PITCHPI_NUMELEM = 3; + // Field YawPI information + /* Array element names for field YawPI */ + typedef enum { YAWPI_KP=0, YAWPI_KI=1, YAWPI_ILIMIT=2 } YawPIElem; + /* Number of elements for field YawPI */ + static const quint32 YAWPI_NUMELEM = 3; // Constants - static const quint32 OBJID = 1346414844U; + static const quint32 OBJID = 3792991236U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 1; diff --git a/ground/src/plugins/uavobjects/stabilizationsettings.py b/ground/src/plugins/uavobjects/stabilizationsettings.py index 3adb9f8a4..52634dc85 100644 --- a/ground/src/plugins/uavobjects/stabilizationsettings.py +++ b/ground/src/plugins/uavobjects/stabilizationsettings.py @@ -68,153 +68,97 @@ _fields = [ \ } ), uavobject.UAVObjectField( - 'YawMode', - 'b', - 1, - [ - '0', - ], - { - '0' : 'rate', - '1' : 'heading', - } - ), - uavobject.UAVObjectField( - 'ThrottleMax', + 'ManualRate', 'f', - 1, + 3, [ - '0', + 'Roll', + 'Pitch', + 'Yaw', ], { } ), uavobject.UAVObjectField( - 'ThrottleMin', + 'MaximumRate', 'f', - 1, + 3, [ - '0', + 'Roll', + 'Pitch', + 'Yaw', ], { } ), uavobject.UAVObjectField( - 'RollIntegralLimit', + 'RollRatePI', 'f', - 1, + 3, [ - '0', + 'Kp', + 'Ki', + 'ILimit', ], { } ), uavobject.UAVObjectField( - 'PitchIntegralLimit', + 'PitchRatePI', 'f', - 1, + 3, [ - '0', + 'Kp', + 'Ki', + 'ILimit', ], { } ), uavobject.UAVObjectField( - 'YawIntegralLimit', + 'YawRatePI', 'f', - 1, + 3, [ - '0', + 'Kp', + 'Ki', + 'ILimit', ], { } ), uavobject.UAVObjectField( - 'PitchKp', + 'RollPI', 'f', - 1, + 3, [ - '0', + 'Kp', + 'Ki', + 'ILimit', ], { } ), uavobject.UAVObjectField( - 'PitchKi', + 'PitchPI', 'f', - 1, + 3, [ - '0', + 'Kp', + 'Ki', + 'ILimit', ], { } ), uavobject.UAVObjectField( - 'PitchKd', + 'YawPI', 'f', - 1, + 3, [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'RollKp', - 'f', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'RollKi', - 'f', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'RollKd', - 'f', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'YawKp', - 'f', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'YawKi', - 'f', - 1, - [ - '0', - ], - { - } - ), - uavobject.UAVObjectField( - 'YawKd', - 'f', - 1, - [ - '0', + 'Kp', + 'Ki', + 'ILimit', ], { } @@ -224,7 +168,7 @@ _fields = [ \ class StabilizationSettings(uavobject.UAVObject): ## Object constants - OBJID = 1346414844 + OBJID = 3792991236 NAME = "StabilizationSettings" METANAME = "StabilizationSettingsMeta" ISSINGLEINST = 1 diff --git a/ground/src/plugins/uavobjects/uavobjects.pro b/ground/src/plugins/uavobjects/uavobjects.pro index 6ebbfa811..eb2903821 100644 --- a/ground/src/plugins/uavobjects/uavobjects.pro +++ b/ground/src/plugins/uavobjects/uavobjects.pro @@ -43,7 +43,6 @@ HEADERS += uavobjects_global.h \ velocityactual.h \ guidancesettings.h \ positiondesired.h \ - lesstabilizationsettings.h \ firmwareiapobj.h SOURCES += uavobject.cpp \ @@ -85,6 +84,5 @@ SOURCES += uavobject.cpp \ velocityactual.cpp \ guidancesettings.cpp \ positiondesired.cpp \ - lesstabilizationsettings.cpp \ firmwareiapobj.cpp OTHER_FILES += UAVObjects.pluginspec diff --git a/ground/src/plugins/uavobjects/uavobjectsinit.cpp b/ground/src/plugins/uavobjects/uavobjectsinit.cpp index 15c203ba9..d200ec630 100644 --- a/ground/src/plugins/uavobjects/uavobjectsinit.cpp +++ b/ground/src/plugins/uavobjects/uavobjectsinit.cpp @@ -49,7 +49,6 @@ #include "gpstime.h" #include "guidancesettings.h" #include "homelocation.h" -#include "lesstabilizationsettings.h" #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" #include "mixersettings.h" @@ -91,7 +90,6 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr) objMngr->registerObject( new GPSTime() ); objMngr->registerObject( new GuidanceSettings() ); objMngr->registerObject( new HomeLocation() ); - objMngr->registerObject( new LesStabilizationSettings() ); objMngr->registerObject( new ManualControlCommand() ); objMngr->registerObject( new ManualControlSettings() ); objMngr->registerObject( new MixerSettings() ); diff --git a/ground/src/shared/uavobjectdefinition/lesstabilizationsettings.xml b/ground/src/shared/uavobjectdefinition/lesstabilizationsettings.xml deleted file mode 100644 index a18640811..000000000 --- a/ground/src/shared/uavobjectdefinition/lesstabilizationsettings.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - - - - - - - - - - - - - - - - - - - - diff --git a/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml b/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml index f34ffaa91..4afae71ec 100644 --- a/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml @@ -1,27 +1,22 @@ - - PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - - - - - - - - - - - - - - - - - - - - - - + + PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired + + + + + + + + + + + + + + + + +