Bool is a number??

Projects Programming
1

Hi everyone,

I built a balancing drone last summer using at STM32. Video here. It flies OK, but I want to fine tune the control loops and need some in-flight data of it to do so. I can't seem to get the STM32 to work with any Arduino SD library, so I have used an Arduino Nano to receive packets of data via I2c from the STM32 then dump them onto an SD card.

I started to notice some pretty strange behaviour, inside the drone code there is a section that sends data over I2C when a certain switch is moved on the controller.

/////////// i2c data trasnfer begins here...../////////

  if (ch5 > 1300){
    
    String dataString1 = String(OPStartT) + "," + String(compangleY) + "," + String(RollTarget) + "," + String(RollError)
    + "," + String(dE_Roll);
    String dataString2 = "," + String(P_term_Roll) + "," +String(D_term_Roll) + "," + String(PID_Roll) + "," + String(PID_Roll_Avg); 
    //String dataString3 =  + "," + String(flag)
    
    dataString1.toCharArray(shortString1, 32);
    dataString2.toCharArray(shortString2, 27);

    Wire.beginTransmission(4);              // transmit to device #4
    Wire.write(shortString1);              // sends one string
    Wire.endTransmission(); 

    Wire.beginTransmission(4);              // transmit to device #4
    Wire.write(shortString2);              // sends second string 
    Wire.endTransmission();                // stop transmitting
  }

  /////////// i2c data trasnfer ends here...../////////

But I started to notice that even with the Arduino Nano not connected, when the data-logging switch was moved the propellers would start to spin on and off. The motors can spin for 2 reasons, you've added throttle, or the HDG PID loop has commanded it. (it yaws by differential thrust).

There is a statement which should inhibit the HDG PID value to '0' if the throttle is below a certain limit.

  if (!Flight) PID_HDG = 0;     

   TV_Pitch.writeMicroseconds(1555 + PID_Pitch);           //1555 uSec is mid-point of PWM signal
   TV_Roll.writeMicroseconds(1400 + PID_Roll);             //1400 uSec is mid-point of PWM signal
   Motor1.writeMicroseconds(throttle - PID_HDG + 15);      // Bottom motor (2), top propeller
   Motor2.writeMicroseconds(throttle + PID_HDG - 40);      // Top motor, bottom propeller

This code runs at 50Hz.

This bool value is called 'Flight' and only when the throttle PWM signal gets above 1350uS does it become 'True'.

  if (ch3 >= 1350 && !Flight){                           // Sets Current HDG as target
    Flight = true;
    HDG_Target = comp_True_HDG;
  }
  else if (ch3 < 1350){                                  // Test whether in flight or not
    Flight = false;
  }

This code above runs at 10Hz so I think that is why 'Flight' is getting reset to '0' every 4-5 cycles.

I did some tests and saw that this was in fact working normally with data-logging off but when data-logging is enabled the value of 'Flight' (bool) becomes erratic. (see photo)

Apart from initialising the variable as a bool these are the only instances 'Flight' is called.

How can a bool variable that isn't getting called change its value? Am I overloading the I2C bus or something? Is something corrupting inside the software? I'm really drawing a blank on it. All other functions of the drone seem to work apart from this.

The whole code is pretty long so not sure how much more people would like to see but happy to share as much as people need.

Any help or direction would be appreciated!

bool value?.png
bool value?.png1680×1050 352 KB

bool value?.png
bool value?.png1680×1050 352 KB

2

No way to know if there is a possibility of corruption unless you post your entire sketch.

3

Ok will do, hopefully by tonight.

4

I split the code into tabs, so each of these is a tab.

