disarm only at timeout
This commit is contained in:
parent
26353508aa
commit
886ecc5b7a
310
controller.ino
310
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
|
||||
|
||||
|
||||
|
|
@ -31,11 +31,11 @@ long last_imuupdated=0;
|
|||
//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;
|
||||
|
||||
|
||||
|
||||
|
|
@ -100,7 +101,7 @@ void setup() {
|
|||
|
||||
|
||||
pinMode(PIN_LED, OUTPUT);
|
||||
digitalWrite(PIN_LED,HIGH);
|
||||
digitalWrite(PIN_LED, HIGH);
|
||||
|
||||
|
||||
Serial.println("Initializing nrf24");
|
||||
|
|
@ -111,11 +112,11 @@ void setup() {
|
|||
|
||||
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
|
||||
}
|
||||
|
||||
//parse commands
|
||||
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
|
||||
if (!motorenabled){ //disable motors?
|
||||
armed=false;
|
||||
if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered
|
||||
armed = true; //arm at first received packet
|
||||
}
|
||||
|
||||
|
||||
|
||||
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();
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef DEBUG
|
||||
#ifdef DEBUG
|
||||
Serial.print("Received:");
|
||||
Serial.print(" st=");
|
||||
Serial.print(lastnrfdata.steer);
|
||||
|
|
@ -177,147 +180,162 @@ void loop() {
|
|||
|
||||
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
|
||||
*/
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue