change armed to controlmode

This commit is contained in:
interfisch 2019-06-13 18:59:03 +02:00
parent fc8f082179
commit 216db6080d
2 changed files with 43 additions and 26 deletions

View File

@ -9,8 +9,8 @@
//PA2 may be defective on my bluepill //PA2 may be defective on my bluepill
//#define DEBUG #define DEBUG
#define PARAMETEROUTPUT //#define PARAMETEROUTPUT
uint8_t error = 0; uint8_t error = 0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long #define IMU_NO_CHANGE 2 //IMU values did not change for too long
uint8_t imu_no_change_counter = 0; 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 uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255
#define NRFDATA_CENTER 127 #define NRFDATA_CENTER 127
boolean armed = false; //boolean armed = false;
boolean lastpacketOK = false; boolean lastpacketOK = false;
//Gametrak //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() { void setup() {
@ -248,7 +253,7 @@ void loop() {
done = radio.read( &lastnrfdata, sizeof(nrfdata) ); done = radio.read( &lastnrfdata, sizeof(nrfdata) );
if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered 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(); last_nrfreceive = millis();
//parse commands //parse commands
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0 motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
if (!motorenabled) { //disable motors?
armed = false;
}
} }
/*
#ifdef DEBUG #ifdef DEBUG
Serial.print("Received:"); Serial.print("Received:");
Serial.print(" st="); Serial.print(" st=");
@ -281,6 +284,7 @@ void loop() {
Serial.print(nrf_delay); Serial.print(nrf_delay);
Serial.println(); Serial.println();
#endif #endif
*/
//y positive = forward //y positive = forward
//x positive = right //x positive = right
@ -304,17 +308,16 @@ void loop() {
} }
if (error > 0) { //disarm if error occured
armed = false;
} if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive controlmode = MODE_DISARMED;
armed = false;
#ifdef DEBUG #ifdef DEBUG
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
#endif #endif
} }
if (armed) { //is armed if (controlmode == MODE_RADIONRF) { //is armed in nrf mode
armed_gt = false; //if remote used disable gametrak
if (lastpacketOK) { //if lastnrfdata is valid if (lastpacketOK) { //if lastnrfdata is valid
if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) { 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 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 #ifdef DEBUG
if (!lastpacketOK) { if (!lastpacketOK) {
Serial.println("Armed but packet not ok"); Serial.println("Armed but packet not ok");
} }
#endif #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 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 //Gametrak Control Code
motorenabled=true; motorenabled=true;
if (gt_length<=GT_LENGTH_MIN){ //let go if (gt_length<=GT_LENGTH_MIN){ //let go
armed_gt=false; controlmode=MODE_DISARMED;
motorenabled=false; motorenabled=false;
} }
int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward 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_steer = 0;
out_speed = 0; out_speed = 0;
} }
if (millis() - last_send > SENDPERIOD) { if (millis() - last_send > SENDPERIOD) {
//calculate checksum //calculate checksum
@ -422,8 +431,12 @@ void loop() {
Serial.print(out_speed); Serial.print(out_speed);
Serial.print(" checksum="); Serial.print(" checksum=");
Serial.print(out_checksum); Serial.print(out_checksum);
Serial.print(" controlmode=");
Serial.print(controlmode);
Serial.println(); Serial.println();
#endif #endif
} }
@ -436,9 +449,13 @@ void loop() {
//Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes //Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes
//Serial.write((uint8_t *) &floatvalue, sizeof(floatvalue)); //float, 4 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_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 *) &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 *) &vbat, sizeof(vbat)); //float, 4 bytes
//Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes //Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes
float yaw_float=yaw; float yaw_float=yaw;

@ -1 +1 @@
Subproject commit 68c6cf81d6c4e1625e76aec9414cdfbd041c94ed Subproject commit 56266c35e92385d051728bad3288c05f1e1ab3f4