/* STM32 WITH PPM INPUT AND DUAL SERVO/ESC OUTPUT    BALANCING DRONE
 *  6.3 GPS library added
 *  6.4 Moved Alt PID to Input check, Error=2.dp, - Flies well
 *  6.5 Flies ok
 *  6.6 New dE/dT calc, change in measurement not Error - Flies well. Bit of tuning still
 *  6.7 atan,(!asin) Failsafe detect, New IMU gains, yaw inhibit, I2C_Read func. PID_Yaw fixed
 *  6.8 Compass & GPS ok, flies ok; twitch in flight angle
 *  6.9 comp yaw angle, 20Hz alt+mag, dE_Filt removed, HDG_PID, Flies ok, HDG PID needs tuning
 *  6.10 New angle cacls, Alt/GPS hold moved to flap switch, state estimation
 *  6.11 i2c data logging on ch5, causes 'Flight' to become a number??
 */

#include <Wire.h>
#include <Servo.h>
#include <Adafruit_BMP280.h>
#include <NMEAGPS.h>
#include <QMC5883LCompass.h>

#define gpsPort Serial1         // Connect GPS to A2 & A3

const int MPU_Address = 0x68;   // I2C address of the MPU-6050
const int Mag_Address = 0x0D;   // I2C address of the MPU-6050
static NMEAGPS  gps;            // This parses the GPS characters
Adafruit_BMP280 bmp;            // Barometer
QMC5883LCompass compass;        // Compass

uint8_t channel = 1, Fail_Test = 0;
int16_t ch1, ch2, ch3, ch4, ch5, ch6, ch7, PID_Alt_Filter; 
int32_t Lat, Long, Lat_estimate, Long_estimate; float Lat_vel, Long_vel;
bool Crash = false, Failsafe = false, Flight = false, Alt_Hold = false, GPS_Hold = false;
float compangleX, compangleY, comp_True_HDG = 0, YawRate_Avg, p0, Altitude_m;
float Target_Alt, Last_Altitude_m, HDG_Target;

bool flag = false;
char shortString1[32];
char shortString2[18];

Servo TV_Pitch;  Servo TV_Roll; Servo Motor1; Servo Motor2;    //Attaches servos to variables

void setup() {
  
  Serial.begin(57600);         
  while(!Serial){}              // Wait for Serial connection to begin
  
  pinMode(PA0, INPUT);          // PA0 needed for timer 2/PPM input. 
  pinMode(PA1, OUTPUT);         // Fail LED
  digitalWrite(PA1, HIGH);      // Set fail LED to on 
  PPM_Init();

  Wire.setClock(400000);        // 400kHz I2c bus Clk
  Wire.begin();
  delay(250);
  I2C_Sensor_Init();            // Initialises MPU9250 & BMP280, p0
  gpsPort.begin(38400);
  
  TV_Pitch.attach(PA6);         // Attaach TVservo to pin 
  TV_Roll.attach(PA7);          // Attaach TVservo to pin 
  Motor1.attach(PB0);           // Attaach ESC1 to pin 
  Motor2.attach(PB1);           // Attaach ESC2 to pin 

  if(Fail_Test == 0){
    digitalWrite(PA1, LOW); 
  }
}

void loop() {
  
 GetIMU();                      // 500Hz
 Outputs();                     // 50Hz
 Sensors();                     // 20Hz
 InputCheck();                  // 10Hz
 GPS();                         // 5Hz

}

// 5Hz

void GPS(){
  
    while (gps.available(gpsPort)) {
    gps_fix fix = gps.read();
    if (fix.valid.location) {
      int32_t Lat_int = fix.latitudeL();
      int32_t Long_int = fix.longitudeL();
      //Serial.print( fix.speed_mph(), 2 );

      Lat  = Lat_int  / 10000000.0;
      Long = Long_int / 1000000.0;



      // With GPS data, update state estimate here. //
      /* Update velocity averages from GPS, over 3ish values. 
       * Use this to update state estimated velocity
       * 
       * Use a timer to see how long since last GPS reading
       * Bias more towards GPS if timer > X, otherwise comp filter. 
       * if delta between lat and lat-estimate > X, converge quicker. 
       * 
       * Comp Filter for Lat/Long_estimate
       * 
       * 
       * 
       */





     //
      }
    }
    
}
5 
// 500Hz loop
uint32_t IMUFinT;
float gain=0.999, filter=0.1, accel_gain = 0.1, Acc_Angle_X_Filter, Acc_Angle_Y_Filter;
int16_t IMUlooptime = 2000;   // Length of IMU loop in MICRO seconds, uS.
int16_t Accel_X_offset = 389, Accel_Y_offset = 2, Accel_Z_offset = -100;
float AccelX_Avg, AccelY_Avg, AccelZ_Avg;
float Acc_Resultant, Acc_Angle_X, Acc_Angle_Y;

