Merge branch 'RemoteControl'

Makefile

@@ -3,9 +3,9 @@
 
 GCC_BIN = 
 PROJECT = FlyBed
-OBJECTS = ./main.o ./PC/PC.o ./RC/RC_Channel.o ./IMU/IMU_10DOF.o ./IMU/IMU_Filter/IMU_Filter.o ./IMU/MPU9250/MPU9250.o ./IMU/MPU6050/MPU6050.o ./IMU/MPU6050/I2C_Sensor.o ./LED/LED.o ./PID/PID.o ./Servo/Servo.o ./ParameterSystem/ParameterSystem.o
+OBJECTS = ./main.o ./PC/PC.o ./RemoteControl/RemoteControl.o ./RemoteControl/RC_Channel.o ./IMU/IMU_10DOF.o ./IMU/IMU_Filter/IMU_Filter.o ./IMU/MPU9250/MPU9250.o ./IMU/MPU6050/MPU6050.o ./IMU/MPU6050/I2C_Sensor.o ./LED/LED.o ./PID/PID.o ./Servo/Servo.o ./ParameterSystem/ParameterSystem.o
 SYS_OBJECTS = ./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM/board.o ./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM/cmsis_nvic.o ./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM/retarget.o ./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM/startup_LPC17xx.o ./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM/system_LPC17xx.o
-INCLUDE_PATHS = -I. -I./PC -I./RC -I./IMU -I./IMU/IMU_Filter -I./IMU/MPU9250 -I./IMU/MPU6050 -I./LED -I./PID -I./Servo -I./ParameterSystem -I./mbed -I./mbed/TARGET_LPC1768 -I./mbed/TARGET_LPC1768/TARGET_NXP -I./mbed/TARGET_LPC1768/TARGET_NXP/TARGET_LPC176X -I./mbed/TARGET_LPC1768/TARGET_NXP/TARGET_LPC176X/TARGET_MBED_LPC1768 -I./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM
+INCLUDE_PATHS = -I. -I./PC -I./RemoteControl -I./IMU -I./IMU/IMU_Filter -I./IMU/MPU9250 -I./IMU/MPU6050 -I./LED -I./PID -I./Servo -I./ParameterSystem -I./mbed -I./mbed/TARGET_LPC1768 -I./mbed/TARGET_LPC1768/TARGET_NXP -I./mbed/TARGET_LPC1768/TARGET_NXP/TARGET_LPC176X -I./mbed/TARGET_LPC1768/TARGET_NXP/TARGET_LPC176X/TARGET_MBED_LPC1768 -I./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM
 LIBRARY_PATHS = -L./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM
 LIBRARIES = -lmbed
 LINKER_SCRIPT = ./mbed/TARGET_LPC1768/TOOLCHAIN_GCC_ARM/LPC1768.ld

RemoteControl/RC_Channel.cpp

@@ -0,0 +1,33 @@
+#include "RC_Channel.h"
+
+#include "mbed.h"
+
+RC_Channel::RC_Channel(PinName pin) : _interrupt(pin) {
+    _time = -100; // start value to see if there was any value yet
+
+    _interrupt.rise(this, &RC_Channel::rise);	// attach interrupts such that the driver starts processing the signal
+    _interrupt.fall(this, &RC_Channel::fall);
+    _timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
+}
+
+int RC_Channel::read() {
+    return _time;
+}
+
+void RC_Channel::rise() {
+    _timer.start();
+}
+
+void RC_Channel::fall() {
+    _timer.stop();
+    int tester = _timer.read_us();
+    if(tester >= 1000 && tester <= 2000)
+        _time = tester-1000;  // we want only the signal from 1000 - 2000 as 0 - 1000 for easier scaling
+    _timer.reset();
+    _timer.start();
+}
+
+void RC_Channel::timeoutcheck() {
+    if (_timer.read() > 0.3)
+        _time = -100;
+}

RemoteControl/RC_Channel.h

@@ -0,0 +1,24 @@
+#ifndef RC_CHANNEL_H
+#define RC_CHANNEL_H
+
+#include "mbed.h"
+
+class RC_Channel {
+    public:
+        RC_Channel(PinName mypin);	// NO p19/p20!!!!, they don't support InterruptIn
+        int read();					// read the last measured data
+        bool present() {return !(read() == -100);}
+
+    //private:
+        int _time;					// last measurement data
+
+        InterruptIn _interrupt;		// interrupt on the pin to react when signal falls or rises
+        void rise();				// start the time measurement when signal rises
+        void fall();				// stop the time measurement and save the value when signal falls
+        Timer _timer;				// timer to measure the up time of the signal and if the signal timed out
+
+        Ticker _timeoutchecker;		// Ticker to see if signal broke down
+        void timeoutcheck();		// to check for timeout, checked every second
+};
+
+#endif

RemoteControl/RemoteControl.cpp

