roll control-ish

GPS_IMU.cpp

@@ -268,7 +268,12 @@ void IMU_join_data()
 	intUnion.byte[1] = IMU_buffer[j++];
 	ground_course = intUnion.word;
 
+	// clear error
+	GPS_update &= ~GPS_IMU_ERROR;
+
+	// set bit
 	GPS_update |= GPS_IMU;
+
 }
 
 

RC.cpp

@@ -13,7 +13,6 @@ RC::RC() {
 
 	// initialize class members
 	for(i=0;i<9;i++) _ppm_sum[i]=0;
-//	for(i=0;i<8;i++) *(float *)(&_rcCmd+i) = 0.0;
 	_sp = -1;
 	_sync_error = ERROR_SYNC;
 
@@ -51,6 +50,10 @@ void RC::update() {
 	_pitch = _rcCmd[1];
 	_roll = _rcCmd[0];
 	_throttle = _rcCmd[2];
+	_aux1 = _rcCmd[4];
+	_aux2 = _rcCmd[5];
+	_aux3 = _rcCmd[6];
+	_aux4 = _rcCmd[7];
 
 }
 
@@ -85,6 +88,17 @@ float RC::get_channel(int channel) {
 	case CH_YAW:
 		return _yaw;
 
+	case CH_AUX1:
+		return _aux1;
+
+	case CH_AUX2:
+		return _aux2;
+
+	case CH_AUX3:
+		return _aux3;
+
+	case CH_AUX4:
+		return _aux4;
 	}
 
 	return -999.9;

RC.h

@@ -15,6 +15,10 @@
 #define CH_PITCH 2
 #define CH_THROTTLE 3
 #define CH_YAW 4
+#define CH_AUX1 5
+#define CH_AUX2 6
+#define CH_AUX3 7
+#define CH_AUX4 8
 
 #define CH_INVALID 0.0
 #define ERROR_SYNC_PULSE -1
@@ -48,6 +52,10 @@ class RC {
 	float 	_pitch;
 	float 	_yaw;
 	float 	_roll;
+	float 	_aux1;
+	float 	_aux2;
+	float 	_aux3;
+	float 	_aux4;
 };
 
 #endif /* RC_H_ */

esc-control.cpp

@@ -49,7 +49,11 @@ void set_rotor_throttle(int rotor, float rate)
 		return;
 	}
 
-	if(rate < 0 || rate > 1) return;
+	if(rate < 0)
+		rate = 0.0;
+
+	if(rate > 1)
+		rate = 1.0;
 
 	// 1.00ms = 3430counts = 0% (0deg)
 	// 1.25ms = 4096counts =

main.cpp

@@ -57,7 +57,8 @@ int main(void) {
 	uint32 t_prev, t_delta;
 	uint32 cpu_util, cpu_util2;
 	uint32 tick50hz=0;
-	float error;
+	float errorRoll, desiredRoll, KpRoll;
+	float m1, m2, m3;
 
 	RC rc;
 	AHRS ahrs;
@@ -82,11 +83,24 @@ int main(void) {
     		rc.update();  // update RC commands
     		ahrs.update();
 
-    		error = rc.get_channel( CH_THROTTLE );
-
-    		set_rotor_throttle(1, rc.get_channel( CH_THROTTLE ));
-			set_rotor_throttle(2, rc.get_channel( CH_THROTTLE ));
-			set_rotor_throttle(3, rc.get_channel( CH_THROTTLE ));
+    		desiredRoll = 50*2*(rc.get_channel( CH_ROLL )-0.5);
+    		errorRoll = desiredRoll - ahrs.get_roll();
+    		KpRoll = rc.get_channel( CH_AUX1 )*-0.1;
+
+    		if(rc.get_channel(CH_AUX4) < 0.5 )
+    		{
+				m1 = rc.get_channel( CH_THROTTLE ) + (-1)*KpRoll*errorRoll;
+				m2 = rc.get_channel( CH_THROTTLE ) + (+1)*KpRoll*errorRoll;
+				m3 = 0.0;
+    		} else {
+    			m1 = 0.0;
+    			m2 = 0.0;
+    			m3 = 0.0;
+    		}
+
+    		set_rotor_throttle( 1, m1 );
+			set_rotor_throttle( 2, m2 );
+			set_rotor_throttle( 3, m2 );
 
 			// 10Hz loop
 			if(tick50hz %  5 == 0)
@@ -95,7 +109,8 @@ int main(void) {
 			}
 
 			// 2Hz loop
-			if(tick50hz % 25 == 0)
+//			if(tick50hz % 25 == 0)
+			if(tick50hz % 5 == 0) // 10 hz
 			{
 				// cpu utilization based on the 2000 microseconds (50 Hz) loop
 				cpu_util = (micros()-t)*100/t_delta;
@@ -117,16 +132,21 @@ int main(void) {
 				{
 					printkv("rc:", ! rc.status());
 					printkv("imu:", ahrs.get_status());
+					printkv("aux1:", rc.get_channel(CH_AUX1));
+					printkv("kp:", KpRoll);
+					printkv("dRoll:", desiredRoll);
+					printkv("m1:", m1);
+					printkv("m2:", m2);
 
-					printkv("  thr:", rc.get_channel( CH_THROTTLE));
+					printkv("thr:", rc.get_channel( CH_THROTTLE));
 					printkv("roll:", ahrs.get_roll());
 					printkv("pitch:", ahrs.get_pitch());
 					printkv("yaw:", ahrs.get_yaw());
 
 					// cpu utilization after printing data
-					printkv("  util:", cpu_util);
-					cpu_util2 = (micros()-t)*100/t_delta;
-					printkv("util2:",cpu_util2);
+//					printkv("  util:", cpu_util);
+//					cpu_util2 = (micros()-t)*100/t_delta;
+//					printkv("util2:",cpu_util2);
 
 					SerialUSB.println("");
 				}

sc

@@ -5,5 +5,7 @@ if [ -e $SCREENLOG_FILE ]; then
     rm screenlog.0
 fi
 
-screen -L /dev/ttyACM0
+if [ -e /dev/ttyACM0 ]; then
+    screen -L /dev/ttyACM0
+fi