void GetIMU(){   
  
  uint32_t IMUStartT = micros();
  uint16_t  IMUdT = IMUStartT - IMUFinT;                       //dT for each IMU loop
 
  if(IMUdT >= IMUlooptime){

    //Serial.println(IMUdT);

    I2C_Read(MPU_Address, 0x3B, 6);                            // I2C Reading function
    int16_t Accel_X = Wire.read()<<8 | Wire.read();
    int16_t Accel_Y = Wire.read()<<8 | Wire.read();
    int16_t Accel_Z = Wire.read()<<8 | Wire.read();
  

    I2C_Read(MPU_Address, 0x43, 6);
    int16_t Gyro_X =  Wire.read()<<8 | Wire.read();
    int16_t Gyro_Y =  Wire.read()<<8 | Wire.read();
    int16_t Gyro_Z =  Wire.read()<<8 | Wire.read();

    if (Accel_Z > (32767 + Accel_Z_offset))                    //Stops Accel_Z over flowing
      Accel_Z = 32767 + Accel_Z_offset;

    Accel_X -= Accel_X_offset;
    Accel_Y -= Accel_Y_offset;
    Accel_Z -= Accel_Z_offset;  


    AccelX_Avg += accel_gain * (Accel_X - AccelX_Avg);          // Smoothes accel values
    AccelY_Avg += accel_gain * (Accel_Y - AccelY_Avg);
    AccelZ_Avg += accel_gain * (Accel_Z - AccelZ_Avg);

 
    //// Gyro Angle calculations 
    float IMUdT_mS = IMUdT / 1000.0;
    float IMUdt_Const = IMUdT_mS / 65536;   //0.0000305;        //65LSB/d/s, micros, 1/65540000= 0.00001525.

  
    float Gyro_Angle_X = compangleX + (Gyro_X * IMUdt_Const);   // calculates first bit of comp. filter   
    float Gyro_Angle_Y = compangleY + (Gyro_Y * IMUdt_Const);
    comp_True_HDG -= (Gyro_Z * IMUdt_Const);
    if (comp_True_HDG >= 360) comp_True_HDG -= 360;
    if (comp_True_HDG < 0)    comp_True_HDG += 360;
  
    float Gyro_Z_Yaw = Gyro_Z * 0.000000533;                    //0.000000533 = 0.0000305 * (3.142/180)
    Gyro_Angle_X += Gyro_Angle_Y * sin(Gyro_Z_Yaw);   
    Gyro_Angle_Y -= Gyro_Angle_X * sin(Gyro_Z_Yaw);


    //// Accelerometer angle calculations ////
    Acc_Resultant = sqrt(sq(AccelY_Avg) + sq(AccelZ_Avg));
    Acc_Angle_X = atan(AccelY_Avg / AccelZ_Avg ) * 57.30;
    Acc_Angle_Y = atan(AccelX_Avg / Acc_Resultant ) * 57.30;

    Acc_Angle_X_Filter += filter * (Acc_Angle_X - Acc_Angle_X_Filter);
    Acc_Angle_Y_Filter += filter * (Acc_Angle_Y - Acc_Angle_Y_Filter); 
    
 
    compangleX = gain * Gyro_Angle_X + (1-gain) * Acc_Angle_X_Filter;     // Pitch
    compangleY = gain * Gyro_Angle_Y - (1-gain) * Acc_Angle_Y_Filter;     // Roll
 
  
    YawRate_Avg += 0.2 * ((-Gyro_Z/65.54) - YawRate_Avg);
    

    //////// State estimator Update ////////

    float Pitch_rad = compangleX / 57.30;
    float Roll_rad  = compangleY / 57.30;
    float Yaw_rad = comp_True_HDG / 57.30;
    float sinHDG = sin(Yaw_rad);
    float cosHDG = cos(Yaw_rad);

    float accel_N_raw = cosHDG * AccelY_Avg - (sinHDG + Roll_rad * Pitch_rad * cosHDG) * AccelX_Avg
    + (Roll_rad * sinHDG + Pitch_rad * cosHDG) * AccelZ_Avg;

    float accel_E_raw = sinHDG * AccelY_Avg + (cosHDG + Roll_rad * Pitch_rad * sinHDG) * AccelX_Avg
    - (Roll_rad * cosHDG + Pitch_rad * sinHDG) * AccelZ_Avg;


    float accel_N = accel_N_raw / 1670;                                   // meters/second^2
    float accel_E = accel_E_raw / 1670;

    float IMU_S = IMUdT_mS / 1000.0;

    // Update Lat_vel, Long_vel
    // Update Lat_estimate, Long_estimate (lat constant)

    /* 
     * Update state estimate position and velocity with accels
     * Longitude lengths reduce with higher Latitude
     * Lat_estimate, Long_estimate, Lat_vel, Long_vel;
     * 
     */
 

    IMUFinT = IMUStartT;   
  } 
}
6 
byte GFS_SEL = 8; // creates binary 0b00001000, this makes gyro sensitivity 500 degs/sec. 65.536 LSBs/deg/sec
// for 250degs/sec use 0.Would need to change comp. filter calcs too.


