From 216db6080d4e2538190ea6fadc116cad06a7652c Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Thu, 13 Jun 2019 18:59:03 +0200
Subject: [PATCH] change armed to controlmode

---
 controller/controller.ino | 67 ++++++++++++++++++++++++---------------
 serialVisualization       |  2 +-
 2 files changed, 43 insertions(+), 26 deletions(-)

diff --git a/controller/controller.ino b/controller/controller.ino
index f286d1a..c002a4f 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -9,8 +9,8 @@
 
 //PA2 may be defective on my bluepill
 
-//#define DEBUG
-#define PARAMETEROUTPUT
+#define DEBUG
+//#define PARAMETEROUTPUT
 uint8_t error = 0;
 #define IMU_NO_CHANGE 2 //IMU values did not change for too long
 uint8_t imu_no_change_counter = 0;
@@ -140,11 +140,16 @@ int16_t lastsend_out_speed = 0;
 uint8_t out_checksum = 0; //0= disable motors, 255=reserved,  1<=checksum<255
 #define NRFDATA_CENTER 127
 
-boolean armed = false;
+//boolean armed = false;
 boolean lastpacketOK = false;
 
 //Gametrak
-boolean armed_gt = false;
+//boolean armed_gt = false;
+
+uint8_t controlmode=0;
+#define MODE_DISARMED 0
+#define MODE_RADIONRF 1
+#define MODE_GAMETRAK 2
 
 void setup() {
 
@@ -248,7 +253,7 @@ void loop() {
       done = radio.read( &lastnrfdata, sizeof(nrfdata) );
 
       if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered
-        armed = true; //arm at first received packet
+        controlmode = MODE_RADIONRF;//set radionrf mode at first received packet
       }
 
 
@@ -260,12 +265,10 @@ void loop() {
         last_nrfreceive = millis();
 
         //parse commands
-        motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
-        if (!motorenabled) { //disable motors?
-          armed = false;
-        }
+        motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
       }
 
+/*
 #ifdef DEBUG
       Serial.print("Received:");
       Serial.print(" st=");
@@ -281,6 +284,7 @@ void loop() {
       Serial.print(nrf_delay);
       Serial.println();
 #endif
+*/
 
       //y positive = forward
       //x positive = right
@@ -304,17 +308,16 @@ void loop() {
   }
 
 
-  if (error > 0) { //disarm if error occured
-    armed = false;
-  }
-  if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
-    armed = false;
+  
+  
+  if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
+    controlmode = MODE_DISARMED;
     #ifdef DEBUG
     Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
     #endif
   }
-  if (armed) { //is armed
-    armed_gt = false; //if remote used disable gametrak
+  if (controlmode == MODE_RADIONRF) { //is armed in nrf mode
+
     
     if (lastpacketOK) { //if lastnrfdata is valid
       if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
@@ -355,27 +358,30 @@ void loop() {
       }
     }//if pastpacket not ok, keep last out_steer and speed values until disarmed
 
+    if (!motorenabled) { //radio connected but not actively driving, keep values reset
+      setYaw = yaw;
+      magalign_multiplier = 0;
+    }
+
 #ifdef DEBUG
     if (!lastpacketOK) {
       Serial.println("Armed but packet not ok");
     }
 #endif
-  }else{ //not armed, keep values reset
-    setYaw = yaw;
-    magalign_multiplier = 0;
+
   }
 
 
 
-  if (!armed_gt) { //check if gametrak will be armed
+  if (controlmode==MODE_DISARMED) { //check if gametrak can be armed
     if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
-      armed_gt=true;
+      controlmode=MODE_GAMETRAK; //enable gametrak mode
     }
-  }else if (armed_gt && !armed){ //gametrak control active and not remote active
+  }else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
     //Gametrak Control Code
     motorenabled=true;
     if (gt_length<=GT_LENGTH_MIN){ //let go
-      armed_gt=false;
+      controlmode=MODE_DISARMED;
       motorenabled=false;
     }
     int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
@@ -388,13 +394,16 @@ void loop() {
   }
 
 
-  if (!armed && !armed_gt){ //all disarmed
+  if (error > 0) { //disarm if error occured
+    controlmode = MODE_DISARMED; //force disarmed
+  }
+
+  if (controlmode == MODE_DISARMED){ //all disarmed
     out_steer = 0;
     out_speed = 0;
   }
 
 
-
   
   if (millis() - last_send > SENDPERIOD) {
     //calculate checksum
@@ -422,8 +431,12 @@ void loop() {
     Serial.print(out_speed);
     Serial.print(" checksum=");
     Serial.print(out_checksum);
+
+    Serial.print(" controlmode=");
+    Serial.print(controlmode);
   
     Serial.println();
+    
   #endif
   }
 
@@ -436,9 +449,13 @@ void loop() {
     //Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes
     //Serial.write((uint8_t *) &floatvalue, sizeof(floatvalue)); //float, 4 bytes
 
+    uint8_t booleanvalues=0; //reset
+    booleanvalues |= motorenabled<<0; //bit 0
+    booleanvalues |= (controlmode&0b00000011)<<1; //bit 1 and 2 (2bit number for controlmodes (3)
     
     Serial.write((uint8_t *) &out_steer, sizeof(out_steer)); //int16_t, 2 bytes
     Serial.write((uint8_t *) &out_speed, sizeof(out_speed)); //int16_t, 2 bytes
+    Serial.write((uint8_t *) &booleanvalues, sizeof(booleanvalues)); //uint8_t, 1 byte //booleanvalues
     Serial.write((uint8_t *) &vbat, sizeof(vbat)); //float, 4 bytes
     //Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes
     float yaw_float=yaw;
diff --git a/serialVisualization b/serialVisualization
index 68c6cf8..56266c3 160000
--- a/serialVisualization
+++ b/serialVisualization
@@ -1 +1 @@
-Subproject commit 68c6cf81d6c4e1625e76aec9414cdfbd041c94ed
+Subproject commit 56266c35e92385d051728bad3288c05f1e1ab3f4