Merge branch 'steve/OP-1803_s.bus_dropped_frames' into steve/OP-1849_flexio_com_bridge

.gitignore

@@ -2,6 +2,7 @@
 /downloads
 /tools
 /build
+/3rdparty
 
 # Exclude temporary and system files
 Thumbs.db
@@ -11,6 +12,7 @@ GRTAGS
 GSYMS
 GTAGS
 core
+*~
 
 # flight
 /flight/*.pnproj

Makefile

@@ -113,6 +113,9 @@ endif
 # Include tools installers
 include $(ROOT_DIR)/make/tools.mk
 
+# Include third party builders if available
+-include $(ROOT_DIR)/make/3rdparty/3rdparty.mk
+
 # We almost need to consider autoconf/automake instead of this
 ifeq ($(UNAME), Linux)
     QT_SPEC = linux-g++
@@ -206,10 +209,9 @@ export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics
 DIRS += $(OPGCSSYNTHDIR)
 
 # Define supported board lists
-ALL_BOARDS    := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
+ALL_BOARDS    := oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
 
 # Short names of each board (used to display board name in parallel builds)
-coptercontrol_short    := 'cc  '
 oplinkmini_short       := 'oplm'
 revolution_short       := 'revo'
 osd_short              := 'osd '
@@ -482,6 +484,8 @@ openpilotgcs_clean:
 	@$(ECHO) " CLEAN      $(call toprel, $(OPENPILOTGCS_DIR))"
 	$(V1) [ ! -d "$(OPENPILOTGCS_DIR)" ] || $(RM) -r "$(OPENPILOTGCS_DIR)"
 
+
+
 ################################
 #
 # Serial Uploader tool

WHATSNEW.txt

@@ -1,3 +1,22 @@
+--- RELEASE-15.02.02 ---
+This release fixes a bug that prevents revo onboard mag to work correctly.
+
+Release Notes - OpenPilot - Version RELEASE-15.02.02
+
+The full list of bugfixes in this release is accessible here:
+https://progress.openpilot.org/issues/?filter=12262
+
+** Bug
+    * [OP-1820] - fix onboard mag orientation
+    * [OP-1821] - Tricopter tail servo wrong speed on wizard
+    * [OP-1827] - Version ID wrong in Windows uninstaller
+    * [OP-1857] - PPM on Flexi does not work on CC/CC3D
+
+** Task
+    * [OP-1831] - due to oneshot higher pid values ki now shows "red" warning in stabilization page 
+
+
+
 --- RELEASE-15.02.01 ---
 This release fixes an in important bug that may prevent failsafe to work correctly using CC3D/CC with a PWM receiver.
 

flight/libraries/pid/pidcontroldown.cpp

@@ -45,7 +45,6 @@ extern "C" {
 #include <vtolselftuningstats.h>
 }
 
-#include "pathfollowerfsm.h"
 #include "pidcontroldown.h"
 
 #define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
@@ -59,17 +58,17 @@ extern "C" {
 
 PIDControlDown::PIDControlDown()
     : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
-    mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
+    mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
-    mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
+    mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true)
 {
     Deactivate();
 }
 
 PIDControlDown::~PIDControlDown() {}
 
-void PIDControlDown::Initialize(PathFollowerFSM *fsm)
+void PIDControlDown::Initialize(PIDControlDownCallback *callback)
 {
-    mFSM = fsm;
+    mCallback = callback;
 }
 
 void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
@@ -80,7 +79,6 @@ void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
 
 void PIDControlDown::Deactivate()
 {
-    // pid_zero(&PID);
     mActive = false;
 }
 
@@ -333,9 +331,9 @@ void PIDControlDown::UpdateVelocityState(float pv)
 {
     mVelocityState = pv;
 
-    if (mFSM) {
+    if (mCallback) {
         // The FSM controls the actual descent velocity and introduces step changes as required
-        float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget);
+        float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget);
         // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
         mVelocitySetpointCurrent = velocitySetpointDesired;
     } else {
@@ -354,8 +352,8 @@ float PIDControlDown::GetDownCommand(void)
     float ulow  = mMinThrust;
     float uhigh = mMaxThrust;
 
-    if (mFSM) {
+    if (mCallback) {
-        mFSM->BoundThrust(ulow, uhigh);
+        mCallback->BoundThrust(ulow, uhigh);
     }
     float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
     pidStatus.setpoint = mVelocitySetpointCurrent;

flight/libraries/pid/pidcontroldown.h

@@ -34,16 +34,20 @@ extern "C" {
 #include <pid.h>
 #include <stabilizationdesired.h>
 }
-#include "pathfollowerfsm.h"
+#include "pidcontroldowncallback.h"
 
 class PIDControlDown {
 public:
     PIDControlDown();
     ~PIDControlDown();
-    void Initialize(PathFollowerFSM *fsm);
+    void Initialize(PIDControlDownCallback *callback);
     void SetThrustLimits(float min_thrust, float max_thrust);
     void Deactivate();
     void Activate();
+    bool IsActive()
+    {
+        return mActive;
+    }
     void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
     void UpdateNeutralThrust(float neutral);
     void UpdateVelocitySetpoint(float setpoint);
@@ -58,6 +62,14 @@ public:
     void ControlPositionWithPath(struct path_status *progress);
     void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
     void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
+    void DisableNeutralThrustCalc()
+    {
+        mAllowNeutralThrustCalc = false;
+    }
+    void EnableNeutralThrustCalc()
+    {
+        mAllowNeutralThrustCalc = true;
+    }
 
 private:
     void setup_neutralThrustCalc();
@@ -69,7 +81,7 @@ private:
     float mVelocitySetpointCurrent;
     float mVelocityState;
     float mDownCommand;
-    PathFollowerFSM *mFSM;
+    PIDControlDownCallback *mCallback;
     float mNeutral;
     float mVelocityMax;
     struct pid PIDpos;
@@ -77,7 +89,6 @@ private:
     float mPositionState;
     float mMinThrust;
     float mMaxThrust;
-    uint8_t mActive;
 
     struct NeutralThrustEstimation {
         uint32_t count;
@@ -90,6 +101,8 @@ private:
         bool     have_correction;
     };
     struct NeutralThrustEstimation neutralThrustEst;
+    bool mActive;
+    bool mAllowNeutralThrustCalc;
 };
 
 #endif // PIDCONTROLDOWN_H

flight/libraries/pid/pidcontroldowncallback.h

@@ -1,10 +1,15 @@
 /**
  ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup PID Library
+ * @brief Thrust control callback pure virtual
+ * @{
+ *
+ * @file       pidcontroldowncallback.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Interface class for PathFollower FSMs
  *
- * @file       pios_config.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      PiOS configuration header.
- *             Central compile time config for the project.
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -23,26 +28,15 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#ifndef PIDCONTROLDOWNCALLBACK_H
+#define PIDCONTROLDOWNCALLBACK_H
 
+class PIDControlDownCallback {
+public:
+    // PIDControlDownCalback() {};
+    virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0;
+    virtual float BoundVelocityDown(float velocity) = 0;
+    // virtual ~PIDControlDownCalback() = 0;
+};
 
-#ifndef PIOS_CONFIG_POSIX_H
+#endif // PIDCONTROLDOWNCALLBACK_H
-#define PIOS_CONFIG_POSIX_H
-
-
-/* Enable/Disable PiOS Modules */
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_FREERTOS
-#define PIOS_INCLUDE_CALLBACKSCHEDULER
-#define PIOS_INCLUDE_TASK_MONITOR
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_UDP
-#define PIOS_INCLUDE_SERVO
-
-
-/* Defaults for Logging */
-#define LOG_FILENAME        "PIOS.LOG"
-#define STARTUP_LOG_ENABLED 1
-
-#endif /* PIOS_CONFIG_POSIX_H */

flight/libraries/plans.c

@@ -157,7 +157,7 @@ void plan_setup_returnToBase()
     pathDesired.StartingVelocity = 0.0f;
     pathDesired.EndingVelocity   = 0.0f;
 
-    uint8_t ReturnToBaseNextCommand;
+    FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
     FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1]     = 0.0f;
@@ -294,6 +294,14 @@ void plan_run_AutoTakeoff()
     break;
 
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
+    {
+        FlightStatusData flightStatus;
+        FlightStatusGet(&flightStatus);
+        if (!flightStatus.Armed) {
+            autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
+        }
+    }
+    break;
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
     // nothing to do. land has been requested. stay here for forever until mode change.
     default:

flight/libraries/sanitycheck.c

@@ -74,7 +74,7 @@ int32_t configuration_check()
     // Classify navigation capability
 #ifdef REVOLUTION
     RevoSettingsInitialize();
-    uint8_t revoFusion;
+    RevoSettingsFusionAlgorithmOptions revoFusion;
     RevoSettingsFusionAlgorithmGet(&revoFusion);
     bool navCapableFusion;
     switch (revoFusion) {
@@ -104,8 +104,8 @@ int32_t configuration_check()
     // For each available flight mode position sanity check the available
     // modes
     uint8_t num_modes;
-    uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
+    FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
-    uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
+    StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
     ManualControlSettingsFlightModeNumberGet(&num_modes);
     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
     FlightModeSettingsFlightModePositionGet(modes);
@@ -209,7 +209,7 @@ int32_t configuration_check()
         }
     }
 
-    uint8_t checks_disabled;
+    FlightModeSettingsDisableSanityChecksOptions checks_disabled;
     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
         severity = SYSTEMALARMS_ALARM_WARNING;
@@ -237,22 +237,22 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
     // Get the different axis modes for this switch position
     switch (index) {
     case 1:
-        FlightModeSettingsStabilization1SettingsArrayGet(modes);
+        FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
         break;
     case 2:
-        FlightModeSettingsStabilization2SettingsArrayGet(modes);
+        FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
         break;
     case 3:
-        FlightModeSettingsStabilization3SettingsArrayGet(modes);
+        FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
         break;
     case 4:
-        FlightModeSettingsStabilization4SettingsArrayGet(modes);
+        FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
         break;
     case 5:
-        FlightModeSettingsStabilization5SettingsArrayGet(modes);
+        FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
         break;
     case 6:
-        FlightModeSettingsStabilization6SettingsArrayGet(modes);
+        FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
         break;
     default:
         return false;
@@ -326,7 +326,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
 
 FrameType_t GetCurrentFrameType()
 {
-    uint8_t airframe_type;
+    SystemSettingsAirframeTypeOptions airframe_type;
 
     SystemSettingsAirframeTypeGet(&airframe_type);
     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {

flight/modules/Actuator/actuator.c

@@ -40,6 +40,7 @@
 #include "actuatordesired.h"
 #include "actuatorcommand.h"
 #include "flightstatus.h"
+#include <flightmodesettings.h>
 #include "mixersettings.h"
 #include "mixerstatus.h"
 #include "cameradesired.h"
@@ -98,9 +99,10 @@ static int mixer_settings_count = 2;
 // Private functions
 static void actuatorTask(void *parameters);
 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
+static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired);
 static void setFailsafe();
-static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements);
+static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor);
-static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements);
+static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor);
 static bool set_channel(uint8_t mixer_channel, uint16_t value);
 static void actuator_update_rate_if_changed(bool force_update);
 static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
@@ -108,7 +110,7 @@ static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
 float ProcessMixer(const int index, const float curve1, const float curve2,
                    ActuatorDesiredData *desired,
-                   const float period);
+                   const float period, bool multirotor);
 
 // this structure is equivalent to the UAVObjects for one mixer.
 typedef struct {
@@ -199,6 +201,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
     ActuatorCommandData command;
     ActuatorDesiredData desired;
     MixerStatusData mixerStatus;
+    FlightModeSettingsData settings;
     FlightStatusData flightStatus;
     float throttleDesired;
     float collectiveDesired;
@@ -245,6 +248,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         dTSeconds = dTMilliseconds * 0.001f;
 
         FlightStatusGet(&flightStatus);
+        FlightModeSettingsGet(&settings);
         ActuatorDesiredGet(&desired);
         ActuatorCommandGet(&command);
 
@@ -266,22 +270,32 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
         bool activeThrottle   = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
         bool positiveThrottle = (throttleDesired > 0.00f);
+        bool multirotor  = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor.
+        bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED;
+        bool AlwaysStabilizeWhenArmed = settings.AlwaysStabilizeWhenArmed == FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMED_TRUE;
 
+        if (alwaysArmed) {
+            AlwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous.
+        }
         // safety settings
         if (!armed) {
-            throttleDesired = 0;
+            throttleDesired = 0.00f; // this also happens in scaleMotors as a per axis check
         }
 
         if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
+            // throttleDesired should never be 0 or go below 0.
             // force set all other controls to zero if throttle is cut (previously set in Stabilization)
-            if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+            // todo: can probably remove this
-                desired.Roll = 0;
+            if (!(multirotor && AlwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed
-            }
+                if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
-            if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                    desired.Roll = 0.00f;
-                desired.Pitch = 0;
+                }
-            }
+                if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
-            if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                    desired.Pitch = 0.00f;
-                desired.Yaw = 0;
+                }
+                if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
+                    desired.Yaw = 0.00f;
+                }
             }
         }
 
@@ -296,13 +310,13 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
 
         AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
 
-        float curve1 = 0.0f;
+        float curve1 = 0.0f; // curve 1 is the throttle curve applied to all motors.
         float curve2 = 0.0f;
 
         // Interpolate curve 1 from throttleDesired as input.
         // assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values
         // map differently
-        curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
+        curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor);
 
         // The source for the secondary curve is selectable
         AccessoryDesiredData accessory;
@@ -310,25 +324,38 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         switch (curve2Source) {
         case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
             // assume reversible motor/mixer initially
-            curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_ROLL:
             // Throttle curve contribution the same for +ve vs -ve roll
-            curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            if (multirotor) {
+                curve2 = MixerCurveFullRangeProportional(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            } else {
+                curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            }
             break;
         case MIXERSETTINGS_CURVE2SOURCE_PITCH:
             // Throttle curve contribution the same for +ve vs -ve pitch
-            curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
+            if (multirotor) {
-                                                 MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+                curve2 = MixerCurveFullRangeProportional(desired.Pitch, mixerSettings.ThrottleCurve2,
+                                                         MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            } else {
+                curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
+                                                     MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            }
             break;
         case MIXERSETTINGS_CURVE2SOURCE_YAW:
             // Throttle curve contribution the same for +ve vs -ve yaw
-            curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+            if (multirotor) {
+                curve2 = MixerCurveFullRangeProportional(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            } else {
+                curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
+            }
             break;
         case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
             // assume reversible motor/mixer initially
             curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
-                                                     MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+                                                     MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
             break;
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
@@ -338,7 +365,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
         case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
             if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) {
                 // Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
-                curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+                curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
             } else {
                 curve2 = 0.0f;
             }
@@ -350,6 +377,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
 
         float *status   = (float *)&mixerStatus; // access status objects as an array of floats
         Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
+        float maxMotor  = -1.0f; // highest motor value. Addition method needs this to be -1.0f, division method needs this to be 1.0f
+        float minMotor  = 1.0f; // lowest motor value Addition method needs this to be 1.0f, division method needs this to be -1.0f
 
         for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
             // During boot all camera actuators should be completely disabled (PWM pulse = 0).
@@ -373,9 +402,11 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                     nonreversible_curve1 = 0.0f;
                 }
                 if (nonreversible_curve2 < 0.0f) {
-                    nonreversible_curve2 = 0.0f;
+                    if (!multirotor) { // allow negative throttle if multirotor. function scaleMotors handles the sanity checks.
+                        nonreversible_curve2 = 0.0f;
+                    }
                 }
-                status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds);
+                status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds, multirotor);
                 // If not armed or motors aren't meant to spin all the time
                 if (!armed ||
                     (!spinWhileArmed && !positiveThrottle)) {
@@ -386,10 +417,14 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                 // If armed meant to keep spinning,
                 else if ((spinWhileArmed && !positiveThrottle) ||
                          (status[ct] < 0)) {
-                    status[ct] = 0;
+                    if (!multirotor) {
+                        status[ct] = 0;
+                        // allow throttle values lower than 0 if multirotor.
+                        // Values will be scaled to 0 if they need to be in the scaleMotor function
+                    }
                 }
             } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
-                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
+                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor);
                 // Reversable Motors are like Motors but go to neutral instead of minimum
                 // If not armed or motor is inactive - no "spinwhilearmed" for this engine type
                 if (!armed || !activeThrottle) {
@@ -398,7 +433,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                     status[ct] = 0; // force neutral throttle
                 }
             } else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
-                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
+                status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor);
             } else {
                 status[ct] = -1;
 
@@ -444,16 +479,39 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
                     }
                 }
             }
+
+            // If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value.
+            // For use in function scaleMotor
+            if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
+                if (maxMotor < status[ct]) {
+                    maxMotor = status[ct];
+                }
+                if (minMotor > status[ct]) {
+                    minMotor = status[ct];
+                }
+            }
         }
 
         // Set real actuator output values scaling them from mixers. All channels
         // will be set except explicitly disabled (which will have PWM pulse = 0).
         for (int i = 0; i < MAX_MIX_ACTUATORS; i++) {
             if (command.Channel[i]) {
-                command.Channel[i] = scaleChannel(status[i],
+                if (mixers[i].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If mixer is for a motor we need to find the highest value of all motors
-                                                  actuatorSettings.ChannelMax[i],
+                    command.Channel[i] = scaleMotor(status[i],
-                                                  actuatorSettings.ChannelMin[i],
+                                                    actuatorSettings.ChannelMax[i],
-                                                  actuatorSettings.ChannelNeutral[i]);
+                                                    actuatorSettings.ChannelMin[i],
+                                                    actuatorSettings.ChannelNeutral[i],
+                                                    maxMotor,
+                                                    minMotor,
+                                                    armed,
+                                                    AlwaysStabilizeWhenArmed,
+                                                    throttleDesired);
+                } else { // else we scale the channel
+                    command.Channel[i] = scaleChannel(status[i],
+                                                      actuatorSettings.ChannelMax[i],
+                                                      actuatorSettings.ChannelMin[i],
+                                                      actuatorSettings.ChannelNeutral[i]);
+                }
             }
         }
 
@@ -497,7 +555,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
  * Process mixing for one actuator
  */
 float ProcessMixer(const int index, const float curve1, const float curve2,
-                   ActuatorDesiredData *desired, const float period)
+                   ActuatorDesiredData *desired, const float period, bool multirotor)
 {
     static float lastFilteredResult[MAX_MIX_ACTUATORS];
     const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
@@ -511,8 +569,10 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 
     // note: no feedforward for reversable motors yet for safety reasons
     if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
-        if (result < 0.0f) { // idle throttle
+        if (!multirotor) { // we allow negative throttle with a multirotor
-            result = 0.0f;
+            if (result < 0.0f) { // zero throttle
+                result = 0.0f;
+            }
         }
 
         // feed forward
@@ -560,9 +620,9 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
  * Input of 0  ->  lookup(0)
  * Input of 1  ->  lookup(1)
  */
-static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements)
+static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor)
 {
-    float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements);
+    float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor);
 
     if (input < 0.0f) {
         return -unsigned_value;
@@ -580,7 +640,7 @@ static float MixerCurveFullRangeProportional(const float input, const float *cur
  * Input of 0  -> lookup(0)
  * Input of 1  -> lookup(1)
  */
-static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements)
+static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor)
 {
     float abs_input = fabsf(input);
     float scale     = abs_input * (float)(elements - 1);
@@ -594,6 +654,16 @@ static float MixerCurveFullRangeAbsolute(const float input, const float *curve,
     if (idx2 >= elements) {
         idx2 = elements - 1; // clamp to highest entry in table
         if (idx1 >= elements) {
+            if (multirotor) {
+                // if multirotor frame we can return throttle values higher than 100%.
+                // Since the we don't have elements in the curve higher than 100% we return
+                // the last element multiplied by the throttle float
+                if (input < 2.0f) { // this limits positive throttle to 200% of max value in table (Maybe this is too much allowance)
+                    return curve[idx2] * input;
+                } else {
+                    return curve[idx2] * 2.0f; // return 200% of max value in table
+                }
+            }
             idx1 = elements - 1;
         }
     }
@@ -636,6 +706,73 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
     return valueScaled;
 }
 
+/**
+ * Constrain motor values to keep any one motor value from going too far out of range of another motor
+ */
+static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired)
+{
+    int16_t valueScaled;
+    int16_t maxMotorScaled;
+    int16_t minMotorScaled;
+    int16_t diff;
+
+    // Scale
+    valueScaled    = (int16_t)(value * ((float)(max - neutral))) + neutral;
+    maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral;
+    minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral;
+
+
+    if (max > min) {
+        diff = max - maxMotorScaled; // difference between max allowed and actual max motor
+        if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value
+            valueScaled += diff;
+        }
+        diff = neutral - minMotorScaled; // difference between min allowed and actual min motor
+        if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value
+            valueScaled += diff;
+        }
+        // todo: make this flow easier to understand
+        if (valueScaled > max) {
+            valueScaled = max; // clamp to max value only after scaling is done.
+        }
+
+        if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) {
+            valueScaled = neutral; // clamp to neutral value only after scaling is done.
+        } else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) {
+            valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below
+        } else if (valueScaled < neutral) {
+            valueScaled = min; // clamp to min value only after scaling is done.
+        }
+    } else {
+        // not sure what to do about reversed polarity right now. Why would anyone do this?
+        if (valueScaled < max) {
+            valueScaled = max; // clamp to max value only after scaling is done.
+        }
+        if (valueScaled > min) {
+            valueScaled = min; // clamp to min value only after scaling is done.
+        }
+    }
+
+    // I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max.
+    // NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option
+    // This prevents motors startup sync issues causing possible ESC failures.
+
+    // safety checks
+    if (!armed) {
+        // if not armed return min EVERYTIME!
+        valueScaled = min;
+    } else if (!AlwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) {
+        // all motors idle is AlwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed
+        // stabilize when armed?
+        valueScaled = neutral;
+    } else if (!spinWhileArmed && (throttleDesired <= 0.0f)) {
+        // soft disarm
+        valueScaled = min;
+    }
+
+    return valueScaled;
+}
+
 /**
  * Set actuator output to the neutral values (failsafe)
  */

flight/modules/Airspeed/airspeed.c

@@ -98,7 +98,7 @@ int32_t AirspeedInitialize()
 #else
 
     HwSettingsInitialize();
-    uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+    HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
     HwSettingsOptionalModulesArrayGet(optionalModules);
 
 
@@ -110,7 +110,7 @@ int32_t AirspeedInitialize()
     }
 #endif
 
-    uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
+    HwSettingsADCRoutingOptions adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
     HwSettingsADCRoutingArrayGet(adcRouting);
 
     // Determine if the barometric airspeed sensor is routed to an ADC pin
@@ -169,6 +169,8 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
                     imu_airspeedInitialize(&airspeedSettings);
                 }
                 break;
+            default:
+                break;
             }
         }
         switch (airspeedSettings.AirspeedSensorType) {

flight/modules/GPS/GPS.c

@@ -50,6 +50,7 @@
 #include "UBX.h"
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
 #include "inc/ubx_autoconfig.h"
+// #include "../../libraries/inc/fifo_buffer.h"
 #endif
 
 #include <pios_instrumentation_helper.h>
@@ -60,7 +61,7 @@ PERF_DEFINE_COUNTER(counterParse);
 // Private functions
 
 static void gpsTask(void *parameters);
-static void updateHwSettings();
+static void updateHwSettings(UAVObjEvent *ev);
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 static void setHomeLocation(GPSPositionSensorData *gpsData);
@@ -111,6 +112,8 @@ void updateGpsSettings(UAVObjEvent *ev);
 // ****************
 // Private variables
 
+static GPSSettingsData gpsSettings;
+
 static uint32_t gpsPort;
 static bool gpsEnabled = false;
 
@@ -150,15 +153,15 @@ int32_t GPSStart(void)
  * \return -1 if initialisation failed
  * \return 0 on success
  */
+
 int32_t GPSInitialize(void)
 {
     gpsPort = PIOS_COM_GPS;
-    uint8_t gpsProtocol;
 
+    HwSettingsInitialize();
 #ifdef MODULE_GPS_BUILTIN
     gpsEnabled = true;
 #else
-    HwSettingsInitialize();
     HwSettingsOptionalModulesData optionalModules;
 
     HwSettingsOptionalModulesGet(&optionalModules);
@@ -188,8 +191,12 @@ int32_t GPSInitialize(void)
     AuxMagSettingsUpdatedCb(NULL);
     AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
 #endif
-    updateHwSettings();
+    GPSSettingsInitialize();
-#else
+    // updateHwSettings() uses gpsSettings
+    GPSSettingsGet(&gpsSettings);
+    // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
+    updateHwSettings(0);
+#else /* if defined(REVOLUTION) */
     if (gpsPort && gpsEnabled) {
         GPSPositionSensorInitialize();
         GPSVelocitySensorInitialize();
@@ -200,27 +207,29 @@ int32_t GPSInitialize(void)
 #if defined(PIOS_GPS_SETS_HOMELOCATION)
         HomeLocationInitialize();
 #endif
-        updateHwSettings();
+        GPSSettingsInitialize();
+        // updateHwSettings() uses gpsSettings
+        GPSSettingsGet(&gpsSettings);
+        // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
+        updateHwSettings(0);
     }
 #endif /* if defined(REVOLUTION) */
 
     if (gpsPort && gpsEnabled) {
-        GPSSettingsInitialize();
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-        GPSSettingsDataProtocolGet(&gpsProtocol);
+        gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
-        switch (gpsProtocol) {
+#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
+        gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
-        case GPSSETTINGS_DATAPROTOCOL_NMEA:
+#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
-            gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
+        gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
-            break;
+#else
+        gps_rx_buffer = NULL;
 #endif
-        case GPSSETTINGS_DATAPROTOCOL_UBX:
+#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
-            gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
-            break;
-        default:
-            gps_rx_buffer = NULL;
-        }
         PIOS_Assert(gps_rx_buffer);
+#endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
         GPSSettingsConnectCallback(updateGpsSettings);
 #endif
         return 0;
@@ -245,15 +254,13 @@ static void gpsTask(__attribute__((unused)) void *parameters)
     portTickType homelocationSetDelay = 0;
 #endif
     GPSPositionSensorData gpspositionsensor;
-    GPSSettingsData gpsSettings;
-
-    GPSSettingsGet(&gpsSettings);
 
     timeOfLastUpdateMs  = timeNowMs;
     timeOfLastCommandMs = timeNowMs;
 
     GPSPositionSensorGet(&gpspositionsensor);
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+    // this should be done in the task because it calls out to actually start the GPS serial reads
     updateGpsSettings(0);
 #endif
 
@@ -270,9 +277,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
         if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
             char *buffer   = 0;
             uint16_t count = 0;
-            uint8_t status;
+
-            GPSPositionSensorStatusGet(&status);
+            ubx_autoconfig_run(&buffer, &count);
-            ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
             // Something to send?
             if (count) {
                 // clear ack/nak
@@ -284,10 +290,28 @@ static void gpsTask(__attribute__((unused)) void *parameters)
                 PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
             }
         }
-#endif
+
-        // This blocks the task until there is something on the buffer
+        // need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct
+        int32_t ac_status = ubx_autoconfig_get_status();
+        static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
+                                    + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
+                                    + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
+                                    + GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
+        gpspositionsensor.AutoConfigStatus =
+            ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
+            ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
+            ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
+            GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
+        if (gpspositionsensor.AutoConfigStatus != lastStatus) {
+            GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
+            lastStatus = gpspositionsensor.AutoConfigStatus;
+        }
+#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
+
+        // This blocks the task until there is something on the buffer (or 100ms? passes)
         uint16_t cnt;
-        while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) {
+        cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
+        if (cnt > 0) {
             PERF_TIMED_SECTION_START(counterParse);
             PERF_TRACK_VALUE(counterBytesIn, cnt);
             PERF_MEASURE_PERIOD(counterRate);
@@ -300,24 +324,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
 #endif
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
             case GPSSETTINGS_DATAPROTOCOL_UBX:
-            {
-#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
-                int32_t ac_status = ubx_autoconfig_get_status();
-                static uint8_t lastStatus = 0;
-
-                gpspositionsensor.AutoConfigStatus =
-                    ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
-                    ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
-                    ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
-                    GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
-                if (gpspositionsensor.AutoConfigStatus != lastStatus) {
-                    GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
-                    lastStatus = gpspositionsensor.AutoConfigStatus;
-                }
-#endif
                 res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
-            }
+                break;
-            break;
 #endif
             default:
                 res = NO_PARSER; // this should not happen
@@ -338,7 +346,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
             (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
             // we have not received any valid GPS sentences for a while.
             // either the GPS is not plugged in or a hardware problem or the GPS has locked up.
-            uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
+            GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
             GPSPositionSensorStatusSet(&status);
             AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
         } else {
@@ -435,40 +443,136 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
  * like protocol, etc. Thus the HwSettings object which contains the
  * GPS port speed is used for now.
  */
-static void updateHwSettings()
-{
-    if (gpsPort) {
-        // Retrieve settings
-        uint8_t speed;
-        HwSettingsGPSSpeedGet(&speed);
 
-        // Set port speed
+// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
-        switch (speed) {
+static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
-        case HWSETTINGS_GPSSPEED_2400:
+{
-            PIOS_COM_ChangeBaud(gpsPort, 2400);
+    // with these changes, no one (not even OP board makers) should ever need u-center (the complex UBlox configuration program)
-            break;
+    // no booting of Revo should be required during any setup or testing, just Send the settings you want to play with
-        case HWSETTINGS_GPSSPEED_4800:
+    // with autoconfig enabled, just change the baud rate in HwSettings and it will change the GPS internal baud and then the Revo port
-            PIOS_COM_ChangeBaud(gpsPort, 4800);
+    // with autoconfig disabled, it will only change the Revo port, so you can try to find the current GPS baud rate if you don't know it
-            break;
+    // GPSPositionSensor.SensorType and .UbxAutoConfigStatus now give correct values at all times, so use .SensorType to prove it is
-        case HWSETTINGS_GPSSPEED_9600:
+    // connected, even to an incorrectly configured (e.g. factory default) GPS
-            PIOS_COM_ChangeBaud(gpsPort, 9600);
+
-            break;
+    // Use cases:
-        case HWSETTINGS_GPSSPEED_19200:
+    // - the user can plug in a default GPS, use autoconfig-store once and then always use autoconfig-disabled
-            PIOS_COM_ChangeBaud(gpsPort, 19200);
+    // - the user can plug in a default GPS that can't store settings (defaults to 9600 baud) and always use autoconfig-nostore
-            break;
+    // - - this is one reason for coding this: cheap eBay GPS's that loose their settings sometimes
-        case HWSETTINGS_GPSSPEED_38400:
+    // - the user can plug in a default GPS that _can_ store settings and always use autoconfig-nostore with that too
-            PIOS_COM_ChangeBaud(gpsPort, 38400);
+    // - - if settings change in newer releases of OP, this will make sure to use the correct settings with whatever code you are running
-            break;
+    // - the user can plug in a correctly configured GPS and use autoconfig-disabled
-        case HWSETTINGS_GPSSPEED_57600:
+    // - the user can recover an old or incorrectly configured GPS (internal GPS baud rate or other GPS settings wrong)
-            PIOS_COM_ChangeBaud(gpsPort, 57600);
+    // - - disable UBX GPS autoconfig
-            break;
+    // - - set HwSettings GPS baud rate to the current GPS internal baud rate to get it to connect at current (especially non-9600) baud rate
-        case HWSETTINGS_GPSSPEED_115200:
+    // - - - you can tell the baud rate is correct when GPSPositionSensor.SensorType is known (not "Unknown") (e.g. UBX6)
-            PIOS_COM_ChangeBaud(gpsPort, 115200);
+    // - - - the SensorType and AutoConfigStatus may fail at 9600 and will fail at 4800 or lower if the GPS is configured to send OP data
-            break;
+    // - - - (way more than default) at 4800 baud or slower
-        case HWSETTINGS_GPSSPEED_230400:
+    // - - enable autoconfig-nostore GPSSettings to set correct messages and enable further magic baud rate settings
-            PIOS_COM_ChangeBaud(gpsPort, 230400);
+    // - - change the baud rate to what you want in HwSettings (it will change the baud rate in the GPS and then in the Revo port)
-            break;
+    // - - wait for the baud rate change to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
+    // - - enable autoconfig-store and it will save the new baud rate and the correct message configuration
+    // - - wait for the store to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
+    // - - for the new baud rate, the user should either choose 9600 for use with autoconfig-nostore or can choose 57600 for use with autoconfig-disabled
+    // - the user (Dave :)) can configure a whole bunch of default GPS's with no intervention by using autoconfig-store as a saved Revo setting
+    // - - just plug the default (9600 baud) GPS in to an unpowered Revo, power the Revo/GPS through the servo connector, wait some time, unplug
+    // - - or with this code, OP could and even should just ship GPS's with default settings
+
+    // if we add an "initial baud rate" instead of assuming 9600 at boot up for autoconfig-nostore/store
+    // - the user can use the same GPS with both an older release of OP (e.g. GPS settings at 38400) and the current release (autoconfig-nostore)
+    // - the 57600 could be fixed instead of the 9600 as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate?
+
+    // About using UBlox GPS's with default settings (9600 baud and NEMA data):
+    // - the default uBlox settings (different than OP settings) are NEMA and 9600 baud
+    // - - and that is OK and you use autoconfig-nostore
+    // - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings,
+    // - - you get them automatically changed to match whatever version of OP code you are running
+    // - - but 9600 is only OK for this autoconfig startup
+    // - by my testing, the 9600 initial to 57600 final baud startup takes:
+    // - - 0:10 if the GPS has been configured to send OP data at 9600
+    // - - 0:06 if the GPS has default data settings (NEMA) at 9600
+    // - - reminder that even 0:10 isn't that bad.  You need to wait for the GPS to acquire satellites,
+
+    // Some things you want to think about if you want to play around with this:
+    // - don't play with low baud rates, with OP data settings (lots of data) it can take a long time to auto-configure
+    // - - at 2400 it could take a long time to autoconfig or error out
+    // - - at 9600 or lower the autoconfig is skipped and only the baud rate is changed
+    // - if you autoconfigure an OP configuration at 9600 baud or lower
+    // - - it will actually make a factory default configuration (not an OP configuration) at that baud rate
+    // - remember that there are two baud rates (inside the GPS and the Revo port) and you can get them out of sync
+    // - - rebooting the Revo from the Ground Control Station (without powering the GPS down too)
+    // - - can leave the baud rates out of sync if you are using autoconfig
+    // - - just power off and on both the Revo and the GPS
+    // - my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400
+    // - - so you could use autoconfig.nostore with this high baud rate, but not autoconfig.store (once) followed by autoconfig.disabled
+    // - - I haven't tested other GPS's in regard to this
+
+    // since 9600 baud and lower are not usable, and are best left at NEMA, I could have coded it to do a factory reset
+    // - if set to 9600 baud (or lower)
+
+    if (gpsPort) {
+        HwSettingsGPSSpeedOptions speed;
+
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        // use 9600 baud during startup if autoconfig is enabled
+        // that is the flag that the code should assumes a factory default baud rate
+        if (ev == NULL && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
+            speed = HWSETTINGS_GPSSPEED_9600;
+        } else
+#endif
+        {
+            // Retrieve settings
+            HwSettingsGPSSpeedGet(&speed);
         }
+
+        // must always set the baud here, even if it looks like it is the same
+        // e.g. startup sets 9600 here, ubx_autoconfig sets 57600 there, then the user selects a change back to 9600 here
+
+        // on    startup (ev == NULL) we must set the Revo port baud rate
+        // after startup (ev != NULL) we must set the Revo port baud rate only if autoconfig is disabled
+        // always we must set the Revo port baud rate if not using UBX
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
+#endif
+        {
+            // Set Revo port speed
+            switch (speed) {
+            case HWSETTINGS_GPSSPEED_2400:
+                PIOS_COM_ChangeBaud(gpsPort, 2400);
+                break;
+            case HWSETTINGS_GPSSPEED_4800:
+                PIOS_COM_ChangeBaud(gpsPort, 4800);
+                break;
+            case HWSETTINGS_GPSSPEED_9600:
+                PIOS_COM_ChangeBaud(gpsPort, 9600);
+                break;
+            case HWSETTINGS_GPSSPEED_19200:
+                PIOS_COM_ChangeBaud(gpsPort, 19200);
+                break;
+            case HWSETTINGS_GPSSPEED_38400:
+                PIOS_COM_ChangeBaud(gpsPort, 38400);
+                break;
+            case HWSETTINGS_GPSSPEED_57600:
+                PIOS_COM_ChangeBaud(gpsPort, 57600);
+                break;
+            case HWSETTINGS_GPSSPEED_115200:
+                PIOS_COM_ChangeBaud(gpsPort, 115200);
+                break;
+            case HWSETTINGS_GPSSPEED_230400:
+                PIOS_COM_ChangeBaud(gpsPort, 230400);
+                break;
+            }
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+            // even changing the baud rate will make it re-verify the sensor type
+            // that way the user can just try some baud rates and it when the sensor type is valid, the baud rate is correct
+            ubx_reset_sensor_type();
+#endif
+        }
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
+        else {
+            // it will never do this during startup because ev == NULL
+            ubx_autoconfig_set(NULL);
+        }
+#endif
     }
 }
 
@@ -480,32 +584,26 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 
 void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
 {
-    uint8_t ubxAutoConfig;
-    uint8_t ubxDynamicModel;
-    uint8_t ubxSbasMode;
     ubx_autoconfig_settings_t newconfig;
-    uint8_t ubxSbasSats;
-    uint8_t ubxGnssMode;
 
-    GPSSettingsUbxRateGet(&newconfig.navRate);
+    GPSSettingsGet(&gpsSettings);
 
-    GPSSettingsUbxAutoConfigGet(&ubxAutoConfig);
+    newconfig.navRate = gpsSettings.UbxRate;
-    newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
-    newconfig.storeSettings     = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
 
-    GPSSettingsUbxDynamicModelGet(&ubxDynamicModel);
+    newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
-    newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
+    newconfig.storeSettings     = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
+
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
+    newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
-                             ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
+                             gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
                              UBX_DYNMODEL_AIRBORNE1G;
 
-    GPSSettingsUbxSBASModeGet(&ubxSbasMode);
+    switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
-    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
     case GPSSETTINGS_UBXSBASMODE_CORRECTION:
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
@@ -516,7 +614,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         newconfig.SBASCorrection = false;
     }
 
-    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
     case GPSSETTINGS_UBXSBASMODE_RANGING:
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
     case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
@@ -527,7 +625,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         newconfig.SBASRanging = false;
     }
 
-    switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
+    switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
     case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
     case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
     case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
@@ -538,19 +636,15 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         newconfig.SBASIntegrity = false;
     }
 
-    GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed);
+    newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed;
 
-    GPSSettingsUbxSBASSatsGet(&ubxSbasSats);
+    newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
+                         gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
 
-    newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
+    switch (gpsSettings.UbxGNSSMode) {
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
-                         ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
-
-    GPSSettingsUbxGNSSModeGet(&ubxGnssMode);
-
-    switch (ubxGnssMode) {
     case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS:
         newconfig.enableGPS     = true;
         newconfig.enableGLONASS = true;
@@ -583,7 +677,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
         break;
     }
 
-    ubx_autoconfig_set(newconfig);
+    ubx_autoconfig_set(&newconfig);
 }
 #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
 /**

flight/modules/GPS/UBX.c

@@ -43,7 +43,7 @@
 
 static bool useMag = false;
 #endif
-static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
+GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
 
 static bool usePvt = false;
 static uint32_t lastPvtTime = 0;
@@ -466,9 +466,11 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS
     struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
 
     ubxHwVersion = atoi(mon_ver->hwVersion);
-
     sensorType   = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
                    ((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
+    // send sensor type right now because on UBX NEMA we don't get a full set of messages
+    // and we want to be able to see sensor type even on UBX NEMA GPS's
+    GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
 }
 
 static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)

flight/modules/GPS/inc/UBX.h

@@ -102,7 +102,8 @@ typedef enum {
     UBX_ID_CFG_MSG  = 0x01,
     UBX_ID_CFG_CFG  = 0x09,
     UBX_ID_CFG_SBAS = 0x16,
-    UBX_ID_CFG_GNSS = 0x3E
+    UBX_ID_CFG_GNSS = 0x3E,
+    UBX_ID_CFG_PRT  = 0x00
 } ubx_class_cfg_id;
 
 typedef enum {
@@ -129,6 +130,39 @@ typedef enum {
     UBX_ID_RXM_SFRB = 0x11,
     UBX_ID_RXM_SVSI = 0x20,
 } ubx_class_rxm_id;
+
+typedef enum {
+    UBX_GNSS_ID_GPS     = 0,
+    UBX_GNSS_ID_SBAS    = 1,
+    UBX_GNSS_ID_GALILEO = 2,
+    UBX_GNSS_ID_BEIDOU  = 3,
+    UBX_GNSS_ID_IMES    = 4,
+    UBX_GNSS_ID_QZSS    = 5,
+    UBX_GNSS_ID_GLONASS = 6,
+    UBX_GNSS_ID_MAX
+} ubx_config_gnss_id_t;
+
+// Enumeration options for field UBXDynamicModel
+typedef enum {
+    UBX_DYNMODEL_PORTABLE   = 0,
+    UBX_DYNMODEL_STATIONARY = 2,
+    UBX_DYNMODEL_PEDESTRIAN = 3,
+    UBX_DYNMODEL_AUTOMOTIVE = 4,
+    UBX_DYNMODEL_SEA = 5,
+    UBX_DYNMODEL_AIRBORNE1G = 6,
+    UBX_DYNMODEL_AIRBORNE2G = 7,
+    UBX_DYNMODEL_AIRBORNE4G = 8
+} ubx_config_dynamicmodel_t;
+
+typedef enum {
+    UBX_SBAS_SATS_AUTOSCAN = 0,
+    UBX_SBAS_SATS_WAAS     = 1,
+    UBX_SBAS_SATS_EGNOS    = 2,
+    UBX_SBAS_SATS_MSAS     = 3,
+    UBX_SBAS_SATS_GAGAN    = 4,
+    UBX_SBAS_SATS_SDCM     = 5
+} ubx_config_sats_t;
+
 // private structures
 
 // Geodetic Position Solution
@@ -140,10 +174,9 @@ struct UBX_NAV_POSLLH {
     int32_t  hMSL;   // Height above mean sea level (mm)
     uint32_t hAcc; // Horizontal Accuracy Estimate (mm)
     uint32_t vAcc; // Vertical Accuracy Estimate (mm)
-};
+} __attribute__((packed));
 
 // Receiver Navigation Status
-
 #define STATUS_GPSFIX_NOFIX    0x00
 #define STATUS_GPSFIX_DRONLY   0x01
 #define STATUS_GPSFIX_2DFIX    0x02
@@ -164,7 +197,7 @@ struct UBX_NAV_STATUS {
     uint8_t  flags2;  // Additional navigation output information
     uint32_t ttff; // Time to first fix (ms)
     uint32_t msss; // Milliseconds since startup/reset (ms)
-};
+} __attribute__((packed));
 
 // Dilution of precision
 struct UBX_NAV_DOP {
@@ -176,10 +209,9 @@ struct UBX_NAV_DOP {
     uint16_t hDOP; // Horizontal DOP
     uint16_t nDOP; // Northing DOP
     uint16_t eDOP; // Easting DOP
-};
+} __attribute__((packed));
 
 // Navigation solution
-
 struct UBX_NAV_SOL {
     uint32_t iTOW; // GPS Millisecond Time of Week (ms)
     int32_t  fTOW;       // fractional nanoseconds (ns)
@@ -198,41 +230,9 @@ struct UBX_NAV_SOL {
     uint8_t  reserved1;  // Reserved
     uint8_t  numSV;      // Number of SVs used in Nav Solution
     uint32_t reserved2; // Reserved
-};
+} __attribute__((packed));
-
-// North/East/Down velocity
-
-struct UBX_NAV_VELNED {
-    uint32_t iTOW; // ms GPS Millisecond Time of Week
-    int32_t  velN;     // cm/s NED north velocity
-    int32_t  velE;     // cm/s NED east velocity
-    int32_t  velD;     // cm/s NED down velocity
-    uint32_t speed; // cm/s Speed (3-D)
-    uint32_t gSpeed; // cm/s Ground Speed (2-D)
-    int32_t  heading;  // 1e-5 *deg Heading of motion 2-D
-    uint32_t sAcc; // cm/s Speed Accuracy Estimate
-    uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
-};
-
-// UTC Time Solution
-
-#define TIMEUTC_VALIDTOW (1 << 0)
-#define TIMEUTC_VALIDWKN (1 << 1)
-#define TIMEUTC_VALIDUTC (1 << 2)
-
-struct UBX_NAV_TIMEUTC {
-    uint32_t iTOW; // GPS Millisecond Time of Week (ms)
-    uint32_t tAcc; // Time Accuracy Estimate (ns)
-    int32_t  nano;   // Nanoseconds of second
-    uint16_t year;
-    uint8_t  month;
-    uint8_t  day;
-    uint8_t  hour;
-    uint8_t  min;
-    uint8_t  sec;
-    uint8_t  valid;  // Validity Flags
-};
 
+// PVT Navigation Position Velocity Time Solution
 #define PVT_VALID_VALIDDATE            0x01
 #define PVT_VALID_VALIDTIME            0x02
 #define PVT_VALID_FULLYRESOLVED        0x04
@@ -252,7 +252,6 @@ struct UBX_NAV_TIMEUTC {
 #define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2)
 #define PVT_FLAGS_PSMSTATE_INACTIVE    (5 << 2)
 
-// PVT Navigation Position Velocity Time Solution
 struct UBX_NAV_PVT {
     uint32_t iTOW;
     uint16_t year;
@@ -286,10 +285,39 @@ struct UBX_NAV_PVT {
     uint32_t reserved3;
 } __attribute__((packed));
 
+// North/East/Down velocity
+struct UBX_NAV_VELNED {
+    uint32_t iTOW; // ms GPS Millisecond Time of Week
+    int32_t  velN;     // cm/s NED north velocity
+    int32_t  velE;     // cm/s NED east velocity
+    int32_t  velD;     // cm/s NED down velocity
+    uint32_t speed; // cm/s Speed (3-D)
+    uint32_t gSpeed; // cm/s Ground Speed (2-D)
+    int32_t  heading;  // 1e-5 *deg Heading of motion 2-D
+    uint32_t sAcc; // cm/s Speed Accuracy Estimate
+    uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
+} __attribute__((packed));
+
+// UTC Time Solution
+#define TIMEUTC_VALIDTOW (1 << 0)
+#define TIMEUTC_VALIDWKN (1 << 1)
+#define TIMEUTC_VALIDUTC (1 << 2)
+
+struct UBX_NAV_TIMEUTC {
+    uint32_t iTOW; // GPS Millisecond Time of Week (ms)
+    uint32_t tAcc; // Time Accuracy Estimate (ns)
+    int32_t  nano;   // Nanoseconds of second
+    uint16_t year;
+    uint8_t  month;
+    uint8_t  day;
+    uint8_t  hour;
+    uint8_t  min;
+    uint8_t  sec;
+    uint8_t  valid;  // Validity Flags
+} __attribute__((packed));
+
 // Space Vehicle (SV) Information
-
 // Single SV information block
-
 #define SVUSED     (1 << 0) // This SV is used for navigation
 #define DIFFCORR   (1 << 1) // Differential correction available
 #define ORBITAVAIL (1 << 2) // Orbit information available
@@ -308,7 +336,7 @@ struct UBX_NAV_SVINFO_SV {
     int8_t  elev;     // Elevation (integer degrees)
     int16_t azim; // Azimuth	(integer degrees)
     int32_t prRes; // Pseudo range residual (cm)
-};
+} __attribute__((packed));
 
 // SV information message
 #define MAX_SVS 32
@@ -319,19 +347,180 @@ struct UBX_NAV_SVINFO {
     uint8_t  globalFlags;  //
     uint16_t reserved2; // Reserved
     struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
-};
+} __attribute__((packed));
 
 // ACK message class
-
 struct UBX_ACK_ACK {
     uint8_t clsID; // ClassID
     uint8_t msgID; // MessageID
-};
+} __attribute__((packed));
 
 struct UBX_ACK_NAK {
     uint8_t clsID; // ClassID
     uint8_t msgID; // MessageID
-};
+} __attribute__((packed));
+
+// Communication port information
+// default cfg_prt packet at 9600,N,8,1 (from u-center)
+// 0000  B5 62 06 00 14 00-01 00
+// 0008  00 00 D0 08 00 00 80 25
+// 0010  00 00 07 00 03 00 00 00
+// 0018  00 00-A2 B5
+#define UBX_CFG_PRT_PORTID_DDC       0
+#define UBX_CFG_PRT_PORTID_UART1     1
+#define UBX_CFG_PRT_PORTID_UART2     2
+#define UBX_CFG_PRT_PORTID_USB       3
+#define UBX_CFG_PRT_PORTID_SPI       4
+#define UBX_CFG_PRT_MODE_DATABITS5   0x00
+#define UBX_CFG_PRT_MODE_DATABITS6   0x40
+#define UBX_CFG_PRT_MODE_DATABITS7   0x80
+#define UBX_CFG_PRT_MODE_DATABITS8   0xC0
+#define UBX_CFG_PRT_MODE_EVENPARITY  0x000
+#define UBX_CFG_PRT_MODE_ODDPARITY   0x200
+#define UBX_CFG_PRT_MODE_NOPARITY    0x800
+#define UBX_CFG_PRT_MODE_STOPBITS1_0 0x0000
+#define UBX_CFG_PRT_MODE_STOPBITS1_5 0x1000
+#define UBX_CFG_PRT_MODE_STOPBITS2_0 0x2000
+#define UBX_CFG_PRT_MODE_STOPBITS0_5 0x3000
+#define UBX_CFG_PRT_MODE_RESERVED    0x10
+
+#define UBX_CFG_PRT_MODE_DEFAULT     (UBX_CFG_PRT_MODE_DATABITS8 | UBX_CFG_PRT_MODE_NOPARITY | UBX_CFG_PRT_MODE_STOPBITS1_0 | UBX_CFG_PRT_MODE_RESERVED)
+
+struct UBX_CFG_PRT {
+    uint8_t  portID;       // 1 or 2 for UART ports
+    uint8_t  res0;         // reserved
+    uint16_t res1; // reserved
+    uint32_t mode; // bit masks for databits, stopbits, parity, and non-zero reserved
+    uint32_t baudRate; // bits per second, 9600 means 9600
+    uint16_t inProtoMask; // bit 0 on = UBX, bit 1 on = NEMA
+    uint16_t outProtoMask; // bit 0 on = UBX, bit 1 on = NEMA
+    uint16_t flags; // reserved
+    uint16_t pad; // reserved
+} __attribute__((packed));
+
+struct UBX_CFG_MSG {
+    uint8_t msgClass;
+    uint8_t msgID;
+    uint8_t rate;
+} __attribute__((packed));
+
+struct UBX_CFG_RATE {
+    uint16_t measRate;
+    uint16_t navRate;
+    uint16_t timeRef;
+} __attribute__((packed));
+
+// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
+#define UBX_CFG_CFG_DEVICE_BBR        0x01
+#define UBX_CFG_CFG_DEVICE_FLASH      0x02
+#define UBX_CFG_CFG_DEVICE_EEPROM     0x04
+#define UBX_CFG_CFG_DEVICE_SPIFLASH   0x10
+#define UBX_CFG_CFG_DEVICE_ALL        (UBX_CFG_CFG_DEVICE_BBR | UBX_CFG_CFG_DEVICE_FLASH | UBX_CFG_CFG_DEVICE_EEPROM | UBX_CFG_CFG_DEVICE_SPIFLASH)
+#define UBX_CFG_CFG_SETTINGS_NONE     0x000
+#define UBX_CFG_CFG_SETTINGS_IOPORT   0x001
+#define UBX_CFG_CFG_SETTINGS_MSGCONF  0x002
+#define UBX_CFG_CFG_SETTINGS_INFMSG   0x004
+#define UBX_CFG_CFG_SETTINGS_NAVCONF  0x008
+#define UBX_CFG_CFG_SETTINGS_TPCONF   0x010
+#define UBX_CFG_CFG_SETTINGS_SFDRCONF 0x100
+#define UBX_CFG_CFG_SETTINGS_RINVCONF 0x200
+#define UBX_CFG_CFG_SETTINGS_ANTCONF  0x400
+
+#define UBX_CFG_CFG_SETTINGS_ALL \
+    (UBX_CFG_CFG_SETTINGS_IOPORT | \
+     UBX_CFG_CFG_SETTINGS_MSGCONF | \
+     UBX_CFG_CFG_SETTINGS_INFMSG | \
+     UBX_CFG_CFG_SETTINGS_NAVCONF | \
+     UBX_CFG_CFG_SETTINGS_TPCONF | \
+     UBX_CFG_CFG_SETTINGS_SFDRCONF | \
+     UBX_CFG_CFG_SETTINGS_RINVCONF | \
+     UBX_CFG_CFG_SETTINGS_ANTCONF)
+
+// Sent messages for configuration support
+struct UBX_CFG_CFG {
+    uint32_t clearMask;
+    uint32_t saveMask;
+    uint32_t loadMask;
+    uint8_t  deviceMask;
+} __attribute__((packed));
+
+#define UBX_CFG_SBAS_MODE_ENABLED    0x01
+#define UBX_CFG_SBAS_MODE_TEST       0x02
+#define UBX_CFG_SBAS_USAGE_RANGE     0x01
+#define UBX_CFG_SBAS_USAGE_DIFFCORR  0x02
+#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
+
+// SBAS used satellite PNR bitmask (120-151)
+// -------------------------------------1---------1---------1---------1
+// -------------------------------------5---------4---------3---------2
+// ------------------------------------10987654321098765432109876543210
+// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_WAAS  0b00000000000001001110000000000100
+// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
+// MSAS 129, 137------------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_MSAS  0b00000000000000100000001000000000
+// GAGAN 127, 128-----------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
+// SDCM 125, 140, 141-------------------|---------|---------|---------|
+#define UBX_CFG_SBAS_SCANMODE1_SDCM  0b00000000001100000000000000100000
+
+#define UBX_CFG_SBAS_SCANMODE2       0x00
+struct UBX_CFG_SBAS {
+    uint8_t  mode;
+    uint8_t  usage;
+    uint8_t  maxSBAS;
+    uint8_t  scanmode2;
+    uint32_t scanmode1;
+} __attribute__((packed));
+
+#define UBX_CFG_GNSS_FLAGS_ENABLED      0x000001
+#define UBX_CFG_GNSS_FLAGS_GPS_L1CA     0x010000
+#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA    0x010000
+#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I   0x010000
+#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA    0x010000
+#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF  0x040000
+#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000
+
+#define UBX_CFG_GNSS_NUMCH_VER7         22
+#define UBX_CFG_GNSS_NUMCH_VER8         32
+
+struct UBX_CFG_GNSS_CFGBLOCK {
+    uint8_t  gnssId;
+    uint8_t  resTrkCh;
+    uint8_t  maxTrkCh;
+    uint8_t  resvd;
+    uint32_t flags;
+} __attribute__((packed));
+
+struct UBX_CFG_GNSS {
+    uint8_t msgVer;
+    uint8_t numTrkChHw;
+    uint8_t numTrkChUse;
+    uint8_t numConfigBlocks;
+    struct UBX_CFG_GNSS_CFGBLOCK cfgBlocks[UBX_GNSS_ID_MAX];
+} __attribute__((packed));
+
+struct UBX_CFG_NAV5 {
+    uint16_t mask;
+    uint8_t  dynModel;
+    uint8_t  fixMode;
+    int32_t  fixedAlt;
+    uint32_t fixedAltVar;
+    int8_t   minElev;
+    uint8_t  drLimit;
+    uint16_t pDop;
+    uint16_t tDop;
+    uint16_t pAcc;
+    uint16_t tAcc;
+    uint8_t  staticHoldThresh;
+    uint8_t  dgpsTimeOut;
+    uint8_t  cnoThreshNumSVs;
+    uint8_t  cnoThresh;
+    uint16_t reserved2;
+    uint32_t reserved3;
+    uint32_t reserved4;
+} __attribute__((packed));
 
 // MON message Class
 #define UBX_MON_MAX_EXT 5
@@ -341,8 +530,7 @@ struct UBX_MON_VER {
 #if UBX_MON_MAX_EXT > 0
     char extension[UBX_MON_MAX_EXT][30];
 #endif
-};
+} __attribute__((packed));
-
 
 // OP custom messages
 struct UBX_OP_SYSINFO {
@@ -360,7 +548,7 @@ struct UBX_OP_MAG {
     int16_t  y;
     int16_t  z;
     uint16_t Status;
-};
+} __attribute__((packed));
 
 typedef union {
     uint8_t payload[0];
@@ -374,6 +562,7 @@ typedef union {
     struct UBX_NAV_PVT     nav_pvt;
     struct UBX_NAV_TIMEUTC nav_timeutc;
     struct UBX_NAV_SVINFO  nav_svinfo;
+    struct UBX_CFG_PRT     cfg_prt;
     // Ack Class
     struct UBX_ACK_ACK     ack_ack;
     struct UBX_ACK_NAK     ack_nak;
@@ -390,15 +579,40 @@ struct UBXHeader {
     uint16_t len;
     uint8_t  ck_a;
     uint8_t  ck_b;
-};
+} __attribute__((packed));
 
 struct UBXPacket {
     struct UBXHeader header;
     UBXPayload payload;
-};
+} __attribute__((packed));
+
+struct UBXSENTHEADER {
+    uint8_t  prolog[2];
+    uint8_t  class;
+    uint8_t  id;
+    uint16_t len;
+} __attribute__((packed));
+
+union UBXSENTPACKET {
+    uint8_t buffer[0];
+    struct {
+        struct UBXSENTHEADER header;
+        union {
+            struct UBX_CFG_CFG  cfg_cfg;
+            struct UBX_CFG_MSG  cfg_msg;
+            struct UBX_CFG_NAV5 cfg_nav5;
+            struct UBX_CFG_PRT  cfg_prt;
+            struct UBX_CFG_RATE cfg_rate;
+            struct UBX_CFG_SBAS cfg_sbas;
+            struct UBX_CFG_GNSS cfg_gnss;
+        } payload;
+        uint8_t resvd[2]; // added space for checksum bytes
+    } message;
+} __attribute__((packed));
 
 // Used by AutoConfig code
 extern int32_t ubxHwVersion;
+extern GPSPositionSensorSensorTypeOptions sensorType;
 extern struct UBX_ACK_ACK ubxLastAck;
 extern struct UBX_ACK_NAK ubxLastNak;
 

flight/modules/GPS/inc/ubx_autoconfig.h

@@ -33,20 +33,48 @@
 
 // defines
 // TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that.
-#define UBX_MAX_RATE_VER8       18
+#define UBX_MAX_RATE_VER8 18
-#define UBX_MAX_RATE_VER7       10
+#define UBX_MAX_RATE_VER7 10
-#define UBX_MAX_RATE            5
+#define UBX_MAX_RATE      5
+
+// reset to factory (and 9600 baud), and baud changes are not acked
+// it could be 31 (full PIOS buffer) + 60 (gnss) + overhead bytes at 240 bytes per second
+// = .42 seconds for the longest sentence to be fully transmitted
+// and then it may have to do something like erase flash before replying...
+
+// at low baud rates and high data rates the ubx gps simply must drop some outgoing data
+// and when a lot of data is being dropped, the MON VER reply often gets dropped
+// on the other hand, uBlox documents that some versions discard data that is over a second old
+// implying that it could be over 1 second before a reply is received
+// later versions dropped this and drop data when the send buffer is full and that could be even longer
+// rather than have long timeouts, we will let timeouts * retries handle that if it happens
 
-// time to wait before reinitializing the fsm due to disconnection
-#define UBX_CONNECTION_TIMEOUT  (2000 * 1000)
-// times between retries in case an error does occurs
-#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000)
 // timeout for ack reception
-#define UBX_REPLY_TIMEOUT       (500 * 1000)
+#define UBX_REPLY_TIMEOUT             (500 * 1000)
+// timeout for a settings save, in case it has to erase flash
+#define UBX_REPLY_TO_SAVE_TIMEOUT     (3000 * 1000)
 // max retries in case of timeout
-#define UBX_MAX_RETRIES         5
+#define UBX_MAX_RETRIES               5
-// pause between each configuration step
+// pause between each verifiably correct configuration step
-#define UBX_STEP_WAIT_TIME      (10 * 1000)
+#define UBX_VERIFIED_STEP_WAIT_TIME   (50 * 1000)
+// pause between each unverifiably correct configuration step
+#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
+
+#define UBX_CFG_CFG_OP_STORE_SETTINGS \
+    (UBX_CFG_CFG_SETTINGS_IOPORT | \
+     UBX_CFG_CFG_SETTINGS_MSGCONF | \
+     UBX_CFG_CFG_SETTINGS_NAVCONF)
+#define UBX_CFG_CFG_OP_CLEAR_SETTINGS ((~UBX_CFG_CFG_OP_STORE_SETTINGS) & UBX_CFG_CFG_SETTINGS_ALL)
+// don't clear rinv as that is just text as configured by the owner
+#define UBX_CFG_CFG_OP_RESET_SETTINGS \
+    (UBX_CFG_CFG_SETTINGS_IOPORT | \
+     UBX_CFG_CFG_SETTINGS_MSGCONF | \
+     UBX_CFG_CFG_SETTINGS_INFMSG | \
+     UBX_CFG_CFG_SETTINGS_NAVCONF | \
+     UBX_CFG_CFG_SETTINGS_TPCONF | \
+     UBX_CFG_CFG_SETTINGS_SFDRCONF | \
+     UBX_CFG_CFG_SETTINGS_ANTCONF)
+
 // types
 typedef enum {
     UBX_AUTOCONFIG_STATUS_DISABLED = 0,
@@ -54,26 +82,6 @@ typedef enum {
     UBX_AUTOCONFIG_STATUS_DONE,
     UBX_AUTOCONFIG_STATUS_ERROR
 } ubx_autoconfig_status_t;
-// Enumeration options for field UBXDynamicModel
-typedef enum {
-    UBX_DYNMODEL_PORTABLE   = 0,
-    UBX_DYNMODEL_STATIONARY = 2,
-    UBX_DYNMODEL_PEDESTRIAN = 3,
-    UBX_DYNMODEL_AUTOMOTIVE = 4,
-    UBX_DYNMODEL_SEA = 5,
-    UBX_DYNMODEL_AIRBORNE1G = 6,
-    UBX_DYNMODEL_AIRBORNE2G = 7,
-    UBX_DYNMODEL_AIRBORNE4G = 8
-} ubx_config_dynamicmodel_t;
-
-typedef enum {
-    UBX_SBAS_SATS_AUTOSCAN = 0,
-    UBX_SBAS_SATS_WAAS     = 1,
-    UBX_SBAS_SATS_EGNOS    = 2,
-    UBX_SBAS_SATS_MSAS     = 3,
-    UBX_SBAS_SATS_GAGAN    = 4,
-    UBX_SBAS_SATS_SDCM     = 5
-} ubx_config_sats_t;
 
 #define UBX_
 typedef struct {
@@ -94,142 +102,21 @@ typedef struct {
     bool    enableBeiDou;
 } ubx_autoconfig_settings_t;
 
-// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
-#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10)
-
 // Sent messages for configuration support
-typedef struct {
+typedef struct UBX_CFG_CFG ubx_cfg_cfg_t;
-    uint32_t clearMask;
+typedef struct UBX_CFG_NAV5 ubx_cfg_nav5_t;
-    uint32_t saveMask;
+typedef struct UBX_CFG_RATE ubx_cfg_rate_t;
-    uint32_t loadMask;
+typedef struct UBX_CFG_MSG ubx_cfg_msg_t;
-    uint8_t  deviceMask;
+typedef struct UBX_CFG_PRT ubx_cfg_prt_t;
-} __attribute__((packed)) ubx_cfg_cfg_t;
+typedef struct UBX_CFG_SBAS ubx_cfg_sbas_t;
+typedef struct UBX_CFG_GNSS_CFGBLOCK ubx_cfg_gnss_cfgblock_t;
+typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t;
+typedef struct UBXSENTHEADER UBXSentHeader_t;
+typedef union  UBXSENTPACKET UBXSentPacket_t;
 
-typedef struct {
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
-    uint16_t mask;
+void ubx_autoconfig_set(ubx_autoconfig_settings_t *config);
-    uint8_t  dynModel;
+void ubx_reset_sensor_type();
-    uint8_t  fixMode;
-    int32_t  fixedAlt;
-    uint32_t fixedAltVar;
-    int8_t   minElev;
-    uint8_t  drLimit;
-    uint16_t pDop;
-    uint16_t tDop;
-    uint16_t pAcc;
-    uint16_t tAcc;
-    uint8_t  staticHoldThresh;
-    uint8_t  dgpsTimeOut;
-    uint8_t  cnoThreshNumSVs;
-    uint8_t  cnoThresh;
-    uint16_t reserved2;
-    uint32_t reserved3;
-    uint32_t reserved4;
-} __attribute__((packed)) ubx_cfg_nav5_t;
 
-typedef struct {
-    uint16_t measRate;
-    uint16_t navRate;
-    uint16_t timeRef;
-} __attribute__((packed)) ubx_cfg_rate_t;
-
-typedef struct {
-    uint8_t msgClass;
-    uint8_t msgID;
-    uint8_t rate;
-} __attribute__((packed)) ubx_cfg_msg_t;
-
-#define UBX_CFG_SBAS_MODE_ENABLED    0x01
-#define UBX_CFG_SBAS_MODE_TEST       0x02
-#define UBX_CFG_SBAS_USAGE_RANGE     0x01
-#define UBX_CFG_SBAS_USAGE_DIFFCORR  0x02
-#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
-
-// SBAS used satellite PNR bitmask (120-151)
-// -------------------------------------1---------1---------1---------1
-// -------------------------------------5---------4---------3---------2
-// ------------------------------------10987654321098765432109876543210
-// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_WAAS  0b00000000000001001110000000000100
-// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
-// MSAS 129, 137------------------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_MSAS  0b00000000000000100000001000000000
-// GAGAN 127, 128-----------------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
-// SDCM 125, 140, 141-------------------|---------|---------|---------|
-#define UBX_CFG_SBAS_SCANMODE1_SDCM  0b00000000001100000000000000100000
-
-#define UBX_CFG_SBAS_SCANMODE2       0x00
-typedef struct {
-    uint8_t  mode;
-    uint8_t  usage;
-    uint8_t  maxSBAS;
-    uint8_t  scanmode2;
-    uint32_t scanmode1;
-} __attribute__((packed)) ubx_cfg_sbas_t;
-
-typedef enum {
-    UBX_GNSS_ID_GPS     = 0,
-    UBX_GNSS_ID_SBAS    = 1,
-    UBX_GNSS_ID_GALILEO = 2,
-    UBX_GNSS_ID_BEIDOU  = 3,
-    UBX_GNSS_ID_IMES    = 4,
-    UBX_GNSS_ID_QZSS    = 5,
-    UBX_GNSS_ID_GLONASS = 6,
-    UBX_GNSS_ID_MAX
-} ubx_config_gnss_id_t;
-
-#define UBX_CFG_GNSS_FLAGS_ENABLED      0x01
-#define UBX_CFG_GNSS_FLAGS_GPS_L1CA     0x010000
-#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA    0x010000
-#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I   0x010000
-#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA    0x010000
-#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF  0x040000
-#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000
-
-#define UBX_CFG_GNSS_NUMCH_VER7         22
-#define UBX_CFG_GNSS_NUMCH_VER8         32
-
-typedef struct {
-    uint8_t  gnssId;
-    uint8_t  resTrkCh;
-    uint8_t  maxTrkCh;
-    uint8_t  resvd;
-    uint32_t flags;
-} __attribute__((packed)) ubx_cfg_gnss_cfgblock_t;
-
-typedef struct {
-    uint8_t msgVer;
-    uint8_t numTrkChHw;
-    uint8_t numTrkChUse;
-    uint8_t numConfigBlocks;
-    ubx_cfg_gnss_cfgblock_t cfgBlocks[UBX_GNSS_ID_MAX];
-} __attribute__((packed)) ubx_cfg_gnss_t;
-
-typedef struct {
-    uint8_t  prolog[2];
-    uint8_t  class;
-    uint8_t  id;
-    uint16_t len;
-} __attribute__((packed)) UBXSentHeader_t;
-
-typedef union {
-    uint8_t buffer[0];
-    struct {
-        UBXSentHeader_t header;
-        union {
-            ubx_cfg_cfg_t  cfg_cfg;
-            ubx_cfg_msg_t  cfg_msg;
-            ubx_cfg_nav5_t cfg_nav5;
-            ubx_cfg_rate_t cfg_rate;
-            ubx_cfg_sbas_t cfg_sbas;
-            ubx_cfg_gnss_t cfg_gnss;
-        } payload;
-        uint8_t resvd[2]; // added space for checksum bytes
-    } message;
-} __attribute__((packed)) UBXSentPacket_t;
-
-void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected);
-void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
 int32_t ubx_autoconfig_get_status();
 #endif /* UBX_AUTOCONFIG_H_ */

flight/modules/GPS/ubx_autoconfig.c

@@ -27,29 +27,48 @@
  * 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 "hwsettings.h"
+
 #include "inc/ubx_autoconfig.h"
 #include <pios_mem.h>
+
 // private type definitions
+
 typedef enum {
     INIT_STEP_DISABLED = 0,
     INIT_STEP_START,
-    INIT_STEP_ASK_VER,
+    INIT_STEP_RESET_GPS,
-    INIT_STEP_WAIT_VER,
+    INIT_STEP_REVO_9600_BAUD,
+    INIT_STEP_GPS_BAUD,
+    INIT_STEP_REVO_BAUD,
     INIT_STEP_ENABLE_SENTENCES,
     INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
     INIT_STEP_CONFIGURE,
     INIT_STEP_CONFIGURE_WAIT_ACK,
+    INIT_STEP_SAVE,
+    INIT_STEP_SAVE_WAIT_ACK,
     INIT_STEP_DONE,
-    INIT_STEP_ERROR,
+    INIT_STEP_ERROR
 } initSteps_t;
 
 typedef struct {
-    initSteps_t        currentStep; // Current configuration "fsm" status
+    initSteps_t currentStep; // Current configuration "fsm" status
-    uint32_t           lastStepTimestampRaw; // timestamp of last operation
+    initSteps_t currentStepSave; // Current configuration "fsm" status
-    uint32_t           lastConnectedRaw; // timestamp of last time gps was connected
+    uint32_t    lastStepTimestampRaw; // timestamp of last operation
-    UBXSentPacket_t    working_packet; // outbound "buffer"
+    uint32_t    lastConnectedRaw; // timestamp of last time gps was connected
-    ubx_autoconfig_settings_t currentSettings;
+    struct {
-    int8_t lastConfigSent; // index of last configuration string sent
+        UBXSentPacket_t working_packet; // outbound "buffer"
+        // bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
+        // failure mode otherwise:
+        // - send message with baud rate change
+        // - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly)
+        // - change Revo port baud rate
+        // sometimes fails (much worse for lowest baud rates)
+        uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
+    } __attribute__((packed));
+    volatile ubx_autoconfig_settings_t currentSettings;
+    int8_t  lastConfigSent;          // index of last configuration string sent
     struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
     uint8_t retryCount;
 } status_t;
@@ -114,15 +133,24 @@ ubx_cfg_msg_t msg_config_ubx7[] = {
 };
 
 // private defines
+
 #define LAST_CONFIG_SENT_START     (-1)
 #define LAST_CONFIG_SENT_COMPLETED (-2)
+// always reset the stored GPS configuration, even when doing autoconfig.nostore
+// that is required to do a 100% correct configuration
+// but is unexpected because it changes the stored configuration when doing autoconfig.nostore
+// note that a reset is always done with autoconfig.store
+// #define ALWAYS_RESET
 
 // private variables
 
 // enable the autoconfiguration system
-static bool enabled;
+static volatile bool enabled = false;
+static volatile bool current_step_touched = false;
+// both the pointer and what it points to are volatile.  Yuk.
+static volatile status_t *volatile status = 0;
+static uint8_t hwsettings_baud;
 
-static status_t *status = 0;
 
 static void append_checksum(UBXSentPacket_t *packet)
 {
@@ -139,29 +167,152 @@ static void append_checksum(UBXSentPacket_t *packet)
     packet->buffer[len]     = ck_a;
     packet->buffer[len + 1] = ck_b;
 }
+
+
 /**
  * prepare a packet to be sent, fill the header and appends the checksum.
  * return the total packet lenght comprising header and checksum
  */
 static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
 {
+    memset((uint8_t *)status->working_packet.buffer + len + sizeof(UBXSentHeader_t) + 2, 0, sizeof(status->bufferPaddingForPiosBugAt2400Baud));
     packet->message.header.prolog[0] = UBX_SYNC1;
     packet->message.header.prolog[1] = UBX_SYNC2;
     packet->message.header.class     = classID;
     packet->message.header.id  = messageID;
     packet->message.header.len = len;
     append_checksum(packet);
-    return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum
+
+    status->requiredAck.clsID  = classID;
+    status->requiredAck.msgID  = messageID;
+
+    return len + sizeof(UBXSentHeader_t) + 2 + sizeof(status->bufferPaddingForPiosBugAt2400Baud); // payload + header + checksum + extra bytes
 }
 
+
 static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send)
 {
     *bytes_to_send = prepare_packet(packet, classID, messageID, 0);
 }
 
-void config_rate(uint16_t *bytes_to_send)
+
+static void set_current_step_if_untouched(initSteps_t new_steps)
 {
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
+    // assume this one byte initSteps_t is atomic
+    // take care of some but not all concurrency issues
+
+    if (!current_step_touched) {
+        status->currentStep = new_steps;
+    }
+    if (current_step_touched) {
+        status->currentStep = status->currentStepSave;
+    }
+}
+
+
+void ubx_reset_sensor_type()
+{
+    ubxHwVersion = -1;
+    sensorType   = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
+    GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
+}
+
+
+static void config_reset(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+    // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
+    // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400
+    // first: reset (permanent settings to default) all but rinv = e.g. owner name
+    status->working_packet.message.payload.cfg_cfg.clearMask  = UBX_CFG_CFG_OP_RESET_SETTINGS;
+    // then: don't store any current settings to permanent
+    status->working_packet.message.payload.cfg_cfg.saveMask   = UBX_CFG_CFG_SETTINGS_NONE;
+    // lastly: load (immediately start to use) all but rinv = e.g. owner name
+    status->working_packet.message.payload.cfg_cfg.loadMask   = UBX_CFG_CFG_OP_RESET_SETTINGS;
+    // all devices
+    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL;
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+}
+
+
+// set the GPS baud rate to the user specified baud rate
+// because we may have started up with 9600 baud (for a GPS with no permanent settings)
+static void config_gps_baud(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t));
+    status->working_packet.message.payload.cfg_prt.mode   = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved
+    status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2
+    status->working_packet.message.payload.cfg_prt.inProtoMask  = 1; // 1 = UBX only (bit 0)
+    status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // 1 = UBX only (bit 0)
+    // Ask GPS to change it's speed
+    switch (hwsettings_baud) {
+    case HWSETTINGS_GPSSPEED_2400:
+        status->working_packet.message.payload.cfg_prt.baudRate = 2400;
+        break;
+    case HWSETTINGS_GPSSPEED_4800:
+        status->working_packet.message.payload.cfg_prt.baudRate = 4800;
+        break;
+    case HWSETTINGS_GPSSPEED_9600:
+        status->working_packet.message.payload.cfg_prt.baudRate = 9600;
+        break;
+    case HWSETTINGS_GPSSPEED_19200:
+        status->working_packet.message.payload.cfg_prt.baudRate = 19200;
+        break;
+    case HWSETTINGS_GPSSPEED_38400:
+        status->working_packet.message.payload.cfg_prt.baudRate = 38400;
+        break;
+    case HWSETTINGS_GPSSPEED_57600:
+        status->working_packet.message.payload.cfg_prt.baudRate = 57600;
+        break;
+    case HWSETTINGS_GPSSPEED_115200:
+        status->working_packet.message.payload.cfg_prt.baudRate = 115200;
+        break;
+    case HWSETTINGS_GPSSPEED_230400:
+        status->working_packet.message.payload.cfg_prt.baudRate = 230400;
+        break;
+    }
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t));
+}
+
+
+// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
+static void config_baud(uint8_t baud)
+{
+    // Set Revo port hwsettings_baud
+    switch (baud) {
+    case HWSETTINGS_GPSSPEED_2400:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 2400);
+        break;
+    case HWSETTINGS_GPSSPEED_4800:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 4800);
+        break;
+    case HWSETTINGS_GPSSPEED_9600:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 9600);
+        break;
+    case HWSETTINGS_GPSSPEED_19200:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 19200);
+        break;
+    case HWSETTINGS_GPSSPEED_38400:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 38400);
+        break;
+    case HWSETTINGS_GPSSPEED_57600:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 57600);
+        break;
+    case HWSETTINGS_GPSSPEED_115200:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 115200);
+        break;
+    case HWSETTINGS_GPSSPEED_230400:
+        PIOS_COM_ChangeBaud(PIOS_COM_GPS, 230400);
+        break;
+    }
+}
+
+
+static void config_rate(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
     // if rate is less than 1 uses the highest rate for current hardware
     uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99;
     if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) {
@@ -172,36 +323,31 @@ void config_rate(uint16_t *bytes_to_send)
         rate = UBX_MAX_RATE_VER8;
     }
     uint16_t period = 1000 / rate;
-
     status->working_packet.message.payload.cfg_rate.measRate = period;
     status->working_packet.message.payload.cfg_rate.navRate  = 1; // must be set to 1
     status->working_packet.message.payload.cfg_rate.timeRef  = 1; // 0 = UTC Time, 1 = GPS Time
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
+
-    status->requiredAck.clsID = UBX_CLASS_CFG;
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
-    status->requiredAck.msgID = UBX_ID_CFG_RATE;
 }
 
-void config_nav(uint16_t *bytes_to_send)
-{
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
 
+static void config_nav(uint16_t *bytes_to_send)
+{
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
     status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel;
     status->working_packet.message.payload.cfg_nav5.fixMode  = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D
     // mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB
-
     status->working_packet.message.payload.cfg_nav5.mask     = 0x01 + 0x04; // Dyn Model | posFixMode configuration
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
+
-    status->requiredAck.clsID = UBX_CLASS_CFG;
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
-    status->requiredAck.msgID = UBX_ID_CFG_NAV5;
 }
 
-void config_sbas(uint16_t *bytes_to_send)
+
+static void config_sbas(uint16_t *bytes_to_send)
 {
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
-
+    status->working_packet.message.payload.cfg_sbas.maxSBAS =
-    status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ?
+        status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3;
-                                                              status->currentSettings.SBASChannelsUsed : 3;
-
     status->working_packet.message.payload.cfg_sbas.usage   =
         (status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) |
         (status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) |
@@ -209,34 +355,28 @@ void config_sbas(uint16_t *bytes_to_send)
     // If sbas is used for anything then set mode as enabled
     status->working_packet.message.payload.cfg_sbas.mode =
         status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0;
-
     status->working_packet.message.payload.cfg_sbas.scanmode1 =
         status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN :
         status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN;
+    status->working_packet.message.payload.cfg_sbas.scanmode2 =
+        UBX_CFG_SBAS_SCANMODE2;
 
-    status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2;
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
-
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
-    status->requiredAck.clsID = UBX_CLASS_CFG;
-    status->requiredAck.msgID = UBX_ID_CFG_SBAS;
 }
 
-void config_gnss(uint16_t *bytes_to_send)
+
+static void config_gnss(uint16_t *bytes_to_send)
 {
-    int32_t i;
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t));
-
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t));
-
     status->working_packet.message.payload.cfg_gnss.numConfigBlocks = UBX_GNSS_ID_MAX;
     status->working_packet.message.payload.cfg_gnss.numTrkChHw = (ubxHwVersion > UBX_HW_VERSION_7) ? UBX_CFG_GNSS_NUMCH_VER8 : UBX_CFG_GNSS_NUMCH_VER7;
     status->working_packet.message.payload.cfg_gnss.numTrkChUse     = status->working_packet.message.payload.cfg_gnss.numTrkChHw;
 
-    for (i = 0; i < UBX_GNSS_ID_MAX; i++) {
+    for (int32_t i = 0; i < UBX_GNSS_ID_MAX; i++) {
         status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].gnssId = i;
-
         switch (i) {
         case UBX_GNSS_ID_GPS:
             if (status->currentSettings.enableGPS) {
@@ -278,30 +418,35 @@ void config_gnss(uint16_t *bytes_to_send)
         }
     }
 
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t));
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t));
-    status->requiredAck.clsID = UBX_CLASS_CFG;
-    status->requiredAck.msgID = UBX_ID_CFG_GNSS;
 }
 
-void config_save(uint16_t *bytes_to_send)
+
+static void config_save(uint16_t *bytes_to_send)
 {
-    memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
+    memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
     // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
-    status->working_packet.message.payload.cfg_cfg.saveMask   = 0x02 | 0x08; // msgConf + navConf
+    // ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400
-    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
+    status->working_packet.message.payload.cfg_cfg.saveMask   = UBX_CFG_CFG_OP_STORE_SETTINGS; // a list of settings we just set
-    *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
+    status->working_packet.message.payload.cfg_cfg.clearMask  = UBX_CFG_CFG_OP_CLEAR_SETTINGS; // everything else gets factory default
-    status->requiredAck.clsID = UBX_CLASS_CFG;
+    status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL;
-    status->requiredAck.msgID = UBX_ID_CFG_CFG;
+
+    *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
 }
+
+
 static void configure(uint16_t *bytes_to_send)
 {
     switch (status->lastConfigSent) {
     case LAST_CONFIG_SENT_START:
+        // increase message rates to 5 fixes per second
         config_rate(bytes_to_send);
         break;
+
     case LAST_CONFIG_SENT_START + 1:
         config_nav(bytes_to_send);
         break;
+
     case LAST_CONFIG_SENT_START + 2:
         if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) {
             config_gnss(bytes_to_send);
@@ -310,24 +455,19 @@ static void configure(uint16_t *bytes_to_send)
             // Skip and fall through to next step
             status->lastConfigSent++;
         }
+    // in the else case we must fall through because we must send something each time because successful send is tested externally
+
     case LAST_CONFIG_SENT_START + 3:
         config_sbas(bytes_to_send);
-        if (!status->currentSettings.storeSettings) {
-            // skips saving
-            status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
-        }
         break;
-    case LAST_CONFIG_SENT_START + 4:
-        config_save(bytes_to_send);
-        status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
-        return;
 
     default:
         status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
-        return;
+        break;
     }
 }
 
+
 static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
 {
     int8_t msg = status->lastConfigSent + 1;
@@ -338,66 +478,303 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
 
     if (msg >= 0 && msg < msg_count) {
         status->working_packet.message.payload.cfg_msg = msg_config[msg];
-
+        *bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
-        *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
-        status->requiredAck.clsID = UBX_CLASS_CFG;
-        status->requiredAck.msgID = UBX_ID_CFG_MSG;
     } else {
         status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
     }
 }
 
-void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected)
+
+// End User Documentation
+
+// There are two baud rates of interest
+// The baud rate the GPS is talking at
+// The baud rate Revo is talking at
+// These two must match for the GPS to work
+// You only have direct control of the Revo baud rate
+// The two baud rates must be the same for the Revo to send a command to the GPS
+// to tell the GPS to change it's baud rate
+// So you start out by changing Revo's baud rate to match the GPS's
+// and then enable UbxAutoConfig to tell Revo to change the GPS baud every time, just before it changes the Revo baud
+// That is the basis of these instructions
+
+// There are microprocessors and they each have internal settings
+// Revo
+// GPS
+// and each of these settings can be temporary or permanent
+
+// To change a Revo setting
+// Use the System tab in the GCS for all the following
+// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled
+// Click on UbxAutoConfig itself and the line will turn green and blue
+// To change this setting permanently, press the  red  up arrow (Save) at the top of the screen
+// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on
+// To change this setting temporarily, press the green up arrow (Send) at the top of the screen
+// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on
+
+// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes
+// This only works correctly after you have matching baud rates so Revo and GPS can talk together
+// "Settings->GPSSettings->UbxAutoConfig = Configure"         sets the internal GPS setting temporarily
+// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently
+
+// You want to wind up with a set of permanent settings that work together
+// There are two different sets of permanent settings that work together
+// GPS at 9600 baud and factory defaults
+// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud
+// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway)
+// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings
+// GPS at 57600 baud with all the settings for the current release stored in the GPS
+// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly
+// May require reconfiguring in a future release
+
+// Changable settings of interest
+// AutoConfig mode
+// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure)
+// Disabled means that changes to the GPS baud setting only affect the Revo port
+// It doesn't try to change the GPS's internal baud rate setting
+// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on)
+// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on)
+// GPS baud rate
+// Settings->HwSettings->GPSSpeed
+// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate
+// If the baud rates are not the same and an AutoConfig mode is enabled it will fail
+// If AutoConfig mode is disabled this will only change the Revo baud rate
+
+// View only settings of interest
+// Detected GPS type
+// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8)
+// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking
+// Real time progress of the GPS detection process
+// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR)
+
+// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo port setting
+// This is necessary for the GPS to work with Revo
+// To sync to and find out an unknown GPS baud rate (or sync to and use a known GPS baud rate)
+// Temporarily change the AutoConfig mode to Disabled
+// Temporarily change the GPS baud rate to a value you think it might be (or go up the list)
+// See if that baud rate is correct (Data Objects->GPSPositionSensor->SensorType will be something besides Unknown)
+// Repeat, changing the GPS baud rate, until found
+
+// Some very important facts:
+// For 9600 baud or lower, the autoconfig will configure it to factory default settings
+// For 19200 baud or higher, the autoconfig will configure it to OP required settings
+// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
+// 57600 baud is recommended for the current release
+// That can be achieved either by
+// autoconfiging the GPS from a permanent 9600 baud (and factory settings) to a temporary 57600 (with OP settings) on each power up
+// or by configuring the GPS with a permanent 57600 (with OP settings) and then permanently disabling autoconfig
+// Some previous releases used 38400 and had some other settings differences
+
+// The user should either:
+// Permanently configure their GPS to 9600 baud factory settings and tell the Revo configuration to load volatile settings at each startup by:
+// (Recommended method because new versions could require new settings and this handles future changes automatically)
+// Syncing the baud rates
+// Setting it to autoconfig.nostore and waiting for it to complete
+// Setting HwSettings.GPSSpeed to 9600 and waiting for it to complete
+// Setting it to autoconfig.store and waiting for it to complete (this tells the GPS to store the 9600 permanently)
+// Permanently setting it to autoconfig.nostore and waiting for it to complete
+// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
+// Permanently configure their GPS to 57600 baud, including OpenPilot settings and telling the Revo configuration to just set the baud to 57600 at each startup by:
+// (Less recommended method because new versions could require new settings so you would have to do this again)
+// Syncing the baud rates
+// Setting it to autoconfig.nostore and waiting for it to complete
+// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
+// Setting it to autoconfig.store
+// Permanently setting it to autoconfig.disabled
+
+// The algorithm is:
+// If autoconfig is enabled at all
+// It will assume that the GPS boot up baud rate is 9600 and the user wants that changed to HwSettings.GPSSpeed
+// and that change can be either volatile (must be done each boot up) or non-volatile (stored in GPS's non-volatile settings storage)
+// according to whether CONFIGURE is used or CONFIGUREANDSTORE is used
+// The only user who should need CONFIGUREANDSTORE stored permanently in Revo is Dave, who configures many OP GPS's before shipping
+// plug a factory default GPS in to a Revo, power up, wait for it to configure and permanently store in the GPS, power down, ship
+// If autoconfig is not enabled
+// it will use HwSettings.GPSSpeed for the baud rate and not do any configuration changes
+// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE it will
+// 1 Reset the permanent configuration back to factory default
+// 2 Disable NEMA message settings
+// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
+// 4 Save the current volatile settings to non-volatile storage
+// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will
+// 2 Disable NEMA message settings
+// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
+// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings
+
+// Talking points to point out:
+// U-center is no longer needed for any use case with this code
+// 9600 is factory default for GPS's
+// Some GPS can't even permanently store settings and must start at 9600 baud?
+// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :)
+// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings)
+// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud)
+// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings
+// GPS's with 9600 baud and factory default settings would be a good default for future OP releases
+// Changing the GPS internal settings multiple times in the future is handled automatically
+// This code is written to do a configure from 9600 to 57600
+// (actually 9600 to whatever is stored in HwSettings.GPSSpeed)
+// if autoconfig is enabled at boot up
+// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings
+// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default
+// For 19200 baud or higher, the autoconfig will configure it to OP required settings
+// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
+// This is good for factory default GPS's
+// This is good in case we change some settings in a future release
+
+
+void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
 {
     *bytes_to_send = 0;
     *buffer = (char *)status->working_packet.buffer;
-    if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) {
+    current_step_touched = false;
-        return; // autoconfig not enabled
+
-    }
+    // autoconfig struct not yet allocated
-    if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) {
+    if (!status) {
         return;
     }
-    // when gps is disconnected it will replay the autoconfig sequence.
+    // smallest delay between each step
-    if (!gps_connected) {
+    if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_VERIFIED_STEP_WAIT_TIME) {
-        if (status->currentStep == INIT_STEP_DONE) {
+        return;
-            // if some operation is in progress and it is not running into issues it maybe that
+    }
-            // the disconnection is only due to wrong message rates, so reinit only when done.
+    // get UBX version whether autoconfig is enabled or not
-            // errors caused by disconnection are handled by error retry logic
+    // this allows the user to try some baud rates and visibly see when it works
-            if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) {
+    // ubxHwVersion is a global set externally by the caller of this function
-                status->currentStep = INIT_STEP_START;
+    if (ubxHwVersion <= 0) {
-                return;
+        // at low baud rates and high data rates the ubx gps simply must drop some outgoing data
-            }
+        // this isn't really an error
+        // and when a lot of data is being dropped, the MON VER reply often gets dropped
+        // on the other hand, uBlox documents that some versions discard data that is over 1 second old
+        // implying a 1 second send buffer and that it could be over 1 second before a reply is received
+        // later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full
+        // and that could be even longer than 1 second
+        // send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped
+
+        // wait for the normal reply timeout before sending it over and over
+        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) {
+            return;
         }
-    } else {
+        build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
-        // reset connection timer
+        // keep timeouts running properly, we (will have) just sent a packet that generates a reply
-        status->lastConnectedRaw = PIOS_DELAY_GetRaw();
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        return;
+    }
+    if (!enabled) {
+        // keep resetting the timeouts here if we are not actually going to run the configure code
+        // not really necessary, but it keeps the timer from wrapping every 50 seconds
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        return; // autoconfig not enabled
+    }
+
+    // replaying constantly could wear the settings memory out
+    // don't allow constant reconfiging when offline
+    // don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring
+    if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) {
+        return;
     }
 
     switch (status->currentStep) {
-    case INIT_STEP_ERROR:
-    case INIT_STEP_DISABLED:
-    case INIT_STEP_DONE:
-        return;
-
     case INIT_STEP_START:
-    case INIT_STEP_ASK_VER:
+        // we should look for the GPS version again
-        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        ubx_reset_sensor_type();
-        build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
+        // do not fall through to next state
-        status->currentStep = INIT_STEP_WAIT_VER;
+        // or it might try to get the sensor type when the baud rate is half changed
+        set_current_step_if_untouched(INIT_STEP_RESET_GPS);
+        // allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
         break;
 
-    case INIT_STEP_WAIT_VER:
+    case INIT_STEP_RESET_GPS:
-        if (ubxHwVersion > 0) {
+        // make sure we don't change the baud rate too soon and garble the packet being sent
-            status->lastConfigSent = LAST_CONFIG_SENT_START;
+        // even after pios says the buffer is empty, the serial port buffer still has data in it
-            status->currentStep    = INIT_STEP_ENABLE_SENTENCES;
+        // and changing the baud will screw it up
+        // when the GPS is configured to send a lot of data, but has a low baud rate
+        // it has way too many messages to send and has to drop most of them
+
+        // Retrieve desired GPS baud rate once for use throughout this module
+        HwSettingsGPSSpeedGet(&hwsettings_baud);
+#if !defined(ALWAYS_RESET)
+        // ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore
+        // but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
+        // if user requests a low baud rate then we just reset and leave it set to NEMA
+        // because low baud and high OP data rate doesn't play nice
+        // if user requests that settings be saved, we will reset here too
+        // that makes sure that all strange settings are reset to factory default
+        // else these strange settings may persist because we don't reset all settings by table
+        if (status->currentSettings.storeSettings)
+#endif
+        {
+            // reset all GPS parameters to factory default (configure low rate NEMA for low baud rates)
+            // this is not usable by OP code for either baud rate or types of messages sent
+            // but it starts up very quickly for use with autoconfig-nostore (which sets a high baud and enables all the necessary messages)
+            config_reset(bytes_to_send);
             status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        }
+        // else allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD);
+        break;
+
+    case INIT_STEP_REVO_9600_BAUD:
+#if !defined(ALWAYS_RESET)
+        // if user requests a low baud rate then we just reset and leave it set to NEMA
+        // because low baud and high OP data rate doesn't play nice
+        // if user requests that settings be saved, we will reset here too
+        // that makes sure that all strange settings are reset to factory default
+        // else these strange settings may persist because we don't reset all settings by hand
+        if (status->currentSettings.storeSettings)
+#endif
+        {
+            // wait for previous step
+            if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
+                return;
+            }
+            // set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done
+            config_baud(HWSETTINGS_GPSSPEED_9600);
+        }
+    // at most, we just set Revo baud and that doesn't send any data
+    // fall through to next state
+    // we can do that if we choose because we haven't sent any data in this state
+    // set_current_step_if_untouched(INIT_STEP_GPS_BAUD);
+    // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+    // break;
+
+    case INIT_STEP_GPS_BAUD:
+        // https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
+        // It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could
+        // affect baud rate and other transmission parameters. Because there may be messages queued for transmission
+        // there may be uncertainty about which protocol applies to such messages. In addition a message currently in
+        // transmission may be corrupted by a protocol change. Host data reception parameters may have to be changed to
+        // be able to receive future messages, including the acknowledge message associated with the UBX-CFG-CFG message.
+
+        // so the message that changes the baud rate will send it's acknowledgement back at the new baud rate; this is not good.
+        // if your message was corrupted, you didn't change the baud rate and you have to guess; try pinging at both baud rates.
+        // also, you would have to change the baud rate instantly after the last byte of the sentence was sent,
+        // and you would have to poll the port in real time for that, and there may be messages ahead of the baud rate change.
+        //
+        // so we ignore the ack from this.  it has proven to be reliable (with the addition of two dummy bytes after the packet)
+
+        // set the GPS internal baud rate to the user configured value
+        config_gps_baud(bytes_to_send);
+        set_current_step_if_untouched(INIT_STEP_REVO_BAUD);
+        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        break;
+
+    case INIT_STEP_REVO_BAUD:
+        // wait for previous step
+        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
             return;
         }
-
+        // set the Revo GPS port baud rate to the (same) user configured value
-        if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
+        config_baud(hwsettings_baud);
-            status->currentStep = INIT_STEP_ASK_VER;
+        status->lastConfigSent = LAST_CONFIG_SENT_START;
+        status->retryCount     = 0;
+        // skip enabling UBX sentences for low baud rates
+        // low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration
+        if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) {
+            set_current_step_if_untouched(INIT_STEP_SAVE);
+        } else {
+            set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES);
         }
-        return;
+        // allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        break;
 
     case INIT_STEP_ENABLE_SENTENCES:
     case INIT_STEP_CONFIGURE:
@@ -409,61 +786,136 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec
             enable_sentences(bytes_to_send);
         }
 
-        status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        // for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
         if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
-            status->lastConfigSent = LAST_CONFIG_SENT_START;
+            if (step_configure) {
-            status->currentStep    = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE;
+                // zero retries for the next state that needs it (INIT_STEP_SAVE)
+                status->retryCount = 0;
+                set_current_step_if_untouched(INIT_STEP_SAVE);
+            } else {
+                // finished enabling sentences, now configure() needs to start at the beginning
+                status->lastConfigSent = LAST_CONFIG_SENT_START;
+                set_current_step_if_untouched(INIT_STEP_CONFIGURE);
+            }
         } else {
-            status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
+            set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK);
+            status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
         }
-        return;
+        break;
     }
+
     case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
     case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
     {
         bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
         if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
             // Continue with next configuration option
+            // start retries over for the next setting to be sent
             status->retryCount = 0;
             status->lastConfigSent++;
-        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT ||
+        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT &&
-                   (ubxLastNak.clsID == status->requiredAck.clsID && ubxLastNak.msgID == status->requiredAck.msgID)) {
+                   (ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
+            // allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            break;
+        } else {
             // timeout or NAK, resend the message or abort
             status->retryCount++;
             if (status->retryCount > UBX_MAX_RETRIES) {
-                status->currentStep = INIT_STEP_ERROR;
+                set_current_step_if_untouched(INIT_STEP_ERROR);
                 status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
-                return;
+                break;
             }
         }
-        // no abort condition, continue or retries.,
+        // success or failure here, retries are handled elsewhere
         if (step_configure) {
-            status->currentStep = INIT_STEP_CONFIGURE;
+            set_current_step_if_untouched(INIT_STEP_CONFIGURE);
         } else {
-            status->currentStep = INIT_STEP_ENABLE_SENTENCES;
+            set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES);
         }
-        return;
+        break;
     }
+
+    case INIT_STEP_SAVE:
+        if (status->currentSettings.storeSettings) {
+            config_save(bytes_to_send);
+            set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK);
+            status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        } else {
+            set_current_step_if_untouched(INIT_STEP_DONE);
+            // allow it enter INIT_STEP_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+        }
+        break;
+
+    // we could remove this state
+    // if we retry, it writes to settings storage a few more times
+    // and it is probably the ack that was dropped, with the save actually performed correctly
+    case INIT_STEP_SAVE_WAIT_ACK:
+        if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
+            // Continue with next configuration option
+            set_current_step_if_untouched(INIT_STEP_DONE);
+            // note that we increase the reply timeout in case the GPS must do a flash erase
+        } else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TO_SAVE_TIMEOUT &&
+                   (ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
+            // allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            break;
+        } else {
+            // timeout or NAK, resend the message or abort
+            status->retryCount++;
+            if (status->retryCount > UBX_MAX_RETRIES / 2) {
+                // give up on the retries
+                set_current_step_if_untouched(INIT_STEP_ERROR);
+                status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
+            } else {
+                // retry a few times
+                set_current_step_if_untouched(INIT_STEP_SAVE);
+            }
+        }
+        break;
+
+    case INIT_STEP_ERROR:
+        // on error we should get the GPS version immediately
+        ubx_reset_sensor_type();
+    // fall through
+    case INIT_STEP_DISABLED:
+    case INIT_STEP_DONE:
+        break;
     }
 }
 
-void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
+
+void ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
 {
+    initSteps_t new_step;
+
     enabled = false;
-    if (config.autoconfigEnabled) {
+
-        if (!status) {
+    if (!status) {
-            status = (status_t *)pios_malloc(sizeof(status_t));
+        status = (status_t *)pios_malloc(sizeof(status_t));
-            PIOS_Assert(status);
+        PIOS_Assert(status);
-            memset(status, 0, sizeof(status_t));
+        memset((status_t *)status, 0, sizeof(status_t));
-            status->currentStep = INIT_STEP_DISABLED;
+    }
-        }
+
-        status->currentSettings = config;
+    // if caller used NULL, just use current settings to restart autoconfig process
-        status->currentStep     = INIT_STEP_START;
+    if (config != NULL) {
-        enabled = true;
+        status->currentSettings = *config;
+    }
+    if (status->currentSettings.autoconfigEnabled) {
+        new_step = INIT_STEP_START;
     } else {
-        if (status) {
+        new_step = INIT_STEP_DISABLED;
-            status->currentStep = INIT_STEP_DISABLED;
+    }
-        }
+
+    // assume this one byte initSteps_t is atomic
+    // take care of some but not all concurrency issues
+
+    status->currentStep     = new_step;
+    status->currentStepSave = new_step;
+    current_step_touched    = true;
+    status->currentStep     = new_step;
+    status->currentStepSave = new_step;
+
+    if (status->currentSettings.autoconfigEnabled) {
+        enabled = true;
     }
 }
 

flight/modules/Logging/Logging.c

@@ -134,7 +134,7 @@ static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
         }
         DebugLogEntrySet(entry);
     } else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
-        uint8_t armed;
+        FlightStatusArmedOptions armed;
         FlightStatusArmedGet(&armed);
         if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
             PIOS_DEBUGLOG_Format();

flight/modules/ManualControl/armhandler.c

@@ -182,6 +182,8 @@ void armHandler(bool newinit, FrameType_t frameType)
     case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
         armingInputLevel = -1.0f * acc.AccessoryVal;
         break;
+    default:
+        break;
     }
 
     bool manualArm    = false;
@@ -317,6 +319,7 @@ static bool okToArm(void)
         return false;
 
     case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
+    case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
         return true;
 
     default:

flight/modules/ManualControl/manualcontrol.c

@@ -182,7 +182,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     frameType = GetCurrentFrameType();
 #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
-    uint8_t TreatCustomCraftAs;
+    VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
 
 
@@ -486,7 +486,7 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
 {
     uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
     uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
-    uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
+    StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 
     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
     if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {

flight/modules/ManualControl/pathfollowerhandler.c

@@ -55,9 +55,9 @@ void pathFollowerHandler(bool newinit)
         plan_initialize();
     }
 
-    uint8_t flightMode;
+    FlightStatusFlightModeOptions flightMode;
-    uint8_t assistedControlFlightMode;
+    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
-    uint8_t flightModeAssist;
+    FlightStatusFlightModeAssistOptions flightModeAssist;
     FlightStatusFlightModeGet(&flightMode);
     FlightStatusFlightModeAssistGet(&flightModeAssist);
     FlightStatusAssistedControlStateGet(&assistedControlFlightMode);

flight/modules/ManualControl/stabilizedhandler.c

@@ -96,33 +96,34 @@ void stabilizedHandler(bool newinit)
 
     switch (flightStatus.FlightMode) {
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-        stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-        stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-        stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
-        stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
-        stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
         break;
     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
-        stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
         break;
     default:
         // Major error, this should not occur because only enter this block when one of these is true
         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-        stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
+        stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
         return;
     }
 
     stabilization.Roll =
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
@@ -134,6 +135,7 @@ void stabilizedHandler(bool newinit)
     stabilization.Pitch =
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
@@ -155,6 +157,7 @@ void stabilizedHandler(bool newinit)
         stabilization.Yaw =
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :

flight/modules/ManualControl/takeofflocationhandler.c

@@ -42,8 +42,8 @@ void takeOffLocationHandlerInit()
 {
     TakeOffLocationInitialize();
     // check whether there is a preset/valid takeoff location
-    uint8_t mode;
+    TakeOffLocationModeOptions mode;
-    uint8_t status;
+    TakeOffLocationStatusOptions status;
     TakeOffLocationModeGet(&mode);
     TakeOffLocationStatusGet(&status);
     // preset with invalid location will actually behave like FirstTakeoff
@@ -61,8 +61,8 @@ void takeOffLocationHandlerInit()
  */
 void takeOffLocationHandler()
 {
-    uint8_t armed;
+    FlightStatusArmedOptions armed;
-    uint8_t status;
+    TakeOffLocationStatusOptions status;
 
     FlightStatusArmedGet(&armed);
 
@@ -77,7 +77,7 @@ void takeOffLocationHandler()
     case FLIGHTSTATUS_ARMED_ARMING:
     case FLIGHTSTATUS_ARMED_ARMED:
         if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
-            uint8_t mode;
+            TakeOffLocationModeOptions mode;
             TakeOffLocationModeGet(&mode);
 
             if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
@@ -91,7 +91,7 @@ void takeOffLocationHandler()
     case FLIGHTSTATUS_ARMED_DISARMED:
         // unset if location is to be acquired at each arming
         if (locationSet) {
-            uint8_t mode;
+            TakeOffLocationModeOptions mode;
             TakeOffLocationModeGet(&mode);
             if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
                 locationSet = false;

flight/modules/PathFollower/inc/pathfollowerfsm.h

@@ -34,6 +34,7 @@
 extern "C" {
 #include <stabilizationdesired.h>
 }
+#include <pidcontroldowncallback.h>
 
 typedef enum {
     PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
@@ -42,7 +43,7 @@ typedef enum {
     PFFSM_STATE_ABORT // Abort on error
 } PathFollowerFSMState_T;
 
-class PathFollowerFSM {
+class PathFollowerFSM : public PIDControlDownCallback {
 public:
     // PathFollowerFSM() {};
     virtual void Inactive(void) {}

flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h

@@ -64,7 +64,7 @@ public:
 private:
     void UpdateVelocityDesired(void);
     int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
-    void setArmedIfChanged(uint8_t val);
+    void setArmedIfChanged(FlightStatusArmedOptions val);
 
     VtolAutoTakeoffFSM *fsm;
     VtolPathFollowerSettingsData *vtolPathFollowerSettings;

flight/modules/PathFollower/inc/vtolautotakeofffsm.h

@@ -82,7 +82,7 @@ protected:
     // FSM instance data type
     typedef struct {
         StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
-        PathFollowerFSM_AutoTakeoffState_T currentState;
+        StatusVtolAutoTakeoffStateOptions currentState;
         TakeOffLocationData takeOffLocation;
         uint32_t stateRunCount;
         uint32_t stateTimeoutCount;
@@ -142,7 +142,7 @@ protected:
     void run_abort(uint8_t);
 
     void initFSM(void);
-    void setState(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
+    void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
     int32_t runState();
     int32_t runAlways();
 

flight/modules/PathFollower/inc/vtollandcontroller.h

@@ -64,6 +64,7 @@ public:
 private:
     void UpdateVelocityDesired(void);
     int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
+    void setArmedIfChanged(FlightStatusArmedOptions val);
 
     PathFollowerFSM *fsm;
     VtolPathFollowerSettingsData *vtolPathFollowerSettings;

flight/modules/PathFollower/inc/vtollandfsm.h

@@ -82,7 +82,7 @@ protected:
     // FSM instance data type
     typedef struct {
         StatusVtolLandData  fsmLandStatus;
-        PathFollowerFSM_LandState_T currentState;
+        StatusVtolLandStateOptions currentState;
         TakeOffLocationData takeOffLocation;
         uint32_t stateRunCount;
         uint32_t stateTimeoutCount;
@@ -133,7 +133,7 @@ protected:
     void setup_abort(void);
     void run_abort(uint8_t);
     void initFSM(void);
-    void setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason);
+    void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason);
     int32_t runState();
     int32_t runAlways();
     void updateVtolLandFSMStatus();

flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

@@ -101,33 +101,12 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
 {
     // Set the objective's target velocity
 
-    if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
-        controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
-        controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
-                                         pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
+    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
-        controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+    controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
-        controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
+    fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
-        fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
-    } else {
-        float velocity_down;
-        FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
-        controlDown.UpdateVelocitySetpoint(-velocity_down);
-        controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
-        // pathplanner mode the waypoint provides the final destination including altitude. Takeoff would
-        // often be waypoint 0, in which case the starting location is the current location, and the end location
-        // is the waypoint location which really needs to be the same as the end in north and east. If it is not,
-        // it will fly to that location the during ascent.
-        // Note in pathplanner mode we use the start location which is the current initial location as the
-        // target NE to control.  Takeoff only ascends vertically.
-        controlNE.UpdatePositionSetpoint(pathDesired->Start.North, pathDesired->Start.East);
-        // Sanity check that the end location is at least a reasonable height above the start location in the down direction
-        float autotakeoff_height = pathDesired->Start.Down - pathDesired->End.Down;
-        if (autotakeoff_height < 2.0f) {
-            pathDesired->End.Down = pathDesired->Start.Down - 2.0f;
-        }
-        controlDown.UpdatePositionSetpoint(pathDesired->End.Down); // the altitude is set by the end location.
-        fsm->setControlState(STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE);
-    }
 }
 void VtolAutoTakeoffController::Deactivate(void)
 {
@@ -147,7 +126,7 @@ void VtolAutoTakeoffController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
 
@@ -299,7 +278,7 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
     PathStatusSet(pathStatus);
 }
 
-void VtolAutoTakeoffController::setArmedIfChanged(uint8_t val)
+void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val)
 {
     if (flightStatus->Armed != val) {
         flightStatus->Armed = val;

flight/modules/PathFollower/vtolautotakeofffsm.cpp

@@ -128,23 +128,23 @@ void VtolAutoTakeoffFSM::Inactive(void)
 void VtolAutoTakeoffFSM::initFSM(void)
 {
     if (vtolPathFollowerSettings != 0) {
-        setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
     } else {
-        mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
+        mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
     }
 }
 
 void VtolAutoTakeoffFSM::Activate()
 {
     memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
-    mAutoTakeoffData->currentState   = AUTOTAKEOFF_STATE_INACTIVE;
+    mAutoTakeoffData->currentState   = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
     mAutoTakeoffData->flLowAltitude  = true;
     mAutoTakeoffData->flAltitudeHold = false;
     mAutoTakeoffData->boundThrustMin = 0.0f;
     mAutoTakeoffData->boundThrustMax = 0.0f;
     mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
     TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation));
-    mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[AUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
+    mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
     mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
     assessAltitude();
 
@@ -153,31 +153,31 @@ void VtolAutoTakeoffFSM::Activate()
     StabilizationDesiredData stabDesired;
     StabilizationDesiredGet(&stabDesired);
     if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
-        setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         return;
     }
 
     // initially inactive and wait for a change in controlstate.
-    setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 }
 
 void VtolAutoTakeoffFSM::Abort(void)
 {
-    setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 }
 
 PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
 {
     switch (mAutoTakeoffData->currentState) {
-    case AUTOTAKEOFF_STATE_INACTIVE:
+    case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
         return PFFSM_STATE_INACTIVE;
 
         break;
-    case AUTOTAKEOFF_STATE_ABORT:
+    case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
         return PFFSM_STATE_ABORT;
 
         break;
-    case AUTOTAKEOFF_STATE_DISARMED:
+    case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
         return PFFSM_STATE_DISARMED;
 
         break;
@@ -248,7 +248,7 @@ void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDes
 // Set the new state and perform setup for subsequent state run calls
 // This is called by state run functions on event detection that drive
 // state transitions.
-void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
+void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
 {
     mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
 
@@ -257,7 +257,7 @@ void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, S
     }
     mAutoTakeoffData->currentState = newState;
 
-    if (newState != AUTOTAKEOFF_STATE_INACTIVE) {
+    if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
         PositionStateData positionState;
         PositionStateGet(&positionState);
         float takeOffDown = 0.0f;
@@ -324,22 +324,22 @@ void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOption
 
     switch (controlState) {
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
-        setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         break;
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
-        setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         break;
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
-        setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         break;
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
-        setState(AUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         break;
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
-        setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         break;
     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
-        setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         break;
     }
 }
@@ -367,7 +367,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
 
     StabilizationDesiredGet(&stabDesired);
     if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
-        setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
         return;
     }
 
@@ -377,7 +377,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
     mAutoTakeoffData->boundThrustMin    = -0.1f;
     mAutoTakeoffData->boundThrustMax    = 0.0f;
 
-    setState(AUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
 }
 
 // STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
@@ -415,7 +415,7 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout
     }
 
     if (flTimeout) {
-        setState(AUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
     }
 }
 
@@ -442,7 +442,7 @@ void VtolAutoTakeoffFSM::run_thrustup(__attribute__((unused)) uint8_t flTimeout)
     }
 
     if (flTimeout) {
-        setState(AUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
     }
 }
 
@@ -458,7 +458,7 @@ void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
 
     StabilizationDesiredGet(&stabDesired);
     if (stabDesired.Thrust < 0.0f) {
-        setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
         return;
     }
 
@@ -470,11 +470,11 @@ void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
     float down_error    = pathDesired->End.Down - positionState.Down;
     float positionError = sqrtf(north_error * north_error + east_error * east_error);
     if (positionError > 3.0f) {
-        setState(AUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
         return;
     }
     if (fabsf(down_error) < 0.5f) {
-        setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
         return;
     }
 }
@@ -515,11 +515,11 @@ void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeou
     StabilizationDesiredData stabDesired;
     StabilizationDesiredGet(&stabDesired);
     if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) {
-        setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
     }
 
     if (flTimeout) {
-        setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
     }
 }
 
@@ -534,7 +534,7 @@ void VtolAutoTakeoffFSM::setup_thrustoff(void)
 
 void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
 {
-    setState(AUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
+    setState(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 }
 
 // STATE: DISARMED

flight/modules/PathFollower/vtolbrakecontroller.cpp

@@ -132,7 +132,7 @@ void VtolBrakeController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta,
                                dT,
                                10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
@@ -312,8 +312,10 @@ int8_t VtolBrakeController::UpdateStabilizationDesired(void)
             // and a better throttle management to the standard Position Hold.
             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
             break;
+        default:
+            break;
         }
-        stabDesired.StabilizationMode.Thrust = thrustMode;
+        stabDesired.StabilizationMode.Thrust = (StabilizationDesiredStabilizationModeOptions)thrustMode;
     }
 
     // set the thrust value

flight/modules/PathFollower/vtolflycontroller.cpp

@@ -127,7 +127,7 @@ void VtolFlyController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
@@ -136,7 +136,7 @@ void VtolFlyController::SettingsUpdated(void)
     controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
                                  vtolPathFollowerSettings->VerticalVelPID.Ki,
                                  vtolPathFollowerSettings->VerticalVelPID.Kd,
-                                 vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA
+                                 vtolPathFollowerSettings->VerticalVelPID.Beta,
                                  dT,
                                  vtolPathFollowerSettings->VerticalVelMax);
     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
@@ -145,6 +145,9 @@ void VtolFlyController::SettingsUpdated(void)
     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
     controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+
+    // disable neutral thrust calcs which should only be done in a hold mode.
+    controlDown.DisableNeutralThrustCalc();
 }
 
 /**

flight/modules/PathFollower/vtollandcontroller.cpp

@@ -1,275 +1,275 @@
-/*
+/*
- ******************************************************************************
+ ******************************************************************************
- *
+ *
- * @file       vtollandcontroller.cpp
+ * @file       vtollandcontroller.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      Vtol landing controller loop
+ * @brief      Vtol landing controller loop
- * @see        The GNU Public License (GPL) Version 3
+ * @see        The GNU Public License (GPL) Version 3
- *
+ *
- *****************************************************************************/
+ *****************************************************************************/
-/*
+/*
- * This program is free software; you can redistribute it and/or modify
+ * 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
+ * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
+ * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
+ * (at your option) any later version.
- *
+ *
- * This program is distributed in the hope that it will be useful, but
+ * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
+ * for more details.
- *
+ *
- * You should have received a copy of the GNU General Public License along
+ * 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.,
+ * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
+ */
-
+
-extern "C" {
+extern "C" {
-#include <openpilot.h>
+#include <openpilot.h>
-
+
-#include <callbackinfo.h>
+#include <callbackinfo.h>
-
+
-#include <math.h>
+#include <math.h>
-#include <pid.h>
+#include <pid.h>
-#include <CoordinateConversions.h>
+#include <CoordinateConversions.h>
-#include <sin_lookup.h>
+#include <sin_lookup.h>
-#include <pathdesired.h>
+#include <pathdesired.h>
-#include <paths.h>
+#include <paths.h>
-#include "plans.h"
+#include "plans.h"
-#include <sanitycheck.h>
+#include <sanitycheck.h>
-
+
-#include <accelstate.h>
+#include <accelstate.h>
-#include <vtolpathfollowersettings.h>
+#include <vtolpathfollowersettings.h>
-#include <flightstatus.h>
+#include <flightstatus.h>
-#include <flightmodesettings.h>
+#include <flightmodesettings.h>
-#include <pathstatus.h>
+#include <pathstatus.h>
-#include <positionstate.h>
+#include <positionstate.h>
-#include <velocitystate.h>
+#include <velocitystate.h>
-#include <velocitydesired.h>
+#include <velocitydesired.h>
-#include <stabilizationdesired.h>
+#include <stabilizationdesired.h>
-#include <attitudestate.h>
+#include <attitudestate.h>
-#include <takeofflocation.h>
+#include <takeofflocation.h>
-#include <manualcontrolcommand.h>
+#include <manualcontrolcommand.h>
-#include <systemsettings.h>
+#include <systemsettings.h>
-#include <stabilizationbank.h>
+#include <stabilizationbank.h>
-#include <stabilizationdesired.h>
+#include <stabilizationdesired.h>
-#include <vtolselftuningstats.h>
+#include <vtolselftuningstats.h>
-#include <pathsummary.h>
+#include <pathsummary.h>
-}
+}
-
+
-// C++ includes
+// C++ includes
-#include "vtollandcontroller.h"
+#include "vtollandcontroller.h"
-#include "pathfollowerfsm.h"
+#include "pathfollowerfsm.h"
-#include "vtollandfsm.h"
+#include "vtollandfsm.h"
-#include "pidcontroldown.h"
+#include "pidcontroldown.h"
-
+
-// Private constants
+// Private constants
-
+
-// pointer to a singleton instance
+// pointer to a singleton instance
-VtolLandController *VtolLandController::p_inst = 0;
+VtolLandController *VtolLandController::p_inst = 0;
-
+
-VtolLandController::VtolLandController()
+VtolLandController::VtolLandController()
-    : fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
+    : fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
-{}
+{}
-
+
-// Called when mode first engaged
+// Called when mode first engaged
-void VtolLandController::Activate(void)
+void VtolLandController::Activate(void)
-{
+{
-    if (!mActive) {
+    if (!mActive) {
-        mActive = true;
+        mActive = true;
-        SettingsUpdated();
+        SettingsUpdated();
-        fsm->Activate();
+        fsm->Activate();
-        controlDown.Activate();
+        controlDown.Activate();
-        controlNE.Activate();
+        controlNE.Activate();
-    }
+    }
-}
+}
-
+
-uint8_t VtolLandController::IsActive(void)
+uint8_t VtolLandController::IsActive(void)
-{
+{
-    return mActive;
+    return mActive;
-}
+}
-
+
-uint8_t VtolLandController::Mode(void)
+uint8_t VtolLandController::Mode(void)
-{
+{
-    return PATHDESIRED_MODE_LAND;
+    return PATHDESIRED_MODE_LAND;
-}
+}
-
+
-// Objective updated in pathdesired, e.g. same flight mode but new target velocity
+// Objective updated in pathdesired, e.g. same flight mode but new target velocity
-void VtolLandController::ObjectiveUpdated(void)
+void VtolLandController::ObjectiveUpdated(void)
-{
+{
-    // Set the objective's target velocity
+    // Set the objective's target velocity
-    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
+    controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
-    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
+    controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
-                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
+                                     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
-    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
+    controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
-}
+}
-void VtolLandController::Deactivate(void)
+void VtolLandController::Deactivate(void)
-{
+{
-    if (mActive) {
+    if (mActive) {
-        mActive = false;
+        mActive = false;
-        fsm->Inactive();
+        fsm->Inactive();
-        controlDown.Deactivate();
+        controlDown.Deactivate();
-        controlNE.Deactivate();
+        controlNE.Deactivate();
-    }
+    }
-}
+}
-
+
-
+
-void VtolLandController::SettingsUpdated(void)
+void VtolLandController::SettingsUpdated(void)
-{
+{
-    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
+    const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
-
+
-    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
+    controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
-                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
+                               vtolPathFollowerSettings->HorizontalVelPID.Ki,
-                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
+                               vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
-                               dT,
+                               dT,
-                               vtolPathFollowerSettings->HorizontalVelMax);
+                               vtolPathFollowerSettings->HorizontalVelMax);
-
+
-
+
-    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
+    controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
-    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
+    controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
-
+
-    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
+    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
-                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
-                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
-                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
-                                 dT,
+                                 dT,
-                                 vtolPathFollowerSettings->VerticalVelMax);
+                                 vtolPathFollowerSettings->VerticalVelMax);
-
+
-    // The following is not currently used in the landing control.
+    // The following is not currently used in the landing control.
-    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
+    controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
-
+
-    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsData vtolSelfTuningStats;
-    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
-    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
-    // initialise limits on thrust but note the FSM can override.
+    // initialise limits on thrust but note the FSM can override.
-    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+    controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
-    fsm->SettingsUpdated();
+    fsm->SettingsUpdated();
-}
+}
-
+
-/**
+/**
- * Initialise the module, called on startup
+ * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
+ * \returns 0 on success or -1 if initialisation failed
- */
+ */
-int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
+int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
-{
+{
-    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
-    if (fsm == 0) {
+    if (fsm == 0) {
-        fsm = (PathFollowerFSM *)VtolLandFSM::instance();
+        fsm = (PathFollowerFSM *)VtolLandFSM::instance();
-        VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
+        VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
-        vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+        vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
-        controlDown.Initialize(fsm);
+        controlDown.Initialize(fsm);
-    }
+    }
-
+
-    return 0;
+    return 0;
-}
+}
-
+
-
+
-void VtolLandController::UpdateVelocityDesired()
+void VtolLandController::UpdateVelocityDesired()
-{
+{
-    VelocityStateData velocityState;
+    VelocityStateData velocityState;
-
+
-    VelocityStateGet(&velocityState);
+    VelocityStateGet(&velocityState);
-    VelocityDesiredData velocityDesired;
+    VelocityDesiredData velocityDesired;
-
+
-    controlDown.UpdateVelocityState(velocityState.Down);
+    controlDown.UpdateVelocityState(velocityState.Down);
-    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
-
+
-    // Implement optional horizontal position hold.
+    // Implement optional horizontal position hold.
-    if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
+    if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
-        (flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
+        (flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
-        // landing flight mode has stored original horizontal position in pathdesired
+        // landing flight mode has stored original horizontal position in pathdesired
-        PositionStateData positionState;
+        PositionStateData positionState;
-        PositionStateGet(&positionState);
+        PositionStateGet(&positionState);
-        controlNE.UpdatePositionState(positionState.North, positionState.East);
+        controlNE.UpdatePositionState(positionState.North, positionState.East);
-        controlNE.ControlPosition();
+        controlNE.ControlPosition();
-    }
+    }
-
+
-    velocityDesired.Down  = controlDown.GetVelocityDesired();
+    velocityDesired.Down  = controlDown.GetVelocityDesired();
-    float north, east;
+    float north, east;
-    controlNE.GetVelocityDesired(&north, &east);
+    controlNE.GetVelocityDesired(&north, &east);
-    velocityDesired.North = north;
+    velocityDesired.North = north;
-    velocityDesired.East  = east;
+    velocityDesired.East  = east;
-
+
-    // update pathstatus
+    // update pathstatus
-    pathStatus->error     = 0.0f;
+    pathStatus->error     = 0.0f;
-    pathStatus->fractional_progress = 0.0f;
+    pathStatus->fractional_progress = 0.0f;
-    if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
+    if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
-        pathStatus->fractional_progress = 1.0f;
+        pathStatus->fractional_progress = 1.0f;
-    }
+    }
-    pathStatus->path_direction_north = velocityDesired.North;
+    pathStatus->path_direction_north = velocityDesired.North;
-    pathStatus->path_direction_east  = velocityDesired.East;
+    pathStatus->path_direction_east  = velocityDesired.East;
-    pathStatus->path_direction_down  = velocityDesired.Down;
+    pathStatus->path_direction_down  = velocityDesired.Down;
-
+
-    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
+    pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
-    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
+    pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
-    pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
+    pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
-
+
-
+
-    VelocityDesiredSet(&velocityDesired);
+    VelocityDesiredSet(&velocityDesired);
-}
+}
-
+
-int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
+int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
-{
+{
-    uint8_t result = 1;
+    uint8_t result = 1;
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-    AttitudeStateData attitudeState;
+    AttitudeStateData attitudeState;
-    StabilizationBankData stabSettings;
+    StabilizationBankData stabSettings;
-    float northCommand;
+    float northCommand;
-    float eastCommand;
+    float eastCommand;
-
+
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-    AttitudeStateGet(&attitudeState);
+    AttitudeStateGet(&attitudeState);
-    StabilizationBankGet(&stabSettings);
+    StabilizationBankGet(&stabSettings);
-
+
-    controlNE.GetNECommand(&northCommand, &eastCommand);
+    controlNE.GetNECommand(&northCommand, &eastCommand);
-    stabDesired.Thrust = controlDown.GetDownCommand();
+    stabDesired.Thrust = controlDown.GetDownCommand();
-
+
-    float angle_radians = DEG2RAD(attitudeState.Yaw);
+    float angle_radians = DEG2RAD(attitudeState.Yaw);
-    float cos_angle     = cosf(angle_radians);
+    float cos_angle     = cosf(angle_radians);
-    float sine_angle    = sinf(angle_radians);
+    float sine_angle    = sinf(angle_radians);
-    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
+    float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
-    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
+    stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
-    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
+    stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
-
+
-    ManualControlCommandData manualControl;
+    ManualControlCommandData manualControl;
-    ManualControlCommandGet(&manualControl);
+    ManualControlCommandGet(&manualControl);
-
+
-    if (yaw_attitude) {
+    if (yaw_attitude) {
-        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-        stabDesired.Yaw = yaw_direction;
+        stabDesired.Yaw = yaw_direction;
-    } else {
+    } else {
-        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
+        stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
-    }
+    }
-
+
-    // default thrust mode to cruise control
+    // default thrust mode to cruise control
-    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
+    stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
-
+
-    fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
+    fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
-    StabilizationDesiredSet(&stabDesired);
+    StabilizationDesiredSet(&stabDesired);
-
+
-    return result;
+    return result;
-}
+}
-
+
-void VtolLandController::UpdateAutoPilot()
+void VtolLandController::UpdateAutoPilot()
-{
+{
-    fsm->Update();
+    fsm->Update();
-
+
-    UpdateVelocityDesired();
+    UpdateVelocityDesired();
-
+
-    // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
+    // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
-    bool yaw_attitude = false;
+    bool yaw_attitude = false;
-    float yaw = 0.0f;
+    float yaw = 0.0f;
-
+
-    fsm->GetYaw(yaw_attitude, yaw);
+    fsm->GetYaw(yaw_attitude, yaw);
-
+
-    int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
+    int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
-
+
-    if (!result) {
+    if (!result) {
-        fsm->Abort();
+        fsm->Abort();
-    }
+    }
-
+
-    PathStatusSet(pathStatus);
+    PathStatusSet(pathStatus);
-}
+}

flight/modules/PathFollower/vtollandfsm.cpp

@@ -1,701 +1,701 @@
-/*
+/*
- ******************************************************************************
+ ******************************************************************************
- *
+ *
- * @file       vtollandfsm.cpp
+ * @file       vtollandfsm.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
- * @brief      This landing state machine is a helper state machine to the
+ * @brief      This landing state machine is a helper state machine to the
- *             VtolLandController.
+ *             VtolLandController.
- * @see        The GNU Public License (GPL) Version 3
+ * @see        The GNU Public License (GPL) Version 3
- *
+ *
- *****************************************************************************/
+ *****************************************************************************/
-/*
+/*
- * This program is free software; you can redistribute it and/or modify
+ * 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
+ * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
+ * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
+ * (at your option) any later version.
- *
+ *
- * This program is distributed in the hope that it will be useful, but
+ * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
+ * for more details.
- *
+ *
- * You should have received a copy of the GNU General Public License along
+ * 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.,
+ * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
+ */
-
+
-extern "C" {
+extern "C" {
-#include <openpilot.h>
+#include <openpilot.h>
-
+
-#include <callbackinfo.h>
+#include <callbackinfo.h>
-
+
-#include <math.h>
+#include <math.h>
-#include <pid.h>
+#include <pid.h>
-#include <CoordinateConversions.h>
+#include <CoordinateConversions.h>
-#include <sin_lookup.h>
+#include <sin_lookup.h>
-#include <pathdesired.h>
+#include <pathdesired.h>
-#include <paths.h>
+#include <paths.h>
-#include "plans.h"
+#include "plans.h"
-#include <sanitycheck.h>
+#include <sanitycheck.h>
-
+
-#include <homelocation.h>
+#include <homelocation.h>
-#include <accelstate.h>
+#include <accelstate.h>
-#include <vtolpathfollowersettings.h>
+#include <vtolpathfollowersettings.h>
-#include <flightstatus.h>
+#include <flightstatus.h>
-#include <flightmodesettings.h>
+#include <flightmodesettings.h>
-#include <pathstatus.h>
+#include <pathstatus.h>
-#include <positionstate.h>
+#include <positionstate.h>
-#include <velocitystate.h>
+#include <velocitystate.h>
-#include <velocitydesired.h>
+#include <velocitydesired.h>
-#include <stabilizationdesired.h>
+#include <stabilizationdesired.h>
-#include <airspeedstate.h>
+#include <airspeedstate.h>
-#include <attitudestate.h>
+#include <attitudestate.h>
-#include <takeofflocation.h>
+#include <takeofflocation.h>
-#include <poilocation.h>
+#include <poilocation.h>
-#include <manualcontrolcommand.h>
+#include <manualcontrolcommand.h>
-#include <systemsettings.h>
+#include <systemsettings.h>
-#include <stabilizationbank.h>
+#include <stabilizationbank.h>
-#include <stabilizationdesired.h>
+#include <stabilizationdesired.h>
-#include <vtolselftuningstats.h>
+#include <vtolselftuningstats.h>
-#include <statusvtolland.h>
+#include <statusvtolland.h>
-#include <pathsummary.h>
+#include <pathsummary.h>
-}
+}
-
+
-// C++ includes
+// C++ includes
-#include <vtollandfsm.h>
+#include <vtollandfsm.h>
-
+
-
+
-// Private constants
+// Private constants
-#define TIMER_COUNT_PER_SECOND            (1000 / vtolPathFollowerSettings->UpdatePeriod)
+#define TIMER_COUNT_PER_SECOND            (1000 / vtolPathFollowerSettings->UpdatePeriod)
-#define MIN_LANDRATE                      0.1f
+#define MIN_LANDRATE                      0.1f
-#define MAX_LANDRATE                      0.6f
+#define MAX_LANDRATE                      0.6f
-#define LOW_ALT_DESCENT_REDUCTION_FACTOR  0.7f  // TODO Need to make the transition smooth
+#define LOW_ALT_DESCENT_REDUCTION_FACTOR  0.7f  // TODO Need to make the transition smooth
-#define LANDRATE_LOWLIMIT_FACTOR          0.5f
+#define LANDRATE_LOWLIMIT_FACTOR          0.5f
-#define LANDRATE_HILIMIT_FACTOR           1.5f
+#define LANDRATE_HILIMIT_FACTOR           1.5f
-#define TIMEOUT_INIT_ALTHOLD              (3 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_INIT_ALTHOLD              (3 * TIMER_COUNT_PER_SECOND)
-#define TIMEOUT_WTG_FOR_DESCENTRATE       (10 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_WTG_FOR_DESCENTRATE       (10 * TIMER_COUNT_PER_SECOND)
-#define WTG_FOR_DESCENTRATE_COUNT_LIMIT   10
+#define WTG_FOR_DESCENTRATE_COUNT_LIMIT   10
-#define TIMEOUT_AT_DESCENTRATE            10
+#define TIMEOUT_AT_DESCENTRATE            10
-#define TIMEOUT_GROUNDEFFECT              (1 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_GROUNDEFFECT              (1 * TIMER_COUNT_PER_SECOND)
-#define TIMEOUT_THRUSTDOWN                (2 * TIMER_COUNT_PER_SECOND)
+#define TIMEOUT_THRUSTDOWN                (2 * TIMER_COUNT_PER_SECOND)
-#define LANDING_PID_SCALAR_P              2.0f
+#define LANDING_PID_SCALAR_P              2.0f
-#define LANDING_PID_SCALAR_I              10.0f
+#define LANDING_PID_SCALAR_I              10.0f
-#define LANDING_SLOWDOWN_HEIGHT           -5.0f
+#define LANDING_SLOWDOWN_HEIGHT           -5.0f
-#define BOUNCE_VELOCITY_TRIGGER_LIMIT     -0.3f
+#define BOUNCE_VELOCITY_TRIGGER_LIMIT     -0.3f
-#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
+#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
-#define BOUNCE_TRIGGER_COUNT              4
+#define BOUNCE_TRIGGER_COUNT              4
-#define GROUNDEFFECT_SLOWDOWN_FACTOR      0.3f
+#define GROUNDEFFECT_SLOWDOWN_FACTOR      0.3f
-#define GROUNDEFFECT_SLOWDOWN_COUNT       4
+#define GROUNDEFFECT_SLOWDOWN_COUNT       4
-
+
-VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
+VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
-    [LAND_STATE_INACTIVE]       =       { .setup = &VtolLandFSM::setup_inactive,             .run = 0                                      },
+    [LAND_STATE_INACTIVE]       =       { .setup = &VtolLandFSM::setup_inactive,             .run = 0                                      },
-    [LAND_STATE_INIT_ALTHOLD]   =       { .setup = &VtolLandFSM::setup_init_althold,         .run = &VtolLandFSM::run_init_althold         },
+    [LAND_STATE_INIT_ALTHOLD]   =       { .setup = &VtolLandFSM::setup_init_althold,         .run = &VtolLandFSM::run_init_althold         },
-    [LAND_STATE_WTG_FOR_DESCENTRATE] =  { .setup = &VtolLandFSM::setup_wtg_for_descentrate,  .run = &VtolLandFSM::run_wtg_for_descentrate  },
+    [LAND_STATE_WTG_FOR_DESCENTRATE] =  { .setup = &VtolLandFSM::setup_wtg_for_descentrate,  .run = &VtolLandFSM::run_wtg_for_descentrate  },
-    [LAND_STATE_AT_DESCENTRATE] =       { .setup = &VtolLandFSM::setup_at_descentrate,       .run = &VtolLandFSM::run_at_descentrate       },
+    [LAND_STATE_AT_DESCENTRATE] =       { .setup = &VtolLandFSM::setup_at_descentrate,       .run = &VtolLandFSM::run_at_descentrate       },
-    [LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
+    [LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
-    [LAND_STATE_GROUNDEFFECT]   =       { .setup = &VtolLandFSM::setup_groundeffect,         .run = &VtolLandFSM::run_groundeffect         },
+    [LAND_STATE_GROUNDEFFECT]   =       { .setup = &VtolLandFSM::setup_groundeffect,         .run = &VtolLandFSM::run_groundeffect         },
-    [LAND_STATE_THRUSTDOWN]     =       { .setup = &VtolLandFSM::setup_thrustdown,           .run = &VtolLandFSM::run_thrustdown           },
+    [LAND_STATE_THRUSTDOWN]     =       { .setup = &VtolLandFSM::setup_thrustdown,           .run = &VtolLandFSM::run_thrustdown           },
-    [LAND_STATE_THRUSTOFF]      =       { .setup = &VtolLandFSM::setup_thrustoff,            .run = &VtolLandFSM::run_thrustoff            },
+    [LAND_STATE_THRUSTOFF]      =       { .setup = &VtolLandFSM::setup_thrustoff,            .run = &VtolLandFSM::run_thrustoff            },
-    [LAND_STATE_DISARMED]       =       { .setup = &VtolLandFSM::setup_disarmed,             .run = &VtolLandFSM::run_disarmed             },
+    [LAND_STATE_DISARMED]       =       { .setup = &VtolLandFSM::setup_disarmed,             .run = &VtolLandFSM::run_disarmed             },
-    [LAND_STATE_ABORT] =                { .setup = &VtolLandFSM::setup_abort,                .run = &VtolLandFSM::run_abort                }
+    [LAND_STATE_ABORT] =                { .setup = &VtolLandFSM::setup_abort,                .run = &VtolLandFSM::run_abort                }
-};
+};
-
+
-// pointer to a singleton instance
+// pointer to a singleton instance
-VtolLandFSM *VtolLandFSM::p_inst = 0;
+VtolLandFSM *VtolLandFSM::p_inst = 0;
-
+
-
+
-VtolLandFSM::VtolLandFSM()
+VtolLandFSM::VtolLandFSM()
-    : mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
+    : mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
-{}
+{}
-
+
-// Private types
+// Private types
-
+
-// Private functions
+// Private functions
-// Public API methods
+// Public API methods
-/**
+/**
- * Initialise the module, called on startup
+ * Initialise the module, called on startup
- * \returns 0 on success or -1 if initialisation failed
+ * \returns 0 on success or -1 if initialisation failed
- */
+ */
-int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
+int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
-                                PathDesiredData *ptr_pathDesired,
+                                PathDesiredData *ptr_pathDesired,
-                                FlightStatusData *ptr_flightStatus)
+                                FlightStatusData *ptr_flightStatus)
-{
+{
-    PIOS_Assert(ptr_vtolPathFollowerSettings);
+    PIOS_Assert(ptr_vtolPathFollowerSettings);
-    PIOS_Assert(ptr_pathDesired);
+    PIOS_Assert(ptr_pathDesired);
-    PIOS_Assert(ptr_flightStatus);
+    PIOS_Assert(ptr_flightStatus);
-
+
-    if (mLandData == 0) {
+    if (mLandData == 0) {
-        mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
+        mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
-        PIOS_Assert(mLandData);
+        PIOS_Assert(mLandData);
-    }
+    }
-    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
+    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
-    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
+    vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
-    pathDesired  = ptr_pathDesired;
+    pathDesired  = ptr_pathDesired;
-    flightStatus = ptr_flightStatus;
+    flightStatus = ptr_flightStatus;
-    initFSM();
+    initFSM();
-
+
-    return 0;
+    return 0;
-}
+}
-
+
-void VtolLandFSM::Inactive(void)
+void VtolLandFSM::Inactive(void)
-{
+{
-    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
+    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
-    initFSM();
+    initFSM();
-}
+}
-
+
-// Initialise the FSM
+// Initialise the FSM
-void VtolLandFSM::initFSM(void)
+void VtolLandFSM::initFSM(void)
-{
+{
-    if (vtolPathFollowerSettings != 0) {
+    if (vtolPathFollowerSettings != 0) {
-        setState(LAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
+        setState(STATUSVTOLLAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
-    } else {
+    } else {
-        mLandData->currentState = LAND_STATE_INACTIVE;
+        mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
-    }
+    }
-}
+}
-
+
-void VtolLandFSM::Activate()
+void VtolLandFSM::Activate()
-{
+{
-    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
+    memset(mLandData, 0, sizeof(VtolLandFSMData_T));
-    mLandData->currentState   = LAND_STATE_INACTIVE;
+    mLandData->currentState   = STATUSVTOLLAND_STATE_INACTIVE;
-    mLandData->flLowAltitude  = false;
+    mLandData->flLowAltitude  = false;
-    mLandData->flAltitudeHold = false;
+    mLandData->flAltitudeHold = false;
-    mLandData->fsmLandStatus.averageDescentRate      = MIN_LANDRATE;
+    mLandData->fsmLandStatus.averageDescentRate      = MIN_LANDRATE;
-    mLandData->fsmLandStatus.averageDescentThrust    = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->fsmLandStatus.averageDescentThrust    = vtolPathFollowerSettings->ThrustLimits.Neutral;
-    mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
-    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
-    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
-    TakeOffLocationGet(&(mLandData->takeOffLocation));
+    TakeOffLocationGet(&(mLandData->takeOffLocation));
-    mLandData->fsmLandStatus.AltitudeAtState[LAND_STATE_INACTIVE] = 0.0f;
+    mLandData->fsmLandStatus.AltitudeAtState[STATUSVTOLLAND_STATE_INACTIVE] = 0.0f;
-    assessAltitude();
+    assessAltitude();
-
+
-    if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
+    if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
-#ifndef DEBUG_GROUNDIMPACT
+#ifndef DEBUG_GROUNDIMPACT
-        setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
+        setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
-#else
+#else
-        setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+        setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
-#endif
+#endif
-    } else {
+    } else {
-        // move to error state and callback to position hold
+        // move to error state and callback to position hold
-        setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+        setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
-    }
+    }
-}
+}
-
+
-void VtolLandFSM::Abort(void)
+void VtolLandFSM::Abort(void)
-{
+{
-    setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
-}
+}
-
+
-PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
+PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
-{
+{
-    switch (mLandData->currentState) {
+    switch (mLandData->currentState) {
-    case LAND_STATE_INACTIVE:
+    case STATUSVTOLLAND_STATE_INACTIVE:
-        return PFFSM_STATE_INACTIVE;
+        return PFFSM_STATE_INACTIVE;
-
+
-        break;
+        break;
-    case LAND_STATE_ABORT:
+    case STATUSVTOLLAND_STATE_ABORT:
-        return PFFSM_STATE_ABORT;
+        return PFFSM_STATE_ABORT;
-
+
-        break;
+        break;
-    case LAND_STATE_DISARMED:
+    case STATUSVTOLLAND_STATE_DISARMED:
-        return PFFSM_STATE_DISARMED;
+        return PFFSM_STATE_DISARMED;
-
+
-        break;
+        break;
-    default:
+    default:
-        return PFFSM_STATE_ACTIVE;
+        return PFFSM_STATE_ACTIVE;
-
+
-        break;
+        break;
-    }
+    }
-}
+}
-
+
-void VtolLandFSM::Update()
+void VtolLandFSM::Update()
-{
+{
-    runState();
+    runState();
-    if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
+    if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
-        runAlways();
+        runAlways();
-    }
+    }
-}
+}
-
+
-int32_t VtolLandFSM::runState(void)
+int32_t VtolLandFSM::runState(void)
-{
+{
-    uint8_t flTimeout = false;
+    uint8_t flTimeout = false;
-
+
-    mLandData->stateRunCount++;
+    mLandData->stateRunCount++;
-
+
-    if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
+    if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
-        flTimeout = true;
+        flTimeout = true;
-    }
+    }
-
+
-    // If the current state has a static function, call it
+    // If the current state has a static function, call it
-    if (sLandStateTable[mLandData->currentState].run) {
+    if (sLandStateTable[mLandData->currentState].run) {
-        (this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
+        (this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
-    }
+    }
-    return 0;
+    return 0;
-}
+}
-
+
-int32_t VtolLandFSM::runAlways(void)
+int32_t VtolLandFSM::runAlways(void)
-{
+{
-    void assessAltitude(void);
+    void assessAltitude(void);
-
+
-    return 0;
+    return 0;
-}
+}
-
+
-// PathFollower implements the PID scheme and has a objective
+// PathFollower implements the PID scheme and has a objective
-// set by a PathDesired object.  Based on the mode, pathfollower
+// set by a PathDesired object.  Based on the mode, pathfollower
-// uses FSM's as helper functions that manage state and event detection.
+// uses FSM's as helper functions that manage state and event detection.
-// PathFollower calls into FSM methods to alter its commands.
+// PathFollower calls into FSM methods to alter its commands.
-
+
-void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
+void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
-{
+{
-    ulow  = mLandData->boundThrustMin;
+    ulow  = mLandData->boundThrustMin;
-    uhigh = mLandData->boundThrustMax;
+    uhigh = mLandData->boundThrustMax;
-
+
-
+
-    if (mLandData->flConstrainThrust) {
+    if (mLandData->flConstrainThrust) {
-        uhigh = mLandData->thrustLimit;
+        uhigh = mLandData->thrustLimit;
-    }
+    }
-}
+}
-
+
-void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
+void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
-{
+{
-    if (mLandData->flZeroStabiHorizontal && stabDesired) {
+    if (mLandData->flZeroStabiHorizontal && stabDesired) {
-        stabDesired->Pitch = 0.0f;
+        stabDesired->Pitch = 0.0f;
-        stabDesired->Roll  = 0.0f;
+        stabDesired->Roll  = 0.0f;
-        stabDesired->Yaw   = 0.0f;
+        stabDesired->Yaw   = 0.0f;
-    }
+    }
-}
+}
-
+
-void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
+void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
-{
+{
-    if (mLandData->flLowAltitude) {
+    if (mLandData->flLowAltitude) {
-        local_scaler->p = LANDING_PID_SCALAR_P;
+        local_scaler->p = LANDING_PID_SCALAR_P;
-        local_scaler->i = LANDING_PID_SCALAR_I;
+        local_scaler->i = LANDING_PID_SCALAR_I;
-    }
+    }
-}
+}
-
+
-
+
-// Set the new state and perform setup for subsequent state run calls
+// Set the new state and perform setup for subsequent state run calls
-// This is called by state run functions on event detection that drive
+// This is called by state run functions on event detection that drive
-// state transitions.
+// state transitions.
-void VtolLandFSM::setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason)
+void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason)
-{
+{
-    mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
+    mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
-
+
-    if (mLandData->currentState == newState) {
+    if (mLandData->currentState == newState) {
-        return;
+        return;
-    }
+    }
-    mLandData->currentState = newState;
+    mLandData->currentState = newState;
-
+
-    if (newState != LAND_STATE_INACTIVE) {
+    if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
-        PositionStateData positionState;
+        PositionStateData positionState;
-        PositionStateGet(&positionState);
+        PositionStateGet(&positionState);
-        float takeOffDown = 0.0f;
+        float takeOffDown = 0.0f;
-        if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+        if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
-            takeOffDown = mLandData->takeOffLocation.Down;
+            takeOffDown = mLandData->takeOffLocation.Down;
-        }
+        }
-        mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
+        mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
-        assessAltitude();
+        assessAltitude();
-    }
+    }
-
+
-    // Restart state timer counter
+    // Restart state timer counter
-    mLandData->stateRunCount     = 0;
+    mLandData->stateRunCount     = 0;
-
+
-    // Reset state timeout to disabled/zero
+    // Reset state timeout to disabled/zero
-    mLandData->stateTimeoutCount = 0;
+    mLandData->stateTimeoutCount = 0;
-
+
-    if (sLandStateTable[mLandData->currentState].setup) {
+    if (sLandStateTable[mLandData->currentState].setup) {
-        (this->*sLandStateTable[mLandData->currentState].setup)();
+        (this->*sLandStateTable[mLandData->currentState].setup)();
-    }
+    }
-
+
-    updateVtolLandFSMStatus();
+    updateVtolLandFSMStatus();
-}
+}
-
+
-
+
-// Timeout utility function for use by state init implementations
+// Timeout utility function for use by state init implementations
-void VtolLandFSM::setStateTimeout(int32_t count)
+void VtolLandFSM::setStateTimeout(int32_t count)
-{
+{
-    mLandData->stateTimeoutCount = count;
+    mLandData->stateTimeoutCount = count;
-}
+}
-
+
-void VtolLandFSM::updateVtolLandFSMStatus()
+void VtolLandFSM::updateVtolLandFSMStatus()
-{
+{
-    mLandData->fsmLandStatus.State = mLandData->currentState;
+    mLandData->fsmLandStatus.State = mLandData->currentState;
-    if (mLandData->flLowAltitude) {
+    if (mLandData->flLowAltitude) {
-        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
+        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
-    } else {
+    } else {
-        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
+        mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
-    }
+    }
-    StatusVtolLandSet(&mLandData->fsmLandStatus);
+    StatusVtolLandSet(&mLandData->fsmLandStatus);
-}
+}
-
+
-
+
-float VtolLandFSM::BoundVelocityDown(float velocity_down)
+float VtolLandFSM::BoundVelocityDown(float velocity_down)
-{
+{
-    velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
+    velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
-    if (mLandData->flLowAltitude) {
+    if (mLandData->flLowAltitude) {
-        velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
+        velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
-    }
+    }
-    mLandData->fsmLandStatus.targetDescentRate = velocity_down;
+    mLandData->fsmLandStatus.targetDescentRate = velocity_down;
-
+
-    if (mLandData->flAltitudeHold) {
+    if (mLandData->flAltitudeHold) {
-        return 0.0f;
+        return 0.0f;
-    } else {
+    } else {
-        return velocity_down;
+        return velocity_down;
-    }
+    }
-}
+}
-
+
-void VtolLandFSM::assessAltitude(void)
+void VtolLandFSM::assessAltitude(void)
-{
+{
-    float positionDown;
+    float positionDown;
-
+
-    PositionStateDownGet(&positionDown);
+    PositionStateDownGet(&positionDown);
-    float takeOffDown = 0.0f;
+    float takeOffDown = 0.0f;
-    if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
+    if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
-        takeOffDown = mLandData->takeOffLocation.Down;
+        takeOffDown = mLandData->takeOffLocation.Down;
-    }
+    }
-    float positionDownRelativeToTakeoff = positionDown - takeOffDown;
+    float positionDownRelativeToTakeoff = positionDown - takeOffDown;
-    if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
+    if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
-        mLandData->flLowAltitude = false;
+        mLandData->flLowAltitude = false;
-    } else {
+    } else {
-        mLandData->flLowAltitude = true;
+        mLandData->flLowAltitude = true;
-    }
+    }
-}
+}
-
+
-
+
-// FSM Setup and Run method implementation
+// FSM Setup and Run method implementation
-
+
-// State: INACTIVE
+// State: INACTIVE
-void VtolLandFSM::setup_inactive(void)
+void VtolLandFSM::setup_inactive(void)
-{
+{
-    // Re-initialise local variables
+    // Re-initialise local variables
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    mLandData->flConstrainThrust     = false;
+    mLandData->flConstrainThrust     = false;
-}
+}
-
+
-// State: INIT ALTHOLD
+// State: INIT ALTHOLD
-void VtolLandFSM::setup_init_althold(void)
+void VtolLandFSM::setup_init_althold(void)
-{
+{
-    setStateTimeout(TIMEOUT_INIT_ALTHOLD);
+    setStateTimeout(TIMEOUT_INIT_ALTHOLD);
-    // get target descent velocity
+    // get target descent velocity
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
+    mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
-    mLandData->flConstrainThrust     = false;
+    mLandData->flConstrainThrust     = false;
-    mLandData->flAltitudeHold = true;
+    mLandData->flAltitudeHold = true;
-    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
-    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
-}
+}
-
+
-void VtolLandFSM::run_init_althold(uint8_t flTimeout)
+void VtolLandFSM::run_init_althold(uint8_t flTimeout)
-{
+{
-    if (flTimeout) {
+    if (flTimeout) {
-        mLandData->flAltitudeHold = false;
+        mLandData->flAltitudeHold = false;
-        setState(LAND_STATE_WTG_FOR_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLLAND_STATE_WTGFORDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
-    }
+    }
-}
+}
-
+
-
+
-// State: WAITING FOR DESCENT RATE
+// State: WAITING FOR DESCENT RATE
-void VtolLandFSM::setup_wtg_for_descentrate(void)
+void VtolLandFSM::setup_wtg_for_descentrate(void)
-{
+{
-    setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
+    setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
-    // get target descent velocity
+    // get target descent velocity
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    mLandData->observationCount = 0;
+    mLandData->observationCount = 0;
-    mLandData->observation2Count     = 0;
+    mLandData->observation2Count     = 0;
-    mLandData->flConstrainThrust     = false;
+    mLandData->flConstrainThrust     = false;
-    mLandData->flAltitudeHold = false;
+    mLandData->flAltitudeHold = false;
-    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
-    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
-}
+}
-
+
-void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
+void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
-{
+{
-    // Look at current actual thrust...are we already shutdown??
+    // Look at current actual thrust...are we already shutdown??
-    VelocityStateData velocityState;
+    VelocityStateData velocityState;
-
+
-    VelocityStateGet(&velocityState);
+    VelocityStateGet(&velocityState);
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-
+
-    // We don't expect PID to get exactly the target descent rate, so have a lower
+    // We don't expect PID to get exactly the target descent rate, so have a lower
-    // water mark but need to see 5 observations to be confident that we have semi-stable
+    // water mark but need to see 5 observations to be confident that we have semi-stable
-    // descent achieved
+    // descent achieved
-
+
-    // we need to see velocity down within a range of control before we proceed, without which we
+    // we need to see velocity down within a range of control before we proceed, without which we
-    // really don't have confidence to allow later states to run.
+    // really don't have confidence to allow later states to run.
-    if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
+    if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
-        velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
+        velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
-        if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
+        if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
-            setState(LAND_STATE_AT_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
+            setState(STATUSVTOLLAND_STATE_ATDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
-            return;
+            return;
-        }
+        }
-    }
+    }
-
+
-    if (flTimeout) {
+    if (flTimeout) {
-        setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
-    }
+    }
-}
+}
-
+
-
+
-// State: AT DESCENT RATE
+// State: AT DESCENT RATE
-void VtolLandFSM::setup_at_descentrate(void)
+void VtolLandFSM::setup_at_descentrate(void)
-{
+{
-    setStateTimeout(TIMEOUT_AT_DESCENTRATE);
+    setStateTimeout(TIMEOUT_AT_DESCENTRATE);
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    mLandData->observationCount  = 0;
+    mLandData->observationCount  = 0;
-    mLandData->sum1 = 0.0f;
+    mLandData->sum1 = 0.0f;
-    mLandData->sum2 = 0.0f;
+    mLandData->sum2 = 0.0f;
-    mLandData->flConstrainThrust = false;
+    mLandData->flConstrainThrust = false;
-    mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
+    mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
-    mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
+    mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
-    mLandData->boundThrustMin    = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMin    = vtolPathFollowerSettings->ThrustLimits.Min;
-    mLandData->boundThrustMax    = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->boundThrustMax    = vtolPathFollowerSettings->ThrustLimits.Max;
-}
+}
-
+
-void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
+void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
-{
+{
-    VelocityStateData velocityState;
+    VelocityStateData velocityState;
-
+
-    VelocityStateGet(&velocityState);
+    VelocityStateGet(&velocityState);
-
+
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-
+
-    mLandData->sum1 += velocityState.Down;
+    mLandData->sum1 += velocityState.Down;
-    mLandData->sum2 += stabDesired.Thrust;
+    mLandData->sum2 += stabDesired.Thrust;
-    mLandData->observationCount++;
+    mLandData->observationCount++;
-    if (flTimeout) {
+    if (flTimeout) {
-        mLandData->fsmLandStatus.averageDescentRate   = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
+        mLandData->fsmLandStatus.averageDescentRate   = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
-        mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
+        mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
-
+
-        // We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
+        // We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
-        // As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
+        // As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
-        // detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
+        // detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
-        mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
+        mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
-        mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
+        mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
-
+
-
+
-        setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
+        setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
-    }
+    }
-}
+}
-
+
-
+
-// State: WAITING FOR GROUND EFFECT
+// State: WAITING FOR GROUND EFFECT
-void VtolLandFSM::setup_wtg_for_groundeffect(void)
+void VtolLandFSM::setup_wtg_for_groundeffect(void)
-{
+{
-    // No timeout
+    // No timeout
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    mLandData->observationCount = 0;
+    mLandData->observationCount = 0;
-    mLandData->observation2Count     = 0;
+    mLandData->observation2Count     = 0;
-    mLandData->sum1 = 0.0f;
+    mLandData->sum1 = 0.0f;
-    mLandData->sum2 = 0.0f;
+    mLandData->sum2 = 0.0f;
-    mLandData->flConstrainThrust     = false;
+    mLandData->flConstrainThrust     = false;
-    mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
+    mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
-    mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel    = 0.0f;
+    mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel    = 0.0f;
-    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
-    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
-}
+}
-
+
-void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
+void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
-{
+{
-    // detect material downrating in thrust for 1 second.
+    // detect material downrating in thrust for 1 second.
-    VelocityStateData velocityState;
+    VelocityStateData velocityState;
-
+
-    VelocityStateGet(&velocityState);
+    VelocityStateGet(&velocityState);
-    AccelStateData accelState;
+    AccelStateData accelState;
-    AccelStateGet(&accelState);
+    AccelStateGet(&accelState);
-
+
-    // +ve 9.8 expected
+    // +ve 9.8 expected
-    float g_e;
+    float g_e;
-    HomeLocationg_eGet(&g_e);
+    HomeLocationg_eGet(&g_e);
-
+
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-
+
-    // detect bounce
+    // detect bounce
-    uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
+    uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
-    if (flBounce) {
+    if (flBounce) {
-        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
-    } else {
+    } else {
-        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
-    }
+    }
-
+
-    // invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
+    // invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
-    // a relative acceleration term.
+    // a relative acceleration term.
-    float bounceAccel     = -accelState.z - g_e;
+    float bounceAccel     = -accelState.z - g_e;
-    uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
+    uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
-    if (flBounceAccel) {
+    if (flBounceAccel) {
-        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
-    } else {
+    } else {
-        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
+        mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
-    }
+    }
-
+
-    if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
+    if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
-        mLandData->observation2Count++;
+        mLandData->observation2Count++;
-        if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
+        if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
-            setState(LAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
+            setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
-            return;
+            return;
-        }
+        }
-    } else {
+    } else {
-        mLandData->observation2Count = 0;
+        mLandData->observation2Count = 0;
-    }
+    }
-
+
-    // detect low descent rate
+    // detect low descent rate
-    uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
+    uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
-    if (flDescentRateLow) {
+    if (flDescentRateLow) {
-        mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
+        mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
-        mLandData->observationCount++;
+        mLandData->observationCount++;
-        if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
+        if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
-#ifndef DEBUG_GROUNDIMPACT
+#ifndef DEBUG_GROUNDIMPACT
-            setState(LAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
+            setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
-#endif
+#endif
-            return;
+            return;
-        }
+        }
-    } else {
+    } else {
-        mLandData->observationCount = 0;
+        mLandData->observationCount = 0;
-    }
+    }
-
+
-    updateVtolLandFSMStatus();
+    updateVtolLandFSMStatus();
-}
+}
-
+
-// STATE: GROUNDEFFET
+// STATE: GROUNDEFFET
-void VtolLandFSM::setup_groundeffect(void)
+void VtolLandFSM::setup_groundeffect(void)
-{
+{
-    setStateTimeout(TIMEOUT_GROUNDEFFECT);
+    setStateTimeout(TIMEOUT_GROUNDEFFECT);
-    mLandData->flZeroStabiHorizontal     = false;
+    mLandData->flZeroStabiHorizontal     = false;
-    PositionStateData positionState;
+    PositionStateData positionState;
-    PositionStateGet(&positionState);
+    PositionStateGet(&positionState);
-    mLandData->expectedLandPositionNorth = positionState.North;
+    mLandData->expectedLandPositionNorth = positionState.North;
-    mLandData->expectedLandPositionEast  = positionState.East;
+    mLandData->expectedLandPositionEast  = positionState.East;
-    mLandData->flConstrainThrust = false;
+    mLandData->flConstrainThrust = false;
-
+
-    // now that we have ground effect limit max thrust to neutral
+    // now that we have ground effect limit max thrust to neutral
-    mLandData->boundThrustMin    = -0.1f;
+    mLandData->boundThrustMin    = -0.1f;
-    mLandData->boundThrustMax    = mLandData->fsmLandStatus.calculatedNeutralThrust;
+    mLandData->boundThrustMax    = mLandData->fsmLandStatus.calculatedNeutralThrust;
-}
+}
-void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
+void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
-{
+{
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-
+
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-    if (stabDesired.Thrust < 0.0f) {
+    if (stabDesired.Thrust < 0.0f) {
-        setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
+        setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
-        return;
+        return;
-    }
+    }
-
+
-    // Stay in this state until we get a low altitude flag.
+    // Stay in this state until we get a low altitude flag.
-    if (mLandData->flLowAltitude == false) {
+    if (mLandData->flLowAltitude == false) {
-        // worst case scenario is that we land and the pid brings thrust down to zero.
+        // worst case scenario is that we land and the pid brings thrust down to zero.
-        return;
+        return;
-    }
+    }
-
+
-    // detect broad sideways drift.  If for some reason we have a hard landing that the bounce detection misses, this will kick in
+    // detect broad sideways drift.  If for some reason we have a hard landing that the bounce detection misses, this will kick in
-    PositionStateData positionState;
+    PositionStateData positionState;
-    PositionStateGet(&positionState);
+    PositionStateGet(&positionState);
-    float north_error   = mLandData->expectedLandPositionNorth - positionState.North;
+    float north_error   = mLandData->expectedLandPositionNorth - positionState.North;
-    float east_error    = mLandData->expectedLandPositionEast - positionState.East;
+    float east_error    = mLandData->expectedLandPositionEast - positionState.East;
-    float positionError = sqrtf(north_error * north_error + east_error * east_error);
+    float positionError = sqrtf(north_error * north_error + east_error * east_error);
-    if (positionError > 1.5f) {
+    if (positionError > 1.5f) {
-        setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
+        setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
-        return;
+        return;
-    }
+    }
-
+
-    if (flTimeout) {
+    if (flTimeout) {
-        setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
-    }
+    }
-}
+}
-
+
-// STATE: THRUSTDOWN
+// STATE: THRUSTDOWN
-void VtolLandFSM::setup_thrustdown(void)
+void VtolLandFSM::setup_thrustdown(void)
-{
+{
-    setStateTimeout(TIMEOUT_THRUSTDOWN);
+    setStateTimeout(TIMEOUT_THRUSTDOWN);
-    mLandData->flZeroStabiHorizontal = true;
+    mLandData->flZeroStabiHorizontal = true;
-    mLandData->flConstrainThrust     = true;
+    mLandData->flConstrainThrust     = true;
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-    mLandData->thrustLimit    = stabDesired.Thrust;
+    mLandData->thrustLimit    = stabDesired.Thrust;
-    mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
+    mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
-    mLandData->boundThrustMin = -0.1f;
+    mLandData->boundThrustMin = -0.1f;
-    mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
+    mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
-}
+}
-
+
-void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
+void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
-{
+{
-    // reduce thrust setpoint step by step
+    // reduce thrust setpoint step by step
-    mLandData->thrustLimit -= mLandData->sum1;
+    mLandData->thrustLimit -= mLandData->sum1;
-
+
-    StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-    StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-    if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
+    if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
-        setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
+        setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
-    }
+    }
-
+
-    if (flTimeout) {
+    if (flTimeout) {
-        setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
+        setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
-    }
+    }
-}
+}
-
+
-// STATE: THRUSTOFF
+// STATE: THRUSTOFF
-void VtolLandFSM::setup_thrustoff(void)
+void VtolLandFSM::setup_thrustoff(void)
-{
+{
-    mLandData->thrustLimit       = -1.0f;
+    mLandData->thrustLimit       = -1.0f;
-    mLandData->flConstrainThrust = true;
+    mLandData->flConstrainThrust = true;
-    mLandData->boundThrustMin    = -0.1f;
+    mLandData->boundThrustMin    = -0.1f;
-    mLandData->boundThrustMax    = 0.0f;
+    mLandData->boundThrustMax    = 0.0f;
-}
+}
-
+
-void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
+void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
-{
+{
-    setState(LAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
+    setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
-}
+}
-
+
-// STATE: DISARMED
+// STATE: DISARMED
-void VtolLandFSM::setup_disarmed(void)
+void VtolLandFSM::setup_disarmed(void)
-{
+{
-    // nothing to do
+    // nothing to do
-    mLandData->flConstrainThrust     = false;
+    mLandData->flConstrainThrust     = false;
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    mLandData->observationCount = 0;
+    mLandData->observationCount = 0;
-    mLandData->boundThrustMin   = -0.1f;
+    mLandData->boundThrustMin   = -0.1f;
-    mLandData->boundThrustMax   = 0.0f;
+    mLandData->boundThrustMax   = 0.0f;
-}
+}
-
+
-void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
+void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
-{
+{
-#ifdef DEBUG_GROUNDIMPACT
+#ifdef DEBUG_GROUNDIMPACT
-    if (mLandData->observationCount++ > 100) {
+    if (mLandData->observationCount++ > 100) {
-        setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
+        setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
-    }
+    }
-#endif
+#endif
-}
+}
-
+
-void VtolLandFSM::fallback_to_hold(void)
+void VtolLandFSM::fallback_to_hold(void)
-{
+{
-    PositionStateData positionState;
+    PositionStateData positionState;
-
+
-    PositionStateGet(&positionState);
+    PositionStateGet(&positionState);
-    pathDesired->End.North        = positionState.North;
+    pathDesired->End.North        = positionState.North;
-    pathDesired->End.East         = positionState.East;
+    pathDesired->End.East         = positionState.East;
-    pathDesired->End.Down         = positionState.Down;
+    pathDesired->End.Down         = positionState.Down;
-    pathDesired->Start.North      = positionState.North;
+    pathDesired->Start.North      = positionState.North;
-    pathDesired->Start.East       = positionState.East;
+    pathDesired->Start.East       = positionState.East;
-    pathDesired->Start.Down       = positionState.Down;
+    pathDesired->Start.Down       = positionState.Down;
-    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->StartingVelocity = 0.0f;
-    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
-    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
+    pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
-
+
-    PathDesiredSet(pathDesired);
+    PathDesiredSet(pathDesired);
-}
+}
-
+
-// abort repeatedly overwrites pathfollower's objective on a landing abort and
+// abort repeatedly overwrites pathfollower's objective on a landing abort and
-// continues to do so until a flight mode change.
+// continues to do so until a flight mode change.
-void VtolLandFSM::setup_abort(void)
+void VtolLandFSM::setup_abort(void)
-{
+{
-    mLandData->boundThrustMin        = vtolPathFollowerSettings->ThrustLimits.Min;
+    mLandData->boundThrustMin        = vtolPathFollowerSettings->ThrustLimits.Min;
-    mLandData->boundThrustMax        = vtolPathFollowerSettings->ThrustLimits.Max;
+    mLandData->boundThrustMax        = vtolPathFollowerSettings->ThrustLimits.Max;
-    mLandData->flConstrainThrust     = false;
+    mLandData->flConstrainThrust     = false;
-    mLandData->flZeroStabiHorizontal = false;
+    mLandData->flZeroStabiHorizontal = false;
-    fallback_to_hold();
+    fallback_to_hold();
-}
+}
-
+
-void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
+void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
-{}
+{}

flight/modules/PathFollower/vtolvelocitycontroller.cpp

@@ -107,7 +107,7 @@ void VtolVelocityController::SettingsUpdated(void)
     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
-                               vtolPathFollowerSettings->HorizontalVelPID.ILimit,
+                               vtolPathFollowerSettings->HorizontalVelPID.Beta,
                                dT,
                                vtolPathFollowerSettings->HorizontalVelMax);
 
@@ -181,7 +181,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)
     ManualControlCommandData manualControl;
     ManualControlCommandGet(&manualControl);
 
-    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
     stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 
     // default thrust mode to altvario

flight/modules/PathPlanner/pathplanner.c

@@ -48,6 +48,9 @@
 #include "plans.h"
 #include <sanitycheck.h>
 #include <vtolpathfollowersettings.h>
+#include <statusvtolautotakeoff.h>
+#include <statusvtolland.h>
+#include <manualcontrolcommand.h>
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -77,6 +80,9 @@ static uint8_t conditionPointingTowardsNext();
 static uint8_t conditionPythonScript();
 static uint8_t conditionImmediate();
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
+static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
+static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position);
 
 
 // Private variables
@@ -126,6 +132,8 @@ int32_t PathPlannerInitialize()
     VelocityStateInitialize();
     WaypointInitialize();
     WaypointActiveInitialize();
+    StatusVtolAutoTakeoffInitialize();
+    StatusVtolLandInitialize();
 
     pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
     pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
@@ -138,7 +146,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
 
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    uint8_t TreatCustomCraftAs;
+    VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
 
     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
 
@@ -167,6 +175,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     }
 }
 
+#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
 
 /**
  * Module task
@@ -219,6 +228,17 @@ static void pathPlannerTask()
 
     WaypointActiveGet(&waypointActive);
 
+    // with the introduction of takeoff, we allow for arming
+    // whilst in pathplanner mode.  Previously it was just an assumption that
+    // a user never armed in pathplanner mode.  This check allows a user to select
+    // pathplanner, to upload waypoints, and then arm in pathplanner.
+    if (!flightStatus.Armed) {
+        return;
+    }
+
+
+    // the transition from pathplanner to another flightmode back to pathplanner
+    // triggers a reset back to 0 index in the waypoint list
     if (pathplanner_active == false) {
         pathplanner_active   = true;
 
@@ -245,6 +265,22 @@ static void pathPlannerTask()
         return;
     }
 
+    // check start conditions
+    // autotakeoff requires midpoint thrust if we are in a pending takeoff situation
+    if (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF) {
+        pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
+        if ((uint8_t)pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE) {
+            ManualControlCommandData cmd;
+            ManualControlCommandGet(&cmd);
+            if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
+                pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
+                PathDesiredSet(&pathDesired);
+            }
+            return;
+        }
+    }
+
+
     // check if condition has been met
     endCondition = pathConditionCheck();
     // decide what to do
@@ -283,6 +319,40 @@ static void pathPlannerTask()
     }
 }
 
+// callback function when waypoints changed in any way, update pathDesired
+void updatePathDesired()
+{
+    // only ever touch pathDesired if pathplanner is enabled
+    if (!pathplanner_active) {
+        return;
+    }
+
+    // find out current waypoint
+    WaypointActiveGet(&waypointActive);
+    WaypointInstGet(waypointActive.Index, &waypoint);
+
+    // Capture if current mode is takeoff
+    bool autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
+
+    PathActionInstGet(waypoint.Action, &pathAction);
+
+    PathDesiredData pathDesired;
+    switch (pathAction.Mode) {
+    case PATHACTION_MODE_AUTOTAKEOFF:
+        planner_setup_pathdesired_takeoff(&pathDesired);
+        break;
+    case PATHACTION_MODE_LAND:
+        planner_setup_pathdesired_land(&pathDesired);
+        break;
+    default:
+        planner_setup_pathdesired(&pathDesired, autotakeoff);
+        break;
+    }
+
+    PathDesiredSet(&pathDesired);
+}
+
+
 // safety checks for path plan integrity
 static uint8_t checkPathPlan()
 {
@@ -367,35 +437,22 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 }
 
-
+// Standard setup of a pathDesired command from the waypoint path plan
-// callback function when waypoints changed in any way, update pathDesired
+static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position)
-void updatePathDesired()
 {
-    // only ever touch pathDesired if pathplanner is enabled
+    pathDesired->End.North = waypoint.Position.North;
-    if (!pathplanner_active) {
+    pathDesired->End.East  = waypoint.Position.East;
-        return;
+    pathDesired->End.Down  = waypoint.Position.Down;
-    }
+    pathDesired->EndingVelocity    = waypoint.Velocity;
+    pathDesired->Mode = pathAction.Mode;
+    pathDesired->ModeParameters[0] = pathAction.ModeParameters[0];
+    pathDesired->ModeParameters[1] = pathAction.ModeParameters[1];
+    pathDesired->ModeParameters[2] = pathAction.ModeParameters[2];
+    pathDesired->ModeParameters[3] = pathAction.ModeParameters[3];
+    pathDesired->UID = waypointActive.Index;
 
-    PathDesiredData pathDesired;
 
-    // find out current waypoint
+    if (waypointActive.Index == 0 || overwrite_start_position) {
-    WaypointActiveGet(&waypointActive);
-
-    WaypointInstGet(waypointActive.Index, &waypoint);
-    PathActionInstGet(waypoint.Action, &pathAction);
-
-    pathDesired.End.North = waypoint.Position.North;
-    pathDesired.End.East  = waypoint.Position.East;
-    pathDesired.End.Down  = waypoint.Position.Down;
-    pathDesired.EndingVelocity    = waypoint.Velocity;
-    pathDesired.Mode = pathAction.Mode;
-    pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
-    pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
-    pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
-    pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
-    pathDesired.UID = waypointActive.Index;
-
-    if (waypointActive.Index == 0) {
         PositionStateData positionState;
         PositionStateGet(&positionState);
         // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@@ -405,23 +462,88 @@ void updatePathDesired()
            pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
         // note takeoff relies on the start being the current location as it merely ascends and using
         // the start as assumption current NE location
-        pathDesired.Start.North = positionState.North;
+        pathDesired->Start.North = positionState.North;
-        pathDesired.Start.East  = positionState.East;
+        pathDesired->Start.East  = positionState.East;
-        pathDesired.Start.Down  = positionState.Down;
+        pathDesired->Start.Down  = positionState.Down;
-        pathDesired.StartingVelocity = pathDesired.EndingVelocity;
+        pathDesired->StartingVelocity = pathDesired->EndingVelocity;
     } else {
         // Get previous waypoint as start point
         WaypointData waypointPrev;
         WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 
-        pathDesired.Start.North = waypointPrev.Position.North;
+        pathDesired->Start.North = waypointPrev.Position.North;
-        pathDesired.Start.East  = waypointPrev.Position.East;
+        pathDesired->Start.East  = waypointPrev.Position.East;
-        pathDesired.Start.Down  = waypointPrev.Position.Down;
+        pathDesired->Start.Down  = waypointPrev.Position.Down;
-        pathDesired.StartingVelocity = waypointPrev.Velocity;
+        pathDesired->StartingVelocity = waypointPrev.Velocity;
     }
-    PathDesiredSet(&pathDesired);
 }
 
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
+static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+    float autotakeoff_height;
+    FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
+    FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
+    autotakeoff_height = fabsf(autotakeoff_height);
+    if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
+    } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
+    }
+
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = -velocity_down;
+    // initially halt takeoff.
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down - autotakeoff_height;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
+
+    pathDesired->UID  = waypointActive.Index;
+}
+
+static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+
+    FlightModeSettingsLandingVelocityGet(&velocity_down);
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]  = velocity_down;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_LAND;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
+}
+
+
 // helper function to go to a specific waypoint
 static void setWaypoint(uint16_t num)
 {

flight/modules/Receiver/receiver.c

@@ -634,6 +634,9 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
             case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
                 group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
                 break;
+            case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL:
+                group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL;
+                break;
             case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
                 group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
                 break;

flight/modules/Stabilization/altitudeloop.cpp

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
  *
- * @file       altitudeloop.c
+ * @file       altitudeloop.cpp
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
  * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
@@ -26,6 +26,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+extern "C" {
 #include <openpilot.h>
 
 #include <callbackinfo.h>
@@ -37,6 +38,12 @@
 #include <altitudeholdstatus.h>
 #include <velocitystate.h>
 #include <positionstate.h>
+#include <vtolselftuningstats.h>
+#include <stabilization.h>
+}
+
+#include <pidcontroldown.h>
+
 // Private constants
 
 
@@ -50,78 +57,134 @@
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
 #define CBTASK_PRIORITY   CALLBACK_TASK_FLIGHTCONTROL
 
-#define STACK_SIZE_BYTES  512
 // Private types
 
 // Private variables
 static DelayedCallbackInfo *altitudeHoldCBInfo;
+static PIDControlDown controlDown;
 static AltitudeHoldSettingsData altitudeHoldSettings;
-static struct pid pid0, pid1;
 static ThrustModeType thrustMode;
-static PiOSDeltatimeConfig timeval;
+static float thrustDemand = 0.0f;
-static float thrustSetpoint = 0.0f;
-static float thrustDemand   = 0.0f;
-static float startThrust    = 0.5f;
 
 
 // Private functions
-static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
+static void altitudeHoldTask(void);
 static void VelocityStateUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Setup mode and setpoint
+ *
+ * reinit: true when althold/vario mode selected over a previous alternate thrust mode
  */
 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
 {
     static bool newaltitude = true;
 
-    if (reinit) {
+    if (reinit || !controlDown.IsActive()) {
-        startThrust = setpoint;
+        controlDown.Activate();
-        pid_zero(&pid0);
-        pid_zero(&pid1);
         newaltitude = true;
+        // calculate a thrustDemand on reinit only
+        altitudeHoldTask();
     }
 
     const float DEADBAND      = 0.20f;
     const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
     const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
 
-    // this is the max speed in m/s at the extents of thrust
-    float thrustRate;
-    uint8_t thrustExp;
-
-    AltitudeHoldSettingsThrustExpGet(&thrustExp);
-    AltitudeHoldSettingsThrustRateGet(&thrustRate);
-
-    PositionStateData posState;
-    PositionStateGet(&posState);
 
     if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
         // Cut thrust if desired
-        thrustSetpoint = 0.0f;
+        controlDown.UpdateVelocitySetpoint(0.0f);
-        thrustDemand   = 0.0f;
+        thrustDemand = 0.0f;
-        thrustMode     = DIRECT;
+        thrustMode   = DIRECT;
-        newaltitude    = true;
+        newaltitude  = true;
+        return thrustDemand;
     } else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
         // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
         // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
-        thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
+        controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
-        thrustMode     = ALTITUDEVARIO;
+        thrustMode  = ALTITUDEVARIO;
-        newaltitude    = true;
+        newaltitude = true;
     } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
-        thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
+        controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
-        thrustMode     = ALTITUDEVARIO;
+        thrustMode  = ALTITUDEVARIO;
-        newaltitude    = true;
+        newaltitude = true;
     } else if (newaltitude == true) {
-        thrustSetpoint = posState.Down;
+        controlDown.UpdateVelocitySetpoint(0.0f);
-        thrustMode     = ALTITUDEHOLD;
+        PositionStateData posState;
-        newaltitude    = false;
+        PositionStateGet(&posState);
+        controlDown.UpdatePositionSetpoint(posState.Down);
+        thrustMode  = ALTITUDEHOLD;
+        newaltitude = false;
     }
 
+    thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
+
     return thrustDemand;
 }
 
+/**
+ * Disable the alt hold task loop velocity controller to save cpu cycles
+ */
+void stabilizationDisableAltitudeHold(void)
+{
+    controlDown.Deactivate();
+}
+
+
+/**
+ * Module thread, should not return.
+ */
+static void altitudeHoldTask(void)
+{
+    if (!controlDown.IsActive()) {
+        return;
+    }
+
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
+    float velocityStateDown;
+    VelocityStateDownGet(&velocityStateDown);
+    controlDown.UpdateVelocityState(velocityStateDown);
+
+    float local_thrustDemand = 0.0f;
+
+    switch (thrustMode) {
+    case ALTITUDEHOLD:
+    {
+        float positionStateDown;
+        PositionStateDownGet(&positionStateDown);
+        controlDown.UpdatePositionState(positionStateDown);
+        controlDown.ControlPosition();
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
+        local_thrustDemand = controlDown.GetDownCommand();
+    }
+    break;
+
+    case ALTITUDEVARIO:
+        altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
+        local_thrustDemand = controlDown.GetDownCommand();
+        break;
+
+    case DIRECT:
+        altitudeHoldStatus.VelocityDesired = 0.0f;
+        altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
+        break;
+    }
+
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
+    thrustDemand = local_thrustDemand;
+}
+
+static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+}
+
 /**
  * Initialise the module, called on startup
  */
@@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit()
     AltitudeHoldStatusInitialize();
     PositionStateInitialize();
     VelocityStateInitialize();
+    VtolSelfTuningStatsInitialize();
 
-    PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
+    AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
-    // Create object queue
+    VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
+    SettingsUpdatedCb(NULL);
 
     altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
-    AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
     VelocityStateConnectCallback(&VelocityStateUpdatedCb);
-
-    // Start main task
-    SettingsUpdatedCb(NULL);
 }
 
 
-/**
- * Module thread, should not return.
- */
-static void altitudeHoldTask(void)
-{
-    AltitudeHoldStatusData altitudeHoldStatus;
-
-    AltitudeHoldStatusGet(&altitudeHoldStatus);
-
-    // do the actual control loop(s)
-    float positionStateDown;
-    PositionStateDownGet(&positionStateDown);
-    float velocityStateDown;
-    VelocityStateDownGet(&velocityStateDown);
-
-    float dT;
-    dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
-    switch (thrustMode) {
-    case ALTITUDEHOLD:
-    {
-        // altitude control loop
-        // No scaling.
-        const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
-        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
-    }
-    break;
-    case ALTITUDEVARIO:
-        altitudeHoldStatus.VelocityDesired = thrustSetpoint;
-        break;
-    default:
-        altitudeHoldStatus.VelocityDesired = 0;
-        break;
-    }
-
-    AltitudeHoldStatusSet(&altitudeHoldStatus);
-
-    switch (thrustMode) {
-    case DIRECT:
-        thrustDemand = thrustSetpoint;
-        break;
-    default:
-    {
-        // velocity control loop
-        // No scaling.
-        const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
-        thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
-    }
-    break;
-    }
-}
-
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     AltitudeHoldSettingsGet(&altitudeHoldSettings);
-    pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
-    pid_zero(&pid0);
-    pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
-    pid_zero(&pid1);
-}
 
-static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+    controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
-{
+                                 altitudeHoldSettings.VerticalVelPID.Ki,
-    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
+                                 altitudeHoldSettings.VerticalVelPID.Kd,
+                                 altitudeHoldSettings.VerticalVelPID.Beta,
+                                 (float)(UPDATE_EXPECTED),
+                                 altitudeHoldSettings.ThrustRate);
+
+    controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
+
+    VtolSelfTuningStatsData vtolSelfTuningStats;
+    VtolSelfTuningStatsGet(&vtolSelfTuningStats);
+    controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
+
+    // initialise limits on thrust but note the FSM can override.
+    controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
+
+    // disable neutral thrust calcs which should only be done in a hold mode.
+    controlDown.DisableNeutralThrustCalc();
 }
 
 

flight/modules/Stabilization/inc/altitudeloop.h

@@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
 
 void stabilizationAltitudeloopInit();
 float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
+void stabilizationDisableAltitudeHold(void);
 
 #endif /* ALTITUDELOOP_H */

flight/modules/Stabilization/innerloop.c

@@ -242,6 +242,7 @@ static void stabilizationInnerloopTask()
     float *actuatorDesiredAxis = &actuator.Roll;
     int t;
     float dT;
+    bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor
     dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
 
     StabilizationStatusOuterLoopData outerLoop;
@@ -324,7 +325,12 @@ static void stabilizationInnerloopTask()
             }
         }
 
-        actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
+        if (!multirotor) {
+            // we only need to clamp the desired axis to a sane range if the frame is not a multirotor type
+            // we don't want to do any clamping until after the motors are calculated and scaled.
+            // need to figure out what to do with a tricopter tail servo.
+            actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
+        }
     }
 
     actuator.UpdateTime = dT * 1000;
@@ -350,7 +356,7 @@ static void stabilizationInnerloopTask()
     }
 
     {
-        uint8_t armed;
+        FlightStatusArmedOptions armed;
         FlightStatusArmedGet(&armed);
         float throttleDesired;
         ManualControlCommandThrottleGet(&throttleDesired);

flight/modules/Stabilization/outerloop.c

@@ -103,7 +103,33 @@ static void stabilizationOuterloopTask()
     float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
     float *rateDesiredAxis = &rateDesired.Roll;
     int t;
-    float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+    float dT    = PIOS_DELTATIME_GetAverageSeconds(&timeval);
+
+    bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
+    previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+#ifdef REVOLUTION
+    if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
+                   previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
+        // disable the altvario velocity control loop
+        stabilizationDisableAltitudeHold();
+    }
+#endif /* REVOLUTION */
+    switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
+#ifdef REVOLUTION
+    case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
+        break;
+    case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
+        break;
+#endif /* REVOLUTION */
+    case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
+    case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
+    default:
+        rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
+        break;
+    }
+
 
     float local_error[3];
     {
@@ -120,6 +146,7 @@ static void stabilizationOuterloopTask()
             case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
                 rpy_desired[t] = stabilizationDesiredAxis[t];
                 break;
+            case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
             case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
             default:
                 rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
@@ -148,123 +175,142 @@ static void stabilizationOuterloopTask()
         }
 #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
     }
-    for (t = 0; t < AXES; t++) {
+
-        bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
+
+    for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
+        reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
         previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
 
-        if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
+        if (reinit) {
-            if (reinit) {
+            stabSettings.outerPids[t].iAccumulator = 0;
-                stabSettings.outerPids[t].iAccumulator = 0;
+        }
+        switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
+        case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
+            rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
+            break;
+        case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
+        {
+            float stickinput[3];
+            stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+            stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+            stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+            float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
+            rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
+                                        -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
+                                        StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
+                                        );
+            // Compute the weighted average rate desired
+            // Using max() rather than sqrt() for cpu speed;
+            // - this makes the stick region into a square;
+            // - this is a feature!
+            // - hold a roll angle and add just pitch without the stick sensitivity changing
+            float magnitude = fabsf(stickinput[t]);
+            if (t < 2) {
+                magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
             }
-            switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
+
-            case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
+            // modify magnitude to move the Att to Rate transition to the place
-                rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
+            // specified by the user
-                break;
+            // we are looking for where the stick angle == transition angle
-            case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
+            // and the Att rate equals the Rate rate
-            {
+            // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
-                float stickinput[3];
+            // == Rate x StickAngle [Rate pulling up according to stick angle]
-                stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+            // * StickAngle [X Ratt proportion]
-                stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+            // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
-                stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+            // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
-                float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            // and quadratic formula says that is 0.618033989f
-                // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
+            // I tested 14.01 and came up with .61 without even remembering this number
-                rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
+            // I thought that moving the P,I, and maxangle terms around would change this value
-                                            -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
+            // and that I would have to take these into account, but varying
-                                            StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
+            // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
-                                            );
+            // and varying maxangle from 4 to 120 didn't either.
-                // Compute the weighted average rate desired
+            // so for now I'm not taking these into account
-                // Using max() rather than sqrt() for cpu speed;
+            // while working with this, it occurred to me that Attitude mode,
-                // - this makes the stick region into a square;
+            // set up with maxangle=190 would be similar to Ratt, and it is.
-                // - this is a feature!
+#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
-                // - hold a roll angle and add just pitch without the stick sensitivity changing
+
-                float magnitude = fabsf(stickinput[t]);
+            // the following assumes the transition would otherwise be at 0.618033989f
-                if (t < 2) {
+            // and THAT assumes that Att ramps up to max roll rate
-                    magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
+            // when a small number of degrees off of where it should be
+
+            // if below the transition angle (still in attitude mode)
+            // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
+            if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
+                magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
+            } else {
+                magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
+                            * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
+                            / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+                            + STICK_VALUE_AT_MODE_TRANSITION;
+            }
+            rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
+        }
+        break;
+        case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
+            // FIXME: local_error[] is rate - attitude for Weak Leveling
+            // The only ramifications are:
+            // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
+            // Changing Rate mode max rate currently requires a change to Kp
+            // That would be changed to Attitude mode max angle affecting Kp
+            // Also does not take dT into account
+        {
+            float stickinput[3];
+            stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
+            stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
+            stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
+            float rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
+            float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
+            weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
+
+            // Compute desired rate as input biased towards leveling
+            rateDesiredAxis[t] = rate_input + weak_leveling;
+        }
+        break;
+        case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
+            // now test limits for pitch and/or roll
+            if (t == 1) { // pitch
+                if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
+                    // attitude exceeds pitch max.
+                    // zero rate desired if also -ve
+                    if (rateDesiredAxis[t] < 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
+                    }
+                } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
+                    // attitude exceeds pitch max
+                    // zero rate desired if also +ve
+                    if (rateDesiredAxis[t] > 0.0f) {
+                        rateDesiredAxis[t] = 0.0f;
+                    }
                 }
-
+            } else if (t == 0) { // roll
-                // modify magnitude to move the Att to Rate transition to the place
+                if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
-                // specified by the user
+                    // attitude exceeds roll max.
-                // we are looking for where the stick angle == transition angle
+                    // zero rate desired if also -ve
-                // and the Att rate equals the Rate rate
+                    if (rateDesiredAxis[t] < 0.0f) {
-                // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
+                        rateDesiredAxis[t] = 0.0f;
-                // == Rate x StickAngle [Rate pulling up according to stick angle]
+                    }
-                // * StickAngle [X Ratt proportion]
+                } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
-                // so 1-x == x*x or x*x+x-1=0 where xE(0,1)
+                    // attitude exceeds roll max
-                // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
+                    // zero rate desired if also +ve
-                // and quadratic formula says that is 0.618033989f
+                    if (rateDesiredAxis[t] > 0.0f) {
-                // I tested 14.01 and came up with .61 without even remembering this number
+                        rateDesiredAxis[t] = 0.0f;
-                // I thought that moving the P,I, and maxangle terms around would change this value
+                    }
-                // and that I would have to take these into account, but varying
-                // all P's and I's by factors of 1/2 to 2 didn't change it noticeably
-                // and varying maxangle from 4 to 120 didn't either.
-                // so for now I'm not taking these into account
-                // while working with this, it occurred to me that Attitude mode,
-                // set up with maxangle=190 would be similar to Ratt, and it is.
-                #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
-
-                // the following assumes the transition would otherwise be at 0.618033989f
-                // and THAT assumes that Att ramps up to max roll rate
-                // when a small number of degrees off of where it should be
-
-                // if below the transition angle (still in attitude mode)
-                // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
-                if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
-                    magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
-                } else {
-                    magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
-                                * (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
-                                / (1.0f - stabSettings.rattitude_mode_transition_stick_position)
-                                + STICK_VALUE_AT_MODE_TRANSITION;
                 }
-                rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
             }
             break;
-            case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
-                // FIXME: local_error[] is rate - attitude for Weak Leveling
-                // The only ramifications are:
-                // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
-                // Changing Rate mode max rate currently requires a change to Kp
-                // That would be changed to Attitude mode max angle affecting Kp
-                // Also does not take dT into account
-            {
-                float stickinput[3];
-                stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
-                stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
-                stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
-                float rate_input    = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
-                float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
-                weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
 
-                // Compute desired rate as input biased towards leveling
+        case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-                rateDesiredAxis[t] = rate_input + weak_leveling;
+        default:
-            }
+            rateDesiredAxis[t] = stabilizationDesiredAxis[t];
             break;
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-            default:
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
-                break;
-            }
-        } else {
-            switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
-#ifdef REVOLUTION
-            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
-                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
-                break;
-            case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
-                rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
-                break;
-#endif /* REVOLUTION */
-            case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
-            default:
-                rateDesiredAxis[t] = stabilizationDesiredAxis[t];
-                break;
-            }
         }
     }
 
     RateDesiredSet(&rateDesired);
     {
-        uint8_t armed;
+        FlightStatusArmedOptions armed;
         FlightStatusArmedGet(&armed);
         float throttleDesired;
         ManualControlCommandThrottleGet(&throttleDesired);
@@ -277,7 +323,7 @@ static void stabilizationOuterloopTask()
         }
     }
 
-    // update cruisecontrol based on attitude
+// update cruisecontrol based on attitude
     cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
     stabSettings.monitor.rateupdates = 0;
 }

flight/modules/Stabilization/stabilization.c

@@ -137,6 +137,10 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
             break;
+        case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER:
+            StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
+            StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
+            break;
         case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
             StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
             StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;

flight/modules/System/systemmod.c

@@ -101,7 +101,9 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
 static bool mallocFailed;
 static HwSettingsData bootHwSettings;
 static FrameType_t bootFrameType;
+#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
 static struct PIOS_FLASHFS_Stats fsStats;
+#endif
 
 // Private functions
 static void objectUpdatedCb(UAVObjEvent *ev);

flight/modules/Telemetry/telemetry.c

@@ -888,7 +888,7 @@ static void updateSettings(channelContext *channel)
 
     if (port) {
         // Retrieve settings
-        uint8_t speed;
+        HwSettingsTelemetrySpeedOptions speed;
         HwSettingsTelemetrySpeedGet(&speed);
 
         // Set port speed

flight/modules/TxPID/txpid.c

@@ -55,6 +55,9 @@
 #include "accessorydesired.h"
 #include "manualcontrolcommand.h"
 #include "stabilizationsettings.h"
+#ifdef REVOLUTION
+#include "altitudeholdsettings.h"
+#endif
 #include "stabilizationbank.h"
 #include "stabilizationsettingsbank1.h"
 #include "stabilizationsettingsbank2.h"
@@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void)
     bool txPIDEnabled;
     HwSettingsOptionalModulesData optionalModules;
 
+#ifdef REVOLUTION
+    AltitudeHoldSettingsInitialize();
+#endif
+
     HwSettingsInitialize();
     HwSettingsOptionalModulesGet(&optionalModules);
 
@@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev)
     }
     StabilizationSettingsData stab;
     StabilizationSettingsGet(&stab);
+#ifdef REVOLUTION
+    AltitudeHoldSettingsData altitude;
+    AltitudeHoldSettingsGet(&altitude);
+#endif
     AccessoryDesiredData accessory;
 
-    uint8_t needsUpdateBank = 0;
+    uint8_t needsUpdateBank     = 0;
-    uint8_t needsUpdateStab = 0;
+    uint8_t needsUpdateStab     = 0;
+#ifdef REVOLUTION
+    uint8_t needsUpdateAltitude = 0;
+#endif
 
     // Loop through every enabled instance
     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev)
             case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
                 needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
                 break;
+#ifdef REVOLUTION
+            case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
+                needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
+                break;
+#endif
             default:
                 PIOS_Assert(0);
             }
@@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev)
     if (needsUpdateStab) {
         StabilizationSettingsSet(&stab);
     }
+#ifdef REVOLUTION
+    if (needsUpdateAltitude) {
+        AltitudeHoldSettingsSet(&altitude);
+    }
+#endif
     if (needsUpdateBank) {
         switch (inst.BankNumber) {
         case 0:

flight/pios/common/pios_srxl.c

@@ -0,0 +1,365 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
+ * @brief Code to read Multiplex SRXL receiver serial stream
+ * @{
+ *
+ * @file       pios_srxl.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Code to read Multiplex SRXL receiver serial stream
+ * @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 "pios.h"
+
+#ifdef PIOS_INCLUDE_SRXL
+
+#include "pios_srxl_priv.h"
+
+// #define PIOS_INSTRUMENT_MODULE
+#include <pios_instrumentation_helper.h>
+
+PERF_DEFINE_COUNTER(crcFailureCount);
+PERF_DEFINE_COUNTER(failsafeCount);
+PERF_DEFINE_COUNTER(successfulCount);
+PERF_DEFINE_COUNTER(messageUnrollTimer);
+PERF_DEFINE_COUNTER(messageReceiveRate);
+PERF_DEFINE_COUNTER(receivedBytesCount);
+PERF_DEFINE_COUNTER(frameStartCount);
+PERF_DEFINE_COUNTER(frameAbortCount);
+PERF_DEFINE_COUNTER(completeMessageCount);
+
+/* Forward Declarations */
+static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel);
+static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
+                                       uint8_t *buf,
+                                       uint16_t buf_len,
+                                       uint16_t *headroom,
+                                       bool *need_yield);
+static void PIOS_SRXL_Supervisor(uint32_t srxl_id);
+
+
+/* Local Variables */
+const struct pios_rcvr_driver pios_srxl_rcvr_driver = {
+    .read = PIOS_SRXL_Get,
+};
+
+enum pios_srxl_dev_magic {
+    PIOS_SRXL_DEV_MAGIC = 0x55545970,
+};
+
+struct pios_srxl_state {
+    uint16_t channel_data[PIOS_SRXL_NUM_INPUTS];
+    uint8_t  received_data[SRXL_FRAME_LENGTH];
+    uint8_t  receive_timer;
+    uint8_t  failsafe_timer;
+    uint8_t  frame_found;
+    uint8_t  byte_count;
+    uint8_t  data_bytes;
+};
+
+struct pios_srxl_dev {
+    enum pios_srxl_dev_magic magic;
+    struct pios_srxl_state   state;
+};
+
+/* Allocate S.Bus device descriptor */
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
+{
+    struct pios_srxl_dev *srxl_dev;
+
+    srxl_dev = (struct pios_srxl_dev *)pios_malloc(sizeof(*srxl_dev));
+    if (!srxl_dev) {
+        return NULL;
+    }
+
+    srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
+    return srxl_dev;
+}
+#else
+static struct pios_srxl_dev pios_srxl_devs[PIOS_SRXL_MAX_DEVS];
+static uint8_t pios_srxl_num_devs;
+static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
+{
+    struct pios_srxl_dev *srxl_dev;
+
+    if (pios_srxl_num_devs >= PIOS_SRXL_MAX_DEVS) {
+        return NULL;
+    }
+
+    srxl_dev = &pios_srxl_devs[pios_srxl_num_devs++];
+    srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
+
+    return srxl_dev;
+}
+#endif /* if defined(PIOS_INCLUDE_FREERTOS) */
+
+/* Validate SRXL device descriptor */
+static bool PIOS_SRXL_Validate(struct pios_srxl_dev *srxl_dev)
+{
+    return srxl_dev->magic == PIOS_SRXL_DEV_MAGIC;
+}
+
+/* Reset channels in case of lost signal or explicit failsafe receiver flag */
+static void PIOS_SRXL_ResetChannels(struct pios_srxl_state *state)
+{
+    for (int i = 0; i < PIOS_SRXL_NUM_INPUTS; i++) {
+        state->channel_data[i] = PIOS_RCVR_TIMEOUT;
+    }
+}
+
+/* Reset SRXL receiver state */
+static void PIOS_SRXL_ResetState(struct pios_srxl_state *state)
+{
+    state->receive_timer  = 0;
+    state->failsafe_timer = 0;
+    state->frame_found    = 0;
+    state->data_bytes     = 0;
+    PIOS_SRXL_ResetChannels(state);
+}
+
+/* Initialize SRXL receiver interface */
+int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
+                       const struct pios_com_driver *driver,
+                       uint32_t lower_id)
+{
+    PIOS_DEBUG_Assert(srxl_id);
+    PIOS_DEBUG_Assert(driver);
+
+    struct pios_srxl_dev *srxl_dev;
+
+    srxl_dev = (struct pios_srxl_dev *)PIOS_SRXL_Alloc();
+    if (!srxl_dev) {
+        goto out_fail;
+    }
+
+    PIOS_SRXL_ResetState(&(srxl_dev->state));
+
+    *srxl_id = (uint32_t)srxl_dev;
+
+    /* Set comm driver callback */
+    (driver->bind_rx_cb)(lower_id, PIOS_SRXL_RxInCallback, *srxl_id);
+
+    if (!PIOS_RTC_RegisterTickCallback(PIOS_SRXL_Supervisor, *srxl_id)) {
+        PIOS_DEBUG_Assert(0);
+    }
+
+    PERF_INIT_COUNTER(crcFailureCount, 0x5551);
+    PERF_INIT_COUNTER(failsafeCount, 0x5552);
+    PERF_INIT_COUNTER(successfulCount, 0x5553);
+    PERF_INIT_COUNTER(messageUnrollTimer, 0x5554);
+    PERF_INIT_COUNTER(messageReceiveRate, 0x5555);
+    PERF_INIT_COUNTER(receivedBytesCount, 0x5556);
+    PERF_INIT_COUNTER(frameStartCount, 0x5557);
+    PERF_INIT_COUNTER(frameAbortCount, 0x5558);
+    PERF_INIT_COUNTER(completeMessageCount, 0x5559);
+
+    return 0;
+
+out_fail:
+    return -1;
+}
+
+/**
+ * Get the value of an input channel
+ * \param[in] channel Number of the channel desired (zero based)
+ * \output PIOS_RCVR_INVALID channel not available
+ * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
+ * \output >=0 channel value
+ */
+static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel)
+{
+    struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)rcvr_id;
+
+    if (!PIOS_SRXL_Validate(srxl_dev)) {
+        return PIOS_RCVR_INVALID;
+    }
+
+    /* return error if channel is not available */
+    if (channel >= PIOS_SRXL_NUM_INPUTS) {
+        return PIOS_RCVR_INVALID;
+    }
+
+    return srxl_dev->state.channel_data[channel];
+}
+
+static void PIOS_SRXL_UnrollChannels(struct pios_srxl_state *state)
+{
+    PERF_TIMED_SECTION_START(messageUnrollTimer);
+    uint8_t *received_data = state->received_data;
+    uint8_t channel;
+    uint16_t channel_value;
+    for (channel = 0; channel < (state->data_bytes / 2); channel++) {
+        channel_value = ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2)]) << 8;
+        channel_value = channel_value + ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2) + 1]);
+        state->channel_data[channel] = (800 + ((channel_value * 1400) >> 12));
+    }
+    PERF_TIMED_SECTION_END(messageUnrollTimer);
+}
+
+static bool PIOS_SRXL_Validate_Checksum(struct pios_srxl_state *state)
+{
+    // Check the CRC16 checksum. The provided checksum is immediately after the channel data.
+    // All data including start byte and version byte is included in crc calculation.
+    uint8_t i    = 0;
+    uint16_t crc = 0;
+
+    for (i = 0; i < SRXL_HEADER_LENGTH + state->data_bytes; i++) {
+        crc = crc ^ (int16_t)state->received_data[i] << 8;
+        uint8_t j = 0;
+        for (j = 0; j < 8; j++) {
+            if (crc & 0x8000) {
+                crc = crc << 1 ^ 0x1021;
+            } else {
+                crc = crc << 1;
+            }
+        }
+    }
+    uint16_t checksum = (((uint16_t)(state->received_data[i] << 8) |
+                          (uint16_t)state->received_data[i + 1]));
+    return crc == checksum;
+}
+
+/* Update decoder state processing input byte from the SRXL stream */
+static void PIOS_SRXL_UpdateState(struct pios_srxl_state *state, uint8_t b)
+{
+    /* should not process any data until new frame is found */
+    if (!state->frame_found) {
+        return;
+    }
+
+    if (state->byte_count < (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
+        if (state->byte_count == 0) {
+            // Set up the length of the channel data according to version received
+            PERF_INCREMENT_VALUE(frameStartCount);
+            if (b == SRXL_V1_HEADER) {
+                state->data_bytes = SRXL_V1_CHANNEL_DATA_BYTES;
+            } else if (b == SRXL_V2_HEADER) {
+                state->data_bytes = SRXL_V2_CHANNEL_DATA_BYTES;
+            } else {
+                /* discard the whole frame if the 1st byte is not correct */
+                state->frame_found = 0;
+                PERF_INCREMENT_VALUE(frameAbortCount);
+                return;
+            }
+        }
+        /* store next byte */
+        state->received_data[state->byte_count] = b;
+        state->byte_count++;
+        if (state->byte_count == (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
+            PERF_INCREMENT_VALUE(completeMessageCount);
+            // We have a complete message, lets decode it
+            if (PIOS_SRXL_Validate_Checksum(state)) {
+                /* data looking good */
+                PIOS_SRXL_UnrollChannels(state);
+                state->failsafe_timer = 0;
+                PERF_INCREMENT_VALUE(successfulCount);
+            } else {
+                /* discard whole frame */
+                PERF_INCREMENT_VALUE(crcFailureCount);
+            }
+            /* prepare for the next frame */
+            state->frame_found = 0;
+            PERF_MEASURE_PERIOD(messageReceiveRate);
+        }
+    }
+}
+
+/* Comm byte received callback */
+static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
+                                       uint8_t *buf,
+                                       uint16_t buf_len,
+                                       uint16_t *headroom,
+                                       bool *need_yield)
+{
+    struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)context;
+
+    bool valid = PIOS_SRXL_Validate(srxl_dev);
+
+    PIOS_Assert(valid);
+
+    struct pios_srxl_state *state = &(srxl_dev->state);
+
+    /* process byte(s) and clear receive timer */
+    for (uint8_t i = 0; i < buf_len; i++) {
+        PIOS_SRXL_UpdateState(state, buf[i]);
+        state->receive_timer = 0;
+        PERF_INCREMENT_VALUE(receivedBytesCount);
+    }
+
+    /* Always signal that we can accept another byte */
+    if (headroom) {
+        *headroom = SRXL_FRAME_LENGTH;
+    }
+
+    /* We never need a yield */
+    *need_yield = false;
+
+    /* Always indicate that all bytes were consumed */
+    return buf_len;
+}
+
+/**
+ * Input data supervisor is called periodically and provides
+ * two functions: frame syncing and failsafe triggering.
+ *
+ * Multiplex SRXL frames come at 14ms (FastResponse ON) or 21ms (FastResponse OFF)
+ * rate at 115200bps.
+ * RTC timer is running at 625Hz (1.6ms). So with divider 2 it gives
+ * 3.2ms pause between frames which is good for both SRXL frame rates.
+ *
+ * Data receive function must clear the receive_timer to confirm new
+ * data reception. If no new data received in 100ms, we must call the
+ * failsafe function which clears all channels.
+ */
+static void PIOS_SRXL_Supervisor(uint32_t srxl_id)
+{
+    struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)srxl_id;
+
+    bool valid = PIOS_SRXL_Validate(srxl_dev);
+
+    PIOS_Assert(valid);
+
+    struct pios_srxl_state *state = &(srxl_dev->state);
+
+    /* waiting for new frame if no bytes were received in 6.4ms */
+
+    if (++state->receive_timer > 4) {
+        state->frame_found   = 1;
+        state->byte_count    = 0;
+        state->receive_timer = 0;
+    }
+
+    /* activate failsafe if no frames have arrived in 102.4ms */
+    if (++state->failsafe_timer > 64) {
+        PIOS_SRXL_ResetChannels(state);
+        state->failsafe_timer = 0;
+        PERF_INCREMENT_VALUE(failsafeCount);
+    }
+}
+
+#endif /* PIOS_INCLUDE_SRXL */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_instrumentation.h

@@ -127,6 +127,29 @@ static inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handl
     counter->lastUpdateTS = PIOS_DELAY_GetRaw();
 }
 
+/**
+ * Increment a counter with a value
+ * @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ * @param increment the value to increment counter with.
+ */
+static inline void PIOS_Instrumentation_incrementCounter(pios_counter_t counter_handle, int32_t increment)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
+    vPortEnterCritical();
+    pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
+    counter->value += increment;
+    counter->max--;
+    if (counter->value > counter->max) {
+        counter->max = counter->value;
+    }
+    counter->min++;
+    if (counter->value < counter->min) {
+        counter->min = counter->value;
+    }
+    counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+    vPortExitCritical();
+}
+
 /**
  * Initialize the Instrumentation infrastructure
  * @param maxCounters maximum number of allowed counters

flight/pios/inc/pios_instrumentation_helper.h

@@ -98,6 +98,8 @@
 #define PERF_TIMED_SECTION_END(x)   PIOS_Instrumentation_TimeEnd(x)
 #define PERF_MEASURE_PERIOD(x)      PIOS_Instrumentation_TrackPeriod(x)
 #define PERF_TRACK_VALUE(x, y)      PIOS_Instrumentation_updateCounter(x, y)
+#define PERF_INCREMENT_VALUE(x)     PIOS_Instrumentation_incrementCounter(x, 1)
+#define PERF_DECREMENT_VALUE(x)     PIOS_Instrumentation_incrementCounter(x, -1)
 
 #else
 
@@ -107,5 +109,7 @@
 #define PERF_TIMED_SECTION_END(x)
 #define PERF_MEASURE_PERIOD(x)
 #define PERF_TRACK_VALUE(x, y)
+#define PERF_INCREMENT_VALUE(x)
+#define PERF_DECREMENT_VALUE(x)
 #endif /* PIOS_INCLUDE_INSTRUMENTATION */
 #endif /* PIOS_INSTRUMENTATION_HELPER_H */

flight/pios/inc/pios_srxl.h

@@ -1,12 +1,14 @@
 /**
  ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
  * @{
- * @addtogroup OpenPilotCore OpenPilot Core
+ * @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
+ * @brief Code to read Multiplex SRXL receiver serial stream
  * @{
- * @file       openpilot.h
+ *
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @file       pios_srxl.h
- * @brief      Main OpenPilot header.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Code to read Multiplex SRXL receiver serial stream
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -26,25 +28,14 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef OPENPILOT_H
+#ifndef PIOS_SRXL_H
-#define OPENPILOT_H
+#define PIOS_SRXL_H
 
-/* PIOS Includes */
+/* Global Types */
-#include <pios.h>
 
-/* OpenPilot Libraries */
+/* Public Functions */
-#include <utlist.h>
-#include <uavobjectmanager.h>
-#include <eventdispatcher.h>
-#include <uavtalk.h>
 
-#include "alarms.h"
+#endif /* PIOS_SRXL_H */
-#include <mathmisc.h>
-
-/* Global Functions */
-void OpenPilotInit(void);
-
-#endif /* OPENPILOT_H */
 
 /**
  * @}

flight/pios/inc/pios_srxl_priv.h

@@ -0,0 +1,95 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
+ * @brief Code to read Multiplex SRXL receiver serial stream
+ * @{
+ *
+ * @file       pios_srxl_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @brief      Code to read Multiplex SRXL receiver serial stream
+ * @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 PIOS_SRXL_PRIV_H
+#define PIOS_SRXL_PRIV_H
+
+#include <pios.h>
+#include <pios_stm32.h>
+#include <pios_usart_priv.h>
+
+/*
+ * Multiplex SRXL serial port settings:
+ *  115200bps inverted serial stream, 8 bits, no parity, 1 stop bits
+ *  frame period is 14ms (FastResponse ON) or 21ms (FastResponse OFF)
+ *
+ * Frame structure:
+ *  1 byte  - start and version
+ *      0xa1 (v1 12-channels)
+ *      0xa2 (v2 16-channels)
+ * 24/32 bytes - channel data (4 + 12 bit/channel, 12/16 channels, MSB first)
+ *      16 bits per channel. 4 first reserved/not used. 12 bits channel data in
+ *          4095 steps, 0x000(800µs) - 0x800(1500µs) - 0xfff(2200µs)
+ *  2 bytes checksum (calculated over all bytes including start and version)
+ *
+ * Checksum calculation:
+ * u16 CRC16(u16 crc, u8 value) {
+ *     u8 i;
+ *     crc = crc ^ (s16)value << 8;
+ *     for(i = 0; i < 8; i++) {
+ *         if(crc & 0x8000) {
+ *             crc = crc << 1 ^ 0x1021;
+ *         } else {
+ *             crc = crc << 1;
+ *         }
+ *     }
+ *     return crc;
+ * }
+ */
+
+#define SRXL_V1_HEADER             0xa1
+#define SRXL_V2_HEADER             0xa2
+
+#define SRXL_HEADER_LENGTH         1
+#define SRXL_CHECKSUM_LENGTH       2
+#define SRXL_V1_CHANNEL_DATA_BYTES (12 * 2)
+#define SRXL_V2_CHANNEL_DATA_BYTES (16 * 2)
+#define SRXL_FRAME_LENGTH          (SRXL_HEADER_LENGTH + SRXL_V2_CHANNEL_DATA_BYTES + SRXL_CHECKSUM_LENGTH)
+
+/*
+ * Multiplex SRXL protocol provides 16 proportional channels.
+ * Do not change unless driver code is updated accordingly.
+ */
+#if (PIOS_SRXL_NUM_INPUTS != 16)
+#error "Multiplex SRXL protocol provides 16 proportional channels."
+#endif
+
+extern const struct pios_rcvr_driver pios_srxl_rcvr_driver;
+
+extern int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
+                              const struct pios_com_driver *driver,
+                              uint32_t lower_id);
+
+#endif /* PIOS_SRXL_PRIV_H */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_usb_defs.h

@@ -353,7 +353,6 @@ enum usb_cdc_notification {
 
 enum usb_product_ids {
     USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A,
-    USB_PRODUCT_ID_COPTERCONTROL  = 0x415B,
     USB_PRODUCT_ID_OPLINK = 0x415C,
     USB_PRODUCT_ID_CC3D = 0x415D,
     USB_PRODUCT_ID_REVOLUTION     = 0x415E,
@@ -364,9 +363,9 @@ enum usb_product_ids {
 enum usb_op_board_ids {
     USB_OP_BOARD_ID_OPENPILOT_MAIN = 1,
     /* Board ID 2 may be unused or AHRS */
-    USB_OP_BOARD_ID_OPLINK = 3,
+    USB_OP_BOARD_ID_OPLINK     = 3,
-    USB_OP_BOARD_ID_COPTERCONTROL  = 4,
+    /* USB_OP_BOARD_ID_COPTERCONTROL  = 4, */
-    USB_OP_BOARD_ID_REVOLUTION     = 5,
+    USB_OP_BOARD_ID_REVOLUTION = 5,
     USB_OP_BOARD_ID_OSD = 6,
 } __attribute__((packed));
 

flight/pios/pios.h

@@ -228,6 +228,10 @@ extern "C" {
 #include <pios_sbus.h>
 #endif
 
+#ifdef PIOS_INCLUDE_SRXL
+#include <pios_srxl.h>
+#endif
+
 /* PIOS abstract receiver interface */
 #ifdef PIOS_INCLUDE_RCVR
 #include <pios_rcvr.h>

flight/targets/boards/coptercontrol/board-info.mk

@@ -1,23 +0,0 @@
-BOARD_TYPE          := 0x04
-BOARD_REVISION      := 0x02
-BOOTLOADER_VERSION  := 0x04
-HW_TYPE             := 0x01
-
-MCU                 := cortex-m3
-CHIP                := STM32F103CBT
-BOARD               := STM32103CB_CC_Rev1
-MODEL               := MD
-MODEL_SUFFIX        := _CC
-
-OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg
-OPENOCD_CONFIG      := stm32f1x.cfg
-
-# Note: These must match the values in link_$(BOARD)_memory.ld
-BL_BANK_BASE        := 0x08000000  # Start of bootloader flash
-BL_BANK_SIZE        := 0x00003000  # Should include BD_INFO region
-FW_BANK_BASE        := 0x08003000  # Start of firmware flash
-FW_BANK_SIZE        := 0x0001D000  # Should include FW_DESC_SIZE
-
-FW_DESC_SIZE        := 0x00000064
-
-OSCILLATOR_FREQ     :=   8000000

flight/targets/boards/coptercontrol/board_hw_defs.c

@@ -1,1525 +0,0 @@
-/**
- ******************************************************************************
- * @file       board_hw_defs.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
- * @author     PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @{
- * @brief Defines board specific static initializers for hardware for the CopterControl board.
- *****************************************************************************/
-/*
- * 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
- */
-
-#define BOARD_REVISION_CC   1
-#define BOARD_REVISION_CC3D 2
-
-#if defined(PIOS_INCLUDE_LED)
-
-#include <pios_led_priv.h>
-static const struct pios_gpio pios_leds_cc[] = {
-    [PIOS_LED_HEARTBEAT] = {
-        .pin                =             {
-            .gpio = GPIOA,
-            .init =             {
-                .GPIO_Pin   = GPIO_Pin_6,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-            },
-        },
-        .active_low         = true
-    },
-};
-
-static const struct pios_gpio_cfg pios_led_cfg_cc = {
-    .gpios     = pios_leds_cc,
-    .num_gpios = NELEMENTS(pios_leds_cc),
-};
-
-static const struct pios_gpio pios_leds_cc3d[] = {
-    [PIOS_LED_HEARTBEAT] = {
-        .pin                =             {
-            .gpio = GPIOB,
-            .init =             {
-                .GPIO_Pin   = GPIO_Pin_3,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-            },
-        },
-        .remap              = GPIO_Remap_SWJ_JTAGDisable,
-        .active_low         = true
-    },
-};
-
-static const struct pios_gpio_cfg pios_led_cfg_cc3d = {
-    .gpios     = pios_leds_cc3d,
-    .num_gpios = NELEMENTS(pios_leds_cc3d),
-};
-
-const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision)
-{
-    switch (board_revision) {
-    case BOARD_REVISION_CC:         return &pios_led_cfg_cc;
-
-    case BOARD_REVISION_CC3D:       return &pios_led_cfg_cc3d;
-
-    default:                        return NULL;
-    }
-}
-
-#endif /* PIOS_INCLUDE_LED */
-
-#if defined(PIOS_INCLUDE_SPI)
-
-#include <pios_spi_priv.h>
-
-/* Gyro interface */
-void PIOS_SPI_gyro_irq_handler(void);
-void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
-void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
-static const struct pios_spi_cfg pios_spi_gyro_cfg = {
-    .regs = SPI1,
-    .init = {
-        .SPI_Mode              = SPI_Mode_Master,
-        .SPI_Direction         = SPI_Direction_2Lines_FullDuplex,
-        .SPI_DataSize          = SPI_DataSize_8b,
-        .SPI_NSS                                   = SPI_NSS_Soft,
-        .SPI_FirstBit          = SPI_FirstBit_MSB,
-        .SPI_CRCPolynomial     = 7,
-        .SPI_CPOL              = SPI_CPOL_High,
-        .SPI_CPHA              = SPI_CPHA_2Edge,
-        .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */
-    },
-    .use_crc = false,
-    .dma     = {
-        .ahb_clk = RCC_AHBPeriph_DMA1,
-
-        .irq     = {
-            .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
-            .init  = {
-                .NVIC_IRQChannel    = DMA1_Channel2_IRQn,
-                .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-                .NVIC_IRQChannelSubPriority        = 0,
-                .NVIC_IRQChannelCmd = ENABLE,
-            },
-        },
-
-        .rx                                        = {
-            .channel = DMA1_Channel2,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&(SPI1->DR),
-                .DMA_DIR                           = DMA_DIR_PeripheralSRC,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-                .DMA_Mode     = DMA_Mode_Normal,
-                .DMA_Priority = DMA_Priority_Medium,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        },
-        .tx                                        = {
-            .channel = DMA1_Channel3,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&(SPI1->DR),
-                .DMA_DIR                           = DMA_DIR_PeripheralDST,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-                .DMA_Mode     = DMA_Mode_Normal,
-                .DMA_Priority = DMA_Priority_Medium,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        },
-    },
-    .sclk                                          = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_5,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-    .miso                                          = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_6,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .mosi                                          = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_7,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-    .slave_count                                   = 1,
-    .ssel                                          = {
-        {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_4,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-            },
-        }
-    },
-};
-
-static uint32_t pios_spi_gyro_id;
-void PIOS_SPI_gyro_irq_handler(void)
-{
-    /* Call into the generic code to handle the IRQ for this specific device */
-    PIOS_SPI_IRQ_Handler(pios_spi_gyro_id);
-}
-
-
-/* Flash/Accel Interface
- *
- * NOTE: Leave this declared as const data so that it ends up in the
- * .rodata section (ie. Flash) rather than in the .bss section (RAM).
- */
-void PIOS_SPI_flash_accel_irq_handler(void);
-void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler")));
-void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler")));
-static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
-    .regs = SPI2,
-    .init = {
-        .SPI_Mode              = SPI_Mode_Master,
-        .SPI_Direction         = SPI_Direction_2Lines_FullDuplex,
-        .SPI_DataSize          = SPI_DataSize_8b,
-        .SPI_NSS                                   = SPI_NSS_Soft,
-        .SPI_FirstBit          = SPI_FirstBit_MSB,
-        .SPI_CRCPolynomial     = 7,
-        .SPI_CPOL              = SPI_CPOL_High,
-        .SPI_CPHA              = SPI_CPHA_2Edge,
-        .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
-    },
-    .use_crc = false,
-    .dma     = {
-        .ahb_clk = RCC_AHBPeriph_DMA1,
-
-        .irq     = {
-            .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
-            .init  = {
-                .NVIC_IRQChannel    = DMA1_Channel4_IRQn,
-                .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-                .NVIC_IRQChannelSubPriority        = 0,
-                .NVIC_IRQChannelCmd = ENABLE,
-            },
-        },
-
-        .rx                                        = {
-            .channel = DMA1_Channel4,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&(SPI2->DR),
-                .DMA_DIR                           = DMA_DIR_PeripheralSRC,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-                .DMA_Mode     = DMA_Mode_Normal,
-                .DMA_Priority = DMA_Priority_High,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        },
-        .tx                                        = {
-            .channel = DMA1_Channel5,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&(SPI2->DR),
-                .DMA_DIR                           = DMA_DIR_PeripheralDST,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-                .DMA_Mode     = DMA_Mode_Normal,
-                .DMA_Priority = DMA_Priority_High,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        },
-    },
-    .sclk                                          = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_13,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-    .miso                                          = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_14,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-        },
-    },
-    .mosi                                          = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_15,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-    .slave_count                                   = 2,
-    .ssel                                          = {
-        {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_12,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-            }
-        },{
-            .gpio = GPIOC,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_15,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-            },
-        }
-    },
-};
-
-static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
-    .regs = SPI2,
-    .init = {
-        .SPI_Mode              = SPI_Mode_Master,
-        .SPI_Direction         = SPI_Direction_2Lines_FullDuplex,
-        .SPI_DataSize          = SPI_DataSize_8b,
-        .SPI_NSS                                   = SPI_NSS_Soft,
-        .SPI_FirstBit          = SPI_FirstBit_MSB,
-        .SPI_CRCPolynomial     = 7,
-        .SPI_CPOL              = SPI_CPOL_High,
-        .SPI_CPHA              = SPI_CPHA_2Edge,
-        .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
-    },
-    .use_crc = false,
-    .dma     = {
-        .ahb_clk = RCC_AHBPeriph_DMA1,
-
-        .irq     = {
-            .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
-            .init  = {
-                .NVIC_IRQChannel    = DMA1_Channel4_IRQn,
-                .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-                .NVIC_IRQChannelSubPriority        = 0,
-                .NVIC_IRQChannelCmd = ENABLE,
-            },
-        },
-
-        .rx                                        = {
-            .channel = DMA1_Channel4,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&(SPI2->DR),
-                .DMA_DIR                           = DMA_DIR_PeripheralSRC,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-                .DMA_Mode     = DMA_Mode_Normal,
-                .DMA_Priority = DMA_Priority_High,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        },
-        .tx                                        = {
-            .channel = DMA1_Channel5,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&(SPI2->DR),
-                .DMA_DIR                           = DMA_DIR_PeripheralDST,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
-                .DMA_Mode     = DMA_Mode_Normal,
-                .DMA_Priority = DMA_Priority_High,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        },
-    },
-    .sclk                                          = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_13,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-    .miso                                          = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_14,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-        },
-    },
-    .mosi                                          = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_15,
-            .GPIO_Speed = GPIO_Speed_50MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-    .slave_count                                   = 2,
-    .ssel                                          = {
-        {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_12,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-            }
-        },{
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_7,
-                .GPIO_Speed = GPIO_Speed_50MHz,
-                .GPIO_Mode  = GPIO_Mode_Out_PP,
-            },
-        }
-    },
-};
-
-static uint32_t pios_spi_flash_accel_id;
-void PIOS_SPI_flash_accel_irq_handler(void)
-{
-    /* Call into the generic code to handle the IRQ for this specific device */
-    PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id);
-}
-
-#endif /* PIOS_INCLUDE_SPI */
-
-#if defined(PIOS_INCLUDE_FLASH)
-#include "pios_flashfs_logfs_priv.h"
-#include "pios_flash_jedec_priv.h"
-
-static const struct flashfs_logfs_cfg flashfs_w25x_cfg = {
-    .fs_magic      = 0x99abcdef,
-    .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */
-    .arena_size    = 0x00010000, /* 256 * slot size */
-    .slot_size     = 0x00000100, /* 256 bytes */
-
-    .start_offset  = 0,          /* start at the beginning of the chip */
-    .sector_size   = 0x00001000, /* 4K bytes */
-    .page_size     = 0x00000100, /* 256 bytes */
-};
-
-
-static const struct flashfs_logfs_cfg flashfs_m25p_cfg = {
-    .fs_magic      = 0x99abceef,
-    .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */
-    .arena_size    = 0x00010000, /* 256 * slot size */
-    .slot_size     = 0x00000100, /* 256 bytes */
-
-    .start_offset  = 0,          /* start at the beginning of the chip */
-    .sector_size   = 0x00010000, /* 64K bytes */
-    .page_size     = 0x00000100, /* 256 bytes */
-};
-
-#include "pios_flash.h"
-
-#endif /* PIOS_INCLUDE_FLASH */
-
-/*
- * ADC system
- */
-#if defined(PIOS_INCLUDE_ADC)
-#include "pios_adc_priv.h"
-extern void PIOS_ADC_handler(void);
-void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler")));
-// Remap the ADC DMA handler to this one
-static const struct pios_adc_cfg pios_adc_cfg = {
-    .dma                                           = {
-        .ahb_clk = RCC_AHBPeriph_DMA1,
-        .irq     = {
-            .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
-            .init  = {
-                .NVIC_IRQChannel    = DMA1_Channel1_IRQn,
-                .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-                .NVIC_IRQChannelSubPriority        = 0,
-                .NVIC_IRQChannelCmd = ENABLE,
-            },
-        },
-        .rx                                        = {
-            .channel = DMA1_Channel1,
-            .init    = {
-                .DMA_PeripheralBaseAddr            = (uint32_t)&ADC1->DR,
-                .DMA_DIR                           = DMA_DIR_PeripheralSRC,
-                .DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
-                .DMA_MemoryInc          = DMA_MemoryInc_Enable,
-                .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
-                .DMA_MemoryDataSize     = DMA_MemoryDataSize_Word,
-                .DMA_Mode     = DMA_Mode_Circular,
-                .DMA_Priority = DMA_Priority_High,
-                .DMA_M2M                           = DMA_M2M_Disable,
-            },
-        }
-    },
-    .half_flag = DMA1_IT_HT1,
-    .full_flag = DMA1_IT_TC1,
-};
-
-void PIOS_ADC_handler()
-{
-    PIOS_ADC_DMA_Handler();
-}
-#endif /* if defined(PIOS_INCLUDE_ADC) */
-
-#include "pios_tim_priv.h"
-
-static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = {
-    .TIM_Prescaler         = (PIOS_MASTER_CLOCK / 1000000) - 1,
-    .TIM_ClockDivision     = TIM_CKD_DIV1,
-    .TIM_CounterMode       = TIM_CounterMode_Up,
-    .TIM_Period            = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
-    .TIM_RepetitionCounter = 0x0000,
-};
-
-static const struct pios_tim_clock_cfg tim_1_cfg = {
-    .timer = TIM1,
-    .time_base_init                            = &tim_1_2_3_4_time_base,
-    .irq   = {
-        .init                                  = {
-            .NVIC_IRQChannel    = TIM1_CC_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-};
-
-static const struct pios_tim_clock_cfg tim_2_cfg = {
-    .timer = TIM2,
-    .time_base_init                            = &tim_1_2_3_4_time_base,
-    .irq   = {
-        .init                                  = {
-            .NVIC_IRQChannel    = TIM2_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-};
-
-static const struct pios_tim_clock_cfg tim_3_cfg = {
-    .timer = TIM3,
-    .time_base_init                            = &tim_1_2_3_4_time_base,
-    .irq   = {
-        .init                                  = {
-            .NVIC_IRQChannel    = TIM3_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-};
-
-static const struct pios_tim_clock_cfg tim_4_cfg = {
-    .timer = TIM4,
-    .time_base_init                            = &tim_1_2_3_4_time_base,
-    .irq   = {
-        .init                                  = {
-            .NVIC_IRQChannel    = TIM4_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-};
-
-static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_6,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_2,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_5,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-        .remap = GPIO_PartialRemap_TIM3,
-    },
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_3,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_0,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_4,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_1,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM2,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_0,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM2,
-        .timer_chan = TIM_Channel_2,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_1,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-};
-
-static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_4,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_9,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_3,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_8,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_2,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_7,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM1,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_8,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_4,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-        .remap = GPIO_PartialRemap_TIM3,
-    },
-    {
-        .timer = TIM2,
-        .timer_chan = TIM_Channel_3,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_2,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-};
-
-
-static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = {
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_4,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_9,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_3,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_8,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM4,
-        .timer_chan = TIM_Channel_2,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_7,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM1,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_8,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_4,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-        .remap = GPIO_PartialRemap_TIM3,
-    },
-    {
-        .timer = TIM2,
-        .timer_chan = TIM_Channel_3,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_2,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-
-    // Receiver port pins
-    // S3-S6 inputs are used as outputs in this case
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_3,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_0,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_4,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_1,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM2,
-        .timer_chan = TIM_Channel_1,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_0,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-    {
-        .timer = TIM2,
-        .timer_chan = TIM_Channel_2,
-        .pin   = {
-            .gpio = GPIOA,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_1,
-                .GPIO_Mode  = GPIO_Mode_AF_PP,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-    },
-};
-
-static const struct pios_tim_channel pios_tim_ppm_flexi_port = {
-    .timer = TIM2,
-    .timer_chan         = TIM_Channel_4,
-    .pin   = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_11,
-            .GPIO_Mode  = GPIO_Mode_IPD,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-        },
-    },
-    .remap              = GPIO_PartialRemap2_TIM2,
-};
-
-#if defined(PIOS_INCLUDE_USART)
-
-#include "pios_usart_priv.h"
-
-static const struct pios_usart_cfg pios_usart_generic_main_cfg = {
-    .regs = USART1,
-    .init = {
-        .USART_BaudRate   = 57600,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_No,
-        .USART_StopBits   = USART_StopBits_1,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx | USART_Mode_Tx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART1_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_9,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-};
-
-static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = {
-    .regs = USART3,
-    .init = {
-        .USART_BaudRate   = 57600,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_No,
-        .USART_StopBits   = USART_StopBits_1,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx | USART_Mode_Tx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART3_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_11,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-};
-
-#if defined(PIOS_INCLUDE_DSM)
-/*
- * Spektrum/JR DSM USART
- */
-#include <pios_dsm_priv.h>
-
-static const struct pios_usart_cfg pios_usart_dsm_main_cfg = {
-    .regs = USART1,
-    .init = {
-        .USART_BaudRate   = 115200,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_No,
-        .USART_StopBits   = USART_StopBits_1,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART1_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_9,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-        },
-    },
-};
-
-static const struct pios_dsm_cfg pios_dsm_main_cfg = {
-    .bind               = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_Out_PP,
-        },
-    },
-};
-
-static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = {
-    .regs = USART3,
-    .init = {
-        .USART_BaudRate   = 115200,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_No,
-        .USART_StopBits   = USART_StopBits_1,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART3_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_11,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-        },
-    },
-};
-
-static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
-    .bind               = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_11,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_Out_PP,
-        },
-    },
-};
-
-#endif /* PIOS_INCLUDE_DSM */
-
-
-#if defined(PIOS_INCLUDE_SBUS)
-/*
- * S.Bus USART
- */
-#include <pios_sbus_priv.h>
-
-static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
-    .regs = USART1,
-    .init = {
-        .USART_BaudRate   = 100000,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_Even,
-        .USART_StopBits   = USART_StopBits_2,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART1_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_9,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-        },
-    },
-};
-
-static const struct pios_sbus_cfg pios_sbus_cfg = {
-    /* Inverter configuration */
-    .inv                = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_2,
-            .GPIO_Mode  = GPIO_Mode_Out_PP,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-        },
-    },
-    .gpio_clk_func   = RCC_APB2PeriphClockCmd,
-    .gpio_clk_periph = RCC_APB2Periph_GPIOB,
-    .gpio_inv_enable = Bit_SET,
-};
-
-#endif /* PIOS_INCLUDE_SBUS */
-
-/*
- * HK OSD
- */
-static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
-    .regs = USART1,
-    .init = {
-        .USART_BaudRate   = 57600,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_No,
-        .USART_StopBits   = USART_StopBits_1,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx | USART_Mode_Tx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART1_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_9,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-};
-
-static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
-    .regs = USART3,
-    .init = {
-        .USART_BaudRate   = 57600,
-        .USART_WordLength = USART_WordLength_8b,
-        .USART_Parity     = USART_Parity_No,
-        .USART_StopBits   = USART_StopBits_1,
-        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
-        .USART_Mode                            = USART_Mode_Rx | USART_Mode_Tx,
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USART3_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .rx                                        = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_11,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_IPU,
-        },
-    },
-    .tx                                        = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_PP,
-        },
-    },
-};
-
-
-#endif /* PIOS_INCLUDE_USART */
-
-#if defined(PIOS_INCLUDE_COM)
-
-#include "pios_com_priv.h"
-
-#endif /* PIOS_INCLUDE_COM */
-
-#if defined(PIOS_INCLUDE_RTC)
-/*
- * Realtime Clock (RTC)
- */
-#include <pios_rtc_priv.h>
-
-void PIOS_RTC_IRQ_Handler(void);
-void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler")));
-static const struct pios_rtc_cfg pios_rtc_main_cfg = {
-    .clksrc    = RCC_RTCCLKSource_HSE_Div128,
-    .prescaler = 100,
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = RTC_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-};
-
-void PIOS_RTC_IRQ_Handler(void)
-{
-    PIOS_RTC_irq_handler();
-}
-
-#endif /* if defined(PIOS_INCLUDE_RTC) */
-
-#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM)
-/*
- * Servo outputs
- */
-#include <pios_servo_priv.h>
-
-const struct pios_servo_cfg pios_servo_cfg = {
-    .tim_oc_init          = {
-        .TIM_OCMode       = TIM_OCMode_PWM1,
-        .TIM_OutputState  = TIM_OutputState_Enable,
-        .TIM_OutputNState = TIM_OutputNState_Disable,
-        .TIM_Pulse        = PIOS_SERVOS_INITIAL_POSITION,
-        .TIM_OCPolarity   = TIM_OCPolarity_High,
-        .TIM_OCNPolarity  = TIM_OCPolarity_High,
-        .TIM_OCIdleState  = TIM_OCIdleState_Reset,
-        .TIM_OCNIdleState = TIM_OCNIdleState_Reset,
-    },
-    .channels     = pios_tim_servoport_all_pins,
-    .num_channels = NELEMENTS(pios_tim_servoport_all_pins),
-};
-
-const struct pios_servo_cfg pios_servo_rcvr_cfg = {
-    .tim_oc_init          = {
-        .TIM_OCMode       = TIM_OCMode_PWM1,
-        .TIM_OutputState  = TIM_OutputState_Enable,
-        .TIM_OutputNState = TIM_OutputNState_Disable,
-        .TIM_Pulse        = PIOS_SERVOS_INITIAL_POSITION,
-        .TIM_OCPolarity   = TIM_OCPolarity_High,
-        .TIM_OCNPolarity  = TIM_OCPolarity_High,
-        .TIM_OCIdleState  = TIM_OCIdleState_Reset,
-        .TIM_OCNIdleState = TIM_OCNIdleState_Reset,
-    },
-    .channels     = pios_tim_servoport_rcvrport_pins,
-    .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins),
-};
-
-#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */
-
-/*
- * PPM Inputs
- */
-#if defined(PIOS_INCLUDE_PPM)
-#include <pios_ppm_priv.h>
-
-const struct pios_ppm_cfg pios_ppm_cfg = {
-    .tim_ic_init         = {
-        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
-        .TIM_ICSelection = TIM_ICSelection_DirectTI,
-        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
-        .TIM_ICFilter    = 0x0,
-    },
-    /* Use only the first channel for ppm */
-    .channels     = &pios_tim_rcvrport_all_channels[0],
-    .num_channels = 1,
-};
-
-const struct pios_ppm_cfg pios_ppm_pin8_cfg = {
-    .tim_ic_init         = {
-        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
-        .TIM_ICSelection = TIM_ICSelection_DirectTI,
-        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
-        .TIM_ICFilter    = 0x0,
-    },
-    /* Use only the first channel for ppm */
-    .channels     = &pios_tim_rcvrport_all_channels[5],
-    .num_channels = 1,
-};
-
-#endif /* PIOS_INCLUDE_PPM */
-
-#if defined(PIOS_INCLUDE_PPM_FLEXI)
-#include <pios_ppm_priv.h>
-
-const struct pios_ppm_cfg pios_ppm_flexi_cfg = {
-    .tim_ic_init         = {
-        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
-        .TIM_ICSelection = TIM_ICSelection_DirectTI,
-        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
-        .TIM_ICFilter    = 0x0,
-    },
-    .channels     = &pios_tim_ppm_flexi_port,
-    .num_channels = 1,
-};
-
-#endif /* PIOS_INCLUDE_PPM_FLEXI */
-
-/*
- * PWM Inputs
- */
-#if defined(PIOS_INCLUDE_PWM)
-#include <pios_pwm_priv.h>
-
-const struct pios_pwm_cfg pios_pwm_cfg = {
-    .tim_ic_init         = {
-        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
-        .TIM_ICSelection = TIM_ICSelection_DirectTI,
-        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
-        .TIM_ICFilter    = 0x0,
-    },
-    .channels     = pios_tim_rcvrport_all_channels,
-    .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
-};
-
-const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = {
-    .tim_ic_init         = {
-        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
-        .TIM_ICSelection = TIM_ICSelection_DirectTI,
-        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
-        .TIM_ICFilter    = 0x0,
-    },
-    /* Leave the first channel for PPM use and use the rest for PWM */
-    .channels     = &pios_tim_rcvrport_all_channels[1],
-    .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1,
-};
-
-#endif /* if defined(PIOS_INCLUDE_PWM) */
-
-
-/*
- * SONAR Inputs
- */
-#if defined(PIOS_INCLUDE_HCSR04)
-#include <pios_hcsr04_priv.h>
-
-static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = {
-    {
-        .timer = TIM3,
-        .timer_chan = TIM_Channel_2,
-        .pin   = {
-            .gpio = GPIOB,
-            .init = {
-                .GPIO_Pin   = GPIO_Pin_5,
-                .GPIO_Mode  = GPIO_Mode_IPD,
-                .GPIO_Speed = GPIO_Speed_2MHz,
-            },
-        },
-        .remap = GPIO_PartialRemap_TIM3,
-    },
-};
-
-const struct pios_hcsr04_cfg pios_hcsr04_cfg = {
-    .tim_ic_init         = {
-        .TIM_ICPolarity  = TIM_ICPolarity_Rising,
-        .TIM_ICSelection = TIM_ICSelection_DirectTI,
-        .TIM_ICPrescaler = TIM_ICPSC_DIV1,
-        .TIM_ICFilter    = 0x0,
-    },
-    .channels     = pios_tim_hcsr04_port_all_channels,
-    .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels),
-    .trigger             = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_6,
-            .GPIO_Mode  = GPIO_Mode_Out_PP,
-            .GPIO_Speed = GPIO_Speed_2MHz,
-        },
-    },
-};
-#endif /* if defined(PIOS_INCLUDE_HCSR04) */
-
-#if defined(PIOS_INCLUDE_I2C)
-
-#include <pios_i2c_priv.h>
-
-/*
- * I2C Adapters
- */
-
-void PIOS_I2C_flexi_adapter_ev_irq_handler(void);
-void PIOS_I2C_flexi_adapter_er_irq_handler(void);
-void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler")));
-void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler")));
-
-static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = {
-    .regs = I2C2,
-    .init = {
-        .I2C_Mode = I2C_Mode_I2C,
-        .I2C_OwnAddress1                       = 0,
-        .I2C_Ack  = I2C_Ack_Enable,
-        .I2C_AcknowledgedAddress               = I2C_AcknowledgedAddress_7bit,
-        .I2C_DutyCycle                         = I2C_DutyCycle_2,
-        .I2C_ClockSpeed                        = 400000,                      /* bits/s */
-    },
-    .transfer_timeout_ms                       = 50,
-    .scl                                       = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_10,
-            .GPIO_Speed = GPIO_Speed_10MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_OD,
-        },
-    },
-    .sda                                       = {
-        .gpio = GPIOB,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_11,
-            .GPIO_Speed = GPIO_Speed_10MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_OD,
-        },
-    },
-    .event                                     = {
-        .flags = 0,               /* FIXME: check this */
-        .init  = {
-            .NVIC_IRQChannel    = I2C2_EV_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .error                                     = {
-        .flags = 0,               /* FIXME: check this */
-        .init  = {
-            .NVIC_IRQChannel    = I2C2_ER_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-};
-
-uint32_t pios_i2c_flexi_adapter_id;
-void PIOS_I2C_flexi_adapter_ev_irq_handler(void)
-{
-    /* Call into the generic code to handle the IRQ for this specific device */
-    PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id);
-}
-
-void PIOS_I2C_flexi_adapter_er_irq_handler(void)
-{
-    /* Call into the generic code to handle the IRQ for this specific device */
-    PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id);
-}
-
-#endif /* PIOS_INCLUDE_I2C */
-
-#if defined(PIOS_INCLUDE_GCSRCVR)
-#include "pios_gcsrcvr_priv.h"
-#endif /* PIOS_INCLUDE_GCSRCVR */
-
-#if defined(PIOS_INCLUDE_RCVR)
-#include "pios_rcvr_priv.h"
-
-#endif /* PIOS_INCLUDE_RCVR */
-
-#if defined(PIOS_INCLUDE_USB)
-#include "pios_usb_priv.h"
-
-static const struct pios_usb_cfg pios_usb_main_cfg_cc = {
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USB_LP_CAN1_RX0_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .vsense                                    = {
-        .gpio = GPIOC,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_15,
-            .GPIO_Speed = GPIO_Speed_10MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_OD,
-        },
-    },
-    .vsense_active_low                         = false
-};
-
-static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = {
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = USB_LP_CAN1_RX0_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .vsense                                    = {
-        .gpio = GPIOC,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_14,
-            .GPIO_Speed = GPIO_Speed_10MHz,
-            .GPIO_Mode  = GPIO_Mode_AF_OD,
-        },
-    },
-    .vsense_active_low                         = false
-};
-
-#include "pios_usb_board_data_priv.h"
-#include "pios_usb_desc_hid_cdc_priv.h"
-#include "pios_usb_desc_hid_only_priv.h"
-
-#endif /* PIOS_INCLUDE_USB */
-
-#if defined(PIOS_INCLUDE_COM_MSG)
-
-#include <pios_com_msg_priv.h>
-
-#endif /* PIOS_INCLUDE_COM_MSG */
-
-#if defined(PIOS_INCLUDE_USB_HID)
-#include <pios_usb_hid_priv.h>
-
-const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
-    .data_if    = 2,
-    .data_rx_ep = 1,
-    .data_tx_ep = 1,
-};
-
-#endif /* PIOS_INCLUDE_USB_HID */
-
-#if defined(PIOS_INCLUDE_USB_RCTX)
-#include <pios_usb_rctx_priv.h>
-
-const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = {
-    .data_if    = 2,
-    .data_tx_ep = 1,
-};
-
-#endif /* PIOS_INCLUDE_USB_RCTX */
-
-#if defined(PIOS_INCLUDE_USB_CDC)
-#include <pios_usb_cdc_priv.h>
-
-const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
-    .ctrl_if    = 0,
-    .ctrl_tx_ep = 2,
-
-    .data_if    = 1,
-    .data_rx_ep = 3,
-    .data_tx_ep = 3,
-};
-#endif /* PIOS_INCLUDE_USB_CDC */

flight/targets/boards/coptercontrol/bootloader/Makefile

@@ -1,32 +0,0 @@
-#
-# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
-#
-# 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 OPENPILOT_IS_COOL
-    $(error Top level Makefile must be used to build this target)
-endif
-
-## The standard CMSIS startup
-SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c
-
-include ../board-info.mk
-include $(ROOT_DIR)/make/firmware-defs.mk
-include $(ROOT_DIR)/make/boot-defs.mk
-include $(ROOT_DIR)/make/common-defs.mk
-
-$(info Making bootloader for CopterControl, board revision $(BOARD_REVISION))
-$(info Use BOARD_REVISION=1 for CC, BOARD_REVISION=2 for CC3D (default))

flight/targets/boards/coptercontrol/bootloader/inc/common.h

@@ -1,115 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup CopterControlBL CopterControl BootLoader
- * @brief These files contain the code to the CopterControl Bootloader.
- *
- * @{
- * @file       common.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      This file contains various common defines for the BootLoader
- * @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 COMMON_H_
-#define COMMON_H_
-
-// #include "board.h"
-
-typedef enum {
-    start, keepgoing,
-} DownloadAction;
-
-/**************************************************/
-/* OP_DFU states                       */
-/**************************************************/
-
-typedef enum {
-    DFUidle, // 0
-    uploading, // 1
-    wrong_packet_received, // 2
-    too_many_packets, // 3
-    too_few_packets, // 4
-    Last_operation_Success, // 5
-    downloading, // 6
-    BLidle, // 7
-    Last_operation_failed, // 8
-    uploadingStarting, // 9
-    outsideDevCapabilities, // 10
-    CRC_Fail, // 11
-    failed_jump,
-// 12
-} DFUStates;
-/**************************************************/
-/* OP_DFU commands                       */
-/**************************************************/
-typedef enum {
-    Reserved, // 0
-    Req_Capabilities, // 1
-    Rep_Capabilities, // 2
-    EnterDFU, // 3
-    JumpFW, // 4
-    Reset, // 5
-    Abort_Operation, // 6
-    Upload, // 7
-    Op_END, // 8
-    Download_Req, // 9
-    Download, // 10
-    Status_Request, // 11
-    Status_Rep
-// 12
-} DFUCommands;
-
-typedef enum {
-    High_Density, Medium_Density
-} DeviceType;
-/**************************************************/
-/* OP_DFU transfer types                       */
-/**************************************************/
-typedef enum {
-    FW, // 0
-    Descript
-// 2
-} DFUTransfer;
-/**************************************************/
-/* OP_DFU transfer port                           */
-/**************************************************/
-typedef enum {
-    Usb, // 0
-    Serial
-// 2
-} DFUPort;
-/**************************************************/
-/* OP_DFU programable programable HW types        */
-/**************************************************/
-typedef enum {
-    Self_flash, // 0
-    Remote_flash_via_spi
-// 1
-} DFUProgType;
-/**************************************************/
-/* OP_DFU programable sources			          */
-/**************************************************/
-#define USB            0
-#define SPI            1
-
-#define DownloadDelay  100000
-
-#define MAX_DEL_RETRYS 3
-#define MAX_WRI_RETRYS 3
-
-#endif /* COMMON_H_ */

flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h

@@ -1,53 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup CopterControlBL CopterControl BootLoader
- * @brief These files contain the code to the CopterControl Bootloader.
- * @{
- * @file       pios_config.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      PiOS configuration header.
- *             Central compile time config for the project.
- *             In particular, pios_config.h is where you define which PiOS libraries
- *             and features are included in the firmware.
- * @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 PIOS_CONFIG_H
-#define PIOS_CONFIG_H
-
-/* Enable/Disable PiOS modules */
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_IRQ
-#define PIOS_INCLUDE_GPIO
-#define PIOS_INCLUDE_USB
-#define PIOS_INCLUDE_USB_HID
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_IAP
-#define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_COM_MSG
-#define PIOS_INCLUDE_BL_HELPER
-#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
-
-#endif /* PIOS_CONFIG_H */
-
-/**
- * @}
- * @}
- */

flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h

@@ -1,53 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup PIOS_USB_BOARD Board specific USB definitions
- * @brief Board specific USB definitions
- * @{
- *
- * @file       pios_usb_board_data.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Board specific USB definitions
- * @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 PIOS_USB_BOARD_DATA_H
-#define PIOS_USB_BOARD_DATA_H
-
-// Note : changing below length will require changes to the USB buffer setup
-#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
-
-#define PIOS_USB_BOARD_EP_NUM          2
-
-#include <pios_usb_defs.h> /* struct usb_* */
-
-#define PIOS_USB_BOARD_PRODUCT_ID      USB_PRODUCT_ID_COPTERCONTROL
-#define PIOS_USB_BOARD_DEVICE_VER      USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL)
-#define PIOS_USB_BOARD_SN_SUFFIX       "+BL"
-
-/*
- * The bootloader uses a simplified report structure
- *   BL: <REPORT_ID><DATA>...<DATA>
- *   FW: <REPORT_ID><LENGTH><DATA>...<DATA>
- * This define changes the behaviour in pios_usb_hid.c
- */
-#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
-
-#endif /* PIOS_USB_BOARD_DATA_H */

flight/targets/boards/coptercontrol/bootloader/main.c

@@ -1,223 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup CopterControlBL CopterControl BootLoader
- * @brief These files contain the code to the CopterControl Bootloader.
- *
- * @{
- * @file       main.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      This is the file with the main function of the OpenPilot BootLoader
- * @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 <pios.h>
-#include <pios_board_info.h>
-#include <op_dfu.h>
-#include <usb_lib.h>
-#include <pios_iap.h>
-#include <fifo_buffer.h>
-#include <pios_com_msg.h>
-
-/* Prototype of PIOS_Board_Init() function */
-extern void PIOS_Board_Init(void);
-extern void FLASH_Download();
-#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
-
-/* Private typedef -----------------------------------------------------------*/
-typedef void (*pFunction)(void);
-/* Private define ------------------------------------------------------------*/
-/* Private macro -------------------------------------------------------------*/
-/* Private variables ---------------------------------------------------------*/
-pFunction Jump_To_Application;
-uint32_t JumpAddress;
-
-/// LEDs PWM
-uint32_t period1 = 5000; // 5 mS
-uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
-uint32_t period2 = 5000; // 5 mS
-uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
-
-
-////////////////////////////////////////
-uint8_t tempcount = 0;
-
-/* Extern variables ----------------------------------------------------------*/
-DFUStates DeviceState;
-int16_t status           = 0;
-uint8_t JumpToApp        = FALSE;
-uint8_t GO_dfu           = FALSE;
-uint8_t USB_connected    = FALSE;
-uint8_t User_DFU_request = FALSE;
-static uint8_t mReceive_Buffer[63];
-/* Private function prototypes -----------------------------------------------*/
-uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
-uint8_t processRX();
-void jump_to_app();
-
-int main()
-{
-    PIOS_SYS_Init();
-    PIOS_Board_Init();
-    PIOS_IAP_Init();
-
-    USB_connected = PIOS_USB_CableConnected(0);
-
-    if (PIOS_IAP_CheckRequest() == TRUE) {
-        PIOS_DELAY_WaitmS(1000);
-        User_DFU_request = TRUE;
-        PIOS_IAP_ClearRequest();
-    }
-
-    GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE);
-
-    if (GO_dfu == TRUE) {
-        PIOS_Board_Init();
-        if (User_DFU_request == TRUE) {
-            DeviceState = DFUidle;
-        } else {
-            DeviceState = BLidle;
-        }
-    } else {
-        JumpToApp = TRUE;
-    }
-
-    uint32_t stopwatch  = 0;
-    uint32_t prev_ticks = PIOS_DELAY_GetuS();
-    while (TRUE) {
-        /* Update the stopwatch */
-        uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
-        prev_ticks += elapsed_ticks;
-        stopwatch  += elapsed_ticks;
-
-        if (JumpToApp == TRUE) {
-            jump_to_app();
-        }
-
-        switch (DeviceState) {
-        case Last_operation_Success:
-        case uploadingStarting:
-        case DFUidle:
-            period1 = 5000;
-            sweep_steps1 = 100;
-            PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-            period2 = 0;
-            break;
-        case uploading:
-            period1 = 5000;
-            sweep_steps1 = 100;
-            period2 = 2500;
-            sweep_steps2 = 50;
-            break;
-        case downloading:
-            period1 = 2500;
-            sweep_steps1 = 50;
-            PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-            period2 = 0;
-            break;
-        case BLidle:
-            period1 = 0;
-            PIOS_LED_On(PIOS_LED_HEARTBEAT);
-            period2 = 0;
-            break;
-        default: // error
-            period1 = 5000;
-            sweep_steps1 = 100;
-            period2 = 5000;
-            sweep_steps2 = 100;
-        }
-
-        if (period1 != 0) {
-            if (LedPWM(period1, sweep_steps1, stopwatch)) {
-                PIOS_LED_On(PIOS_LED_HEARTBEAT);
-            } else {
-                PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-            }
-        } else {
-            PIOS_LED_On(PIOS_LED_HEARTBEAT);
-        }
-
-        if (period2 != 0) {
-            if (LedPWM(period2, sweep_steps2, stopwatch)) {
-                PIOS_LED_On(PIOS_LED_HEARTBEAT);
-            } else {
-                PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-            }
-        } else {
-            PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-        }
-
-        if (stopwatch > 50 * 1000 * 1000) {
-            stopwatch = 0;
-        }
-        if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) {
-            JumpToApp = TRUE;
-        }
-
-        processRX();
-        DataDownload(start);
-    }
-}
-
-void jump_to_app()
-{
-    const struct pios_board_info *bdinfo = &pios_board_info_blob;
-
-    if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
-        FLASH_Lock();
-        RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
-        RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
-        RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
-        RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
-        _SetCNTR(0); // clear interrupt mask
-        _SetISTR(0); // clear all requests
-        JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4);
-        Jump_To_Application = (pFunction)JumpAddress;
-        /* Initialize user application's Stack Pointer */
-        __set_MSP(*(__IO uint32_t *)bdinfo->fw_base);
-        Jump_To_Application();
-    } else {
-        DeviceState = failed_jump;
-        return;
-    }
-}
-uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count)
-{
-    uint32_t curr_step  = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
-    uint32_t pwm_duty   = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
-
-    uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
-
-    if (curr_sweep & 1) {
-        pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
-    }
-    return ((count % pwm_period) > pwm_duty) ? 1 : 0;
-}
-
-uint8_t processRX()
-{
-    if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
-        processComand(mReceive_Buffer);
-    }
-    return TRUE;
-}
-
-int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len)
-{
-    return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len);
-}

flight/targets/boards/coptercontrol/bootloader/pios_board.c

@@ -1,106 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_board.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board specific static initializers for hardware for the CopterControl board.
- * @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 <pios.h>
-#include <pios_board_info.h>
-
-/*
- * Pull in the board-specific static HW definitions.
- * Including .c files is a bit ugly but this allows all of
- * the HW definitions to be const and static to limit their
- * scope.
- *
- * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
- */
-#include "../board_hw_defs.c"
-
-uint32_t pios_com_telem_usb_id;
-
-/**
- * PIOS_Board_Init()
- * initializes all the core subsystems on this specific hardware
- * called from System/openpilot.c
- */
-static bool board_init_complete = false;
-void PIOS_Board_Init(void)
-{
-    if (board_init_complete) {
-        return;
-    }
-
-    /* Enable Prefetch Buffer */
-    FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
-
-    /* Flash 2 wait state */
-    FLASH_SetLatency(FLASH_Latency_2);
-
-    /* Delay system */
-    PIOS_DELAY_Init();
-
-    const struct pios_board_info *bdinfo = &pios_board_info_blob;
-
-#if defined(PIOS_INCLUDE_LED)
-    const struct pios_gpio_cfg *led_cfg  = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
-    PIOS_Assert(led_cfg);
-    PIOS_LED_Init(led_cfg);
-#endif /* PIOS_INCLUDE_LED */
-
-#if defined(PIOS_INCLUDE_USB)
-    /* Initialize board specific USB data */
-    PIOS_USB_BOARD_DATA_Init();
-
-    /* Activate the HID-only USB configuration */
-    PIOS_USB_DESC_HID_ONLY_Init();
-
-    uint32_t pios_usb_id;
-    switch (bdinfo->board_rev) {
-    case BOARD_REVISION_CC:
-        PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
-        break;
-    case BOARD_REVISION_CC3D:
-        PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
-        break;
-    default:
-        PIOS_Assert(0);
-    }
-#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
-    uint32_t pios_usb_hid_id;
-    if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
-        PIOS_Assert(0);
-    }
-    if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
-        PIOS_Assert(0);
-    }
-#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
-
-#endif /* PIOS_INCLUDE_USB */
-
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar
-
-    board_init_complete = true;
-}
-
-void PIOS_ADC_DMA_Handler()
-{}

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -1,149 +0,0 @@
-#
-# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
-# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
-#
-# 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 OPENPILOT_IS_COOL
-    $(error Top level Makefile must be used to build this target)
-endif
-
-include ../board-info.mk
-include $(ROOT_DIR)/make/firmware-defs.mk
-
-# ARM DSP library
-USE_DSP_LIB ?= NO
-
-# Set to YES to build a FW version that will erase data flash memory
-ERASE_FLASH ?= NO
-
-# Set to yes to include Gcsreceiver module
-GCSRECEIVER ?= NO
-
-# Enable Diag tasks ?
-DIAG_TASKS ?= NO
-
-# List of mandatory modules to include
-MODULES += Attitude
-MODULES += Stabilization
-MODULES += Actuator
-MODULES += Receiver
-MODULES += ManualControl
-MODULES += FirmwareIAP
-MODULES += Telemetry
-
-# List of optional modules to include
-OPTMODULES += CameraStab
-OPTMODULES += ComUsbBridge
-OPTMODULES += GPS
-OPTMODULES += TxPID
-OPTMODULES += Osd/osdoutput
-#OPTMODULES += Altitude
-#OPTMODULES += Fault
-
-SRC += $(FLIGHTLIB)/notification.c
-
-# Include all camera options
-CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
-
-
-
-# Erase flash firmware should be buildable from command line
-ifeq ($(ERASE_FLASH), YES)
-    CDEFS += -DERASE_FLASH
-endif
-
-# List C source files here (C dependencies are automatically generated).
-# Use file-extension c for "c-only"-files
-ifndef TESTAPP
-    ## The standard CMSIS startup
-    SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c
-
-    ## Application Core
-    SRC += ../pios_usb_board_data.c
-    SRC += $(OPMODULEDIR)/System/systemmod.c
-    SRC += $(OPSYSTEM)/coptercontrol.c
-    SRC += $(OPSYSTEM)/pios_board.c
-    SRC += $(FLIGHTLIB)/alarms.c
-    SRC += $(FLIGHTLIB)/instrumentation.c
-    SRC += $(OPUAVTALK)/uavtalk.c
-    SRC += $(OPUAVOBJ)/uavobjectmanager.c
-    SRC += $(OPUAVOBJ)/uavobjectpersistence.c
-    SRC += $(OPUAVOBJ)/eventdispatcher.c
-    
-    ## UAVObjects
-    SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
-    SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
-    SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c
-    SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
-    SRC += $(OPUAVSYNTHDIR)/faultsettings.c
-    SRC += $(OPUAVSYNTHDIR)/flightstatus.c
-    SRC += $(OPUAVSYNTHDIR)/systemstats.c
-    SRC += $(OPUAVSYNTHDIR)/systemalarms.c
-    SRC += $(OPUAVSYNTHDIR)/systemsettings.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c
-    SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
-    SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
-    SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
-    SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
-    SRC += $(OPUAVSYNTHDIR)/accelstate.c
-    SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c
-    SRC += $(OPUAVSYNTHDIR)/gyrostate.c
-    SRC += $(OPUAVSYNTHDIR)/attitudestate.c
-    SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
-    SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
-    SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
-    SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c
-    SRC += $(OPUAVSYNTHDIR)/mixersettings.c
-    SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
-    SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
-    SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
-    SRC += $(OPUAVSYNTHDIR)/cameradesired.c
-    SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c
-    SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
-    SRC += $(OPUAVSYNTHDIR)/gpssettings.c
-    SRC += $(OPUAVSYNTHDIR)/hwsettings.c
-    SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
-    SRC += $(OPUAVSYNTHDIR)/receiverstatus.c
-    SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
-    SRC += $(OPUAVSYNTHDIR)/ratedesired.c
-    SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
-    SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
-    # Command line option for Gcsreceiver module
-    ifeq ($(GCSRECEIVER), YES)
-        SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
-    endif
-    # Enable Diag tasks and UAVOs needed
-    ifeq ($(DIAG_TASKS), YES)
-        CDEFS += -DDIAG_TASKS
-        SRC += $(OPUAVSYNTHDIR)/taskinfo.c
-        SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
-        SRC += $(OPUAVSYNTHDIR)/perfcounter.c
-        SRC += $(OPUAVSYNTHDIR)/i2cstats.c
-    endif
-else
-    ## Test Code
-    SRC += $(OPTESTS)/test_common.c
-    SRC += $(OPTESTS)/$(TESTAPP).c
-endif
-
-include $(ROOT_DIR)/make/apps-defs.mk
-include $(ROOT_DIR)/make/common-defs.mk

flight/targets/boards/coptercontrol/firmware/coptercontrol.c

@@ -1,117 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @brief These files are the core system files of OpenPilot.
- * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
- * in the main() function of openpilot.c
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @brief This is where the OP firmware starts. Those files also define the compile-time
- * options of the firmware.
- * @{
- * @file       openpilot.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Sets up and runs main OpenPilot tasks.
- * @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 "inc/openpilot.h"
-#include <uavobjectsinit.h>
-#include <hwsettings.h>
-
-/* Task Priorities */
-#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
-
-/* Global Variables */
-
-/* Prototype of PIOS_Board_Init() function */
-extern void PIOS_Board_Init(void);
-extern void Stack_Change(void);
-
-/**
- * OpenPilot Main function:
- *
- * Initialize PiOS<BR>
- * Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
- * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
- * If something goes wrong, blink LED1 and LED2 every 100ms
- *
- */
-int main()
-{
-    /* NOTE: Do NOT modify the following start-up sequence */
-    /* Any new initialization functions should be added in OpenPilotInit() */
-
-    /* Brings up System using CMSIS functions, enables the LEDs. */
-    PIOS_SYS_Init();
-
-    /* Architecture dependant Hardware and
-     * core subsystem initialisation
-     * (see pios_board.c for your arch)
-     * */
-    PIOS_Board_Init();
-
-#ifdef ERASE_FLASH
-    PIOS_Flash_Jedec_EraseChip();
-#if defined(PIOS_LED_HEARTBEAT)
-    PIOS_LED_Off(PIOS_LED_HEARTBEAT);
-#endif /* PIOS_LED_HEARTBEAT */
-    while (1) {
-        ;
-    }
-#endif
-
-    /* Initialize modules */
-    MODULE_INITIALISE_ALL
-    /* swap the stack to use the IRQ stack */
-    Stack_Change();
-
-    /* Start the FreeRTOS scheduler, which should never return.
-     *
-     * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls
-     * (schedules) function files (modules). These functions never return from their
-     * while loops, which explains why each module has a while(1){} segment. Thus,
-     * the OpenPilot software actually starts at the vTaskStartScheduler() function,
-     * even though this is somewhat obscure.
-     *
-     * In addition, there are many main() functions in the OpenPilot firmware source tree
-     * This is because each main() refers to a separate hardware platform. Of course,
-     * C only allows one main(), so only the relevant main() function is compiled when
-     * making a specific firmware.
-     *
-     */
-    vTaskStartScheduler();
-
-    /* If all is well we will never reach here as the scheduler will now be running. */
-
-    /* Do some indication to user that something bad just happened */
-    while (1) {
-#if defined(PIOS_LED_HEARTBEAT)
-        PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
-#endif /* PIOS_LED_HEARTBEAT */
-        PIOS_DELAY_WaitmS(100);
-    }
-
-    return 0;
-}
-
-/**
- * @}
- * @}
- */

flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h

@@ -1,99 +0,0 @@
-#ifndef FREERTOS_CONFIG_H
-#define FREERTOS_CONFIG_H
-
-/*-----------------------------------------------------------
-* Application specific definitions.
-*
-* These definitions should be adjusted for your particular hardware and
-* application requirements.
-*
-* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
-* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
-*
-* See http://www.freertos.org/a00110.html.
-*----------------------------------------------------------*/
-
-/**
- * @addtogroup PIOS PIOS
- * @{
- * @addtogroup FreeRTOS FreeRTOS
- * @{
- */
-
-/* Notes: We use 5 task priorities */
-
-#define configUSE_PREEMPTION                         1
-#define configUSE_IDLE_HOOK                          1
-#define configUSE_TICK_HOOK                          0
-#define configUSE_MALLOC_FAILED_HOOK                 1
-#define configCPU_CLOCK_HZ                           ((unsigned long)72000000)
-#define configTICK_RATE_HZ                           ((portTickType)1000)
-#define configMAX_PRIORITIES                         ((unsigned portBASE_TYPE)5)
-#define configMINIMAL_STACK_SIZE                     ((unsigned short)48)
-#define configTOTAL_HEAP_SIZE                        ((size_t)(53 * 256))
-#define configMAX_TASK_NAME_LEN                      (6)
-#define configUSE_TRACE_FACILITY                     0
-#define configUSE_16_BIT_TICKS                       0
-#define configIDLE_SHOULD_YIELD                      0
-#define configUSE_MUTEXES                            1
-#define configUSE_RECURSIVE_MUTEXES                  1
-#define configUSE_COUNTING_SEMAPHORES                0
-#define configUSE_ALTERNATIVE_API                    0
-#define configQUEUE_REGISTRY_SIZE                    0
-
-/* Co-routine definitions. */
-#define configUSE_CO_ROUTINES                        0
-#define configMAX_CO_ROUTINE_PRIORITIES              (2)
-
-/* Set the following definitions to 1 to include the API function, or zero
-   to exclude the API function. */
-
-#define INCLUDE_vTaskPrioritySet                     1
-#define INCLUDE_uxTaskPriorityGet                    1
-#define INCLUDE_vTaskDelete                          1
-#define INCLUDE_vTaskCleanUpResources                0
-#define INCLUDE_vTaskSuspend                         1
-#define INCLUDE_vTaskDelayUntil                      1
-#define INCLUDE_vTaskDelay                           1
-#define INCLUDE_xTaskGetSchedulerState               0
-#define INCLUDE_xTaskGetCurrentTaskHandle            1
-#define INCLUDE_uxTaskGetStackHighWaterMark          1
-
-/* This is the raw value as per the Cortex-M3 NVIC.  Values can be 255
-   (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */
-#define configKERNEL_INTERRUPT_PRIORITY              15 << 4 /* equivalent to NVIC priority 15 */
-        #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority  3 */
-
-/* This is the value being used as per the ST library which permits 16
-   priority values, 0 to 15.  This must correspond to the
-   configKERNEL_INTERRUPT_PRIORITY setting.  Here 15 corresponds to the lowest
-   NVIC value of 255. */
-#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY      15
-
-#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
-#define CHECK_IRQ_STACK
-#endif
-
-/* Enable run time stats collection */
-#define configGENERATE_RUN_TIME_STATS  1
-#define INCLUDE_uxTaskGetRunTime       1
-#define INCLUDE_xTaskGetIdleTaskHandle 1
-#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \
-    do { \
-        (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \
-        (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \
-    } \
-    while (0)
-#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */
-
-#ifdef DIAG_TASKS
-#define configCHECK_FOR_STACK_OVERFLOW 2
-#else
-#define configCHECK_FOR_STACK_OVERFLOW 1
-#endif
-
-/**
- * @}
- */
-
-#endif /* FREERTOS_CONFIG_H */

flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h

@@ -1,81 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_board.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board hardware for the OpenPilot Version 1.1 hardware.
- * @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 PIOS_BOARD_H
-#define PIOS_BOARD_H
-
-
-// ------------------------
-// PIOS_LED
-// ------------------------
-// #define PIOS_LED_LED1_GPIO_PORT			GPIOC
-// #define PIOS_LED_LED1_GPIO_PIN			GPIO_Pin_12
-// #define PIOS_LED_LED1_GPIO_CLK			RCC_APB2Periph_GPIOC
-// #define PIOS_LED_LED2_GPIO_PORT			GPIOC
-// #define PIOS_LED_LED2_GPIO_PIN			GPIO_Pin_13
-// #define PIOS_LED_LED2_GPIO_CLK			RCC_APB2Periph_GPIOC
-#define PIOS_LED_NUM 2
-// #define PIOS_LED_PORTS				{ PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
-// #define PIOS_LED_PINS				{ PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
-// #define PIOS_LED_CLKS				{ PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
-
-
-// -------------------------
-// COM
-//
-// See also pios_board_posix.c
-// -------------------------
-// #define PIOS_USART_TX_BUFFER_SIZE		256
-#define PIOS_COM_BUFFER_SIZE    1024
-#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
-
-#define PIOS_COM_TELEM_RF       0
-#define PIOS_COM_GPS            1
-#define PIOS_COM_TELEM_USB      2
-
-#ifdef PIOS_ENABLE_AUX_UART
-#define PIOS_COM_AUX            3
-#define PIOS_COM_DEBUG          PIOS_COM_AUX
-#endif
-
-/**
- * glue macros for file IO
- * STM32 uses DOSFS for file IO
- */
-#define PIOS_FOPEN_READ(filename, file)                 (file = fopen((char *)filename, "r")) == NULL
-
-#define PIOS_FOPEN_WRITE(filename, file)                (file = fopen((char *)filename, "w")) == NULL
-
-#define PIOS_FREAD(file, bufferadr, length, resultadr)  (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length
-
-#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file)
-
-
-#define PIOS_FCLOSE(file)                               fclose(file)
-
-#define PIOS_FUNLINK(file)                              unlink((char *)filename)
-
-#endif /* PIOS_BOARD_H */

flight/targets/boards/coptercontrol/firmware/inc/pios_config.h

@@ -1,188 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @{
- * @file       pios_config.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
- * @brief      PiOS configuration header, the compile time config file for the PIOS.
- *             Defines which PiOS libraries and features are included in the firmware.
- * @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 PIOS_CONFIG_H
-#define PIOS_CONFIG_H
-
-/*
- * Below is a complete list of PIOS configurable options.
- * Please do not remove or rearrange them. Only comment out
- * unused options in the list. See main pios.h header for more
- * details.
- */
-
-/* #define PIOS_INCLUDE_DEBUG_CONSOLE */
-/* #define DEBUG_LEVEL 0 */
-/* #define PIOS_ENABLE_DEBUG_PINS */
-
-/* PIOS FreeRTOS support */
-#define PIOS_INCLUDE_FREERTOS
-
-
-/* PIOS CallbackScheduler support */
-#define PIOS_INCLUDE_CALLBACKSCHEDULER
-
-/* PIOS bootloader helper */
-#define PIOS_INCLUDE_BL_HELPER
-/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */
-
-/* PIOS system functions */
-#define PIOS_INCLUDE_DELAY
-#define PIOS_INCLUDE_INITCALL
-#define PIOS_INCLUDE_SYS
-#define PIOS_INCLUDE_TASK_MONITOR
-// #define PIOS_INCLUDE_INSTRUMENTATION
-#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5
-
-/* PIOS hardware peripherals */
-#define PIOS_INCLUDE_IRQ
-#define PIOS_INCLUDE_RTC
-#define PIOS_INCLUDE_TIM
-#define PIOS_INCLUDE_USART
-#define PIOS_INCLUDE_ADC
-/* #define PIOS_INCLUDE_I2C */
-#define PIOS_INCLUDE_SPI
-#define PIOS_INCLUDE_GPIO
-#define PIOS_INCLUDE_EXTI
-#define PIOS_INCLUDE_WDG
-
-/* PIOS USB functions */
-#define PIOS_INCLUDE_USB
-#define PIOS_INCLUDE_USB_HID
-#define PIOS_INCLUDE_USB_CDC
-#define PIOS_INCLUDE_USB_RCTX
-
-/* PIOS sensor interfaces */
-#define PIOS_INCLUDE_ADXL345
-/* #define PIOS_INCLUDE_BMA180 */
-/* #define PIOS_INCLUDE_L3GD20 */
-#define PIOS_INCLUDE_MPU6000
-#define PIOS_MPU6000_ACCEL
-/* #define PIOS_INCLUDE_HMC5843 */
-/* #define PIOS_INCLUDE_HMC5X83 */
-/* #define PIOS_HMC5883_HAS_GPIOS */
-/* #define PIOS_INCLUDE_BMP085 */
-/* #define PIOS_INCLUDE_MS5611 */
-/* #define PIOS_INCLUDE_MPXV */
-/* #define PIOS_INCLUDE_ETASV3 */
-/* #define PIOS_INCLUDE_HCSR04 */
-
-#define PIOS_SENSOR_RATE 500.0f
-
-/* PIOS receiver drivers */
-#define PIOS_INCLUDE_PWM
-#define PIOS_INCLUDE_PPM
-/* #define PIOS_INCLUDE_PPM_FLEXI */
-#define PIOS_INCLUDE_DSM
-#define PIOS_INCLUDE_SBUS
-/* #define PIOS_INCLUDE_GCSRCVR */
-/* #define PIOS_INCLUDE_OPLINKRCVR */
-
-/* PIOS abstract receiver interface */
-#define PIOS_INCLUDE_RCVR
-
-/* PIOS common peripherals */
-#define PIOS_INCLUDE_LED
-#define PIOS_INCLUDE_IAP
-#define PIOS_INCLUDE_SERVO
-/* #define PIOS_INCLUDE_I2C_ESC */
-/* #define PIOS_INCLUDE_OVERO */
-/* #define PIOS_OVERO_SPI */
-/* #define PIOS_INCLUDE_SDCARD */
-/* #define LOG_FILENAME "startup.log" */
-#define PIOS_INCLUDE_FLASH
-#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS
-/* #define FLASH_FREERTOS */
-/* #define PIOS_INCLUDE_FLASH_EEPROM */
-/* #define PIOS_INCLUDE_FLASH_INTERNAL */
-
-/* PIOS radio modules */
-/* #define PIOS_INCLUDE_RFM22B */
-/* #define PIOS_INCLUDE_RFM22B_COM */
-/* #define PIOS_INCLUDE_PPM_OUT */
-/* #define PIOS_RFM22B_DEBUG_ON_TELEM */
-
-/* PIOS misc peripherals */
-/* #define PIOS_INCLUDE_VIDEO */
-/* #define PIOS_INCLUDE_WAVE */
-/* #define PIOS_INCLUDE_UDP */
-
-/* PIOS abstract comms interface with options */
-#define PIOS_INCLUDE_COM
-/* #define PIOS_INCLUDE_COM_MSG */
-#define PIOS_INCLUDE_TELEMETRY_RF
-/* #define PIOS_INCLUDE_COM_TELEM */
-/* #define PIOS_INCLUDE_COM_FLEXI */
-/* #define PIOS_INCLUDE_COM_AUX */
-/* #define PIOS_TELEM_PRIORITY_QUEUE */
-#define PIOS_INCLUDE_GPS
-#define PIOS_GPS_MINIMAL
-/* #define PIOS_INCLUDE_GPS_NMEA_PARSER */
-#define PIOS_INCLUDE_GPS_UBX_PARSER
-/* #define PIOS_GPS_SETS_HOMELOCATION */
-
-/* Stabilization options */
-/* #define PIOS_QUATERNION_STABILIZATION */
-#define PIOS_EXCLUDE_ADVANCED_FEATURES
-/* Performance counters */
-#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD  1995998
-
-/* Alarm Thresholds */
-#define HEAP_LIMIT_WARNING              220
-#define HEAP_LIMIT_CRITICAL             40
-#define IRQSTACK_LIMIT_WARNING          100
-#define IRQSTACK_LIMIT_CRITICAL         60
-#define CPULOAD_LIMIT_WARNING           85
-#define CPULOAD_LIMIT_CRITICAL          95
-
-/* Task stack sizes */
-#define PIOS_ACTUATOR_STACK_SIZE        820
-#define PIOS_MANUAL_STACK_SIZE          735
-#define PIOS_RECEIVER_STACK_SIZE        620
-#define PIOS_STABILIZATION_STACK_SIZE   400
-
-#ifdef DIAG_TASKS
-#define PIOS_SYSTEM_STACK_SIZE          740
-#else
-#define PIOS_SYSTEM_STACK_SIZE          660
-#endif
-#define PIOS_TELEM_RX_STACK_SIZE        410
-#define PIOS_TELEM_TX_STACK_SIZE        560
-#define PIOS_EVENTDISPATCHER_STACK_SIZE 95
-
-/* This can't be too high to stop eventdispatcher thread overflowing */
-#define PIOS_EVENTDISAPTCHER_QUEUE      10
-
-/* Revolution series */
-/* #define REVOLUTION */
-
-#endif /* PIOS_CONFIG_H */
-/**
- * @}
- * @}
- */

flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h

@@ -1,47 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup PIOS_USB_BOARD Board specific USB definitions
- * @brief Board specific USB definitions
- * @{
- *
- * @file       pios_usb_board_data.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Board specific USB definitions
- * @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 PIOS_USB_BOARD_DATA_H
-#define PIOS_USB_BOARD_DATA_H
-
-// Note : changing below length will require changes to the USB buffer setup
-#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
-#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
-#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
-
-#define PIOS_USB_BOARD_EP_NUM          4
-
-#include <pios_usb_defs.h> /* USB_* macros */
-
-#define PIOS_USB_BOARD_PRODUCT_ID      USB_PRODUCT_ID_COPTERCONTROL
-#define PIOS_USB_BOARD_DEVICE_VER      USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW)
-#define PIOS_USB_BOARD_SN_SUFFIX       "+FW"
-
-#endif /* PIOS_USB_BOARD_DATA_H */

flight/targets/boards/coptercontrol/firmware/pios_board.c

@@ -1,920 +0,0 @@
-/**
- *****************************************************************************
- * @file       pios_board.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @author     PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
- * @addtogroup OpenPilotSystem OpenPilot System
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @{
- * @brief Defines board specific static initializers for hardware for the CopterControl board.
- *****************************************************************************/
-/*
- * 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 "inc/openpilot.h"
-#include <pios_board_info.h>
-#include <uavobjectsinit.h>
-#include <hwsettings.h>
-#include <manualcontrolsettings.h>
-#include <gcsreceiver.h>
-#include <taskinfo.h>
-#include <sanitycheck.h>
-#include <actuatorsettings.h>
-
-#ifdef PIOS_INCLUDE_INSTRUMENTATION
-#include <pios_instrumentation.h>
-#endif
-#if defined(PIOS_INCLUDE_ADXL345)
-#include <pios_adxl345.h>
-#endif
-
-/*
- * Pull in the board-specific static HW definitions.
- * Including .c files is a bit ugly but this allows all of
- * the HW definitions to be const and static to limit their
- * scope.
- *
- * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
- */
-#include "../board_hw_defs.c"
-
-/* One slot per selectable receiver group.
- *  eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS
- * NOTE: No slot in this map for NONE.
- */
-uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
-
-static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook();
-static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
-
-#define PIOS_COM_TELEM_RF_RX_BUF_LEN     32
-#define PIOS_COM_TELEM_RF_TX_BUF_LEN     12
-
-#define PIOS_COM_GPS_RX_BUF_LEN          32
-
-#define PIOS_COM_TELEM_USB_RX_BUF_LEN    65
-#define PIOS_COM_TELEM_USB_TX_BUF_LEN    65
-
-#define PIOS_COM_BRIDGE_RX_BUF_LEN       65
-#define PIOS_COM_BRIDGE_TX_BUF_LEN       12
-
-#define PIOS_COM_HKOSD_TX_BUF_LEN        22
-
-#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
-#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
-uint32_t pios_com_debug_id;
-#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
-
-uint32_t pios_com_telem_rf_id;
-uint32_t pios_com_telem_usb_id;
-uint32_t pios_com_vcp_id;
-uint32_t pios_com_gps_id;
-uint32_t pios_com_bridge_id;
-uint32_t pios_com_hkosd_id;
-
-uint32_t pios_usb_rctx_id;
-
-uintptr_t pios_uavo_settings_fs_id;
-uintptr_t pios_user_fs_id = 0;
-/**
- * Configuration for MPU6000 chip
- */
-#if defined(PIOS_INCLUDE_MPU6000)
-#include "pios_mpu6000.h"
-#include "pios_mpu6000_config.h"
-static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
-    .vector = PIOS_MPU6000_IRQHandler,
-    .line   = EXTI_Line3,
-    .pin    = {
-        .gpio = GPIOA,
-        .init = {
-            .GPIO_Pin   = GPIO_Pin_3,
-            .GPIO_Speed = GPIO_Speed_10MHz,
-            .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
-        },
-    },
-    .irq                                       = {
-        .init                                  = {
-            .NVIC_IRQChannel    = EXTI3_IRQn,
-            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
-            .NVIC_IRQChannelSubPriority        = 0,
-            .NVIC_IRQChannelCmd = ENABLE,
-        },
-    },
-    .exti                                      = {
-        .init                                  = {
-            .EXTI_Line    = EXTI_Line3, // matches above GPIO pin
-            .EXTI_Mode    = EXTI_Mode_Interrupt,
-            .EXTI_Trigger = EXTI_Trigger_Rising,
-            .EXTI_LineCmd = ENABLE,
-        },
-    },
-};
-
-static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
-    .exti_cfg   = &pios_exti_mpu6000_cfg,
-    .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
-    // Clock at 8 khz, downsampled by 8 for 1000 Hz
-    .Smpl_rate_div_no_dlp = 7,
-    // Clock at 1 khz, downsampled by 1 for 1000 Hz
-    .Smpl_rate_div_dlp    = 0,
-    .interrupt_cfg  = PIOS_MPU6000_INT_CLR_ANYRD,
-    .interrupt_en   = PIOS_MPU6000_INTEN_DATA_RDY,
-    .User_ctl             = PIOS_MPU6000_USERCTL_DIS_I2C,
-    .Pwr_mgmt_clk   = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
-    .accel_range    = PIOS_MPU6000_ACCEL_8G,
-    .gyro_range     = PIOS_MPU6000_SCALE_2000_DEG,
-    .filter               = PIOS_MPU6000_LOWPASS_256_HZ,
-    .orientation    = PIOS_MPU6000_TOP_180DEG,
-    .fast_prescaler = PIOS_SPI_PRESCALER_4,
-    .std_prescaler  = PIOS_SPI_PRESCALER_64,
-    .max_downsample = 2
-};
-#endif /* PIOS_INCLUDE_MPU6000 */
-
-/**
- * PIOS_Board_Init()
- * initializes all the core subsystems on this specific hardware
- * called from System/openpilot.c
- */
-int32_t init_test;
-void PIOS_Board_Init(void)
-{
-    /* Delay system */
-    PIOS_DELAY_Init();
-
-    const struct pios_board_info *bdinfo = &pios_board_info_blob;
-
-#if defined(PIOS_INCLUDE_LED)
-    const struct pios_gpio_cfg *led_cfg  = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
-    PIOS_Assert(led_cfg);
-    PIOS_LED_Init(led_cfg);
-#endif /* PIOS_INCLUDE_LED */
-
-#ifdef PIOS_INCLUDE_INSTRUMENTATION
-    PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS);
-#endif
-
-#if defined(PIOS_INCLUDE_SPI)
-    /* Set up the SPI interface to the serial flash */
-
-    switch (bdinfo->board_rev) {
-    case BOARD_REVISION_CC:
-        if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) {
-            PIOS_Assert(0);
-        }
-        break;
-    case BOARD_REVISION_CC3D:
-        if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) {
-            PIOS_Assert(0);
-        }
-        break;
-    default:
-        PIOS_Assert(0);
-    }
-
-#endif
-
-    uintptr_t flash_id;
-    switch (bdinfo->board_rev) {
-    case BOARD_REVISION_CC:
-        if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) {
-            PIOS_DEBUG_Assert(0);
-        }
-        if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) {
-            PIOS_DEBUG_Assert(0);
-        }
-        break;
-    case BOARD_REVISION_CC3D:
-        if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) {
-            PIOS_DEBUG_Assert(0);
-        }
-        if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) {
-            PIOS_DEBUG_Assert(0);
-        }
-        break;
-    default:
-        PIOS_DEBUG_Assert(0);
-    }
-
-    /* Initialize the task monitor */
-    if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
-        PIOS_Assert(0);
-    }
-
-    /* Initialize the delayed callback library */
-    PIOS_CALLBACKSCHEDULER_Initialize();
-
-    /* Initialize UAVObject libraries */
-    EventDispatcherInitialize();
-    UAVObjInitialize();
-
-#if defined(PIOS_INCLUDE_RTC)
-    /* Initialize the real-time clock and its associated tick */
-    PIOS_RTC_Init(&pios_rtc_main_cfg);
-#endif
-    PIOS_IAP_Init();
-    // check for safe mode commands from gcs
-    if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 &&
-        PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 &&
-        PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) {
-        PIOS_FLASHFS_Format(pios_uavo_settings_fs_id);
-        PIOS_IAP_WriteBootCmd(0, 0);
-        PIOS_IAP_WriteBootCmd(1, 0);
-        PIOS_IAP_WriteBootCmd(2, 0);
-    }
-
-    HwSettingsInitialize();
-
-#ifndef ERASE_FLASH
-#ifdef PIOS_INCLUDE_WDG
-    /* Initialize watchdog as early as possible to catch faults during init */
-    PIOS_WDG_Init();
-#endif
-#endif
-
-    /* Initialize the alarms library */
-    AlarmsInitialize();
-
-    /* Check for repeated boot failures */
-    uint16_t boot_count = PIOS_IAP_ReadBootCount();
-    if (boot_count < 3) {
-        PIOS_IAP_WriteBootCount(++boot_count);
-        AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT);
-    } else {
-        /* Too many failed boot attempts, force hwsettings to defaults */
-        HwSettingsSetDefaults(HwSettingsHandle(), 0);
-        AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
-    }
-
-    /* Set up pulse timers */
-    PIOS_TIM_InitClock(&tim_1_cfg);
-    PIOS_TIM_InitClock(&tim_2_cfg);
-    PIOS_TIM_InitClock(&tim_3_cfg);
-    PIOS_TIM_InitClock(&tim_4_cfg);
-
-#if defined(PIOS_INCLUDE_USB)
-    /* Initialize board specific USB data */
-    PIOS_USB_BOARD_DATA_Init();
-
-
-    /* Flags to determine if various USB interfaces are advertised */
-    bool usb_hid_present = false;
-    bool usb_cdc_present = false;
-
-#if defined(PIOS_INCLUDE_USB_CDC)
-    if (PIOS_USB_DESC_HID_CDC_Init()) {
-        PIOS_Assert(0);
-    }
-    usb_hid_present = true;
-    usb_cdc_present = true;
-#else
-    if (PIOS_USB_DESC_HID_ONLY_Init()) {
-        PIOS_Assert(0);
-    }
-    usb_hid_present = true;
-#endif
-
-    uint32_t pios_usb_id;
-
-    switch (bdinfo->board_rev) {
-    case BOARD_REVISION_CC:
-        PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
-        break;
-    case BOARD_REVISION_CC3D:
-        PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
-        break;
-    default:
-        PIOS_Assert(0);
-    }
-
-#if defined(PIOS_INCLUDE_USB_CDC)
-
-    uint8_t hwsettings_usb_vcpport;
-    /* Configure the USB VCP port */
-    HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport);
-
-    if (!usb_cdc_present) {
-        /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */
-        hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED;
-    }
-
-    switch (hwsettings_usb_vcpport) {
-    case HWSETTINGS_USB_VCPPORT_DISABLED:
-        break;
-    case HWSETTINGS_USB_VCPPORT_USBTELEMETRY:
-#if defined(PIOS_INCLUDE_COM)
-        {
-            uint32_t pios_usb_cdc_id;
-            if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
-                PIOS_Assert(0);
-            }
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
-                              rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
-                              tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_COM */
-        break;
-    case HWSETTINGS_USB_VCPPORT_COMBRIDGE:
-#if defined(PIOS_INCLUDE_COM)
-        {
-            uint32_t pios_usb_cdc_id;
-            if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
-                PIOS_Assert(0);
-            }
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
-                              rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
-                              tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_COM */
-        break;
-    case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE:
-#if defined(PIOS_INCLUDE_COM)
-#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
-        {
-            uint32_t pios_usb_cdc_id;
-            if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
-                PIOS_Assert(0);
-            }
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
-                              NULL, 0,
-                              tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
-#endif /* PIOS_INCLUDE_COM */
-        break;
-    }
-#endif /* PIOS_INCLUDE_USB_CDC */
-
-#if defined(PIOS_INCLUDE_USB_HID)
-    /* Configure the usb HID port */
-    uint8_t hwsettings_usb_hidport;
-    HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport);
-
-    if (!usb_hid_present) {
-        /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */
-        hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED;
-    }
-
-    switch (hwsettings_usb_hidport) {
-    case HWSETTINGS_USB_HIDPORT_DISABLED:
-        break;
-    case HWSETTINGS_USB_HIDPORT_USBTELEMETRY:
-#if defined(PIOS_INCLUDE_COM)
-        {
-            uint32_t pios_usb_hid_id;
-            if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
-                PIOS_Assert(0);
-            }
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id,
-                              rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
-                              tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_COM */
-        break;
-    case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER:
-#if defined(PIOS_INCLUDE_USB_RCTX)
-        {
-            if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_USB_RCTX */
-        break;
-    }
-
-#endif /* PIOS_INCLUDE_USB_HID */
-
-#endif /* PIOS_INCLUDE_USB */
-
-    /* Configure the main IO port */
-    uint8_t hwsettings_DSMxBind;
-    HwSettingsDSMxBindGet(&hwsettings_DSMxBind);
-    uint8_t hwsettings_cc_mainport;
-    HwSettingsCC_MainPortGet(&hwsettings_cc_mainport);
-
-    switch (hwsettings_cc_mainport) {
-    case HWSETTINGS_CC_MAINPORT_DISABLED:
-        break;
-    case HWSETTINGS_CC_MAINPORT_TELEMETRY:
-#if defined(PIOS_INCLUDE_TELEMETRY_RF)
-        {
-            uint32_t pios_usart_generic_id;
-            if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id,
-                              rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
-                              tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_TELEMETRY_RF */
-        break;
-    case HWSETTINGS_CC_MAINPORT_SBUS:
-#if defined(PIOS_INCLUDE_SBUS)
-        {
-            uint32_t pios_usart_sbus_id;
-            if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint32_t pios_sbus_id;
-            if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) {
-                PIOS_Assert(0);
-            }
-
-            uint32_t pios_sbus_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_SBUS */
-        break;
-    case HWSETTINGS_CC_MAINPORT_GPS:
-#if defined(PIOS_INCLUDE_GPS)
-        {
-            uint32_t pios_usart_generic_id;
-            if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id,
-                              rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
-                              NULL, 0)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_GPS */
-        break;
-    case HWSETTINGS_CC_MAINPORT_DSM:
-#if defined(PIOS_INCLUDE_DSM)
-        {
-            uint32_t pios_usart_dsm_id;
-            if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint32_t pios_dsm_id;
-            if (PIOS_DSM_Init(&pios_dsm_id,
-                              &pios_dsm_main_cfg,
-                              &pios_usart_com_driver,
-                              pios_usart_dsm_id,
-                              0)) {
-                PIOS_Assert(0);
-            }
-
-            uint32_t pios_dsm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_DSM */
-        break;
-    case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE:
-#if defined(PIOS_INCLUDE_COM)
-#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
-        {
-            uint32_t pios_usart_generic_id;
-            if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id,
-                              NULL, 0,
-                              tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
-#endif /* PIOS_INCLUDE_COM */
-        break;
-    case HWSETTINGS_CC_MAINPORT_COMBRIDGE:
-    {
-        uint32_t pios_usart_generic_id;
-        if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) {
-            PIOS_Assert(0);
-        }
-
-        uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
-        PIOS_Assert(rx_buffer);
-        uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
-        PIOS_Assert(tx_buffer);
-        if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id,
-                          rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
-                          tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
-            PIOS_Assert(0);
-        }
-    }
-    break;
-    case HWSETTINGS_CC_MAINPORT_OSDHK:
-    {
-        uint32_t pios_usart_hkosd_id;
-        if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) {
-            PIOS_Assert(0);
-        }
-
-        uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_HKOSD_TX_BUF_LEN);
-        PIOS_Assert(tx_buffer);
-        if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id,
-                          NULL, 0,
-                          tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) {
-            PIOS_Assert(0);
-        }
-    }
-    break;
-    }
-
-    /* Configure the flexi port */
-    uint8_t hwsettings_cc_flexiport;
-    HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport);
-
-    switch (hwsettings_cc_flexiport) {
-    case HWSETTINGS_CC_FLEXIPORT_DISABLED:
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_TELEMETRY:
-#if defined(PIOS_INCLUDE_TELEMETRY_RF)
-        {
-            uint32_t pios_usart_generic_id;
-            if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
-                PIOS_Assert(0);
-            }
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id,
-                              rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
-                              tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_TELEMETRY_RF */
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE:
-    {
-        uint32_t pios_usart_generic_id;
-        if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
-            PIOS_Assert(0);
-        }
-
-        uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
-        uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
-        PIOS_Assert(rx_buffer);
-        PIOS_Assert(tx_buffer);
-        if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id,
-                          rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
-                          tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
-            PIOS_Assert(0);
-        }
-    }
-    break;
-    case HWSETTINGS_CC_FLEXIPORT_GPS:
-#if defined(PIOS_INCLUDE_GPS)
-        {
-            uint32_t pios_usart_generic_id;
-            if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
-                PIOS_Assert(0);
-            }
-            uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN);
-            PIOS_Assert(rx_buffer);
-            if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id,
-                              rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
-                              NULL, 0)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_GPS */
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_PPM:
-#if defined(PIOS_INCLUDE_PPM_FLEXI)
-        {
-            uint32_t pios_ppm_id;
-            PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg);
-
-            uint32_t pios_ppm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_PPM_FLEXI */
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_DSM:
-#if defined(PIOS_INCLUDE_DSM)
-        {
-            uint32_t pios_usart_dsm_id;
-            if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint32_t pios_dsm_id;
-            if (PIOS_DSM_Init(&pios_dsm_id,
-                              &pios_dsm_flexi_cfg,
-                              &pios_usart_com_driver,
-                              pios_usart_dsm_id,
-                              hwsettings_DSMxBind)) {
-                PIOS_Assert(0);
-            }
-
-            uint32_t pios_dsm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_DSM */
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE:
-#if defined(PIOS_INCLUDE_COM)
-#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
-        {
-            uint32_t pios_usart_generic_id;
-            if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) {
-                PIOS_Assert(0);
-            }
-
-            uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN);
-            PIOS_Assert(tx_buffer);
-            if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id,
-                              NULL, 0,
-                              tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
-#endif /* PIOS_INCLUDE_COM */
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_I2C:
-#if defined(PIOS_INCLUDE_I2C)
-        {
-            if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) {
-                PIOS_Assert(0);
-            }
-        }
-#endif /* PIOS_INCLUDE_I2C */
-        break;
-    case HWSETTINGS_CC_FLEXIPORT_OSDHK:
-    {
-        uint32_t pios_usart_hkosd_id;
-        if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) {
-            PIOS_Assert(0);
-        }
-
-        uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_HKOSD_TX_BUF_LEN);
-        PIOS_Assert(tx_buffer);
-        if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id,
-                          NULL, 0,
-                          tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) {
-            PIOS_Assert(0);
-        }
-    }
-    break;
-    }
-
-    /* Configure the rcvr port */
-    uint8_t hwsettings_rcvrport;
-    HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
-
-    switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {
-    case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
-#if defined(PIOS_INCLUDE_HCSR04)
-        {
-            uint32_t pios_hcsr04_id;
-            PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg);
-        }
-#endif
-        break;
-    case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
-#if defined(PIOS_INCLUDE_PWM)
-        {
-            uint32_t pios_pwm_id;
-            PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
-
-            uint32_t pios_pwm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_PWM */
-        break;
-    case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
-#if defined(PIOS_INCLUDE_PPM)
-        {
-            uint32_t pios_ppm_id;
-            if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) {
-                PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg);
-            } else {
-                PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
-            }
-
-            uint32_t pios_ppm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_PPM */
-        break;
-    case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
-        /* This is a combination of PPM and PWM inputs */
-#if defined(PIOS_INCLUDE_PPM)
-        {
-            uint32_t pios_ppm_id;
-            PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
-
-            uint32_t pios_ppm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_PPM */
-#if defined(PIOS_INCLUDE_PWM)
-        {
-            uint32_t pios_pwm_id;
-            PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg);
-
-            uint32_t pios_pwm_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
-        }
-#endif /* PIOS_INCLUDE_PWM */
-        break;
-    case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
-        break;
-    }
-
-#if defined(PIOS_INCLUDE_GCSRCVR)
-    GCSReceiverInitialize();
-    uint32_t pios_gcsrcvr_id;
-    PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
-    uint32_t pios_gcsrcvr_rcvr_id;
-    if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
-        PIOS_Assert(0);
-    }
-    pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
-#endif /* PIOS_INCLUDE_GCSRCVR */
-
-    /* Remap AFIO pin for PB4 (Servo 5 Out)*/
-    GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE);
-
-#ifndef PIOS_ENABLE_DEBUG_PINS
-    switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {
-    case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
-        PIOS_Servo_Init(&pios_servo_cfg);
-        break;
-    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
-        PIOS_Servo_Init(&pios_servo_rcvr_cfg);
-        break;
-    }
-#else
-    PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins));
-#endif /* PIOS_ENABLE_DEBUG_PINS */
-
-    switch (bdinfo->board_rev) {
-    case BOARD_REVISION_CC:
-        // Revision 1 with invensense gyros, start the ADC
-#if defined(PIOS_INCLUDE_ADC)
-        PIOS_ADC_Init(&pios_adc_cfg);
-#endif
-#if defined(PIOS_INCLUDE_ADXL345)
-        PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0);
-#endif
-        break;
-    case BOARD_REVISION_CC3D:
-        // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it
-        GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
-
-#if defined(PIOS_INCLUDE_MPU6000)
-        // Set up the SPI interface to the serial flash
-        if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
-            PIOS_Assert(0);
-        }
-        PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
-        PIOS_MPU6000_CONFIG_Configure();
-        init_test = !PIOS_MPU6000_Driver.test(0);
-#endif /* PIOS_INCLUDE_MPU6000 */
-
-        break;
-    default:
-        PIOS_Assert(0);
-    }
-
-    /* Make sure we have at least one telemetry link configured or else fail initialization */
-    PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id);
-
-    // Attach the board config check hook
-    SANITYCHECK_AttachHook(&CopterControlConfigHook);
-    // trigger a config check if actuatorsettings are updated
-    ActuatorSettingsInitialize();
-    ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb);
-}
-
-SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook()
-{
-    // inhibit usage of oneshot for non supported RECEIVER port modes
-    uint8_t recmode;
-
-    HwSettingsCC_RcvrPortGet(&recmode);
-    uint8_t flexiMode;
-    uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM];
-    ActuatorSettingsBankModeGet(modes);
-    HwSettingsCC_FlexiPortGet(&flexiMode);
-
-    switch ((HwSettingsCC_RcvrPortOptions)recmode) {
-    // Those modes allows oneshot usage
-    case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT:
-        if ((recmode == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT ||
-             flexiMode == HWSETTINGS_CC_FLEXIPORT_PPM) &&
-            (modes[3] == ACTUATORSETTINGS_BANKMODE_PWMSYNC ||
-             modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT125)) {
-            return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;
-        } else {
-            return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
-        }
-
-    // inhibit oneshot for the following modes
-    case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT:
-    case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT:
-        for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
-            if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC ||
-                modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) {
-                return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;;
-            }
-
-            return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
-        }
-    }
-    return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;;
-}
-// trigger a configuration check if ActuatorSettings are changed.
-void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
-{
-    configuration_check();
-}
-
-/**
- * @}
- */

flight/targets/boards/coptercontrol/firmware/pios_board_posix.c

@@ -1,145 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_board.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board specific static initializers for hardware for the OpenPilot board.
- * @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 "inc/openpilot.h"
-#include <pios_udp_priv.h>
-#include <pios_com_priv.h>
-#include <uavobjectsinit.h>
-
-/**
- * PIOS_Board_Init()
- * initializes all the core systems on this specific hardware
- * called from System/openpilot.c
- */
-void PIOS_Board_Init(void)
-{
-    /* Delay system */
-    PIOS_DELAY_Init();
-
-    /* Initialize the delayed callback library */
-    PIOS_CALLBACKSCHEDULER_Initialize();
-
-    /* Initialize UAVObject libraries */
-    EventDispatcherInitialize();
-    UAVObjInitialize();
-
-    /* Initialize the alarms library */
-    AlarmsInitialize();
-
-    /* Initialize the task monitor */
-    if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
-        PIOS_Assert(0);
-    }
-
-    /* Initialize the PiOS library */
-    PIOS_COM_Init();
-}
-
-
-const struct pios_udp_cfg pios_udp0_cfg = {
-    .ip   = "0.0.0.0",
-    .port = 9000,
-};
-const struct pios_udp_cfg pios_udp1_cfg = {
-    .ip   = "0.0.0.0",
-    .port = 9001,
-};
-const struct pios_udp_cfg pios_udp2_cfg = {
-    .ip   = "0.0.0.0",
-    .port = 9002,
-};
-
-#ifdef PIOS_COM_AUX
-/*
- * AUX USART
- */
-const struct pios_udp_cfg pios_udp3_cfg = {
-    .ip   = "0.0.0.0",
-    .port = 9003,
-};
-#endif
-
-/*
- * Board specific number of devices.
- */
-struct pios_udp_dev pios_udp_devs[] = {
-#define PIOS_UDP_TELEM 0
-    {
-        .cfg = &pios_udp0_cfg,
-    },
-#define PIOS_UDP_GPS   1
-    {
-        .cfg = &pios_udp1_cfg,
-    },
-#define PIOS_UDP_LOCAL 2
-    {
-        .cfg = &pios_udp2_cfg,
-    },
-#ifdef PIOS_COM_AUX
-#define PIOS_UDP_AUX   3
-    {
-        .cfg = &pios_udp3_cfg,
-    },
-#endif
-};
-
-uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
-
-/*
- * COM devices
- */
-
-/*
- * Board specific number of devices.
- */
-extern const struct pios_com_driver pios_serial_com_driver;
-extern const struct pios_com_driver pios_udp_com_driver;
-
-struct pios_com_dev pios_com_devs[] = {
-    {
-        .id     = PIOS_UDP_TELEM,
-        .driver = &pios_udp_com_driver,
-    },
-    {
-        .id     = PIOS_UDP_GPS,
-        .driver = &pios_udp_com_driver,
-    },
-    {
-        .id     = PIOS_UDP_LOCAL,
-        .driver = &pios_udp_com_driver,
-    },
-#ifdef PIOS_COM_AUX
-    {
-        .id     = PIOS_UDP_AUX,
-        .driver = &pios_udp_com_driver,
-    },
-#endif
-};
-
-const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
-
-/**
- * @}
- */

flight/targets/boards/coptercontrol/pios_board.h

@@ -1,279 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       pios_board.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Defines board hardware for the OpenPilot Version 1.1 hardware.
- * @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 PIOS_BOARD_H
-#define PIOS_BOARD_H
-// ------------------------
-// Timers and Channels Used
-// ------------------------
-/*
-   Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
-   ------+-----------+-----------+-----------+----------
-   TIM1  |  Servo 4  |           |           |
-   TIM2  |  RC In 5  |  RC In 6  |  Servo 6  |
-   TIM3  |  Servo 5  |  RC In 2  |  RC In 3  |  RC In 4
-   TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
-   ------+-----------+-----------+-----------+----------
- */
-
-// ------------------------
-// DMA Channels Used
-// ------------------------
-/* Channel 1  -                                 */
-/* Channel 2  -                                 */
-/* Channel 3  -                                 */
-/* Channel 4  -                                 */
-/* Channel 5  -                                 */
-/* Channel 6  -                                 */
-/* Channel 7  -                                 */
-/* Channel 8  -                                 */
-/* Channel 9  -                                 */
-/* Channel 10 -                                 */
-/* Channel 11 -                                 */
-/* Channel 12 -                                 */
-
-// ------------------------
-// BOOTLOADER_SETTINGS
-// ------------------------
-#define BOARD_READABLE           TRUE
-#define BOARD_WRITABLE           TRUE
-#define MAX_DEL_RETRYS           3
-
-// ------------------------
-// WATCHDOG_SETTINGS
-// ------------------------
-#define PIOS_WATCHDOG_TIMEOUT    250
-#define PIOS_WDG_REGISTER        BKP_DR4
-#define PIOS_WDG_ACTUATOR        0x0001
-#define PIOS_WDG_STABILIZATION   0x0002
-#define PIOS_WDG_ATTITUDE        0x0004
-#define PIOS_WDG_MANUAL          0x0008
-#define PIOS_WDG_AUTOTUNE        0x0010
-
-// ------------------------
-// TELEMETRY
-// ------------------------
-#define TELEM_QUEUE_SIZE         10
-
-// ------------------------
-// PIOS_LED
-// ------------------------
-#define PIOS_LED_HEARTBEAT       0
-
-// -------------------------
-// System Settings
-// -------------------------
-#define PIOS_MASTER_CLOCK        72000000
-#define PIOS_PERIPHERAL_CLOCK    (PIOS_MASTER_CLOCK / 2)
-
-// -------------------------
-// Interrupt Priorities
-// -------------------------
-#define PIOS_IRQ_PRIO_LOW        12              // lower than RTOS
-#define PIOS_IRQ_PRIO_MID        8               // higher than RTOS
-#define PIOS_IRQ_PRIO_HIGH       5               // for SPI, ADC, I2C etc...
-#define PIOS_IRQ_PRIO_HIGHEST    4               // for USART etc...
-// ------------------------
-// PIOS_I2C
-// See also pios_board.c
-// ------------------------
-#define PIOS_I2C_MAX_DEVS        1
-extern uint32_t pios_i2c_flexi_adapter_id;
-#define PIOS_I2C_MAIN_ADAPTER    (pios_i2c_flexi_adapter_id)
-#define PIOS_I2C_ESC_ADAPTER     (pios_i2c_flexi_adapter_id)
-#define PIOS_I2C_BMP085_ADAPTER  (pios_i2c_flexi_adapter_id)
-
-// ------------------------
-// PIOS_BMP085
-// ------------------------
-#define PIOS_BMP085_OVERSAMPLING 3
-
-// -------------------------
-// SPI
-//
-// See also pios_board.c
-// -------------------------
-#define PIOS_SPI_MAX_DEVS   2
-
-// -------------------------
-// PIOS_USART
-// -------------------------
-#define PIOS_USART_MAX_DEVS 2
-
-// -------------------------
-// PIOS_COM
-//
-// See also pios_board.c
-// -------------------------
-#define PIOS_COM_MAX_DEVS  3
-
-extern uint32_t pios_com_telem_rf_id;
-#define PIOS_COM_TELEM_RF  (pios_com_telem_rf_id)
-
-#if defined(PIOS_INCLUDE_GPS)
-extern uint32_t pios_com_gps_id;
-#define PIOS_COM_GPS       (pios_com_gps_id)
-#endif /* PIOS_INCLUDE_GPS */
-
-extern uint32_t pios_com_bridge_id;
-#define PIOS_COM_BRIDGE    (pios_com_bridge_id)
-
-extern uint32_t pios_com_vcp_id;
-#define PIOS_COM_VCP       (pios_com_vcp_id)
-
-extern uint32_t pios_com_telem_usb_id;
-#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
-
-#if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
-extern uint32_t pios_com_debug_id;
-#define PIOS_COM_DEBUG     (pios_com_debug_id)
-#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
-
-extern uint32_t pios_com_hkosd_id;
-#define PIOS_COM_OSDHK     (pios_com_hkosd_id)
-
-// -------------------------
-// ADC
-// PIOS_ADC_PinGet(0) = Gyro Z
-// PIOS_ADC_PinGet(1) = Gyro Y
-// PIOS_ADC_PinGet(2) = Gyro X
-// -------------------------
-// #define PIOS_ADC_OVERSAMPLING_RATE		1
-#define PIOS_ADC_USE_TEMP_SENSOR         1
-#define PIOS_ADC_TEMP_SENSOR_ADC         ADC1
-#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1
-
-#define PIOS_ADC_PIN1_GPIO_PORT          GPIOA                   // PA4 (Gyro X)
-#define PIOS_ADC_PIN1_GPIO_PIN           GPIO_Pin_4              // ADC12_IN4
-#define PIOS_ADC_PIN1_GPIO_CHANNEL       ADC_Channel_4
-#define PIOS_ADC_PIN1_ADC                ADC2
-#define PIOS_ADC_PIN1_ADC_NUMBER         1
-
-#define PIOS_ADC_PIN2_GPIO_PORT          GPIOA                   // PA5 (Gyro Y)
-#define PIOS_ADC_PIN2_GPIO_PIN           GPIO_Pin_5              // ADC123_IN5
-#define PIOS_ADC_PIN2_GPIO_CHANNEL       ADC_Channel_5
-#define PIOS_ADC_PIN2_ADC                ADC1
-#define PIOS_ADC_PIN2_ADC_NUMBER         2
-
-#define PIOS_ADC_PIN3_GPIO_PORT          GPIOA                   // PA3 (Gyro Z)
-#define PIOS_ADC_PIN3_GPIO_PIN           GPIO_Pin_3              // ADC12_IN3
-#define PIOS_ADC_PIN3_GPIO_CHANNEL       ADC_Channel_3
-#define PIOS_ADC_PIN3_ADC                ADC2
-#define PIOS_ADC_PIN3_ADC_NUMBER         2
-
-#define PIOS_ADC_NUM_PINS                3
-
-#define PIOS_ADC_PORTS                   { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT }
-#define PIOS_ADC_PINS                    { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN }
-#define PIOS_ADC_CHANNELS                { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL }
-#define PIOS_ADC_MAPPING                 { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC }
-#define PIOS_ADC_CHANNEL_MAPPING         { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER }
-#define PIOS_ADC_NUM_CHANNELS            (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR)
-#define PIOS_ADC_NUM_ADC_CHANNELS        2
-#define PIOS_ADC_USE_ADC2                1
-#define PIOS_ADC_CLOCK_FUNCTION          RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE)
-#define PIOS_ADC_ADCCLK                  RCC_PCLK2_Div8
-/* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */
-/* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */
-/* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */
-/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */
-#define PIOS_ADC_SAMPLE_TIME             ADC_SampleTime_239Cycles5
-/* Sample time: */
-/* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */
-/* Tconv = 239.5 + 12.5 = 252 cycles = 18<31>s */
-/* (1 / (ADCCLK / CYCLES)) = Sample Time (<28>S) */
-#define PIOS_ADC_IRQ_PRIO                PIOS_IRQ_PRIO_LOW
-
-// Currently analog acquistion hard coded at 480 Hz
-// PCKL2 = HCLK / 16
-// ADCCLK = PCLK2 / 2
-#define PIOS_ADC_RATE                    (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2))
-#define PIOS_ADC_MAX_OVERSAMPLING        48
-
-#define PIOS_ADC_TEMPERATURE_PIN         0
-
-// ------------------------
-// PIOS_RCVR
-// See also pios_board.c
-// ------------------------
-#define PIOS_RCVR_MAX_DEVS           3
-#define PIOS_RCVR_MAX_CHANNELS       12
-#define PIOS_GCSRCVR_TIMEOUT_MS      100
-
-// -------------------------
-// Receiver PPM input
-// -------------------------
-#define PIOS_PPM_MAX_DEVS            1
-#define PIOS_PPM_NUM_INPUTS          12
-
-// -------------------------
-// Receiver PWM input
-// -------------------------
-#define PIOS_PWM_MAX_DEVS            1
-#define PIOS_PWM_NUM_INPUTS          6
-
-// -------------------------
-// Receiver DSM input
-// -------------------------
-#define PIOS_DSM_MAX_DEVS            2
-#define PIOS_DSM_NUM_INPUTS          12
-
-// -------------------------
-// Receiver S.Bus input
-// -------------------------
-#define PIOS_SBUS_MAX_DEVS           1
-#define PIOS_SBUS_NUM_INPUTS         (16 + 2)
-
-// -------------------------
-// Servo outputs
-// -------------------------
-#define PIOS_SERVO_UPDATE_HZ         50
-#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
-#define PIOS_SERVO_BANKS             6
-
-// --------------------------
-// Timer controller settings
-// --------------------------
-#define PIOS_TIM_MAX_DEVS            3
-
-// -------------------------
-// GPIO
-// -------------------------
-#define PIOS_GPIO_PORTS              {}
-#define PIOS_GPIO_PINS               {}
-#define PIOS_GPIO_CLKS               {}
-#define PIOS_GPIO_NUM                0
-
-// -------------------------
-// USB
-// -------------------------
-#define PIOS_USB_HID_MAX_DEVS        1
-
-#define PIOS_USB_ENABLED             1
-#define PIOS_USB_DETECT_GPIO_PORT    GPIOC
-#define PIOS_USB_MAX_DEVS            1
-#define PIOS_USB_DETECT_GPIO_PIN     GPIO_Pin_15
-
-#endif /* PIOS_BOARD_H */

flight/targets/boards/coptercontrol/pios_usb_board_data.c

@@ -1,102 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup PIOS_USB_BOARD Board specific USB definitions
- * @brief Board specific USB definitions
- * @{
- *
- * @file       pios_usb_board_data.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Board specific USB definitions
- * @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 "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */
-#include <pios_sys.h> /* PIOS_SYS_SerialNumberGet */
-#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
-#include <pios_usb_util.h> /* PIOS_USB_UTIL_AsciiToUtf8 */
-
-static const uint8_t usb_product_id[28] = {
-    sizeof(usb_product_id),
-    USB_DESC_TYPE_STRING,
-    'C',                   0,
-    'o',                   0,
-    'p',                   0,
-    't',                   0,
-    'e',                   0,
-    'r',                   0,
-    'C',                   0,
-    'o',                   0,
-    'n',                   0,
-    't',                   0,
-    'r',                   0,
-    'o',                   0,
-    'l',                   0,
-};
-
-static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = {
-    sizeof(usb_serial_number),
-    USB_DESC_TYPE_STRING,
-};
-
-static const struct usb_string_langid usb_lang_id = {
-    .bLength = sizeof(usb_lang_id),
-    .bDescriptorType = USB_DESC_TYPE_STRING,
-    .bLangID = htousbs(USB_LANGID_ENGLISH_US),
-};
-
-static const uint8_t usb_vendor_id[28] = {
-    sizeof(usb_vendor_id),
-    USB_DESC_TYPE_STRING,
-    'o',                  0,
-    'p',                  0,
-    'e',                  0,
-    'n',                  0,
-    'p',                  0,
-    'i',                  0,
-    'l',                  0,
-    'o',                  0,
-    't',                  0,
-    '.',                  0,
-    'o',                  0,
-    'r',                  0,
-    'g',                  0
-};
-
-int32_t PIOS_USB_BOARD_DATA_Init(void)
-{
-    /* Load device serial number into serial number string */
-    uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1];
-
-    PIOS_SYS_SerialNumberGet((char *)sn);
-
-    /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */
-    uint8_t *utf8 = &(usb_serial_number[2]);
-    utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN);
-    utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1);
-
-    PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id));
-    PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number));
-
-    PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id));
-    PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id));
-
-    return 0;
-}

flight/targets/boards/discoveryf4bare/firmware/Makefile

@@ -24,6 +24,9 @@ endif
 include ../board-info.mk
 include $(ROOT_DIR)/make/firmware-defs.mk
 
+# REVO C++ support 
+USE_CXX = YES
+
 # ARM DSP library
 USE_DSP_LIB ?= NO
 
@@ -70,7 +73,7 @@ ifndef TESTAPP
     ## Application Core
     SRC += ../pios_usb_board_data.c
     SRC += $(OPMODULEDIR)/System/systemmod.c
-    SRC += $(OPSYSTEM)/discoveryf4bare.c
+    CPPSRC += $(OPSYSTEM)/discoveryf4bare.cpp
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
     SRC += $(OPUAVTALK)/uavtalk.c
@@ -91,6 +94,7 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/insgps13state.c
     SRC += $(FLIGHTLIB)/auxmagsupport.c
     SRC += $(FLIGHTLIB)/lednotification.c    
+    CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
 
     ## UAVObjects
     include ./UAVObjects.inc

flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc

@@ -19,7 +19,10 @@
 # These are the UAVObjects supposed to be build as part of the OpenPilot target
 # (all architectures)
 UAVOBJSRCFILENAMES =
+UAVOBJSRCFILENAMES += statusgrounddrive
+UAVOBJSRCFILENAMES += pidstatus
 UAVOBJSRCFILENAMES += statusvtolland
+UAVOBJSRCFILENAMES += statusvtolautotakeoff
 UAVOBJSRCFILENAMES += vtolselftuningstats
 UAVOBJSRCFILENAMES += accelgyrosettings
 UAVOBJSRCFILENAMES += accessorydesired

flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp

@@ -31,7 +31,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-
+extern "C" {
 #include "inc/openpilot.h"
 #include <uavobjectsinit.h>
 
@@ -74,6 +74,7 @@ static void initTask(void *parameters);
 
 /* Prototype of generated InitModules() function */
 extern void InitModules(void);
+}
 
 /**
  * OpenPilot Main function:

flight/targets/boards/revolution/board_hw_defs.c

@@ -899,7 +899,6 @@ static const struct pios_sbus_cfg pios_sbus_cfg = {
     .gpio_clk_periph  = RCC_AHB1Periph_GPIOC,
 };
 
-
 #ifdef PIOS_INCLUDE_COM_FLEXI
 /*
  * FLEXI PORT
@@ -1007,6 +1006,54 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
 
 #endif /* PIOS_INCLUDE_DSM */
 
+#if defined(PIOS_INCLUDE_SRXL)
+/*
+ * SRXL USART
+ */
+#include <pios_srxl_priv.h>
+
+static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = {
+    .regs  = USART3,
+    .remap = GPIO_AF_USART3,
+    .init  = {
+        .USART_BaudRate   = 115200,
+        .USART_WordLength = USART_WordLength_8b,
+        .USART_Parity     = USART_Parity_No,
+        .USART_StopBits   = USART_StopBits_1,
+        .USART_HardwareFlowControl             = USART_HardwareFlowControl_None,
+        .USART_Mode                            = USART_Mode_Rx,
+    },
+    .irq                                       = {
+        .init                                  = {
+            .NVIC_IRQChannel    = USART3_IRQn,
+            .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+            .NVIC_IRQChannelSubPriority        = 0,
+            .NVIC_IRQChannelCmd = ENABLE,
+        },
+    },
+    .rx                                        = {
+        .gpio = GPIOB,
+        .init = {
+            .GPIO_Pin   = GPIO_Pin_11,
+            .GPIO_Speed = GPIO_Speed_2MHz,
+            .GPIO_Mode  = GPIO_Mode_AF,
+            .GPIO_OType = GPIO_OType_PP,
+            .GPIO_PuPd  = GPIO_PuPd_UP
+        },
+    },
+    .tx                                        = {
+        .gpio = GPIOB,
+        .init = {
+            .GPIO_Pin   = GPIO_Pin_10,
+            .GPIO_Speed = GPIO_Speed_2MHz,
+            .GPIO_Mode  = GPIO_Mode_OUT,
+            .GPIO_OType = GPIO_OType_PP,
+            .GPIO_PuPd  = GPIO_PuPd_UP
+        },
+    },
+};
+
+#endif /* PIOS_INCLUDE_SRXL */
 /*
  * HK OSD
  */

flight/targets/boards/revolution/firmware/inc/pios_config.h

@@ -103,6 +103,7 @@
 /* #define PIOS_INCLUDE_PPM_FLEXI */
 #define PIOS_INCLUDE_DSM
 #define PIOS_INCLUDE_SBUS
+#define PIOS_INCLUDE_SRXL
 #define PIOS_INCLUDE_GCSRCVR
 #define PIOS_INCLUDE_OPLINKRCVR
 

flight/targets/boards/revolution/firmware/pios_board.c

@@ -499,6 +499,27 @@ void PIOS_Board_Init(void)
     case HWSETTINGS_RM_FLEXIPORT_OSDHK:
         PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
         break;
+    case HWSETTINGS_RM_FLEXIPORT_SRXL:
+#if defined(PIOS_INCLUDE_SRXL)
+        {
+            uint32_t pios_usart_srxl_id;
+            if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) {
+                PIOS_Assert(0);
+            }
+
+            uint32_t pios_srxl_id;
+            if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) {
+                PIOS_Assert(0);
+            }
+
+            uint32_t pios_srxl_rcvr_id;
+            if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) {
+                PIOS_Assert(0);
+            }
+            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id;
+        }
+#endif
+        break;
     } /* hwsettings_rm_flexiport */
 
     /* Moved this here to allow binding on flexiport */
@@ -871,7 +892,6 @@ void PIOS_Board_Init(void)
         break;
     }
 
-
 #if defined(PIOS_INCLUDE_GCSRCVR)
     GCSReceiverInitialize();
     uint32_t pios_gcsrcvr_id;

flight/targets/boards/revolution/pios_board.h

@@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler;
 #define PIOS_SBUS_MAX_DEVS           1
 #define PIOS_SBUS_NUM_INPUTS         (16 + 2)
 
+// -------------------------
+// Receiver Multiplex SRXL input
+// -------------------------
+#define PIOS_SRXL_MAX_DEVS           1
+#define PIOS_SRXL_NUM_INPUTS         16
+
 // -------------------------
 // Receiver DSM input
 // -------------------------

flight/targets/boards/revoproto/firmware/Makefile

@@ -24,6 +24,9 @@ endif
 include ../board-info.mk
 include $(ROOT_DIR)/make/firmware-defs.mk
 
+# REVO C++ support 
+USE_CXX = YES
+
 # ARM DSP library
 USE_DSP_LIB ?= NO
 
@@ -65,7 +68,7 @@ ifndef TESTAPP
     ## Application Core
     SRC += ../pios_usb_board_data.c
     SRC += $(OPMODULEDIR)/System/systemmod.c
-    SRC += $(OPSYSTEM)/revolution.c
+    CPPSRC += $(OPSYSTEM)/revolution.cpp
     SRC += $(OPSYSTEM)/pios_board.c
     SRC += $(FLIGHTLIB)/alarms.c
     SRC += $(OPUAVTALK)/uavtalk.c
@@ -84,6 +87,8 @@ ifndef TESTAPP
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
     SRC += $(FLIGHTLIB)/auxmagsupport.c
+    CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
+
 
     ## UAVObjects
     include ./UAVObjects.inc

flight/targets/boards/revoproto/firmware/revolution.cpp

@@ -31,6 +31,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+extern "C" {
 #include "inc/openpilot.h"
 #include <uavobjectsinit.h>
 
@@ -73,6 +74,7 @@ static void initTask(void *parameters);
 
 /* Prototype of generated InitModules() function */
 extern void InitModules(void);
+}
 
 /**
  * OpenPilot Main function:

flight/targets/boards/simposix/firmware/pios_board.c

@@ -152,7 +152,7 @@ void PIOS_Board_Init(void)
     /* Configure IO ports */
 
     /* Configure Telemetry port */
-    uint8_t hwsettings_rv_telemetryport;
+    HwSettingsRV_TelemetryPortOptions hwsettings_rv_telemetryport;
     HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport);
 
     switch (hwsettings_rv_telemetryport) {
@@ -164,10 +164,12 @@ void PIOS_Board_Init(void)
     case HWSETTINGS_RV_TELEMETRYPORT_COMAUX:
         PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id);
         break;
+    default:
+        break;
     } /*        hwsettings_rv_telemetryport */
 
     /* Configure GPS port */
-    uint8_t hwsettings_rv_gpsport;
+    HwSettingsRV_GPSPortOptions hwsettings_rv_gpsport;
     HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport);
     switch (hwsettings_rv_gpsport) {
     case HWSETTINGS_RV_GPSPORT_DISABLED:
@@ -184,10 +186,12 @@ void PIOS_Board_Init(void)
     case HWSETTINGS_RV_GPSPORT_COMAUX:
         PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id);
         break;
+    default:
+        break;
     } /* hwsettings_rv_gpsport */
 
     /* Configure AUXPort */
-    uint8_t hwsettings_rv_auxport;
+    HwSettingsRV_AuxPortOptions hwsettings_rv_auxport;
     HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport);
 
     switch (hwsettings_rv_auxport) {
@@ -201,6 +205,7 @@ void PIOS_Board_Init(void)
     case HWSETTINGS_RV_AUXPORT_COMAUX:
         PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id);
         break;
+    default:
         break;
     } /* hwsettings_rv_auxport */
 }

flight/uavobjects/inc/uavobject.h.template

@@ -52,6 +52,8 @@ int32_t $(NAME)Initialize();
 UAVObjHandle $(NAME)Handle();
 void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId);
 
+$(DATAFIELDINFO)
+
 $(DATASTRUCTURES)
 /*
  * Packed Object data (unaligned).
@@ -86,8 +88,6 @@ static inline int32_t $(NAME)GetMetadata(UAVObjMetadata *dataOut) { return UAVOb
 static inline int32_t $(NAME)SetMetadata(const UAVObjMetadata *dataIn) { return UAVObjSetMetadata($(NAME)Handle(), dataIn); }
 static inline int8_t $(NAME)ReadOnly() { return UAVObjReadOnly($(NAME)Handle()); }
 
-$(DATAFIELDINFO)
-
 /* Set/Get functions */
 $(SETGETFIELDSEXTERN)
 

flight/uavtalk/uavtalk.c

@@ -376,49 +376,48 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
     }
 
     uint8_t processedBytes = (*position);
-    uint8_t count = 0;
 
     // stop processing as soon as a complete packet is received, error is encountered or buffer is processed entirely
-    while ((count = length - (*position)) > 0
+    while ((length > (*position))
            && iproc->state != UAVTALK_STATE_COMPLETE
            && iproc->state != UAVTALK_STATE_ERROR) {
         // Receive state machine
-        if (iproc->state == UAVTALK_STATE_SYNC &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_SYNC &&
             !UAVTalkProcess_SYNC(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_TYPE &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_TYPE &&
             !UAVTalkProcess_TYPE(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_SIZE &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_SIZE &&
             !UAVTalkProcess_SIZE(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_OBJID &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_OBJID &&
             !UAVTalkProcess_OBJID(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_INSTID &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_INSTID &&
             !UAVTalkProcess_INSTID(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_TIMESTAMP &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_TIMESTAMP &&
             !UAVTalkProcess_TIMESTAMP(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_DATA &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_DATA &&
             !UAVTalkProcess_DATA(connection, iproc, rxbuffer, length, position)) {
             break;
         }
 
-        if (iproc->state == UAVTALK_STATE_CS &&
+        if ((length > (*position)) && iproc->state == UAVTALK_STATE_CS &&
             !UAVTalkProcess_CS(connection, iproc, rxbuffer, length, position)) {
             break;
         }

ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts

@@ -125,7 +125,7 @@
     <message>
         <location/>
         <source>Contribute usage statistics:</source>
-        <translation type="unfinished">Contribuer aux statistiques d&apos;utilisation :</translation>
+        <translation>Contribuer aux statistiques d&apos;utilisation :</translation>
     </message>
     <message>
         <location/>
@@ -10712,7 +10712,7 @@ Voulez-vous toujours continuer ?</translation>
 <context>
     <name>ConfigInputWidget</name>
     <message>
-        <location filename="../../../src/plugins/config/configinputwidget.cpp" line="+393"/>
+        <location filename="../../../src/plugins/config/configinputwidget.cpp" line="+396"/>
         <source>http://wiki.openpilot.org/x/04Cf</source>
         <translation></translation>
     </message>
@@ -10837,6 +10837,31 @@ Vous pouvez appuyer à tout moment sur &apos;Précédent&apos; pour revenir à l
         <translatorcomment>Dérive / gouvernail ?</translatorcomment>
         <translation>Pour un Quadricoptère : Profondeur correspond à la Rotation Avant, Ailerons à Roulis et Dérive correspond à Lacet.</translation>
     </message>
+    <message>
+        <location line="+209"/>
+        <source>&lt;p&gt;Please enable throttle hold mode.&lt;/p&gt;&lt;p&gt;Move the Collective Pitch stick.&lt;/p&gt;</source>
+        <translation>&lt;p&gt;Veuillez activer les gaz en position maintenue.&lt;/p&gt;&lt;p&gt;Bougez le manche du Collectif de tangage.&lt;/p&gt;</translation>
+    </message>
+    <message>
+        <location line="+3"/>
+        <source>&lt;p&gt;Please toggle the Flight Mode switch.&lt;/p&gt;&lt;p&gt;For switches you may have to repeat this rapidly.&lt;/p&gt;&lt;p&gt;Alternatively, you can click Next to skip this channel, but you will get only &lt;b&gt;ONE&lt;/b&gt; Flight Mode.&lt;/p&gt;</source>
+        <translation>&lt;p&gt;Veuillez activer l&apos;interrupteur de Mode de Vol.&lt;/p&gt;&lt;p&gt;Pour les interrupteurs vous pourriez avoir à répéter l&apos;opération rapidement.&lt;/p&gt;&lt;p&gt;Vous avez la possibilité d&apos;appuyer sur Suivant pour ignorer ce canal mais vous aurez seulement &lt;b&gt;UN&lt;/b&gt; Mode de Vol.&lt;/p&gt;</translation>
+    </message>
+    <message>
+        <location line="+5"/>
+        <source>&lt;p&gt;Please disable throttle hold mode.&lt;/p&gt;&lt;p&gt;Move the Throttle stick.&lt;/p&gt;</source>
+        <translation>&lt;p&gt;Veuillez désactiver les gaz en position maintenue.&lt;/p&gt;&lt;p&gt;Bougez le manche des Gaz.&lt;/p&gt;</translation>
+    </message>
+    <message>
+        <location line="+3"/>
+        <source>&lt;p&gt;Please move each control one at a time according to the instructions and picture below.&lt;/p&gt;&lt;p&gt;Move the %1 stick.&lt;/p&gt;</source>
+        <translation>&lt;p&gt;Veuillez bouger chaque organe de contrôle, un seul à la fois, en fonction des instructions et de l&apos;image ci-dessous.&lt;/p&gt;&lt;p&gt;Bougez le manche %1.&lt;/p&gt;</translation>
+    </message>
+    <message>
+        <location line="+5"/>
+        <source>&lt;p&gt;Alternatively, click Next to skip this channel.&lt;/p&gt;</source>
+        <translation>&lt;p&gt;Vous avez la possibilité d&apos;appuyer sur Suivant pour ignorer ce canal.&lt;/p&gt;</translation>
+    </message>
     <message>
         <source>Please center all controls and trims and press Next when ready.
 
@@ -10867,7 +10892,7 @@ IMPORTANT: These new settings have not been saved to the board yet. After pressi
 IMPORTANT : Ces nouveaux paramètres ne sont pas encore enregistrés sur la carte. Après avoir appuyé sur Suivant vous serez dirigé vers l&apos;onglet Paramètres d&apos;Armement où vous pourrez choisir votre séquence d&apos;armement et enregistrer la configuration.</translation>
     </message>
     <message>
-        <location line="+23"/>
+        <location line="-202"/>
         <source>Please center all controls and trims and press Next when ready.
 
 For a ground vehicle, this center position will be used as neutral value of each channel.</source>
@@ -10876,52 +10901,47 @@ For a ground vehicle, this center position will be used as neutral value of each
 Pour un véhicule terrestre, ces positions centrales seront utilisées comme neutre de chaque canal.</translation>
     </message>
     <message>
-        <location line="+178"/>
         <source>Please enable throttle hold mode.
 
 Move the Collective Pitch stick.</source>
         <translatorcomment>hélico ? à traduire [Platypus] Ca convient ? [soh] A confirmer par un héliqueux.</translatorcomment>
-        <translation type="unfinished">Veuillez activer les gaz en position maintenue.
+        <translation type="obsolete">Veuillez activer les gaz en position maintenue.
 
 Bougez le manche du Collectif de tangage.</translation>
     </message>
     <message>
-        <location line="+2"/>
         <source>Please toggle the Flight Mode switch.
 
 For switches you may have to repeat this rapidly.</source>
-        <translation>Veuillez activer l&apos;interrupteur de Mode de Vol.
+        <translation type="vanished">Veuillez activer l&apos;interrupteur de Mode de Vol.
 
 Pour les interrupteurs vous pourriez avoir à répéter l&apos;opération rapidement.</translation>
     </message>
     <message>
-        <location line="+2"/>
         <source>Please disable throttle hold mode.
 
 Move the Throttle stick.</source>
         <translatorcomment>hélico ? à traduire</translatorcomment>
-        <translation type="unfinished">Veuillez désactiver les gaz en position maintenue.
+        <translation type="obsolete">Veuillez désactiver les gaz en position maintenue.
 
 Bougez le manche des Gaz.</translation>
     </message>
     <message>
-        <location line="+2"/>
         <source>Please move each control one at a time according to the instructions and picture below.
 
 Move the %1 stick.</source>
-        <translation>Veuillez bouger chaque organe de contrôle, un seul à la fois, en fonction des instructions et de l&apos;image ci-dessous.
+        <translation type="vanished">Veuillez bouger chaque organe de contrôle, un seul à la fois, en fonction des instructions et de l&apos;image ci-dessous.
 
 Bougez le manche %1.</translation>
     </message>
     <message>
-        <location line="+6"/>
+        <location line="+208"/>
         <source>Next / Skip</source>
         <translation>Suivant / Sauter</translation>
     </message>
     <message>
-        <location line="+1"/>
         <source> Alternatively, click Next to skip this channel.</source>
-        <translation> Vous avez la possibilité d&apos;appuyer sur Suivant pour ignorer ce canal.</translation>
+        <translation type="vanished"> Vous avez la possibilité d&apos;appuyer sur Suivant pour ignorer ce canal.</translation>
     </message>
     <message>
         <location line="+748"/>
@@ -16187,69 +16207,87 @@ IMPORTANT : Ces nouveaux paramètres ne sont pas encore enregistrés sur la cart
     <name>ConfigCcpmWidget</name>
     <message>
         <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp" line="+1080"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+1110"/>
         <source>&lt;h1&gt;Swashplate Leveling Routine&lt;/h1&gt;</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+1"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+1"/>
         <source>&lt;b&gt;You are about to start the Swashplate levelling routine.&lt;/b&gt;&lt;p&gt;This process will start by downloading the current configuration from the GCS to the OP hardware and will adjust your configuration at various stages.&lt;p&gt;The final state of your system should match the current configuration in the GCS config gadget.&lt;/p&gt;&lt;p&gt;Please ensure all ccpm settings in the GCS are correct before continuing.&lt;/p&gt;&lt;p&gt;If this process is interrupted, then the state of your OP board may not match the GCS configuration.&lt;/p&gt;&lt;p&gt;&lt;i&gt;After completing this process, please check all settings before attempting to fly.&lt;/i&gt;&lt;/p&gt;&lt;p&gt;&lt;font color=red&gt;&lt;b&gt;Please disconnect your motor to ensure it will not spin up.&lt;/b&gt;&lt;/font&gt;&lt;p&gt;&lt;hr&gt;&lt;i&gt;Do you wish to proceed?&lt;/i&gt;&lt;/p&gt;</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+128"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+128"/>
         <source>&lt;h2&gt;Neutral levelling&lt;/h2&gt;&lt;p&gt;Using adjustment of:&lt;ul&gt;&lt;li&gt;Servo horns,&lt;/li&gt;&lt;li&gt;Link lengths,&lt;/li&gt;&lt;li&gt;Neutral triming spinboxes to the right&lt;/li&gt;&lt;/ul&gt;&lt;br&gt;Ensure that the swashplate is in the center of desired travel range and is level.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+14"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+14"/>
         <source>&lt;h2&gt;Max levelling&lt;/h2&gt;&lt;p&gt;Using adjustment of:&lt;ul&gt;&lt;li&gt;Max triming spinboxes to the right ONLY&lt;/li&gt;&lt;/ul&gt;&lt;br&gt;Ensure that the swashplate is at the top of desired travel range and is level.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+13"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+13"/>
         <source>&lt;h2&gt;Min levelling&lt;/h2&gt;&lt;p&gt;Using adjustment of:&lt;ul&gt;&lt;li&gt;Min triming spinboxes to the right ONLY&lt;/li&gt;&lt;/ul&gt;&lt;br&gt;Ensure that the swashplate is at the bottom of desired travel range and is level.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+19"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+19"/>
         <source>&lt;h2&gt;Levelling verification&lt;/h2&gt;&lt;p&gt;Adjust the slider to the right over it&apos;s full range and observe the swashplate motion. It should remain level over the entire range of travel.&lt;/p&gt;</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+7"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+7"/>
         <source>&lt;h2&gt;Levelling complete&lt;/h2&gt;&lt;p&gt;Press the Finish button to save these settings to the SD card&lt;/p&gt;&lt;p&gt;Press the cancel button to return to the pre-levelling settings&lt;/p&gt;</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+68"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+68"/>
         <source>&lt;h2&gt;Levelling Cancelled&lt;/h2&gt;&lt;p&gt;Previous settings have been restored.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+43"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+43"/>
         <source>&lt;h2&gt;Levelling Completed&lt;/h2&gt;&lt;p&gt;New settings have been saved to the SD card</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+12"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+12"/>
         <source>&lt;font color=red&gt;&lt;h1&gt;Warning!!!&lt;/h2&gt;&lt;/font&gt;</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+6"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+6"/>
         <source>&lt;h2&gt;This code has many configurations.&lt;/h2&gt;&lt;p&gt;Please double check all settings before attempting flight!</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+11"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+11"/>
         <source>&lt;h2&gt;The CCPM mixer code needs more testing!&lt;/h2&gt;&lt;p&gt;&lt;font color=red&gt;Use it at your own risk!&lt;/font&gt;&lt;p&gt;Do you wish to continue?</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
         <location line="+16"/>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+16"/>
         <source>&lt;h2&gt;The CCPM swashplate levelling code is NOT complete!&lt;/h2&gt;&lt;p&gt;&lt;font color=red&gt;DO NOT use it for flight!&lt;/font&gt;</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget_dev.cpp" line="+174"/>
+        <source>Channel already used</source>
+        <translation type="unfinished">Canal déjà utilisé</translation>
+    </message>
 </context>
 <context>
     <name>VehicleTemplateSelectorWidget</name>

ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_zh_CN.ts

@@ -5621,13 +5621,16 @@ Beware of not locking yourself out!</source>
         <source>If enabled, a fast recalibration of gyro zero point will be done
 whenever the frame is armed. Do not move the airframe while
 arming it in that case!</source>
-        <translation>每次解锁航模时，控制器以解锁时摆放状态做一次快速水平校准。
+        <translation>每次解锁航模时，为陀螺仪传感器零点位置做一次快速调整。
-打开此项设置后，请务必保证每次解锁时候航模静止放置！</translation>
+打开此项设置后，请务必保证每次解锁时候航模静止放置！
+陀螺仪传感器的作用是测量旋转量，不是用来确保飞行器维持
+对地水平的。加速度传感器Z轴读取到的重力加速度才是用来维持
+水平飞行稳定的参考值！</translation>
     </message>
     <message>
         <location/>
         <source>Zero gyros while arming aircraft</source>
-        <translation>每次解锁时，重置陀螺仪读取数值</translation>
+        <translation>除非你对自己智商足够自信，且透彻明白陀螺仪传感器和加速度传感器的作用和区别，否则请勿自作聪明取消此项设置</translation>
     </message>
     <message>
         <location/>
@@ -9244,32 +9247,32 @@ Please select the type of multirotor you want to create a configuration for belo
     </message>
     <message>
         <location/>
-        <location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="+435"/>
+        <location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="+561"/>
-        <location line="+104"/>
+        <location line="+131"/>
         <source>Start</source>
         <translation>开始</translation>
     </message>
     <message>
-        <location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="-198"/>
+        <location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="-268"/>
         <location line="+8"/>
-        <location line="+178"/>
+        <location line="+243"/>
-        <location line="+25"/>
+        <location line="+38"/>
         <source>Output value : &lt;b&gt;%1&lt;/b&gt; µs</source>
         <translation>输出值：&lt;b&gt;%1&lt;/b&gt;µs</translation>
     </message>
     <message>
-        <location line="-206"/>
+        <location line="-284"/>
         <source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;To find &lt;/span&gt;&lt;span style=&quot; font-size:10pt; font-weight:600;&quot;&gt;the neutral rate for this reversable motor&lt;/span&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;, press the Start button below and slide the slider to the right or left until you find the value where the motor doesn&apos;t start. &lt;br/&gt;&lt;br/&gt;When done press button again to stop.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <location line="+89"/>
+        <location line="+132"/>
-        <location line="+104"/>
+        <location line="+131"/>
         <source>Stop</source>
         <translation>停止</translation>
     </message>
     <message>
-        <location line="-43"/>
+        <location line="-59"/>
         <source>The actuator module is in an error state.
 
 Please make sure the correct firmware version is used then restart the wizard and try again. If the problem persists please consult the openpilot.org support forum.</source>
@@ -9277,6 +9280,16 @@ Please make sure the correct firmware version is used then restart the wizard an
 
 请确认设备上固件版本是否正确，然后重新启动设置向导。如果问题继续存在，请到openpilot.org官网论坛寻求进一步技术支持。</translation>
     </message>
+    <message>
+        <location line="+132"/>
+        <source>Output value : &lt;b&gt;%1&lt;/b&gt; µs (Min)</source>
+        <translation>输出值：&lt;b&gt;%1&lt;/b&gt;µs (最小)</translation>
+    </message>
+    <message>
+        <location line="+31"/>
+        <source>Output value : &lt;b&gt;%1&lt;/b&gt; µs (Max)</source>
+        <translation>输出值：&lt;b&gt;%1&lt;/b&gt;µs (最大)</translation>
+    </message>
     <message>
         <location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.ui"/>
         <source>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
@@ -9370,6 +9383,11 @@ p, li { white-space: pre-wrap; }
         <source>Output value: 1000µs</source>
         <translation>输出值：1000µs</translation>
     </message>
+    <message>
+        <location/>
+        <source>Calibrate all motor outputs at the same time</source>
+        <translation type="unfinished">一次性给所有电机设置相同怠速点</translation>
+    </message>
 </context>
 <context>
     <name>SavePage</name>
@@ -10268,7 +10286,7 @@ Do you still want to proceed?</source>
         <translation>在设置向导结束以后会停留在“解锁设置”选项页，请务必修改遥控器解锁方式。</translation>
     </message>
     <message>
-        <location line="+210"/>
+        <location line="+212"/>
         <source>Next</source>
         <translation>下一步</translation>
     </message>
@@ -10348,6 +10366,16 @@ You can press &apos;back&apos; at any time to return to the previous screen or p
         <source>For a Quad: Elevator is Pitch, Ailerons are Roll, and Rudder is Yaw.</source>
         <translation>对于多旋翼飞行器，升降舵（ELEV）控制俯仰，副翼（AILE）控制横滚，方向舵（RUDD）控制方向。</translation>
     </message>
+    <message>
+        <location line="+1016"/>
+        <source>Warning</source>
+        <translation>警告</translation>
+    </message>
+    <message>
+        <location line="+0"/>
+        <source>&lt;p&gt;There is something wrong with Throttle range. Please redo calibration and move &lt;b&gt;ALL sticks&lt;/b&gt;, Throttle stick included.&lt;/p&gt;</source>
+        <translation>&lt;p&gt;油门行程识别出错。请重新拨动&lt;b&gt;所有操纵杆&lt;/b&gt;再次进行校准，切记不要忽略油门摇杆。&lt;/p&gt;</translation>
+    </message>
     <message>
         <source>Please center all controls and trims and press Next when ready.
 
@@ -10377,7 +10405,7 @@ IMPORTANT: These new settings have not been saved to the board yet. After pressi
 注意：遥控器校准结果尚未保存，请点击“下一步”进入“解锁设置”页，将解锁动作设置好以后，再点击”保存“按钮将所有遥控器输入设置信息保存。</translation>
     </message>
     <message>
-        <location line="+191"/>
+        <location line="-825"/>
         <source>Please enable throttle hold mode.
 
 Move the Collective Pitch stick.</source>
@@ -10419,7 +10447,7 @@ Move the %1 stick.</source>
         <translation>结束手动校准</translation>
     </message>
     <message>
-        <location line="+3"/>
+        <location line="+4"/>
         <source>&lt;p&gt;Arming Settings are now set to &apos;Always Disarmed&apos; for your safety.&lt;/p&gt;&lt;p&gt;Be sure your receiver is powered with an external source and Transmitter is on.&lt;/p&gt;&lt;p align=&apos;center&apos;&gt;&lt;b&gt;Stop Manual Calibration&lt;/b&gt; when done&lt;/p&gt;</source>
         <translation>&lt;p&gt;安全起见，在后续操作开始之前，强制遥控器进入永久锁定状态。&lt;/p&gt;&lt;p&gt;请确认遥控器发射机和接收器已经打开。&lt;/p&gt;&lt;p align=&apos;center&apos;&gt;完成手动校准后，请点击&lt;b&gt;结束手动校准&lt;/b&gt;&lt;/p&gt;</translation>
     </message>
@@ -10429,12 +10457,12 @@ Move the %1 stick.</source>
         <translation>手动校准结束后，请打开“解锁设置”选项页设置飞行器解锁动作。</translation>
     </message>
     <message>
-        <location line="+33"/>
+        <location line="+51"/>
         <source>Start Manual Calibration</source>
         <translation>开始手动校准</translation>
     </message>
     <message>
-        <location line="-764"/>
+        <location line="-783"/>
         <source> Alternatively, click Next to skip this channel.</source>
         <translation>你也可以点击“下一步/跳过”忽略该项。</translation>
     </message>
@@ -10762,14 +10790,14 @@ Move the %1 stick.</source>
         <location line="+24"/>
         <location line="+6"/>
         <location line="+19"/>
-        <location line="+16"/>
+        <location line="+19"/>
         <location line="+16"/>
         <location line="+38"/>
         <source>Unknown</source>
         <translation>未知设备</translation>
     </message>
     <message>
-        <location line="-182"/>
+        <location line="-185"/>
         <source>Vehicle type: </source>
         <translation>航模类型： </translation>
     </message>
@@ -10927,6 +10955,11 @@ Move the %1 stick.</source>
     </message>
     <message>
         <location line="+9"/>
+        <source>Synched ESC</source>
+        <translation>Synched电调</translation>
+    </message>
+    <message>
+        <location line="+3"/>
         <source>Oneshot ESC</source>
         <translation>Oneshot电调</translation>
     </message>
@@ -11040,7 +11073,7 @@ Move the %1 stick.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <location line="+199"/>
+        <location line="+196"/>
         <location line="+32"/>
         <location line="+37"/>
         <source>Writing actuator settings</source>
@@ -11073,7 +11106,7 @@ Move the %1 stick.</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <location line="+146"/>
+        <location line="+144"/>
         <source>Writing mixer settings</source>
         <translation type="unfinished"></translation>
     </message>
@@ -13400,7 +13433,7 @@ p, li { white-space: pre-wrap; }
     </message>
     <message>
         <location filename="../../../src/plugins/setupwizard/pages/esccalibrationpage.cpp" line="+49"/>
-        <location line="+49"/>
+        <location line="+58"/>
         <location line="+42"/>
         <location line="+7"/>
         <location line="+9"/>
@@ -14305,7 +14338,7 @@ Please first select the area of the map to rip with &lt;CTRL&gt;+Left mouse clic
         <location line="+3"/>
         <location line="+261"/>
         <source>%1 Hz</source>
-        <translation type="unfinished"></translation>
+        <translation></translation>
     </message>
     <message>
         <location line="-201"/>
@@ -14329,8 +14362,12 @@ Please first select the area of the map to rip with &lt;CTRL&gt;+Left mouse clic
     </message>
     <message>
         <location line="+32"/>
+        <source>OneShot and PWMSync output only works with Receiver Port settings marked with &apos;+OneShot&apos;&lt;br&gt;When using Receiver Port setting &apos;PPM_PIN8+OneShot&apos; &lt;b&gt;&lt;font color=&apos;%1&apos;&gt;Bank %2&lt;/font&gt;&lt;/b&gt; must be set to PWM</source>
+        <translation>要打开OneShot以及PWMSync输出，请确保遥控接收器端口设置为“+OneShot”&lt;br&gt;当接收器端口设置为“PPM_PIN8+OneShot”时，&lt;b&gt;&lt;font color=&apos;#C3A8FF&apos;&gt;输出集%2&lt;/font&gt;&lt;/b&gt;必须设置为PWM</translation>
+    </message>
+    <message>
         <source>OneShot only works with Receiver Port settings marked with &apos;+OneShot&apos;&lt;br&gt;When using Receiver Port setting &apos;PPM_PIN8+OneShot&apos; &lt;b&gt;&lt;font color=&apos;%1&apos;&gt;Bank %2&lt;/font&gt;&lt;/b&gt; must be set to PWM</source>
-        <translation>遥控接收器端口设置为“+OneShot”后才能&lt;br&gt;正常工作。假设设置为“PPM_PIN6+OneShot”，则&lt;b&gt;&lt;font color=&apos;%1&apos;&gt;输出集%2&lt;/font&gt;&lt;/b&gt;必须设置为PWM</translation>
+        <translation type="vanished">遥控接收器端口设置为“+OneShot”后才能&lt;br&gt;正常工作。假设设置为“PPM_PIN6+OneShot”，则&lt;b&gt;&lt;font color=&apos;%1&apos;&gt;输出集%2&lt;/font&gt;&lt;/b&gt;必须设置为PWM</translation>
     </message>
     <message>
         <source>OneShot only works with Receiver Port settings marked with &apos;+OneShot&apos;&lt;br&gt;When using Receiver Port setting &apos;PPM_PIN6+OneShot&apos; &lt;b&gt;&lt;font color=&apos;#C3A8FF&apos;&gt;Bank 4 (output 6,9-10)&lt;/font&gt;&lt;/b&gt; must be set to PWM</source>

ground/openpilotgcs/src/libs/juavobjects/templates/uavobject.java.template

@@ -115,6 +115,17 @@ $(INITFIELDS)
 		}
 	}
 
+	/**
+	 * Returns a new instance of this UAVDataObject with default field
+	 * values. This is intended to be used by 'reset to default' functionality.
+	 * 
+	 * @return new instance of this class with default values.
+	 */
+	@Override
+	public UAVDataObject getDefaultInstance(){
+		return new $(NAME)();
+	}
+
 	/**
 	 * Static function to retrieve an instance of the object.
 	 */

ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pro

@@ -2,3 +2,5 @@ TEMPLATE = subdirs
 
 
 SUBDIRS = src \
+
+QT += xml

ground/openpilotgcs/src/libs/opmapcontrol/src/core/alllayersoftype.cpp

@@ -55,13 +55,6 @@ QVector<MapType::Types> AllLayersOfType::GetAllLayersOfType(const MapType::Types
         }
         break;
 
-        case MapType::YahooHybrid:
-        {
-            types.append(MapType::YahooSatellite);
-            types.append(MapType::YahooLabels);
-        }
-        break;
-
         case MapType::ArcGIS_MapsLT_Map_Hybrid:
         {
             types.append(MapType::ArcGIS_MapsLT_OrtoFoto);

ground/openpilotgcs/src/libs/opmapcontrol/src/core/core.pro

@@ -38,3 +38,5 @@ HEADERS += opmaps.h \
     kibertilecache.h \
     debugheader.h \
     diagnostics.h
+
+QT += xml

ground/openpilotgcs/src/libs/opmapcontrol/src/core/maptype.h

@@ -52,11 +52,6 @@ public:
         OpenStreetMapSurfer  = 34,
         OpenStreetMapSurferTerrain = 35,
 
-        YahooMap            = 64,
-        YahooSatellite      = 128,
-        YahooLabels         = 256,
-        YahooHybrid         = 333,
-
         BingMap = 444,
         BingSatellite       = 555,
         BingHybrid          = 666,

ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp

@@ -153,15 +153,6 @@ QByteArray OPMaps::GetImageFrom(const MapType::Types &type, const Point &pos, co
             }
             break;
 
-            case MapType::YahooHybrid:
-            case MapType::YahooLabels:
-            case MapType::YahooMap:
-            case MapType::YahooSatellite:
-            {
-                qheader.setRawHeader("Referrer", "http://maps.yahoo.com/");
-            }
-            break;
-
             case MapType::ArcGIS_MapsLT_Map_Labels:
             case MapType::ArcGIS_MapsLT_Map:
             case MapType::ArcGIS_MapsLT_OrtoFoto:

ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.cpp

@@ -61,11 +61,6 @@ ProviderStrings::ProviderStrings()
     /// </summary>
     GoogleMapsAPIKey      = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA";
 
-    // Yahoo version strings
-    VersionYahooMap       = "4.3";
-    VersionYahooSatellite = "1.9";
-    VersionYahooLabels    = "4.3";
-
     // BingMaps
     VersionBingMaps       = "563";
 

ground/openpilotgcs/src/libs/opmapcontrol/src/core/providerstrings.h

@@ -60,11 +60,6 @@ public:
     /// </summary>
 
 
-    // Yahoo version strings
-    QString VersionYahooMap;
-    QString VersionYahooSatellite;
-    QString VersionYahooLabels;
-
     // BingMaps
     QString VersionBingMaps;
 

ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp

@@ -26,7 +26,8 @@
  */
 #include "urlfactory.h"
 #include <QRegExp>
-
+#include <QDomDocument>
+#include <QtXml>
 
 namespace core {
 const double UrlFactory::EarthRadiusKm = 6378.137; // WGS-84
@@ -97,6 +98,12 @@ void UrlFactory::TryCorrectGoogleVersions()
     if (CorrectGoogleVersions && !IsCorrectGoogleVersions()) {
         QNetworkReply *reply;
         QNetworkRequest qheader;
+	// This SSL Hack is half assed... technically bad *security* joojoo.
+	// Required due to a QT5 bug on linux and Mac
+	// 
+	QSslConfiguration conf = qheader.sslConfiguration();
+	conf.setPeerVerifyMode(QSslSocket::VerifyNone);
+	qheader.setSslConfiguration(conf);
         QNetworkAccessManager network;
         QEventLoop q;
         QTimer tT;
@@ -109,7 +116,10 @@ void UrlFactory::TryCorrectGoogleVersions()
         qDebug() << "Correct GoogleVersion";
 #endif // DEBUG_URLFACTORY
        // setIsCorrectGoogleVersions(true);
-        QString url = "https://maps.google.com/maps?output=classic";
+        // QString url = "https://www.google.com/maps/@0,-0,7z?dg=dbrw&newdg=1";
+	// We need to switch to the Above url... the /lochp method will be depreciated soon
+	// https://productforums.google.com/forum/#!category-topic/maps/navigation/k6EFrp7J7Jk
+        QString url = "https://www.google.com/lochp";
 
         qheader.setUrl(QUrl(url));
         qheader.setRawHeader("User-Agent", UserAgent);
@@ -126,7 +136,12 @@ void UrlFactory::TryCorrectGoogleVersions()
 #endif // DEBUG_URLFACTORY
             return;
         }
+
         QString html = QString(reply->readAll());
+#ifdef DEBUG_URLFACTORY
+	qDebug() << html;
+#endif // DEBUG_URLFACTORY
+
         QRegExp reg("\"*https://mts0.google.com/vt/lyrs=m@(\\d*)", Qt::CaseInsensitive);
         if (reg.indexIn(html) != -1) {
             QStringList gc = reg.capturedTexts();
@@ -148,6 +163,7 @@ void UrlFactory::TryCorrectGoogleVersions()
             qDebug() << "TryCorrectGoogleVersions, VersionGoogleLabels: " << VersionGoogleLabels;
 #endif // DEBUG_URLFACTORY
         }
+
         reg = QRegExp("\"*https://khms0.google.com/kh/v=(\\d*)", Qt::CaseInsensitive);
         if (reg.indexIn(html) != -1) {
             QStringList gc = reg.capturedTexts();
@@ -157,6 +173,7 @@ void UrlFactory::TryCorrectGoogleVersions()
 
             qDebug() << "TryCorrectGoogleVersions, VersionGoogleSatellite: " << VersionGoogleSatellite;
         }
+
         reg = QRegExp("\"*https://mts0.google.com/vt/lyrs=t@(\\d*),r@(\\d*)", Qt::CaseInsensitive);
         if (reg.indexIn(html) != -1) {
             QStringList gc = reg.capturedTexts();
@@ -208,7 +225,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         QString sec2    = ""; // after &zoom=...
         GetSecGoogleWords(pos, sec1, sec2);
         TryCorrectGoogleVersions();
-
+        qDebug() << QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
         return QString("http://%1%2.google.com/%3/lyrs=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabels).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
     }
     break;
@@ -259,6 +276,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         TryCorrectGoogleVersions();
         // http://mt0.google.cn/vt/v=w2t.110&hl=zh-CN&gl=cn&x=12&y=6&z=4&s=Ga
 
+        qDebug() << QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
         return QString("http://%1%2.google.cn/%3/imgtp=png32&lyrs=%4&hl=%5&gl=cn&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsChina).arg("zh-CN").arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
     }
     break;
@@ -318,37 +336,26 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         return QString("https://%1%2.gmaptiles.co.kr/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleLabelsKorea).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
     }
     break;
-    case MapType::YahooMap:
+    // *.yimg.com has been depreciated. "Here" is what Yahoo uses now. https://developer.here.com/rest-apis/documentation/enterprise-map-tile/topics/request-constructing.html
-    {
-        return QString("http://maps%1.yimg.com/hx/tl?v=%2&.intl=%3&x=%4&y=%5&z=%6&r=1").arg(((GetServerNum(pos, 2)) + 1)).arg(VersionYahooMap).arg(language).arg(pos.X()).arg((((1 << zoom) >> 1) - 1 - pos.Y())).arg((zoom + 1));
-    }
-
-    case MapType::YahooSatellite:
-    {
-        return QString("http://maps%1.yimg.com/ae/ximg?v=%2&t=a&s=256&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("3").arg(VersionYahooSatellite).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1);
-    }
-    break;
-    case MapType::YahooLabels:
-    {
-        return QString("http://maps%1.yimg.com/hx/tl?v=%2&t=h&.intl=%3&x=%4&y=%5&z=%6&r=1").arg("1").arg(VersionYahooLabels).arg(language).arg(pos.X()).arg(((1 << zoom) >> 1) - 1 - pos.Y()).arg(zoom + 1);
-    }
-    break;
     case MapType::OpenStreetMap:
     {
         char letter = "abc"[GetServerNum(pos, 3)];
         return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y());
     }
     break;
+    // Need to update tile format to fit http://wiki.openstreetmap.org/wiki/Tile_servers
     case MapType::OpenStreetOsm:
     {
         char letter = "abc"[GetServerNum(pos, 3)];
-        return QString("http://%1.tah.openstreetmap.org/Tiles/tile/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y());
+        qDebug() << QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y());
+        return QString("http://%1.tile.openstreetmap.org/%2/%3/%4.png").arg(letter).arg(zoom).arg(pos.X()).arg(pos.Y());
     }
     break;
     case MapType::OpenStreetMapSurfer:
     {
         // http://tiles1.mapsurfer.net/tms_r.ashx?x=37378&y=20826&z=16
 
+        qDebug() << QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom);
         return QString("http://tiles1.mapsurfer.net/tms_r.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom);
     }
     break;
@@ -356,6 +363,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
     {
         // http://tiles2.mapsurfer.net/tms_t.ashx?x=9346&y=5209&z=14
 
+        qDebug() << QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom);
         return QString("http://tiles2.mapsurfer.net/tms_t.ashx?x=%1&y=%2&z=%3").arg(pos.X()).arg(pos.Y()).arg(zoom);
     }
     break;
@@ -395,7 +403,8 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
     {
         // http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/1/0/1.jpg
 
-        return QString("http://server.arcgisonline.com/ArcGIS/rest/services/ESRI_ShadedRelief_World_2D/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X());
+        qDebug() << QString("http://server.arcgisonline.com/ArcGIS/rest/services/World_Shaded_Relief/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X());
+        return QString("http://server.arcgisonline.com/ArcGIS/rest/services/World_Shaded_Relief/MapServer/tile/%1/%2/%3").arg(zoom).arg(pos.Y()).arg(pos.X());
     }
     break;
     case MapType::ArcGIS_Terrain:
@@ -414,6 +423,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X());
         // http://dc1.maps.lt/cache/mapslt_ortofoto_512/map/_alllayers/L03/R0000001d/C0000002a.jpg
         // TODO verificar
+        qDebug() << QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0');
         return QString("http://dc1.maps.lt/cache/mapslt_ortofoto/map/_alllayers/L%1/R%2/C%3.jpg").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0');
     }
     break;
@@ -426,6 +436,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         // http://dc1.maps.lt/cache/mapslt_512/map/_alllayers/L03/R0000001b/C00000029.png
         // TODO verificar
         // http://dc1.maps.lt/cache/mapslt/map/_alllayers/L02/R0000001c/C00000029.png
+        qDebug() << QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0');
         return QString("http://dc1.maps.lt/cache/mapslt/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0');
     }
     break;
@@ -435,6 +446,7 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         // return string.Format("http://arcgis.maps.lt/ArcGIS/rest/services/mapslt_ortofoto_overlay/MapServer/tile/{0}/{1}/{2}", zoom, pos.Y(), pos.X());
         // http://dc1.maps.lt/cache/mapslt_ortofoto_overlay_512/map/_alllayers/L03/R0000001d/C00000029.png
         // TODO verificar
+        qDebug() << QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0');
         return QString("http://dc1.maps.lt/cache/mapslt_ortofoto_overlay/map/_alllayers/L%1/R%2/C%3.png").arg(zoom, 2, 10, (QChar)'0').arg(pos.Y(), 8, 16, (QChar)'0').arg(pos.X(), 8, 16, (QChar)'0');
     }
     break;
@@ -455,22 +467,33 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         QString y = QString("%1").arg(QString::number(pos.Y()), 9, (QChar)'0');
         y.insert(3, "/").insert(7, "/");
         // "http://map03.pergo.com.tr/tile/2/000/000/003/000/000/002.png"
+	// This has changed map03 does not exist. (neither does map3) Servers have changed to map1 and map2? 
         return QString("http://map%1.pergo.com.tr/tile/%2/%3/%4.png").arg(GetServerNum(pos, 4)).arg(zoom, 2, 10, (QChar)'0').arg(x).arg(y);
     }
     break;
     case MapType::SigPacSpainMap:
     {
-        return QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1);
+	// http://sigpac.magrama.es/fega/h5visor/ is new server location 
+        qDebug() << QString("http://sigpac.mapa.es/kmlserver/raster/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1);
+        return QString("http://sigpac.magrama.es/SDG/%1@3785/%2.%3.%4.img").arg(levelsForSigPacSpainMap[zoom]).arg(zoom).arg(pos.X()).arg((2 << (zoom - 1)) - pos.Y() - 1);
     }
     break;
 
     case MapType::YandexMapRu:
     {
+        /*  
+	    Used "oldmaps" to determine map types - https://old.maps.yandex.ru/?ll=-83.110960%2C40.091250&spn=7.745361%2C6.015476&z=7&l=map
+	    map: 'https:\/\/vec0%d.maps.yandex.net\/tiles?l=map&%c&%l',
+            sat: 'https:\/\/sat0%d.maps.yandex.net\/tiles?l=sat&%c&%l',
+            skl: 'https:\/\/vec0%d.maps.yandex.net\/tiles?l=skl&%c&%l',
+									*/
+
         QString server = "vec";
+        return QString("http://%1").arg(server) + QString("0%2.maps.yandex.net/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom);
 
-        // http://vec01.maps.yandex.ru/tiles?l=map&v=2.10.2&x=1494&y=650&z=11
+// Satllite maps are poor quality, but available. 
-
+//        QString server = "sat";
-        return QString("http://%1").arg(server) + QString("0%2.maps.yandex.ru/tiles?l=map&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom);
+//        return QString("http://%1").arg(server) + QString("0%2.maps.yandex.net/tiles?l=sat&v=%3&x=%4&y=%5&z=%6").arg(GetServerNum(pos, 4) + 1).arg(VersionYandexMap).arg(pos.X()).arg(pos.Y()).arg(zoom);
     }
     break;
     default:
@@ -492,23 +515,29 @@ void UrlFactory::GetSecGoogleWords(const Point &pos, QString &sec1, QString &sec
 QString UrlFactory::MakeGeocoderUrl(QString keywords)
 {
     QString key = keywords.replace(' ', '+');
-
+    // CSV output has been depreciated. API key is no longer needed. 
-    return QString("http://maps.google.com/maps/geo?q=%1&output=csv&key=%2").arg(key).arg(GoogleMapsAPIKey);
+    return QString("https://maps.googleapis.com/maps/api/geocode/xml?sensor=false&address=%1").arg(key);
 }
 QString UrlFactory::MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language)
 {
-    return QString("http://maps.google.com/maps/geo?hl=%1&ll=%2,%3&output=csv&key=%4").arg(language).arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng())).arg(GoogleMapsAPIKey);
+    // CSV output has been depreciated. API key is no longer needed. 
+    return QString("https://maps.googleapis.com/maps/api/geocode/xml?latlng=%1,%2").arg(QString::number(pt.Lat())).arg(QString::number(pt.Lng()));
 }
-internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status)
+internals::PointLatLng UrlFactory::GetLatLngFromGeodecoder(const QString &keywords, QString &status)
 {
     return GetLatLngFromGeocoderUrl(MakeGeocoderUrl(keywords), UseGeocoderCache, status);
 }
-internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status)
+
+QString latxml;
+QString lonxml;
+//QString status;
+
+internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, QString &status)
 {
 #ifdef DEBUG_URLFACTORY
     qDebug() << "Entered GetLatLngFromGeocoderUrl:";
 #endif // DEBUG_URLFACTORY
-    status = GeoCoderStatusCode::Unknow;
+    status = "ZERO_RESULTS";
     internals::PointLatLng ret(0, 0);
     QString urlEnd = url.mid(url.indexOf("geo?q=") + 6);
     urlEnd.replace(QRegExp(
@@ -529,6 +558,10 @@ internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url,
 #endif // DEBUG_URLFACTORY
         QNetworkReply *reply;
         QNetworkRequest qheader;
+        // Lame hack *SSL security == none, bypass QT bug
+        QSslConfiguration conf = qheader.sslConfiguration();
+        conf.setPeerVerifyMode(QSslSocket::VerifyNone);
+        qheader.setSslConfiguration(conf);
         QNetworkAccessManager network;
         network.setProxy(Proxy);
         qheader.setUrl(QUrl(url));
@@ -552,11 +585,86 @@ internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url,
             return internals::PointLatLng(0, 0);
         }
         {
+            geo = reply->readAll();
 #ifdef DEBUG_URLFACTORY
             qDebug() << "GetLatLngFromGeocoderUrl:Reply ok";
+	    qDebug() << geo; // This is the response from the geocode request (no longer in CSV)
 #endif // DEBUG_URLFACTORY
-            geo = reply->readAll();
 
+	    // This is SOOOO horribly hackish, code duplication needs to go. Needed a quick fix. 
+	    QXmlStreamReader reader(geo);
+	    while(!reader.atEnd())
+	    {  
+  		reader.readNext();
+
+  		if(reader.isStartElement())
+  		{
+    		    if(reader.name() == "lat")
+    		    {		
+      			reader.readNext();
+      			if(reader.atEnd()) 
+				break;
+
+      			if(reader.isCharacters())
+      			{
+        		    QString text = reader.text().toString();
+			    qDebug() << text;
+			    latxml = text;
+			    break;
+      			}
+    		    }
+  	        }
+
+	     }
+
+	    while(!reader.atEnd())
+	    {  
+  		reader.readNext();
+
+  		if(reader.isStartElement())
+  		{
+    		    if(reader.name() == "lng")
+    		    {		
+      			reader.readNext();
+      			if(reader.atEnd()) 
+				break;
+
+      			if(reader.isCharacters())
+      			{
+        		    QString text = reader.text().toString();
+			    qDebug() << text;
+			    lonxml = text;
+			    break;
+      			}
+    		    }
+  	        }
+
+	     }
+
+	    QXmlStreamReader reader2(geo);
+	    while(!reader2.atEnd())
+	    {  
+  		reader2.readNext();
+
+  		if(reader2.isStartElement())
+  		{
+    		    if(reader2.name() == "status")
+    		    {		
+      			reader2.readNext();
+      			if(reader2.atEnd()) 
+				break;
+
+      			if(reader2.isCharacters())
+      			{
+        		    QString text = reader2.text().toString();
+			    qDebug() << text;
+			    status = text;
+			    break;
+      			}
+    		    }
+  	        }
+
+	     }
 
             // cache geocoding
             if (useCache && geo.startsWith("200")) {
@@ -567,23 +675,36 @@ internals::PointLatLng UrlFactory::GetLatLngFromGeocoderUrl(const QString &url,
     }
 
 
-    // parse values
-    // true : 200,4,56.1451640,22.0681787
-    // false: 602,0,0,0
     {
-        QStringList values = geo.split(',');
+            if (status == "OK") {
-        if (values.count() == 4) {
+                double lat = QString(latxml).toDouble();
-            status = (GeoCoderStatusCode::Types)QString(values[0]).toInt();
+                double lng = QString(lonxml).toDouble();
-            if (status == GeoCoderStatusCode::G_GEO_SUCCESS) {
-                double lat = QString(values[2]).toDouble();
-                double lng = QString(values[3]).toDouble();
 
                 ret = internals::PointLatLng(lat, lng);
 #ifdef DEBUG_URLFACTORY
+                qDebug() << "Status is: " << status;
                 qDebug() << "Lat=" << lat << " Lng=" << lng;
 #endif // DEBUG_URLFACTORY
             }
-        }
+	    else if (status == "ZERO_RESULTS") {
+		qDebug() << "No results";
+	    }
+	    else if (status == "OVER_QUERY_LIMIT") {
+		qDebug() << "You are over quota on queries";
+	    }
+	    else if (status == "REQUEST_DENIED") {
+		qDebug() << "Request was denied";
+	    }
+	    else if (status == "INVALID_REQUEST") {
+		qDebug() << "Invalid request, missing address, lat long or location";
+	    }
+	    else if (status == "UNKNOWN_ERROR") {
+		qDebug() << "Some sort of server error.";
+	    }
+	    else
+	    {
+	        qDebug() << "UrlFactory loop error";
+	    }
     }
     return ret;
 }

ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.h

@@ -56,7 +56,7 @@ public:
     UrlFactory();
     ~UrlFactory();
     QString MakeImageUrl(const MapType::Types &type, const core::Point &pos, const int &zoom, const QString &language);
-    internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords, GeoCoderStatusCode::Types &status);
+    internals::PointLatLng GetLatLngFromGeodecoder(const QString &keywords, QString &status);
     Placemark GetPlacemarkFromGeocoder(internals::PointLatLng location);
     int Timeout;
 private:
@@ -79,7 +79,7 @@ protected:
     void setIsCorrectGoogleVersions(bool value);
     QString MakeGeocoderUrl(QString keywords);
     QString MakeReverseGeocoderUrl(internals::PointLatLng &pt, const QString &language);
-    internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, GeoCoderStatusCode::Types &status);
+    internals::PointLatLng GetLatLngFromGeocoderUrl(const QString &url, const bool &useCache, QString &status);
     Placemark GetPlacemarkFromReverseGeocoderUrl(const QString &url, const bool &useCache);
 };
 }

ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.cpp

@@ -377,15 +377,18 @@ void Core::OnMapClose()
 
     CancelAsyncTasks();
 }
-GeoCoderStatusCode::Types Core::SetCurrentPositionByKeywords(QString const & keys)
+QString Core::SetCurrentPositionByKeywords(QString const & keys)
 {
-    GeoCoderStatusCode::Types status = GeoCoderStatusCode::Unknow;
+    QString status = "ZERO_RESULTS";
     PointLatLng pos = OPMaps::Instance()->GetLatLngFromGeodecoder(keys, status);
 
-    if (!pos.IsEmpty() && (status == GeoCoderStatusCode::G_GEO_SUCCESS)) {
+    if (!pos.IsEmpty() && (status == "OK")) {
         SetCurrentPosition(pos);
     }
-
+    else
+    {
+	qDebug() << "Status is not OK: " << status; 
+    }
     return status;
 }
 RectLatLng Core::CurrentViewArea()

ground/openpilotgcs/src/libs/opmapcontrol/src/internals/core.h

@@ -266,7 +266,7 @@ public:
 
     void OnMapClose(); // TODO had as slot
 
-    GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys);
+    QString SetCurrentPositionByKeywords(QString const & keys);
 
     RectLatLng CurrentViewArea();
 

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h

@@ -229,7 +229,7 @@ private:
     {
         core->ReloadMap();
     }
-    GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys)
+    QString SetCurrentPositionByKeywords(QString const & keys)
     {
         return core->SetCurrentPositionByKeywords(keys);
     }

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h

@@ -350,7 +350,7 @@ public:
         map->ReloadMap(); map->resize();
     }
 
-    GeoCoderStatusCode::Types SetCurrentPositionByKeywords(QString const & keys)
+    QString SetCurrentPositionByKeywords(QString const & keys)
     {
         return map->SetCurrentPositionByKeywords(keys);
     }

ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui

@@ -1,658 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>CC_HW_Widget</class>
- <widget class="QWidget" name="CC_HW_Widget">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>646</width>
-    <height>596</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>Form</string>
-  </property>
-  <layout class="QVBoxLayout" name="verticalLayout">
-   <item>
-    <widget class="QTabWidget" name="tabWidget">
-     <property name="currentIndex">
-      <number>0</number>
-     </property>
-     <widget class="QWidget" name="tab">
-      <attribute name="title">
-       <string>HW settings</string>
-      </attribute>
-      <layout class="QVBoxLayout" name="verticalLayout_3">
-       <property name="leftMargin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
-        <number>0</number>
-       </property>
-       <item>
-        <widget class="QScrollArea" name="scrollArea">
-         <property name="palette">
-          <palette>
-           <active>
-            <colorrole role="Base">
-             <brush brushstyle="SolidPattern">
-              <color alpha="255">
-               <red>255</red>
-               <green>255</green>
-               <blue>255</blue>
-              </color>
-             </brush>
-            </colorrole>
-            <colorrole role="Window">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-           </active>
-           <inactive>
-            <colorrole role="Base">
-             <brush brushstyle="SolidPattern">
-              <color alpha="255">
-               <red>255</red>
-               <green>255</green>
-               <blue>255</blue>
-              </color>
-             </brush>
-            </colorrole>
-            <colorrole role="Window">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-           </inactive>
-           <disabled>
-            <colorrole role="Base">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-            <colorrole role="Window">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-           </disabled>
-          </palette>
-         </property>
-         <property name="frameShape">
-          <enum>QFrame::NoFrame</enum>
-         </property>
-         <property name="frameShadow">
-          <enum>QFrame::Plain</enum>
-         </property>
-         <property name="widgetResizable">
-          <bool>true</bool>
-         </property>
-         <widget class="QWidget" name="scrollAreaWidgetContents">
-          <property name="geometry">
-           <rect>
-            <x>0</x>
-            <y>0</y>
-            <width>624</width>
-            <height>510</height>
-           </rect>
-          </property>
-          <layout class="QGridLayout" name="gridLayout_3">
-           <property name="leftMargin">
-            <number>12</number>
-           </property>
-           <property name="topMargin">
-            <number>12</number>
-           </property>
-           <property name="rightMargin">
-            <number>12</number>
-           </property>
-           <property name="bottomMargin">
-            <number>12</number>
-           </property>
-           <item row="6" column="0" colspan="3">
-            <widget class="QGroupBox" name="groupBox_2">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="minimumSize">
-              <size>
-               <width>0</width>
-               <height>50</height>
-              </size>
-             </property>
-             <property name="title">
-              <string>Messages</string>
-             </property>
-             <layout class="QHBoxLayout" name="horizontalLayout_3">
-              <item>
-               <widget class="QLabel" name="problems">
-                <property name="font">
-                 <font>
-                  <weight>75</weight>
-                  <bold>true</bold>
-                 </font>
-                </property>
-                <property name="text">
-                 <string/>
-                </property>
-                <property name="textFormat">
-                 <enum>Qt::AutoText</enum>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-             </layout>
-            </widget>
-           </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_6">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="minimumSize">
-              <size>
-               <width>500</width>
-               <height>0</height>
-              </size>
-             </property>
-             <property name="maximumSize">
-              <size>
-               <width>500</width>
-               <height>16777215</height>
-              </size>
-             </property>
-             <property name="font">
-              <font>
-               <weight>75</weight>
-               <bold>true</bold>
-              </font>
-             </property>
-             <property name="text">
-              <string>Changes on this page only take effect after board reset or power cycle</string>
-             </property>
-             <property name="alignment">
-              <set>Qt::AlignCenter</set>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="5" column="1">
-            <spacer name="verticalSpacer">
-             <property name="orientation">
-              <enum>Qt::Vertical</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>20</width>
-               <height>25</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
-           <item row="3" column="1">
-            <layout class="QHBoxLayout" name="horizontalLayout">
-             <item>
-              <layout class="QGridLayout" name="gridLayout_2">
-               <item row="0" column="1">
-                <widget class="QComboBox" name="telemetrySpeed">
-                 <property name="toolTip">
-                  <string>Select the speed here.</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="0">
-                <widget class="QLabel" name="GpsSpeedLabel">
-                 <property name="minimumSize">
-                  <size>
-                   <width>55</width>
-                   <height>0</height>
-                  </size>
-                 </property>
-                 <property name="text">
-                  <string>GPS speed:</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="0">
-                <widget class="QLabel" name="telemetrySpeedLabel">
-                 <property name="text">
-                  <string>Telemetry speed:</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="1">
-                <widget class="QComboBox" name="comUsbBridgeSpeed">
-                 <property name="toolTip">
-                  <string>Select the speed here.</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="1">
-                <widget class="QComboBox" name="gpsSpeed">
-                 <property name="toolTip">
-                  <string>Select the speed here.</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="0">
-                <widget class="QLabel" name="ComUsbBridgeSpeedLabel">
-                 <property name="minimumSize">
-                  <size>
-                   <width>55</width>
-                   <height>0</height>
-                  </size>
-                 </property>
-                 <property name="text">
-                  <string>ComUsbBridge speed:</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="0">
-                <widget class="QLabel" name="GpsProtocolLabel">
-                 <property name="text">
-                  <string>GPS protocol :</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="1">
-                <widget class="QComboBox" name="gpsProtocol"/>
-               </item>
-              </layout>
-             </item>
-             <item>
-              <spacer name="horizontalSpacer_3">
-               <property name="orientation">
-                <enum>Qt::Horizontal</enum>
-               </property>
-               <property name="sizeHint" stdset="0">
-                <size>
-                 <width>40</width>
-                 <height>20</height>
-                </size>
-               </property>
-              </spacer>
-             </item>
-            </layout>
-           </item>
-           <item row="2" column="1">
-            <layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0,0,0,0,0">
-             <item row="4" column="0">
-              <widget class="QComboBox" name="cbFlexi"/>
-             </item>
-             <item row="6" column="0">
-              <widget class="QComboBox" name="cbTele"/>
-             </item>
-             <item row="3" column="3">
-              <widget class="QLabel" name="label_8">
-               <property name="text">
-                <string>USB HID Port</string>
-               </property>
-               <property name="alignment">
-                <set>Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft</set>
-               </property>
-              </widget>
-             </item>
-             <item row="7" column="0">
-              <widget class="QLabel" name="label_4">
-               <property name="text">
-                <string/>
-               </property>
-              </widget>
-             </item>
-             <item row="5" column="3">
-              <widget class="QLabel" name="label_9">
-               <property name="text">
-                <string>USB VCP Port</string>
-               </property>
-               <property name="alignment">
-                <set>Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft</set>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="1" rowspan="5" colspan="2">
-              <widget class="QLabel" name="label_2">
-               <property name="text">
-                <string/>
-               </property>
-               <property name="scaledContents">
-                <bool>false</bool>
-               </property>
-              </widget>
-             </item>
-             <item row="5" column="0">
-              <widget class="QLabel" name="label_3">
-               <property name="text">
-                <string>Main Port</string>
-               </property>
-               <property name="alignment">
-                <set>Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft</set>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="0">
-              <widget class="QLabel" name="label_5">
-               <property name="text">
-                <string>Flexi Port</string>
-               </property>
-               <property name="alignment">
-                <set>Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft</set>
-               </property>
-              </widget>
-             </item>
-             <item row="4" column="3">
-              <widget class="QComboBox" name="cbUsbHid"/>
-             </item>
-             <item row="4" column="4">
-              <spacer name="horizontalSpacer_2">
-               <property name="orientation">
-                <enum>Qt::Horizontal</enum>
-               </property>
-               <property name="sizeHint" stdset="0">
-                <size>
-                 <width>40</width>
-                 <height>20</height>
-                </size>
-               </property>
-              </spacer>
-             </item>
-             <item row="6" column="3">
-              <widget class="QComboBox" name="cbUsbVcp"/>
-             </item>
-             <item row="2" column="1" colspan="2" alignment="Qt::AlignLeft">
-              <widget class="QComboBox" name="cbRcvr"/>
-             </item>
-             <item row="1" column="1" colspan="2" alignment="Qt::AlignLeft">
-              <widget class="QLabel" name="label_7">
-               <property name="text">
-                <string>Receiver Port</string>
-               </property>
-               <property name="alignment">
-                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-               </property>
-              </widget>
-             </item>
-            </layout>
-           </item>
-           <item row="2" column="0">
-            <spacer name="horizontalSpacer_4">
-             <property name="orientation">
-              <enum>Qt::Horizontal</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>40</width>
-               <height>20</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
-           <item row="2" column="2">
-            <spacer name="horizontalSpacer_5">
-             <property name="orientation">
-              <enum>Qt::Horizontal</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>40</width>
-               <height>20</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
-           <item row="1" column="1">
-            <spacer name="verticalSpacer_2">
-             <property name="orientation">
-              <enum>Qt::Vertical</enum>
-             </property>
-             <property name="sizeType">
-              <enum>QSizePolicy::Fixed</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>20</width>
-               <height>10</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
-          </layout>
-         </widget>
-        </widget>
-       </item>
-      </layout>
-     </widget>
-    </widget>
-   </item>
-   <item>
-    <layout class="QHBoxLayout" name="horizontalLayout_2">
-     <property name="spacing">
-      <number>4</number>
-     </property>
-     <item>
-      <spacer name="horizontalSpacer">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>369</width>
-         <height>20</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
-     <item>
-      <widget class="QPushButton" name="cchwHelp">
-       <property name="sizePolicy">
-        <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
-         <horstretch>0</horstretch>
-         <verstretch>0</verstretch>
-        </sizepolicy>
-       </property>
-       <property name="minimumSize">
-        <size>
-         <width>25</width>
-         <height>25</height>
-        </size>
-       </property>
-       <property name="maximumSize">
-        <size>
-         <width>25</width>
-         <height>25</height>
-        </size>
-       </property>
-       <property name="toolTip">
-        <string>Takes you to the wiki page</string>
-       </property>
-       <property name="text">
-        <string/>
-       </property>
-       <property name="icon">
-        <iconset resource="../coreplugin/core.qrc">
-         <normaloff>:/core/images/helpicon.svg</normaloff>:/core/images/helpicon.svg</iconset>
-       </property>
-       <property name="iconSize">
-        <size>
-         <width>25</width>
-         <height>25</height>
-        </size>
-       </property>
-       <property name="flat">
-        <bool>true</bool>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QPushButton" name="saveTelemetryToRAM">
-       <property name="minimumSize">
-        <size>
-         <width>0</width>
-         <height>0</height>
-        </size>
-       </property>
-       <property name="maximumSize">
-        <size>
-         <width>16777215</width>
-         <height>16777215</height>
-        </size>
-       </property>
-       <property name="palette">
-        <palette>
-         <active>
-          <colorrole role="Base">
-           <brush brushstyle="SolidPattern">
-            <color alpha="0">
-             <red>255</red>
-             <green>255</green>
-             <blue>255</blue>
-            </color>
-           </brush>
-          </colorrole>
-          <colorrole role="Window">
-           <brush brushstyle="SolidPattern">
-            <color alpha="255">
-             <red>232</red>
-             <green>232</green>
-             <blue>232</blue>
-            </color>
-           </brush>
-          </colorrole>
-         </active>
-         <inactive>
-          <colorrole role="Base">
-           <brush brushstyle="SolidPattern">
-            <color alpha="0">
-             <red>255</red>
-             <green>255</green>
-             <blue>255</blue>
-            </color>
-           </brush>
-          </colorrole>
-          <colorrole role="Window">
-           <brush brushstyle="SolidPattern">
-            <color alpha="255">
-             <red>232</red>
-             <green>232</green>
-             <blue>232</blue>
-            </color>
-           </brush>
-          </colorrole>
-         </inactive>
-         <disabled>
-          <colorrole role="Base">
-           <brush brushstyle="SolidPattern">
-            <color alpha="255">
-             <red>232</red>
-             <green>232</green>
-             <blue>232</blue>
-            </color>
-           </brush>
-          </colorrole>
-          <colorrole role="Window">
-           <brush brushstyle="SolidPattern">
-            <color alpha="255">
-             <red>232</red>
-             <green>232</green>
-             <blue>232</blue>
-            </color>
-           </brush>
-          </colorrole>
-         </disabled>
-        </palette>
-       </property>
-       <property name="toolTip">
-        <string>Send to OpenPilot but don't write in SD.
-Beware of not locking yourself out!</string>
-       </property>
-       <property name="autoFillBackground">
-        <bool>false</bool>
-       </property>
-       <property name="styleSheet">
-        <string notr="true"/>
-       </property>
-       <property name="text">
-        <string>Apply</string>
-       </property>
-       <property name="iconSize">
-        <size>
-         <width>16</width>
-         <height>16</height>
-        </size>
-       </property>
-       <property name="checkable">
-        <bool>false</bool>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QPushButton" name="saveTelemetryToSD">
-       <property name="minimumSize">
-        <size>
-         <width>0</width>
-         <height>0</height>
-        </size>
-       </property>
-       <property name="maximumSize">
-        <size>
-         <width>16777215</width>
-         <height>16777215</height>
-        </size>
-       </property>
-       <property name="toolTip">
-        <string>Applies and Saves all settings to SD.
-Beware of not locking yourself out!</string>
-       </property>
-       <property name="autoFillBackground">
-        <bool>false</bool>
-       </property>
-       <property name="styleSheet">
-        <string notr="true"/>
-       </property>
-       <property name="text">
-        <string>Save</string>
-       </property>
-      </widget>
-     </item>
-    </layout>
-   </item>
-  </layout>
- </widget>
- <resources>
-  <include location="../coreplugin/core.qrc"/>
- </resources>
- <connections/>
-</ui>

ground/openpilotgcs/src/plugins/config/ccattitude.ui

@@ -1,597 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>ccattitude</class>
- <widget class="QWidget" name="ccattitude">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>780</width>
-    <height>566</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>Form</string>
-  </property>
-  <layout class="QVBoxLayout" name="verticalLayout_5">
-   <item>
-    <widget class="QTabWidget" name="tabWidget">
-     <property name="currentIndex">
-      <number>0</number>
-     </property>
-     <widget class="QWidget" name="Attitude">
-      <attribute name="title">
-       <string>Attitude</string>
-      </attribute>
-      <layout class="QVBoxLayout" name="verticalLayout_3">
-       <property name="leftMargin">
-        <number>0</number>
-       </property>
-       <property name="topMargin">
-        <number>0</number>
-       </property>
-       <property name="rightMargin">
-        <number>0</number>
-       </property>
-       <property name="bottomMargin">
-        <number>0</number>
-       </property>
-       <item>
-        <widget class="QScrollArea" name="scrollArea">
-         <property name="palette">
-          <palette>
-           <active>
-            <colorrole role="Base">
-             <brush brushstyle="SolidPattern">
-              <color alpha="255">
-               <red>255</red>
-               <green>255</green>
-               <blue>255</blue>
-              </color>
-             </brush>
-            </colorrole>
-            <colorrole role="Window">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-           </active>
-           <inactive>
-            <colorrole role="Base">
-             <brush brushstyle="SolidPattern">
-              <color alpha="255">
-               <red>255</red>
-               <green>255</green>
-               <blue>255</blue>
-              </color>
-             </brush>
-            </colorrole>
-            <colorrole role="Window">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-           </inactive>
-           <disabled>
-            <colorrole role="Base">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-            <colorrole role="Window">
-             <brush brushstyle="SolidPattern">
-              <color alpha="0">
-               <red>232</red>
-               <green>232</green>
-               <blue>232</blue>
-              </color>
-             </brush>
-            </colorrole>
-           </disabled>
-          </palette>
-         </property>
-         <property name="frameShape">
-          <enum>QFrame::NoFrame</enum>
-         </property>
-         <property name="frameShadow">
-          <enum>QFrame::Plain</enum>
-         </property>
-         <property name="widgetResizable">
-          <bool>true</bool>
-         </property>
-         <widget class="QWidget" name="scrollAreaWidgetContents">
-          <property name="geometry">
-           <rect>
-            <x>0</x>
-            <y>0</y>
-            <width>758</width>
-            <height>486</height>
-           </rect>
-          </property>
-          <layout class="QVBoxLayout" name="verticalLayout">
-           <property name="leftMargin">
-            <number>12</number>
-           </property>
-           <property name="topMargin">
-            <number>12</number>
-           </property>
-           <property name="rightMargin">
-            <number>12</number>
-           </property>
-           <property name="bottomMargin">
-            <number>12</number>
-           </property>
-           <item>
-            <widget class="QGroupBox" name="groupBox">
-             <property name="title">
-              <string>Rotate virtual attitude relative to board</string>
-             </property>
-             <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,0,1,0,1,0">
-              <item row="0" column="1">
-               <widget class="QLabel" name="label_2">
-                <property name="styleSheet">
-                 <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
-color: rgb(255, 255, 255);
-border-radius: 5;
-margin:1px;
-font:bold;</string>
-                </property>
-                <property name="text">
-                 <string>Roll</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="1">
-               <widget class="QSpinBox" name="rollBias">
-                <property name="minimum">
-                 <number>-180</number>
-                </property>
-                <property name="maximum">
-                 <number>180</number>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="4">
-               <spacer name="horizontalSpacer_9">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>40</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-              <item row="1" column="5">
-               <widget class="QSpinBox" name="yawBias">
-                <property name="minimum">
-                 <number>-180</number>
-                </property>
-                <property name="maximum">
-                 <number>180</number>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="5">
-               <widget class="QLabel" name="label_4">
-                <property name="styleSheet">
-                 <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
-color: rgb(255, 255, 255);
-border-radius: 5;
-margin:1px;
-font:bold;</string>
-                </property>
-                <property name="text">
-                 <string>Yaw</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="2">
-               <spacer name="horizontalSpacer_8">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>40</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-              <item row="0" column="3">
-               <widget class="QLabel" name="label_3">
-                <property name="styleSheet">
-                 <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
-color: rgb(255, 255, 255);
-border-radius: 5;
-margin:1px;
-font:bold;</string>
-                </property>
-                <property name="text">
-                 <string>Pitch</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="3">
-               <widget class="QSpinBox" name="pitchBias">
-                <property name="minimum">
-                 <number>-90</number>
-                </property>
-                <property name="maximum">
-                 <number>90</number>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="0">
-               <spacer name="horizontalSpacer_7">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>40</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-              <item row="1" column="6">
-               <spacer name="horizontalSpacer_10">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>40</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-             </layout>
-            </widget>
-           </item>
-           <item>
-            <widget class="QGroupBox" name="groupBox_2">
-             <property name="title">
-              <string>Calibration</string>
-             </property>
-             <layout class="QGridLayout" name="gridLayout_2">
-              <item row="0" column="0" colspan="3">
-               <layout class="QHBoxLayout" name="horizontalLayout_2">
-                <item>
-                 <spacer name="horizontalSpacer_2">
-                  <property name="orientation">
-                   <enum>Qt::Horizontal</enum>
-                  </property>
-                  <property name="sizeHint" stdset="0">
-                   <size>
-                    <width>40</width>
-                    <height>20</height>
-                   </size>
-                  </property>
-                 </spacer>
-                </item>
-                <item>
-                 <widget class="QLabel" name="label">
-                  <property name="text">
-                   <string>Place aircraft very flat, and then click level to compute the accelerometer and gyro bias</string>
-                  </property>
-                  <property name="wordWrap">
-                   <bool>true</bool>
-                  </property>
-                 </widget>
-                </item>
-                <item>
-                 <spacer name="horizontalSpacer_3">
-                  <property name="orientation">
-                   <enum>Qt::Horizontal</enum>
-                  </property>
-                  <property name="sizeHint" stdset="0">
-                   <size>
-                    <width>40</width>
-                    <height>20</height>
-                   </size>
-                  </property>
-                 </spacer>
-                </item>
-               </layout>
-              </item>
-              <item row="1" column="0">
-               <widget class="QPushButton" name="zeroBias">
-                <property name="maximumSize">
-                 <size>
-                  <width>500</width>
-                  <height>16777215</height>
-                 </size>
-                </property>
-                <property name="toolTip">
-                 <string>Launch horizontal calibration.</string>
-                </property>
-                <property name="text">
-                 <string>Level</string>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="1">
-               <spacer name="horizontalSpacer_4">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeType">
-                 <enum>QSizePolicy::Fixed</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>10</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-              <item row="1" column="2">
-               <widget class="QProgressBar" name="zeroBiasProgress">
-                <property name="minimumSize">
-                 <size>
-                  <width>300</width>
-                  <height>0</height>
-                 </size>
-                </property>
-                <property name="value">
-                 <number>0</number>
-                </property>
-               </widget>
-              </item>
-              <item row="2" column="0" colspan="3">
-               <layout class="QHBoxLayout" name="horizontalLayout_3">
-                <item>
-                 <widget class="QCheckBox" name="zeroGyroBiasOnArming">
-                  <property name="toolTip">
-                   <string>If enabled, a fast recalibration of gyro zero point will be done
-whenever the frame is armed. Do not move the airframe while
-arming it in that case!</string>
-                  </property>
-                  <property name="text">
-                   <string>Zero gyros while arming aircraft</string>
-                  </property>
-                 </widget>
-                </item>
-                <item>
-                 <spacer name="horizontalSpacer_6">
-                  <property name="orientation">
-                   <enum>Qt::Horizontal</enum>
-                  </property>
-                  <property name="sizeHint" stdset="0">
-                   <size>
-                    <width>40</width>
-                    <height>20</height>
-                   </size>
-                  </property>
-                 </spacer>
-                </item>
-               </layout>
-              </item>
-             </layout>
-            </widget>
-           </item>
-           <item>
-            <widget class="QGroupBox" name="groupBox_3">
-             <property name="title">
-              <string>Filtering</string>
-             </property>
-             <layout class="QHBoxLayout" name="horizontalLayout_5">
-              <item>
-               <widget class="QLabel" name="label_5">
-                <property name="minimumSize">
-                 <size>
-                  <width>0</width>
-                  <height>0</height>
-                 </size>
-                </property>
-                <property name="toolTip">
-                 <string/>
-                </property>
-                <property name="text">
-                 <string>Accelerometers</string>
-                </property>
-               </widget>
-              </item>
-              <item>
-               <spacer name="horizontalSpacer_12">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeType">
-                 <enum>QSizePolicy::Fixed</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>10</width>
-                  <height>0</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-              <item>
-               <widget class="QDoubleSpinBox" name="accelTauSpinbox">
-                <property name="minimumSize">
-                 <size>
-                  <width>60</width>
-                  <height>0</height>
-                 </size>
-                </property>
-                <property name="toolTip">
-                 <string>Accelerometer filtering.
-
-Sets the amount of lowpass filtering of accelerometer data 
-for the attitude estimation. Higher values apply a stronger 
-filter, which may help with drifting in attitude mode.
-
-Range: 0.00 - 0.20, Good starting value: 0.05 - 0.10
-Start low and raise until drift stops.
-
-A setting of 0.00 disables the filter.</string>
-                </property>
-                <property name="decimals">
-                 <number>2</number>
-                </property>
-                <property name="maximum">
-                 <double>0.200000000000000</double>
-                </property>
-                <property name="singleStep">
-                 <double>0.010000000000000</double>
-                </property>
-               </widget>
-              </item>
-              <item>
-               <spacer name="horizontalSpacer_5">
-                <property name="orientation">
-                 <enum>Qt::Horizontal</enum>
-                </property>
-                <property name="sizeHint" stdset="0">
-                 <size>
-                  <width>40</width>
-                  <height>20</height>
-                 </size>
-                </property>
-               </spacer>
-              </item>
-             </layout>
-            </widget>
-           </item>
-           <item>
-            <spacer name="verticalSpacer_2">
-             <property name="orientation">
-              <enum>Qt::Vertical</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>20</width>
-               <height>10</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
-          </layout>
-         </widget>
-        </widget>
-       </item>
-      </layout>
-     </widget>
-    </widget>
-   </item>
-   <item>
-    <layout class="QHBoxLayout" name="horizontalLayout">
-     <property name="spacing">
-      <number>4</number>
-     </property>
-     <item>
-      <spacer name="horizontalSpacer">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="sizeHint" stdset="0">
-        <size>
-         <width>380</width>
-         <height>20</height>
-        </size>
-       </property>
-      </spacer>
-     </item>
-     <item>
-      <widget class="QPushButton" name="ccAttitudeHelp">
-       <property name="sizePolicy">
-        <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
-         <horstretch>0</horstretch>
-         <verstretch>0</verstretch>
-        </sizepolicy>
-       </property>
-       <property name="maximumSize">
-        <size>
-         <width>25</width>
-         <height>25</height>
-        </size>
-       </property>
-       <property name="toolTip">
-        <string>Takes you to the wiki page</string>
-       </property>
-       <property name="text">
-        <string/>
-       </property>
-       <property name="icon">
-        <iconset resource="../coreplugin/core.qrc">
-         <normaloff>:/core/images/helpicon.svg</normaloff>:/core/images/helpicon.svg</iconset>
-       </property>
-       <property name="iconSize">
-        <size>
-         <width>25</width>
-         <height>25</height>
-        </size>
-       </property>
-       <property name="flat">
-        <bool>true</bool>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QPushButton" name="applyButton">
-       <property name="minimumSize">
-        <size>
-         <width>0</width>
-         <height>0</height>
-        </size>
-       </property>
-       <property name="text">
-        <string>Apply</string>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QPushButton" name="saveButton">
-       <property name="minimumSize">
-        <size>
-         <width>0</width>
-         <height>0</height>
-        </size>
-       </property>
-       <property name="toolTip">
-        <string>Click to permanently save the accel bias in the CopterControl Flash.</string>
-       </property>
-       <property name="text">
-        <string>Save</string>
-       </property>
-      </widget>
-     </item>
-    </layout>
-   </item>
-  </layout>
- </widget>
- <resources>
-  <include location="../coreplugin/core.qrc"/>
- </resources>
- <connections/>
-</ui>

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp

@@ -420,7 +420,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType)
         m_aircraft->multiThrottleCurve->initCurve(&curveValues);
     } else {
         // no, init a straight curve
-        m_aircraft->multiThrottleCurve->initLinearCurve(curveValues.count(), 0.9);
+        m_aircraft->multiThrottleCurve->initLinearCurve(curveValues.count(), 1.0);
     }
 
     GUIConfigDataUnion config    = getConfigData();

ground/openpilotgcs/src/plugins/config/config.pro

@@ -21,8 +21,6 @@ HEADERS += \
     configinputwidget.h \
     configoutputwidget.h \
     configvehicletypewidget.h \
-    config_cc_hw_widget.h \
-    configccattitudewidget.h \
     configstabilizationwidget.h \
     assertions.h \
     defaultattitudewidget.h \
@@ -68,8 +66,6 @@ SOURCES += \
     configinputwidget.cpp \
     configoutputwidget.cpp \
     configvehicletypewidget.cpp \
-    config_cc_hw_widget.cpp \
-    configccattitudewidget.cpp \
     configstabilizationwidget.cpp \
     defaultattitudewidget.cpp \
     defaulthwsettingswidget.cpp \
@@ -106,12 +102,10 @@ FORMS += \
     airframe_ground.ui \
     airframe_multirotor.ui \
     airframe_custom.ui \
-    cc_hw_settings.ui \
     stabilization.ui \
     input.ui \
     input_wizard.ui \
     output.ui \
-    ccattitude.ui \
     defaultattitude.ui \
     defaulthwsettings.ui \
     inputchannelform.ui \
@@ -121,6 +115,6 @@ FORMS += \
     txpid.ui \
     mixercurve.ui \
     configrevohwwidget.ui \
-    oplink.ui
+    oplink.ui 
 
 RESOURCES += configgadget.qrc