void I2C_Sensor_Init(){                            //Writes gyro offsets to MPU9250 MPU_Address=0x68  

  
  // Valid at 22 degs C
  int16_t Gyro_X_offset = -165; 
  int16_t Gyro_Y_offset = 110; 
  int16_t Gyro_Z_offset = -210; 

  Wire.beginTransmission(MPU_Address);
  if(Wire.endTransmission() != 0)
      //Fail_Test++; 

  Wire.beginTransmission(MPU_Address);
  Wire.write(0x6B), Wire.write(0x00); // sends 0's to PWR MNGMT register 0x68 resetting it. 
  Wire.endTransmission();
  Wire.beginTransmission(MPU_Address);
  Wire.write(0x1B), Wire.write(GFS_SEL); // 0x1B register sets gyro scale, 
  Wire.endTransmission();
  
  int16_t(Gyro_X_offset1) = float(-0.5) * float(Gyro_X_offset);
  int16_t(Gyro_Y_offset1) = float(-0.5) * float(Gyro_Y_offset);
  int16_t(Gyro_Z_offset1) = float(-0.5) * float(Gyro_Z_offset);

  byte Gyro_X_H = highByte(Gyro_X_offset1); byte Gyro_X_L = lowByte(Gyro_X_offset1);
  byte Gyro_Y_H = highByte(Gyro_Y_offset1); byte Gyro_Y_L = lowByte(Gyro_Y_offset1);
  byte Gyro_Z_H = highByte(Gyro_Z_offset1); byte Gyro_Z_L = lowByte(Gyro_Z_offset1);

  Wire.beginTransmission(MPU_Address);
  Wire.write(0x13), Wire.write(Gyro_X_H); // 0x13 register sets gyro_X offset, upper byte 
  Wire.endTransmission();
  Wire.beginTransmission(MPU_Address);
  Wire.write(0x14), Wire.write(Gyro_X_L); // 0x14 register sets gyro_X offset, lower byte
  Wire.endTransmission();

  Wire.beginTransmission(MPU_Address);
  Wire.write(0x15), Wire.write(Gyro_Y_H); // 0x15 register sets gyro_Y offset, upper byte 
  Wire.endTransmission();
  Wire.beginTransmission(MPU_Address);
  Wire.write(0x16), Wire.write(Gyro_Y_L); // 0x16 register sets gyro_Y offset, lower byte
  Wire.endTransmission();

  Wire.beginTransmission(MPU_Address);
  Wire.write(0x17), Wire.write(Gyro_Z_H); // 0x17 register sets gyro_Z offset, upper byte 
  Wire.endTransmission();
  Wire.beginTransmission(MPU_Address);
  Wire.write(0x18), Wire.write(Gyro_Z_L); // 0x18 register sets gyro_Z offset, lower byte
  Wire.endTransmission();
  delay(500);

  /////////////BMP280 Setup Code ///////////

   if (!bmp.begin())
    Fail_Test++; 
  
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X8,      /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_1); /* Standby time. */



  ////////// Calculates p0 /////////////////

  float total = 0;
  for (int counter = 0; counter < 100; counter++){
    total += bmp.readPressure();
    delay(20);
  }
  p0 = total / 10000;


  ////////////// Compass ////////////////////////////


  compass.init();
  compass.setCalibration(-1690, 1190, -1703, 1191, -1687, 1458);

  ///// CALIBRATION TEST DATA ////// 
