From 886ecc5b7a3261a24b24fba6a9523a6d3f03ff11 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sat, 1 Jun 2019 18:51:16 +0200 Subject: [PATCH] disarm only at timeout --- controller.ino | 330 ++++++++++++++++++++++++++----------------------- 1 file changed, 174 insertions(+), 156 deletions(-) diff --git a/controller.ino b/controller.ino index 1f7aaef..7690efa 100644 --- a/controller.ino +++ b/controller.ino @@ -8,19 +8,19 @@ //set boot0 back to 0 to run program on powerup //#define DEBUG -uint8_t error=0; +uint8_t error = 0; #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; #define PIN_LED PC13 #define SENDPERIOD 20 //ms #define CONTROLUPDATEPERIOD 10 -long last_controlupdate=0; +long last_controlupdate = 0; #define IMUUPDATEPERIOD 10 //ms -long last_imuupdated=0; +long last_imuupdated = 0; #define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered @@ -28,14 +28,14 @@ long last_imuupdated=0; //https://github.com/fookingthg/GY85 //ITG3200 and ADXL345 from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino //https://github.com/mechasolution/Mecha_QMC5883L //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer -//in qmc5883L library read values changed from uint16_t to int16_t +//in qmc5883L library read values changed from uint16_t to int16_t IMUGY85 imu; -double ax, ay, az, gx, gy, gz, roll, pitch, yaw,mx,my,mz,ma; -double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw,old_mx,old_my,old_mz,old_ma; +double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx, my, mz, ma; +double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw, old_mx, old_my, old_mz, old_ma; -double setYaw=0; -float magalign_multiplier=0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned +double setYaw = 0; +float magalign_multiplier = 0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned //from left to right. pins at bottom. chips on top @@ -54,7 +54,7 @@ float magalign_multiplier=0; //how much the magnetometer should influence steeri #include "nRF24L01.h" #include "RF24.h" -RF24 radio(PB0,PB1); //ce, cs +RF24 radio(PB0, PB1); //ce, cs //SCK D13 (Pro mini), A5 (bluepill) //Miso D12 (Pro mini), A6 (bluepill) //Mosi D11 (Pro mini), A7 (bluepill) @@ -71,24 +71,25 @@ struct nrfdata { }; nrfdata lastnrfdata; -long last_nrfreceive=0; //last time values were received and checksum ok -long nrf_delay=0; -#define MAX_NRFDELAY 50 +long last_nrfreceive = 0; //last time values were received and checksum ok +long nrf_delay = 0; +#define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm //command variables -boolean motorenabled=false; //set by nrfdata.commands +boolean motorenabled = false; //set by nrfdata.commands -long last_send=0; +long last_send = 0; -int16_t out_steer=0; //between -1000 and 1000 -int16_t out_speed=0; -uint8_t out_checksum=0; //0= disable motors, 255=reserved, 1<=checksum<255 +int16_t out_steer = 0; //between -1000 and 1000 +int16_t 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; @@ -98,24 +99,24 @@ void setup() { Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) - + pinMode(PIN_LED, OUTPUT); - digitalWrite(PIN_LED,HIGH); + digitalWrite(PIN_LED, HIGH); Serial.println("Initializing nrf24"); - + radio.begin(); radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive) - - radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries + + radio.setRetries(15, 15); // optionally, increase the delay between retries & # of retries radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability - + radio.openWritingPipe(pipes[0]); //write on pipe 0 - radio.openReadingPipe(1,pipes[1]); //read on pipe 1 + radio.openReadingPipe(1, pipes[1]); //read on pipe 1 radio.startListening(); @@ -129,42 +130,44 @@ void setup() { void loop() { - if (millis()-last_imuupdated>IMUUPDATEPERIOD){ + if (millis() - last_imuupdated > IMUUPDATEPERIOD) { updateIMU(); - last_imuupdated=millis(); + last_imuupdated = millis(); } - + //NRF24 - nrf_delay=millis()-last_nrfreceive; //update nrf delay + nrf_delay = millis() - last_nrfreceive; //update nrf delay if ( radio.available() ) { //Serial.println("radio available ..."); bool done = false; while (!done) { + lastpacketOK = false; //initialize with false, if checksum ok gets set to true digitalWrite(PIN_LED, !digitalRead(PIN_LED)); done = radio.read( &lastnrfdata, sizeof(nrfdata) ); - if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered - armed=true; //arm at first received packet + if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered + armed = true; //arm at first received packet } - //parse commands - motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0 - if (!motorenabled){ //disable motors? - armed=false; + + + + + uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13)); + if (lastnrfdata.checksum == calcchecksum) { //checksum ok? + lastpacketOK = true; + last_nrfreceive = millis(); + + //parse commands + motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0 + if (!motorenabled) { //disable motors? + 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 Serial.print("Received:"); Serial.print(" st="); Serial.print(lastnrfdata.steer); @@ -174,150 +177,165 @@ void loop() { Serial.print(lastnrfdata.commands); Serial.print(", chks="); Serial.print(lastnrfdata.checksum); - + Serial.print("nrfdelay="); Serial.print(nrf_delay); - #endif + Serial.println(); +#endif //y positive = forward //x positive = right /* - setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127; - while (setYaw<0){ + setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127; + while (setYaw<0){ setYaw+=360; - } - while (setYaw>=360){ + } + while (setYaw>=360){ setYaw-=360; - }*/ + }*/ /* - Serial.print("setYaw="); - Serial.print(setYaw); - Serial.print(" Yaw="); - Serial.println(yaw);*/ - + Serial.print("setYaw="); + Serial.print(setYaw); + Serial.print(" Yaw="); + Serial.println(yaw);*/ + } } - if (error>0){ //disarm if error occured - armed=false; + 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 (armed){ - if (millis()-last_controlupdate>CONTROLUPDATEPERIOD){ - last_controlupdate=millis(); - - //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); - //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); - out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 ); //-1000 to 1000 - out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 ); - - //align to compass - double yawdiff=(setYaw-180)-(yaw-180); //following angle difference works only for angles [-180,180]. yaw here is [0,360] - yawdiff += (yawdiff>180) ? -360 : (yawdiff<-180) ? 360 : 0; - //yawdiff/=2; - int yawdiffsign=1; - if (yawdiff<0){ - yawdiffsign=-1; - } - yawdiff=yawdiff*yawdiff; //square - yawdiff=constrain(yawdiff*1 ,0,800); - yawdiff*=yawdiffsign; //redo sign - 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 - 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=constrain(magalign_multiplier, 0.0,1.0); //safety constrain again - - out_steer = out_steer*(1-magalign_multiplier) + out_steer_mag*magalign_multiplier; - - setYaw=setYaw*magalign_multiplier + yaw*(1-magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw - - /* - Serial.print("Out steer="); - Serial.println(out_steer);*/ - } - - }else{ //took too long since last nrf data - out_steer=0; - out_speed=0; - setYaw=yaw; - magalign_multiplier=0; - } - - - - - if (millis()-last_send>SENDPERIOD){ - //calculate 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 (!motorenabled){ //disable motors? - out_checksum=0; //checksum=0 disables motors - } - - Serial2.write((uint8_t *) &out_steer, sizeof(out_steer)); - Serial2.write((uint8_t *) &out_speed, sizeof(out_speed)); - Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum)); - last_send=millis(); - - + if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive + armed = false; #ifdef DEBUG - Serial.print(" steer="); - Serial.print(out_steer); - Serial.print(" speed="); - Serial.print(out_speed); - Serial.print(" checksum="); - Serial.print(out_checksum); - - Serial.println(); + Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); #endif } + if (armed) { //is armed + if (lastpacketOK) { //if lastnrfdata is valid + if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) { + last_controlupdate = millis(); + + //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); + //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX ); + out_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000 + out_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ); + + //align to compass + double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360] + yawdiff += (yawdiff > 180) ? -360 : (yawdiff < -180) ? 360 : 0; + //yawdiff/=2; + int yawdiffsign = 1; + if (yawdiff < 0) { + yawdiffsign = -1; + } + yawdiff = yawdiff * yawdiff; //square + yawdiff = constrain(yawdiff * 1 , 0, 800); + yawdiff *= yawdiffsign; //redo sign + 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 + new_magalign_multiplier = 0; //Force mag off + 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 = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again + + out_steer = out_steer * (1 - magalign_multiplier) + out_steer_mag * magalign_multiplier; + + setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw + + /* + Serial.print("Out steer="); + Serial.println(out_steer);*/ + } + }//if pastpacket not ok, keep last out_steer and speed values until disarmed + +#ifdef DEBUG + if (!lastpacketOK) + Serial.println("Armed but packet not ok"); + } +#endif + +} else { //disarmed + out_steer = 0; + out_speed = 0; + setYaw = yaw; + magalign_multiplier = 0; +} + + + + +if (millis() - last_send > SENDPERIOD) { + //calculate 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 (!motorenabled) { //disable motors? + out_checksum = 0; //checksum=0 disables motors + } + + Serial2.write((uint8_t *) &out_steer, sizeof(out_steer)); + Serial2.write((uint8_t *) &out_speed, sizeof(out_speed)); + Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum)); + last_send = millis(); + + +#ifdef DEBUG + Serial.print(" steer="); + Serial.print(out_steer); + Serial.print(" speed="); + Serial.print(out_speed); + Serial.print(" checksum="); + Serial.print(out_checksum); + + Serial.println(); +#endif +} } void updateIMU() { - if (old_ax==ax && old_ay==ay && old_az==az && old_gx==gx && old_gy==gy && old_gz==gz && old_mx==mx && old_my==my && old_mz==mz){ + if (old_ax == ax && old_ay == ay && old_az == az && old_gx == gx && old_gy == gy && old_gz == gz && old_mx == mx && old_my == my && old_mz == mz) { imu_no_change_counter++; - if (imu_no_change_counter>10){ - error=IMU_NO_CHANGE; + if (imu_no_change_counter > 10) { + error = IMU_NO_CHANGE; Serial.println("Error: IMU_NO_CHANGE"); } - }else{ - imu_no_change_counter=0; + } else { + imu_no_change_counter = 0; } - old_ax=ax; - old_ay=ay; - old_az=az; - old_gx=gx; - old_gy=gy; - old_gz=gz; - old_mx=mx; - old_my=my; - old_mz=mz; - old_roll=roll; - old_pitch=pitch; - old_yaw=yaw; + old_ax = ax; + old_ay = ay; + old_az = az; + old_gx = gx; + old_gy = gy; + old_gz = gz; + old_mx = mx; + old_my = my; + old_mz = mz; + old_roll = roll; + old_pitch = pitch; + old_yaw = yaw; //Update Imu and write to variables imu.update(); imu.getAcceleration(&ax, &ay, &az); imu.getGyro(&gx, &gy, &gz); - imu.getMag(&mx, &my, &mz,&ma); //calibration data such as bias is set in IMUGY85.h + imu.getMag(&mx, &my, &mz, &ma); //calibration data such as bias is set in IMUGY85.h roll = imu.getRoll(); pitch = imu.getPitch(); yaw = imu.getYaw(); /*Directions: - * Components on top. - * Roll: around Y axis (pointing to the right), left negative - * Pitch: around X axis (pointing forward), up positive - * Yaw: around Z axis, CCW positive, 0 to 360 - */ + Components on top. + Roll: around Y axis (pointing to the right), left negative + Pitch: around X axis (pointing forward), up positive + Yaw: around Z axis, CCW positive, 0 to 360 + */ }