disarm only at timeout

This commit is contained in:
interfisch 2019-06-01 18:51:16 +02:00
parent 26353508aa
commit 886ecc5b7a
1 changed files with 174 additions and 156 deletions

View File

@ -73,7 +73,7 @@ struct nrfdata {
nrfdata lastnrfdata; nrfdata lastnrfdata;
long last_nrfreceive = 0; //last time values were received and checksum ok long last_nrfreceive = 0; //last time values were received and checksum ok
long nrf_delay = 0; long nrf_delay = 0;
#define MAX_NRFDELAY 50 #define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm
//command variables //command variables
boolean motorenabled = false; //set by nrfdata.commands boolean motorenabled = false; //set by nrfdata.commands
@ -89,6 +89,7 @@ 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;
@ -142,26 +143,28 @@ void loop() {
bool done = false; bool done = false;
while (!done) while (!done)
{ {
lastpacketOK = false; //initialize with false, if checksum ok gets set to true
digitalWrite(PIN_LED, !digitalRead(PIN_LED)); digitalWrite(PIN_LED, !digitalRead(PIN_LED));
done = radio.read( &lastnrfdata, sizeof(nrfdata) ); done = radio.read( &lastnrfdata, sizeof(nrfdata) );
if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==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 armed = true; //arm at first received packet
} }
uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13));
if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
lastpacketOK = true;
last_nrfreceive = millis();
//parse commands //parse commands
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0 motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
if (!motorenabled) { //disable motors? if (!motorenabled) { //disable motors?
armed = false; armed = false;
} }
uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
armed=false;
}else{ //checksum ok
last_nrfreceive=millis();
} }
#ifdef DEBUG #ifdef DEBUG
@ -177,6 +180,7 @@ void loop() {
Serial.print("nrfdelay="); Serial.print("nrfdelay=");
Serial.print(nrf_delay); Serial.print(nrf_delay);
Serial.println();
#endif #endif
//y positive = forward //y positive = forward
@ -205,8 +209,12 @@ void loop() {
} }
if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
armed = false; armed = false;
#ifdef DEBUG
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
#endif
} }
if (armed){ if (armed) { //is armed
if (lastpacketOK) { //if lastnrfdata is valid
if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) { if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
last_controlupdate = millis(); last_controlupdate = millis();
@ -229,6 +237,7 @@ void loop() {
int16_t out_steer_mag = (int16_t)( yawdiff ); int16_t out_steer_mag = (int16_t)( yawdiff );
float new_magalign_multiplier = map( abs((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering float new_magalign_multiplier = map( abs((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering
new_magalign_multiplier = 0; //Force mag off
new_magalign_multiplier = constrain(new_magalign_multiplier, 0.0, 1.0); new_magalign_multiplier = constrain(new_magalign_multiplier, 0.0, 1.0);
magalign_multiplier = min(new_magalign_multiplier, min(1.0, magalign_multiplier + 0.01)); //go down fast, slowly increase magalign_multiplier = min(new_magalign_multiplier, min(1.0, magalign_multiplier + 0.01)); //go down fast, slowly increase
@ -242,8 +251,15 @@ void loop() {
Serial.print("Out steer="); Serial.print("Out steer=");
Serial.println(out_steer);*/ Serial.println(out_steer);*/
} }
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
}else{ //took too long since last nrf data #ifdef DEBUG
if (!lastpacketOK)
Serial.println("Armed but packet not ok");
}
#endif
} else { //disarmed
out_steer = 0; out_steer = 0;
out_speed = 0; out_speed = 0;
setYaw = yaw; setYaw = yaw;
@ -256,7 +272,9 @@ void loop() {
if (millis() - last_send > SENDPERIOD) { if (millis() - last_send > SENDPERIOD) {
//calculate checksum //calculate checksum
out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum
if (out_checksum==0 || out_checksum==255){ out_checksum=1; } //cannot be 0 or 255 (special purpose) if (out_checksum == 0 || out_checksum == 255) {
out_checksum = 1; //cannot be 0 or 255 (special purpose)
}
if (!motorenabled) { //disable motors? if (!motorenabled) { //disable motors?
out_checksum = 0; //checksum=0 disables motors out_checksum = 0; //checksum=0 disables motors
@ -315,9 +333,9 @@ void updateIMU()
pitch = imu.getPitch(); pitch = imu.getPitch();
yaw = imu.getYaw(); yaw = imu.getYaw();
/*Directions: /*Directions:
* Components on top. Components on top.
* Roll: around Y axis (pointing to the right), left negative Roll: around Y axis (pointing to the right), left negative
* Pitch: around X axis (pointing forward), up positive Pitch: around X axis (pointing forward), up positive
* Yaw: around Z axis, CCW positive, 0 to 360 Yaw: around Z axis, CCW positive, 0 to 360
*/ */
} }