//compass.setCalibration(-1750, 1241, -1710, 1366, -1731, 1420);
//compass.setCalibration(-1778, 1115, -1625, 1176, -1592, 1478);
//compass.setCalibration(-1737, 1320, -1712, 913, -1686, 1420);
//compass.setCalibration(-1737, 1091, -1725, 1235, -1693, 1541);
//compass.setCalibration(-1412, 1183, -1746, 1265, -1736, 1430);



}  // End of I2C_Sensor_Init



void I2C_Read(int Address, int Register, int Bytes){              // Function to read I2C values
  
    Wire.beginTransmission(Address);
    Wire.write(Register);
    Wire.endTransmission();
    Wire.requestFrom(Address,Bytes);
    
}

// 10 Hz loop
int IPlooptime = 100, fail_counter = 0;
uint32_t IPFinT;                   


void InputCheck(){             

  uint32_t  IPStartT = millis();
 int16_t IPdT = IPStartT - IPFinT;                       //dT for each computation of the Input loop
 
 if(IPdT >= IPlooptime){   //10Hz update rate
  

  if (ch3 < 1030)
    fail_counter++;                                      //Failsafe detected
  else fail_counter = 0;

  if (fail_counter > 5)
    Failsafe = true;

  
  if (abs(compangleX) > 25 || abs(compangleY) > 25){     // detect if fallen over
    Crash = true;
    digitalWrite(PA1, HIGH);                             // Set fail LED to on
  }


  if (ch3 >= 1350 && !Flight){                           // Sets Current HDG as target
    Flight = true;
    HDG_Target = comp_True_HDG;
  }
  else if (ch3 < 1350){                                  // Test whether in flight or not
    Flight = false;
  }


  

  if (ch6 < 1300){                                       // Flap switch, Alt/GPS hold
    Alt_Hold = false;
    GPS_Hold = false;
  }
  else if (ch6 >= 1300 && ch6 < 1750){
    GPS_Hold = false;
    if (!Alt_Hold && Flight){
      Target_Alt = Altitude_m;                           // Sets target Alt as current alt
      Last_Altitude_m = Altitude_m;
      Alt_Hold = true;
    }
  }
  else if (ch6 >= 1750){
    if (!GPS_Hold){
      // Target_pos = Current_pos
      GPS_Hold = true;
      // Lat_estimate = Lat;
      // Long_estimate = Long; /1000...
    }
    
  }




  if(ch5 < 1300 ){                                      // Gear switch => Data logging
    // Data logging off
    flag = false;
  }
  else if (ch5 >= 1300 && ch5 < 1750){
    // Data logging on
    flag = false;
  }
  else if (ch5 >= 1750){
    // Data logging still on
    flag = true;
  }




  //Serial.println(bmp.readTemperature());  Is this needed?
  


  ///// Get Lipo voltage //////////////////////
  //uint16_t Voltage_raw = analogRead(PA4);
  //float Voltage = Voltage_raw * 0.00428;   
  //Serial.println(Voltage);
  //if (Voltage < 11.1){
    //Do something here
  //}



  
  // End of Input check
  IPFinT = IPStartT; 
  
 }
}
7 
// 50Hz loop
float kp = 43, kd = 7.2;                                    // Pitch & Roll coefficients
float kp_hdg = 1.2, ki_hdg = 0.0, kd_hdg = 0.004;           // HDG coefficients

