Merge remote-tracking branch 'origin/master' into mmosca-ublox-satinfo

docs/Programming Framework.md

@@ -161,6 +161,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 40            | Yaw [deg]                             | Current heading (yaw) in `degrees` |
 | 41            | FW Land Sate                          | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
 | 42            | Current battery profile               | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` |
+| 43            | Flown Loiter Radius [m]               | The actual loiter radius flown by a fixed wing during hold modes, in `meters` |
 
 #### FLIGHT_MODE
 

docs/Serial Gimbal.md

@@ -0,0 +1,56 @@
+# Serial Gimbal
+INAV 8.0 introduces support for serial Gimbals. Currently, it is compatible with the protocol used by the Walksnail GM series gimbals.
+
+While these gimbals also support PWM as input, using the Serial protocol gives it more flexibility and saves up to 4 PWM channels. The downside of the Serial protocol vs PWM input is that you don't have access to the full power of INAV's mixers. The main advantage is that you gain easy control of gimbal functions using INAV's modes.
+
+# Axis Input
+The Serial Gimbal supports 2 differents inputs.
+
+## PWM Channels
+This is the simplest way to control the Gimbal, as you can use your radio mixer and sliders to Control the gimbal by assigning RC channels to functions in the ```Configuration``` tab. You can control all 3 gimbal axis and unlike the raw PWM input, gimbal modes are controlled by INAV modes and you can control roll channel as well, instead of wiring 4 servo outputs. If an rc channel is set to 0, that input will be ignore and will be equivalent to a centered RC channel. So, if you setup the serial gimbal and don't assign any rc channels, it will stay centered, with default sensitivity and will obey the Gimbal MODES setup in the Modes tab.
+
+## Headtracker Input
+Headtracker input is only used when you have a Headtracker device configured and the ```Gimbal Headtracker``` mode is active. 
+A Headtracker device is a device that transmits headtracker information by a side channel, instead of relying on your rc link.
+
+In head tracker mode, the Serial Gimbal will ignore the axis rc channel inputs and replace it with the inputs coming from the Headtracker device.
+
+# Gimbal Modes
+## No Gimbal mode selected
+Like ACRO is the default mode for flight modes, the Gimbal will default to ```FPV Mode``` or ```Follow Mode``` when no mode is selected. The gimbal will try to stablized the footag and will follow the aircraft pitch, roll and yaw movements and use user inputs to point the camera where the user wants.
+
+## Gimbal Center
+This locks the gimbal camera to the center position and ignores any user input. Useful to reset the camera if you loose orientation.
+
+## Gimbal Headtracker
+Switches inputs to the configured Headtracker device. If no device is configured it will behave like Gimbal Center.
+
+## Gimbal Level Tilt
+This mode locks the camera tilt (pitch axis) and keeps it level with the horizon. Pitching the aircraft up and down, will move the camera so it stays pointing at the horizon. It can be combined with ```Gimbal Level Roll```.
+
+## Gimbal Level Roll
+This mode locks the camera roll and keeps it level with the horizon. Rolling the aircraft will move the camera so it stays level with the horizon. It can be combined with ```Gimbal Level Tilt```.
+
+# Advanced settings
+The gimbal also supports some advanced settings not exposed in the configurator.
+
+## Gimbal Trim
+You can set a trim setting for the gimbal, the idea is that it will shift the notion of center of the gimbal, like a trim and let you setup a fixed camera up tilt, like you would have in a traditional fpv quad setup.
+
+```
+gimbal_pan_trim = 0
+Allowed range: -500 - 500
+
+gimbal_tilt_trim = 0
+Allowed range: -500 - 500
+
+gimbal_roll_trim = 0
+Allowed range: -500 - 500
+```
+
+## Gimbal and Headtracker on a single uart
+As INAV does not process any inputs from the Walksnail Gimbal, it is possible to share the uard with the Walksnail Headtracking output by connect the fc TX to the gimbal and RX to receive the headtracker input.
+```
+gimbal_serial_single_uart = OFF
+Allowed values: OFF, ON
+```

src/main/common/lulu.c

@@ -28,15 +28,16 @@ void luluFilterInit(luluFilter_t *filter, int N)
 
 FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
 {
-	register float curVal = 0;
+	float curVal = 0;
-	register float curValB = 0;
+	float curValB = 0;
 	for (int N = 1; N <= filterN; N++)
 	{
 		int indexNeg = (index + windowSize - 2 * N) % windowSize;
-		register int curIndex = (indexNeg + 1) % windowSize;
+	    int curIndex = (indexNeg + 1) % windowSize;
-		register float prevVal = series[indexNeg];
+	    float prevVal = series[indexNeg];
-		register float prevValB = seriesB[indexNeg];
+		float prevValB = seriesB[indexNeg];
-		register int indexPos = (curIndex + N) % windowSize;
+		int indexPos = (curIndex + N) % windowSize;
+		
         for (int i = windowSize - 2 * N; i < windowSize - N; i++)
 		{
 			if (indexPos >= windowSize)
@@ -50,8 +51,8 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 			// curIndex = (2 - 1) % 3 = 1
 			curVal = series[curIndex];
 			curValB = seriesB[curIndex];
-			register float nextVal = series[indexPos];
+			float nextVal = series[indexPos];
-			register float nextValB = seriesB[indexPos];
+			float nextValB = seriesB[indexPos];
 			// onbump (s, 1, 1, 3)
 			// if(onBump(series, curIndex, N, windowSize))
 			if (prevVal < curVal && curVal > nextVal)
@@ -59,7 +60,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 				float maxValue = MAX(prevVal, nextVal);
 
 				series[curIndex] = maxValue;
-				register int k = curIndex;
+			    int k = curIndex;
 				for (int j = 1; j < N; j++)
 				{
 					if (++k >= windowSize)
@@ -76,7 +77,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 
 				curVal = maxValue;
 				seriesB[curIndex] = maxValue;
-				register int k = curIndex;
+				int k = curIndex;
 				for (int j = 1; j < N; j++)
 				{
 					if (++k >= windowSize)
@@ -109,8 +110,8 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 			// curIndex = (2 - 1) % 3 = 1
 			curVal = series[curIndex];
 			curValB = seriesB[curIndex];
-			register float nextVal = series[indexPos];
+			float nextVal = series[indexPos];
-			register float nextValB = seriesB[indexPos];
+			float nextValB = seriesB[indexPos];
 
 			if (prevVal > curVal && curVal < nextVal)
 			{
@@ -118,7 +119,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 
 				curVal = minValue;
 				series[curIndex] = minValue;
-				register int k = curIndex;
+				int k = curIndex;
 				for (int j = 1; j < N; j++)
 				{
 					if (++k >= windowSize)
@@ -134,7 +135,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 				float minValue = MIN(prevValB, nextValB);
 				curValB = minValue;
 				seriesB[curIndex] = minValue;
-				register int k = curIndex;
+				int k = curIndex;
 				for (int j = 1; j < N; j++)
 				{
 					if (++k >= windowSize)
@@ -156,13 +157,13 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
 FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
 {
 	// This is the value N of the LULU filter.
-	register int filterN = filter->N;
+	int filterN = filter->N;
 	// This is the total window size for the rolling buffer
-	register int filterWindow = filter->windowSize;
+	int filterWindow = filter->windowSize;
 
-	register int windowIndex = filter->windowBufIndex;
+	int windowIndex = filter->windowBufIndex;
-	register float inputVal = input;
+	float inputVal = input;
-	register int newIndex = (windowIndex + 1) % filterWindow;
+	int newIndex = (windowIndex + 1) % filterWindow;
 	filter->windowBufIndex = newIndex;
 	filter->luluInterim[windowIndex] = inputVal;
 	filter->luluInterimB[windowIndex] = -inputVal;

src/main/navigation/navigation.c

@@ -5074,5 +5074,12 @@ bool canFwLandingBeCancelled(void)
 {
     return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
 }
-
 #endif
+uint16_t getFlownLoiterRadius(void)
+{
+    if (STATE(AIRPLANE) && navGetCurrentStateFlags() & NAV_CTL_HOLD) {
+        return CENTIMETERS_TO_METERS(calculateDistanceToDestination(&posControl.desiredState.pos));
+    }
+
+    return 0;
+}

src/main/navigation/navigation.h

@@ -694,6 +694,7 @@ bool rthAltControlStickOverrideCheck(uint8_t axis);
 
 int8_t navCheckActiveAngleHoldAxis(void);
 uint8_t getActiveWpNumber(void);
+uint16_t getFlownLoiterRadius(void);
 
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles

src/main/programming/logic_condition.c

@@ -830,6 +830,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
             return getLoiterRadius(navConfig()->fw.loiter_radius);
+            break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS:
+            return getFlownLoiterRadius();
+            break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
             return isEstimatedAglTrusted();

src/main/programming/logic_condition.h

@@ -143,6 +143,7 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg                     // 40
     LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE,                           // 41
     LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int                     // 42
+    LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS,                     // 43
 } logicFlightOperands_e;
 
 typedef enum {