Added extra checks to ensure everything is zero before changing mode. F…

…ixes #25
BastiaanGrisel · Oct 13, 2015 · da4b137 · da4b137
1 parent 84ccebd
commit da4b137
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 23 deletions.
diff --git a/fpga/main.c b/fpga/main.c
@@ -128,17 +128,6 @@ bool set_mode(Mode new_mode) {
 		return false;
 	}
 
-	if(new_mode == CALIBRATE) {
-		s_bias[0] = s0 << SENSOR_PRECISION;
-		s_bias[1] = s1 << SENSOR_PRECISION;
-		s_bias[2] = s2 << SENSOR_PRECISION;
-		s_bias[3] = s3 << SENSOR_PRECISION;
-		s_bias[4] = s4 << SENSOR_PRECISION;
-		s_bias[5] = s5 << SENSOR_PRECISION;
-
-		is_calibrated = true;
-	}
-
 	if(new_mode >= MANUAL) {
 		// Make sure that a change to an operational mode can only be done via SAFE
 		if(mode >= MANUAL){
@@ -147,21 +136,33 @@ bool set_mode(Mode new_mode) {
 		}
 
 		// If at least one of the motor's RPM is not zero, return false
-		if(!motors_have_zero_rpm()) {
+		if(!motors_have_zero_rpm() || R != 0 || P != 0 || Y != 0 || T != 0 || !motors_have_zero_offset()) {
 			send_err_message(MODE_CHANGE_ONLY_IF_ZERO_RPM);
 			return false;
 		}
 	}
 
+	if((new_mode == FULL_CONTROL || new_mode == YAW_CONTROL) && !is_calibrated){
+		send_err_message(FIRST_CALIBRATE);
+		return false;
+	}
+
+	if(new_mode == CALIBRATE) {
+		s_bias[0] = s0 << SENSOR_PRECISION;
+		s_bias[1] = s1 << SENSOR_PRECISION;
+		s_bias[2] = s2 << SENSOR_PRECISION;
+		s_bias[3] = s3 << SENSOR_PRECISION;
+		s_bias[4] = s4 << SENSOR_PRECISION;
+		s_bias[5] = s5 << SENSOR_PRECISION;
+
+		is_calibrated = true;
+	}
+
 	if(new_mode == FULL_CONTROL) {
 		R_ACC_BIAS = DECREASE_SHIFT(s_bias[0]*R_ACC_RATIO,SENSOR_PRECISION);
 		Rbias = INCREASE_SHIFT(s_bias[3],C2_R_BIAS_UPDATE-SENSOR_PRECISION);
 	}
 
-	if((new_mode == FULL_CONTROL || new_mode == YAW_CONTROL) && !is_calibrated){
-		send_err_message(FIRST_CALIBRATE);
-		return false;
-	}
 
 	// If everything is OK, change the mode
 	mode = new_mode;

diff --git a/include/motors.h b/include/motors.h
@@ -32,6 +32,14 @@ bool motors_have_zero_rpm() {
 	return true;
 }
 
+bool motors_have_zero_offset() {
+	int32_t i;
+	for(i = 0; i < 4; i++)
+		if(get_motor_offset(i) != 0)
+			return false;
+	return true;
+}
+
 void set_motor_rpm(int32_t motor0, int32_t motor1, int32_t motor2, int32_t motor3) {
 	int32_t i;
 
@@ -48,13 +56,6 @@ void set_motor_rpm(int32_t motor0, int32_t motor1, int32_t motor2, int32_t motor
 	X32_QR_a1 = rpm[1];
 	X32_QR_a2 = rpm[2];
 	X32_QR_a3 = rpm[3];
-
-	/*if(T>0){ TODO: This logic does not belong here
-		motor0 = ((motor0 < T/4 ? T/4 : motor0) > 0x3ff)? 0x3ff : motor0;
-		motor1 = ((motor1 < T/4 ? T/4 : motor1) > 0x3ff)? 0x3ff : motor1;
-		motor2 = ((motor2 < T/4 ? T/4 : motor2) > 0x3ff)? 0x3ff : motor2;
-		motor3 = ((motor3 < T/4 ? T/4 : motor3) > 0x3ff)? 0x3ff : motor3;
-	}*/
 }
 
 void reset_motors() {