int16_t OPlooptime = 20, throttle, PID_Pitch_Avg, PID_Roll_Avg, PID_HDG_Avg;
float I_term_HDG, PID_Gain = 0.35;
float Last_compangleX, Last_compangleY;
uint32_t OPFinT;


void Outputs(){
 
 uint32_t  OPStartT = millis();
 int16_t OPdT = OPStartT - OPFinT;                          // dT for each computation of the IMU loop
 
 if(OPdT >= OPlooptime){   //50Hz update rate

  int16_t pitchmap = map(ch2, 1135, 1904, -500, 500);       // max vehicle pitch angle ±5degs, 5*100=500
  int16_t rollmap  = map(ch1, 1135, 1904, -500, 500);  
  int16_t HDG_Rate = map(ch4, 1170, 1946, -200, 200);       

  if (abs(HDG_Rate) < 8) HDG_Rate = 0;                      // Deadband
  
  float PitchTarget = pitchmap / 100.0;  
  float RollTarget  = rollmap / 100.0;
  float HDG_Rated  = HDG_Rate / 100.0;

  HDG_Target += HDG_Rated;

  // GPS Guidance data here. Update PitchTarget, RollTarget, HDG_Target
  
  if (HDG_Target >= 360) HDG_Target -= 360;
  if (HDG_Target < 0)    HDG_Target += 360;

  
  float PitchError = PitchTarget   - compangleX;            //Error calculation
  float RollError  = RollTarget    - compangleY; 
  float HDG_Error  = HDG_Target - comp_True_HDG; 

  if (HDG_Error > 180)  HDG_Error -= 360;
  if (HDG_Error < -180) HDG_Error += 360;


  float dE_Pitch = compangleX - Last_compangleX;            // Change in Measurement now. 
  float dE_Roll  = compangleY - Last_compangleY;
  float dE_HDG   = YawRate_Avg;


  float P_term_Pitch = kp * PitchError;                     // Proportional terms
  float P_term_Roll  = kp * RollError;
  float P_term_HDG   = kp_hdg * HDG_Error;

             
  I_term_HDG += ki_hdg * HDG_Error * (float(OPdT) / 1000.0);// Integral term
  I_term_HDG = constrain(I_term_HDG, -8, 8);                // Stops integral windup

 
  float D_term_Pitch = (kd * dE_Pitch * 1000) / OPdT;       // Derivitive terms
  float D_term_Roll  = (kd * dE_Roll  * 1000) / OPdT;
  float D_term_HDG   = (kd_hdg * dE_HDG * 1000) / OPdT;


  int16_t PID_Pitch = (P_term_Pitch - D_term_Pitch);            // PID/PD outputs
  int16_t PID_Roll  = (P_term_Roll  - D_term_Roll);
  int16_t PID_HDG   = (P_term_HDG   + I_term_HDG - D_term_HDG);


  PID_Pitch_Avg += PID_Gain * (PID_Pitch - PID_Pitch_Avg);  // Smoothes PID output
  PID_Roll_Avg  += PID_Gain * (PID_Roll  -  PID_Roll_Avg);
  PID_HDG_Avg   += PID_Gain * (PID_HDG   -   PID_HDG_Avg);   // <<<<<<<------   NOT USED?????? /////////////////////////////////


  PID_Pitch = constrain(PID_Pitch, -334, 334);     //Limits PID Ouput, physical limits. 
  PID_Roll  = constrain(PID_Roll,  -350, 350); 
  PID_HDG   = constrain(PID_HDG,   -60,   60);
  

  if (Alt_Hold) throttle = PID_Alt_Filter;                  // Changes throttle from PID to ch3
  else if (!Alt_Hold) throttle = ch3;

  if (!Flight) PID_HDG = 0;                                 // Stops props spinning due to yaw

  
//      Serial.print(P_term_HDG);
//      Serial.print(", ");
//      Serial.print(I_term_HDG);
//      Serial.print(", ");
      Serial.println(Flight);


  if (!Crash){                                              //If vehicle crashed, turns motors off
    TV_Pitch.writeMicroseconds(1555 + PID_Pitch);           //1555 uSec is mid-point of PWM signal
    TV_Roll.writeMicroseconds(1400 + PID_Roll);             //1400 uSec is mid-point of PWM signal
    Motor1.writeMicroseconds(throttle - PID_HDG + 15);      // Bottom motor (2), top propeller
    Motor2.writeMicroseconds(throttle + PID_HDG - 40);      // Top motor, bottom propeller
  }
  else{
    TV_Pitch.writeMicroseconds(1555);       
    TV_Roll.writeMicroseconds(1400);         
    Motor1.writeMicroseconds(1000); 
    Motor2.writeMicroseconds(1000);     
  }


  Last_compangleX = compangleX;
  Last_compangleY = compangleY;
 
  
  OPFinT = OPStartT;  




  /////////// i2c data trasnfer begins here...../////////

  if (ch5 > 1300){
    
    String dataString1 = String(OPStartT) + "," + String(compangleY) + "," + String(RollTarget) + "," + String(RollError)
    + "," + String(dE_Roll);
    String dataString2 = "," + String(P_term_Roll) + "," +String(D_term_Roll) + "," + String(PID_Roll) + "," + String(PID_Roll_Avg); 
    //String dataString3 =  + "," + String(flag)
    
    dataString1.toCharArray(shortString1, 32);
    dataString2.toCharArray(shortString2, 27);

    Wire.beginTransmission(4);              // transmit to device #4
    Wire.write(shortString1);              // sends one string
    Wire.endTransmission(); 

    Wire.beginTransmission(4);              // transmit to device #4
    Wire.write(shortString2);              // sends second string 
    Wire.endTransmission();                // stop transmitting
  }

  /////////// i2c data trasnfer ends here...../////////

  
  } 
}

