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
//#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,14 +394,17 @@ 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
out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum
@ -423,7 +432,11 @@ void loop() {
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;

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