interfisch
/
hoverbrett
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

mag auto align if idle

This commit is contained in:
interfisch 2019-03-20 23:29:09 +01:00
parent 719b0fa7c5
commit 26353508aa
1 changed files with 79 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
controller.ino
View File

@ -8,13 +8,20 @@
//set boot0 back to 0 to run program on powerup
//#define DEBUG
uint8_t error=0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
uint8_t imu_no_change_counter=0;
#define PIN_LED PC13
#define SENDPERIOD 20 //ms
#define CONTROLUPDATEPERIOD 10
long last_controlupdate=0;
#define IMUUPDATEPERIOD 10 //ms
long last_imuupdated=0;
#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered
#include <IMUGY85.h>
@ -24,8 +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 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
//from left to right. pins at bottom. chips on top
@ -51,6 +61,7 @@ RF24 radio(PB0,PB1); //ce, cs
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
#define NRF24CHANNEL 75
struct nrfdata {
uint8_t steer;
@ -97,6 +108,8 @@ void setup() {
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.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
@ -132,15 +145,17 @@ void loop() {
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.speed==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?
@ -185,35 +200,54 @@ 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 (armed){
//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 );
//Test Mag
/*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;
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);*/
}
yawdiff=yawdiff*yawdiff; //square
yawdiff=constrain(yawdiff/2,0,800);
yawdiff*=yawdiffsign; //redo sign
out_steer=(int16_t)( yawdiff );
*/
/*
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;
}
@ -251,6 +285,27 @@ void loop() {
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){
imu_no_change_counter++;
if (imu_no_change_counter>10){
error=IMU_NO_CHANGE;
Serial.println("Error: IMU_NO_CHANGE");
}
}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;
//Update Imu and write to variables
imu.update();
imu.getAcceleration(&ax, &ay, &az);