void PPM_Init(){
  
  delay(250);

  Timer2.attachCompare1Interrupt(PPM_Timer);
  TIMER2_BASE->CR1 = TIMER_CR1_CEN;
  TIMER2_BASE->CR2 = 0;
  TIMER2_BASE->SMCR = 0;
  TIMER2_BASE->DIER = TIMER_DIER_CC1IE;
  TIMER2_BASE->EGR = 0;
  TIMER2_BASE->CCMR1 = TIMER_CCMR1_CC1S_INPUT_TI1;
  TIMER2_BASE->CCMR2 = 0;
  TIMER2_BASE->CCER = TIMER_CCER_CC1E;
  TIMER2_BASE->PSC = 71;
  TIMER2_BASE->ARR = 0xFFFF;
  TIMER2_BASE->DCR = 0;
  
}

uint32_t  previous_time_PPM;

void PPM_Timer(void) {
   
 uint32_t start_time_PPM = TIMER2_BASE->CCR1;
 int32_t PPM_delay_time =  start_time_PPM - previous_time_PPM;
 
 if (PPM_delay_time < 0) PPM_delay_time += 0xFFFF;
    
 if(channel == 1) ch1 = PPM_delay_time;
 if(channel == 2) ch2 = PPM_delay_time;
 if(channel == 3) ch3 = PPM_delay_time;
 if(channel == 4) ch4 = PPM_delay_time;
 if(channel == 5) ch5 = PPM_delay_time;          
 if(channel == 6) ch6 = PPM_delay_time;
 //if(channel == 7) ch7 = PPM_delay_time;

 if(PPM_delay_time > 5000)
   channel = 1;
 else
  channel++;

 previous_time_PPM = start_time_PPM;
 
 }
8 
// 20Hz loop
uint32_t SensorFinT; int Sensorlooptime = 50;
int16_t PID_Alt;
float I_term_Alt, dE_Alt_Filt, last_Alt_m;
float alt_gain = 0.07, kp_a = 180.0, ki_a = 100, kd_a = 70.0;    // Alt coefficients

