From 1ef8f56ed63661043ca135bf83e40eaac84471d8 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Mon, 24 Oct 2016 17:34:44 +0200
Subject: [PATCH] Changes to acc functionality. OBS! Not working correctly

---
 UAV-ControlSystem/src/Flight/pid.c |  8 +++++---
 UAV-ControlSystem/src/config/cli.c | 18 +++++++++++++++++-
 2 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 7814df5..082ef17 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -18,6 +18,7 @@
 #include "drivers/sbus.h"
 #include "scheduler/scheduler.h"
 #include <math.h>
+#include "drivers/failsafe_toggles.h"
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
 #define ITERM_SCALE 0.244381f										/*I-term used as a scale value to the PID controller*/
@@ -87,7 +88,8 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
 **************************************************************************/
 float convertData(int inputRange, int outputRange, int offset, float value)
 {
-	return (outputRange/inputRange)*(value-offset);
+	return ((float)outputRange/(float)inputRange)*(value-(float)offset);
+	//return 1.0;
 }
 
 /**************************************************************************
@@ -154,7 +156,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
 	{
 	case PID_ID_GYRO:
 
-		if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled)
+		if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
 		{
 			desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL]);
 			desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH]);
@@ -326,7 +328,7 @@ void pidRun(uint8_t ID)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled)
+		if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
 		{
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index 22b3005..acc5305 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/UAV-ControlSystem/src/config/cli.c
@@ -266,6 +266,10 @@ typedef enum
 	COMMAND_ID_PID_ACCEL_YAW_P_LIMIT,
 	COMMAND_ID_PID_ACCEL_OUT_LIMIT,
 
+	/* Enable the different pid loops */
+	COMMAND_ID_PID_GYRO_ISENABLED,
+	COMMAND_ID_PID_ACCEL_ISENABLED,
+
 	/* Counter for the amount of commands */
 	COMMAND_ID_COUNT,
 
@@ -634,8 +638,20 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		},
 		[COMMAND_ID_PID_ACCEL_OUT_LIMIT] =
 		{
-				"pid_accel_yaw_p_limit", 	COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000}
+				"pid_accel_out_limit", 	COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000}
 		},
+
+		/* Enable pid loops */
+		[COMMAND_ID_PID_GYRO_ISENABLED] =
+		{
+				"pid_gyro_isenabled", 	COMMAND_ID_PID_GYRO_ISENABLED, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
+		},
+		[COMMAND_ID_PID_ACCEL_ISENABLED] =
+		{
+				"pid_accel_isenabled", 	COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
+		},
+
+
 };
 
 /***********************************************************************