@@ -0,0 +1,92 @@
+/*
+ * RemoteControl.cpp
+ *
+ *  Created on: 04.12.2015
+ *      Author: MaEtUgR
+ */
+
+#include "RemoteControl.h"
+
+RemoteControl::RemoteControl() :
+	_channels({RC_Channel(p8), RC_Channel(p15), RC_Channel(p17), RC_Channel(p16), RC_Channel(p25), RC_Channel(p26), RC_Channel(p29)}),
+	_params("RC")
+{
+	_armed = true;
+	_stickCentring = true;
+	for(int i = 0; i < 3; i++)
+		_angle[i] = 0;
+
+	_params.readASCIIFile();
+	if (_params.size() != CHANNELS * 2) {
+		_params.resize(CHANNELS * 2);
+
+		for(int i = 0; i < CHANNELS; i++) // set all offsets to zero
+			_params.setParameter(i, 0);
+
+		for(int i = CHANNELS; i < CHANNELS*2; i++) // set all scales to 1
+			_params.setParameter(i, 1);
+	}
+}
+
+bool RemoteControl::present() {
+	return (_channels[AILERON].present() && _channels[ELEVATOR].present() && _channels[RUDDER].present() && _channels[THROTTLE].present());
+}
+
+float RemoteControl::getValue(int i) {
+	float calibratedValue = (_channels[i].read() + _params[i]) * _params[CHANNELS + i];
+
+	if(_stickCentring && i != THROTTLE && 485 < calibratedValue && calibratedValue < 515)
+		calibratedValue = 500;
+
+	return _channels[i].present() ? calibratedValue : -100;
+}
+
+void RemoteControl::run(float IMU_yaw) {
+	if(getValue(THROTTLE) < 20 && getValue(RUDDER) > 850) {					// arming / disarming
+		_armed = true;
+		_angle[2] = IMU_yaw;
+	}
+	if((getValue(THROTTLE) < 30 && getValue(RUDDER) < 30) || !present())
+		_armed = false;
+
+	for(int i=0;i<2;i++)													// RC Angle ROLL-PITCH-Part
+		if (present())
+			_angle[i] = (getValue(i)-500)*RC_SENSITIVITY/500.0;
+		else
+			_angle[i] = 0;
+
+	float yaw_adding;                  										// RC Angle YAW-Part
+	if (present() && getValue(THROTTLE) > 20)
+		yaw_adding = -(getValue(RUDDER)-500)*YAWSPEED/500;					// the yaw angle is integrated from stick input		TODO: make this independent of loop frequency
+	else
+		yaw_adding = 0;
+
+	_angle[2] = _angle[2] + yaw_adding < -180 ? _angle[2] + 360 + yaw_adding : _angle[2] + yaw_adding; // make shure it's in the cycle -180 to 180
+	_angle[2] = _angle[2] + yaw_adding > 180 ? _angle[2] - 360 + yaw_adding : _angle[2] + yaw_adding;
+}
+
+void RemoteControl::calibrate(int seconds) {
+	int mins[CHANNELS];
+	int maxs[CHANNELS];
+	fill(mins, mins+CHANNELS-1, 1000);
+	fill(maxs, maxs+CHANNELS-1, 0);
+	Timer calibrationTimer;
+	calibrationTimer.start();
+
+	while(calibrationTimer.read() < seconds)
+		for(int i = 0; i < CHANNELS; i++) {
+			int value = _channels[i].read();
+			printf("%d ", value);
+			if(i == CHANNELS-1)
+				printf("\r\n");
+			mins[i] = fminf(mins[i], value);
+			maxs[i] = fmaxf(maxs[i], value);
+		}
+
+	for(int i = 0; i < CHANNELS; i++) {
+		_params.setParameter(i, -mins[i]); // get the offset	TODO: setParameter with operator overloading params[i] = v;
+		_params.setParameter(CHANNELS + i, 1000/(float)(maxs[i]-mins[i])); // get the scale
+	}
+
+	_params.writeASCIIFile(); // safe the calibration
+}

RemoteControl/RemoteControl.h

@@ -0,0 +1,48 @@
+/*
+ * RemoteControl.h
+ *
+ *  Created on: 04.12.2015
+ *      Author: MaEtUgR
+ */
+
+#ifndef REMOTECONTROL_H_
+#define REMOTECONTROL_H_
+
+#include "mbed.h"
+#include "RC_Channel.h"
+#include "ParameterSystem.h"
+
+#define CHANNELS 7	// TODO: make more general
+#define RC_SENSITIVITY  30      // maximal angle from horizontal that the PID is aming for
+#define YAWSPEED        1.0f    // maximal speed of yaw rotation in degree per Rate
+
+#define AILERON         0       // RC
+#define ELEVATOR        1		// TODO: enum
+#define RUDDER          2
+#define THROTTLE        3
+#define CHANNEL8        4
+#define CHANNEL7        5
+#define CHANNEL6        6
+
+class RemoteControl {
+public:
+	RemoteControl();
+
+	inline bool		armed() {return _armed;}							// for safety (when true the QC is DANGEROUS!! Take this SERIOUS!!)
+	bool			present();											// shows if remote control is working	TODO: Failsafe
+	float			getValue(int i);									// get the Value from a specific channel of the remote control
+	inline float	operator[](int i) {return getValue(i);}
+
+	void			run(float IMU_yaw);									// needs to be called in the main loop to get everything updated
+	void			enableStickCentering() {_stickCentring = true;}		// values near the middle position get pulled to exactly the middle value (so there's no drift when you let go of the sticks)
+	void			calibrate(int seconds);								// calibrate the remote control (move the sticks to all the limits during the seconds)
+
+//private:
+	RC_Channel			_channels[CHANNELS];							// driver to get values for the channels
+	bool				_armed;											// safety flag
+	ParameterSystem		_params;										// calibration parameters which also get saved to the flash
+	bool				_stickCentring;									// centering feature flag
+	float 				_angle[3];										// set pount angle of the RC Sticks, to steer the QC in level mode
+};
+
+#endif