OP-1309 added cruisecontrol function library and outer loop template

flight/modules/Stabilization/cruisecontrol.c

@@ -0,0 +1,94 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief cruisecontrol mode
+ * @note This file implements the logic for a cruisecontrol
+ * @{
+ *
+ * @file       cruisecontrol.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @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 <attitudestate.h>
+
+static float cruisecontrol_factor = 1.0f;
+
+void cruisecontrol_compute_factor(AttitudeStateData *attitude)
+{
+    float angleCosine;
+
+    // get attitude state and calculate angle
+    // spherical right triangle
+    // 0 <= acosf() <= Pi
+
+    // angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
+    // more efficient calculation: see coordinateconversion.h Quaternion2R()
+    angleCosine = (attitude->q1 * attitude->q1) - (attitude->q2 * attitude->q2) - (attitude->q3 * attitude->q3) + (attitude->q4 * attitude->q4);
+
+    // if past the cutoff angle (60 to 180 (180 means never))
+    if (angleCosine < stabSettings.cruiseControl.cruise_control_max_angle_cosine) {
+        // -1 reversed collective, 0 zero power, or 1 normal power
+        // these are all unboosted
+        cruisecontrol_factor = stabSettings.cruiseControl.cruise_control_inverted_power_switch;
+    } else {
+        // avoid singularity
+        if (angleCosine > -1e-4f && angleCosine < 1e-4f) {
+            cruisecontrol_factor = stabSettings.cruiseControl.cruise_control_max_power_factor;
+        } else {
+            cruisecontrol_factor = boundf(1.0f / angleCosine,
+                                          -stabSettings.cruiseControl.cruise_control_max_power_factor,
+                                          stabSettings.cruiseControl.cruise_control_max_power_factor);
+        }
+        // factor in the power trim, no effect at 1.0, linear effect increases with factor
+        cruisecontrol_factor = (cruisecontrol_factor - 1.0f) * stabSettings.cruiseControl.cruise_control_power_trim + 1.0f;
+        // if inverted and they want negative boost
+        if (angleCosine < 0.0f && stabSettings.cruiseControl.cruise_control_inverted_power_switch == (int8_t)-1) {
+            cruisecontrol_factor = -cruisecontrol_factor;
+            // WARNING: might not go well together with power_trim adjustment above, WARNING: might have bad side effects regarding wound up integral accumulators in all 3 axis at transition stage!
+            // as long as thrust is getting reversed
+            // we may as well do pitch and yaw for a complete "invert switch"
+            // WARNING: currently not implementable in this control scheme, needs to be done elsewhere!
+            // actuatorDesired.Pitch = -actuatorDesired.Pitch;
+            // actuatorDesired.Yaw   = -actuatorDesired.Yaw;
+        }
+    }
+}
+
+
+float cruisecontrol_apply_factor(float raw)
+{
+    if (stabSettings.cruiseControl.cruise_control_max_power_factor > 0.0001f) {
+        // don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
+        if (raw > stabSettings.cruiseControl.cruise_control_min_thrust) {
+            // quad    example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
+            // CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
+            raw = boundf((raw - stabSettings.cruiseControl.cruise_control_neutral_thrust) * cruisecontrol_factor + stabSettings.cruiseControl.cruise_control_neutral_thrust,
+                         stabSettings.cruiseControl.cruise_control_min_thrust,
+                         stabSettings.cruiseControl.cruise_control_max_thrust);
+        }
+    }
+    return raw;
+}

flight/modules/Stabilization/inc/cruisecontrol.h

@@ -33,6 +33,10 @@
 #ifndef CRUISECONTROL_H
 #define CRUISECONTROL_H
 
+#include <openpilot.h>
+#include <attitudestate.h>
+
+void cruisecontrol_compute_factor(AttitudeStateData *attitude);
 float cruisecontrol_apply_factor(float raw);
 
 #endif /* CRUISECONTROL_H */

flight/modules/Stabilization/inc/outerloop.h

@@ -0,0 +1,38 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief outerloop mode
+ * @note This file implements the logic for a outerloop
+ * @{
+ *
+ * @file       outerloop.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @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 OUTERLOOP_H
+#define OUTERLOOP_H
+
+void stabilizationOuterloopInit();
+
+#endif /* OUTERLOOP_H */

flight/modules/Stabilization/inc/stabilization.h

@@ -33,6 +33,7 @@
 #ifndef STABILIZATION_H
 #define STABILIZATION_H
 
+#include <openpilot.h>
 #include <stabilizationsettings.h>
 
 int32_t StabilizationInitialize();
@@ -40,6 +41,15 @@ int32_t StabilizationInitialize();
 typedef struct {
     StabilizationSettingsData settings;
     float gyro_alpha;
+    struct {
+        float  cruise_control_min_thrust;
+        float  cruise_control_max_thrust;
+        float  cruise_control_max_angle_cosine;
+        float  cruise_control_max_power_factor;
+        float  cruise_control_power_trim;
+        int8_t cruise_control_inverted_power_switch; // WARNING: currently -1 is not fully implemented !!!
+        float  cruise_control_neutral_thrust;
+    } cruiseControl;
 } StabilizationData;
 
 

flight/modules/Stabilization/innerloop.c

@@ -8,7 +8,7 @@
  * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
  * @{
  *
- * @file       rateloop.c
+ * @file       innerloop.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  * @brief      Attitude stabilization module.
  *
@@ -157,6 +157,9 @@ static void stabilizationInnerloopTask()
 
         actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
     }
+
+    actuator.UpdateTime = dT * 1000;
+
     if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
         ActuatorDesiredSet(&actuator);
     } else {

flight/modules/Stabilization/outerloop.c

@@ -0,0 +1,172 @@
+/**
+ ******************************************************************************
+ * @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 AttitudeState "Attitude State"
+ * @{
+ *
+ * @file       outerloop.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @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 <pios_struct_helper.h>
+#include <pid.h>
+#include <callbackinfo.h>
+#include <ratedesired.h>
+#include <stabilizationdesired.h>
+// #include <gyrostate.h>
+// #include <airspeedstate.h>
+#include <stabilizationstatus.h>
+// #include <flightstatus.h>
+#include <stabilizationbank.h>
+
+#include <stabilization.h>
+#include <cruisecontrol.h>
+
+// Private constants
+#define STACK_SIZE_BYTES  256
+
+#define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
+
+#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_MIN        1.0e-6f
+#define UPDATE_MAX        1.0f
+#define UPDATE_ALPHA      1.0e-2f
+
+#define AXES              4
+
+// Private variables
+static DelayedCallbackInfo *callbackHandle;
+static struct pid pids[3];
+static AttitudeStateData attitude;
+static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
+static PiOSDeltatimeConfig timeval;
+
+// Private functions
+static void stabilizationOuterloopTask();
+static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+
+void stabilizationOuterloopInit()
+{
+    RateDesiredInitialize();
+    StabilizationDesiredInitialize();
+    AttitudeStateInitialize();
+    StabilizationStatusInitialize();
+    StabilizationBankInitialize();
+    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+
+    callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES);
+    AttitudeStateConnectCallback(AttitudeStateUpdatedCb);
+    StabilizationBankConnectCallback(BankUpdatedCb);
+    BankUpdatedCb(NULL);
+}
+
+
+/**
+ * WARNING! This callback executes with critical flight control priority every
+ * time a gyroscope update happens do NOT put any time consuming calculations
+ * in this loop unless they really have to execute with every gyro update
+ */
+static void stabilizationOuterloopTask()
+{
+    RateDesiredData rateDesired;
+    StabilizationDesiredData stabilizationDesired;
+    StabilizationStatusOuterLoopData enabled;
+
+    StabilizationDesiredGet(&stabilizationDesired);
+    RateDesiredGet(&rateDesired);
+    StabilizationStatusOuterLoopGet(&enabled);
+    float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
+    float *rateDesiredAxis = &rateDesired.Roll;
+    int t;
+    // float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+
+    for (t = 0; t < AXES; t++) {
+        bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
+        previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
+
+        if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
+            if (reinit) {
+                pids[t].iAccumulator = 0;
+            }
+            switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
+            case STABILIZATIONSTATUS_OUTERLOOP_MANUAL:
+            default:
+                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
+                break;
+            }
+        } else {
+            switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
+            default:
+                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
+                break;
+            }
+        }
+    }
+
+    RateDesiredSet(&rateDesired);
+}
+
+
+static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    // this does not need mutex protection as both eventdispatcher and stabi run in same callback task!
+    AttitudeStateGet(&attitude);
+
+    PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
+}
+
+static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    StabilizationBankData bank;
+
+    StabilizationBankGet(&bank);
+
+    // Set the roll rate PID constants
+    pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], bank.RollRatePID.Kp,
+                  bank.RollRatePID.Ki,
+                  bank.RollRatePID.Kd,
+                  bank.RollRatePID.ILimit);
+
+    // Set the pitch rate PID constants
+    pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], bank.PitchRatePID.Kp,
+                  bank.PitchRatePID.Ki,
+                  bank.PitchRatePID.Kd,
+                  bank.PitchRatePID.ILimit);
+
+    // Set the yaw rate PID constants
+    pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], bank.YawRatePID.Kp,
+                  bank.YawRatePID.Ki,
+                  bank.YawRatePID.Kd,
+                  bank.YawRatePID.ILimit);
+}
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Stabilization/stabilization.c

@@ -53,6 +53,7 @@
 #include "flightmodesettings.h"
 #include "taskinfo.h"
 
+
 // Math libraries
 #include "CoordinateConversions.h"
 #include "pid.h"

shared/uavobjectdefinition/stabilizationstatus.xml

@@ -3,7 +3,7 @@
         <description>Contains status information to control submodules for stabilization.</description>
 
 
-	<field name="OuterLoop" units="" type="enum" options="None,Attitude,Rattitude,Weakleveling,Axislock,Manual,Altitude">
+	<field name="OuterLoop" units="" type="enum" options="Manual,Attitude,Rattitude,Weakleveling,Axislock,Altitude,VerticalVelocity">
 		<elementnames>
 			<elementname>Roll</elementname>
 			<elementname>Pitch</elementname>