void Sensors(){

 uint32_t  Sensor_StartT = millis();
 int16_t SensordT = Sensor_StartT - SensorFinT;                  //dT for each computation of the Input loop
 
 if(SensordT >= Sensorlooptime){                                 //20Hz update rate

  // Adds 4mS delay to IMU loop every 2.5s
  float Alt_raw = bmp.readAltitude(p0);                          // Raw reading from BMP280
  float delta_Alt = Alt_raw - last_Alt_m;
  
  if (abs(delta_Alt) > 0.25) alt_gain = 0.4;                     // Chose gain depending on situation
  else alt_gain = 0.07;
  
  Altitude_m = last_Alt_m + alt_gain * (Alt_raw - last_Alt_m);   // Filter the output

  
  if (Alt_Hold){
  
    float Alt_Error = Target_Alt - Altitude_m;
    float dE_Alt = Altitude_m - Last_Altitude_m;
    dE_Alt_Filt += 0.2 * (dE_Alt - dE_Alt_Filt);  // CHECK THIS IF NEEDED
    float P_term_Alt = kp_a * Alt_Error;
    
    I_term_Alt += ki_a * Alt_Error * (SensordT / 1000);
    I_term_Alt = constrain(I_term_Alt, -40, 40);                 // Check values//////////////////////////////////////
    
    float D_term_Alt = (kd_a * dE_Alt_Filt * 1000) / SensordT;  
     
    PID_Alt = 1490 + P_term_Alt + I_term_Alt - D_term_Alt;
    PID_Alt_Filter += 0.20 * (PID_Alt - PID_Alt_Filter);
    PID_Alt_Filter = constrain(PID_Alt_Filter, 1420, 1750);
 
    Last_Altitude_m = Altitude_m;
  }




  ////////////////// Get magnatometer data //////////////////
  compass.read();
  int Mag_X = -compass.getX();
  int Mag_Y = compass.getY();
  int Mag_Z = -compass.getZ();

  float pitch_rads = compangleX / 57.3;
  float roll_rads  = compangleY / 57.3;

  float Mag_Y_tilt = Mag_Y * cos(pitch_rads) + Mag_Z * sin(pitch_rads);
  float Mag_X_tilt = Mag_Y * sin(roll_rads) * sin(pitch_rads) + Mag_X * cos(roll_rads)
  - Mag_Z * sin(roll_rads) * cos(pitch_rads);

  float Mag_HDG = atan2(Mag_X_tilt, -Mag_Y_tilt) * 57.3;
  if (Mag_HDG < 0) Mag_HDG += 360;
  float True_HDG = Mag_HDG - 1;
  if (True_HDG > 359) True_HDG -= 360;
  if (True_HDG < 0)   True_HDG += 360;
  
  if (True_HDG >= 0 && True_HDG <= 180) digitalWrite(PA1, HIGH);                  // Turns LED off/on at North
  if (True_HDG <= 359 && True_HDG > 180) digitalWrite(PA1, LOW);
  
  float HDG_gain = 0.98;
  if (!Flight) HDG_gain = 0.4;                                                    // Allows quicker convergance of comp filter
  
  if ((comp_True_HDG - True_HDG) > 340) True_HDG += 360;
  if ((True_HDG - comp_True_HDG) > 340) True_HDG -= 360;
  comp_True_HDG = (1 - HDG_gain) * True_HDG + HDG_gain * comp_True_HDG;
  if (comp_True_HDG >= 360) comp_True_HDG -= 360;
  if (comp_True_HDG < 0)    comp_True_HDG += 360;


  

  SensorFinT = Sensor_StartT;
  last_Alt_m = Altitude_m;

 }
}

If anyone had a general pointers on my style or things I could do better I'd always be glad to hear them.

Sorry Im limited to 9000 characters and a post every 5 minutes. You can probably see now why I didn't post it all at the start :slight_smile:

9

Here is a problem:

char shortString2[18];
.
.
.
    dataString2.toCharArray(shortString2, 27);

Fix that first and see what happens.

10

Ah thank you, I think you've done it. Poor form from myself on that.

11

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.