PPM Signal Interference

Projects Programming
1

Hi all,

Im making a drone that balances on a column of thrust, a little bit like a rocket with a propeller underneath rather than a rocket engine, it receives a PPM signal from a receiver and uses an IMU to calculate body angle then some PID code to tell the thrust vectoring what to do.

It mostly works but having rechecked the PPM signal (range 1100-1900 microseconds per channel) I've seen some erroneous values creep into the signal.

Full code:

/
#include <Wire.h>
#include <libmaple/scb.h>
#include <Servo.h>

byte GFS_SEL = 8; 

const int MPU_Address=0x68;
uint8_t int_flag, channel=1, AltHold, Throttle;
int16_t  Temp,samples,elapsed_time;
uint32_t IMUFinT, OPFinT, previous_time, print_finish, print_total, print_start; 
volatile int16_t ch1, ch2, ch3, ch4, ch5, ch6, ch7;   
float  compangleX, compangleY, gyroangleZ, YawRate, Last_PitchError, Last_RollError, Last_YawError;

float   gain=0.95;
uint16_t Integral_Windup = 0, Integral_Windup_Yaw = 0, gimbal_limit = 0, yaw_limit = 0;
int16_t IMUlooptime = 2000; int16_t OPlooptime = 20;  // Length of IMU loop in MICRO seconds & OP loop in milli secs
int16_t Accel_X_offset = 770; int16_t Accel_Y_offset = 110; int16_t Accel_Z_offset = -230;
int16_t Gyro_X_offset = -124; int16_t Gyro_Y_offset = 111; int16_t Gyro_Z_offset = -216;
int16_t kp = 0, ki = 0, kd = 0, kp_y = 0, ki_y = 0, kd_y = 0;

Servo TV_Pitch;  
Servo TV_Roll;
Servo Motor1;
Servo Motor2;
 
void setup() {
pinMode(PB10, INPUT);
attachInterrupt(digitalPinToInterrupt(PB10), PPM_calc, FALLING);   
Serial.begin(57600);                    
Wire.setClock(400000);                  
Wire.begin();
delay(250);
MPU_Init();                  

TV_Pitch.attach(PA0);          
TV_Roll.attach(PA1);          
Motor1.attach(PA2);           
Motor2.attach(PA2);            
  
}




void loop() {

print_start = millis();
  print_total = print_start - print_finish;
  if(print_total > 100){
    Serial.println(ch3);
    print_finish = print_start;
  }
  
GetIMU();
Outputs();
}


void GetIMU(){
uint32_t IMUStartT = micros();
uint16_t  IMUdT = IMUStartT - IMUFinT;      
 
if(IMUdT >= IMUlooptime){
Wire.beginTransmission(MPU_Address);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU_Address,14);
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();
int16_t Temp =    Wire.read()<<8 | Wire.read();
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();

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

float Acc_Angle_X = atan2(Accel_Y, sqrt( pow(Accel_X,2) + pow(Accel_Z,2) )  )*57.30;
float Acc_Angle_Y = atan2(Accel_X, sqrt( pow(Accel_Y,2) + pow(Accel_Z,2) )  )*57.30;

float IMUdt_Const = (IMUdT*0.0000000152579);         
float Xdelta = compangleX + (Gyro_X*IMUdt_Const);     
compangleX = gain*Xdelta + (1-gain)*Acc_Angle_X;   
float Ydelta = compangleY + (Gyro_Y*IMUdt_Const);    
compangleY = gain*Ydelta + (1-gain)*Acc_Angle_Y;

YawRate = (-Gyro_Z/65.54); 

IMUFinT = IMUStartT;    // leave in IF loop, then only computed when If=true. 
  } 
}




void PPM_calc() {                   // Reads PPM I/P & calculates PWM signal for up to 7  channels.
uint32_t  systick_counter = SYSTICK_BASE->CNT;                //Read SysTick counter
  if (0b1 & SCB_BASE->ICSR >> 26) {                           //If SysTick interrupt pending flag is set
    int_flag = 1;                                             //Set interrupt flag
    systick_counter = SYSTICK_BASE->CNT;                      //Re-read the SysTick counter
  }
  else int_flag = 0;                                 //SysTick interrupt flag is not set during reading
 uint32_t start_time = (systick_uptime_millis * 1000) + 
    (SYSTICK_RELOAD_VAL + 1 - systick_counter) / CYCLES_PER_MICROSECOND; //Calculate  total microseconds
  
  if (int_flag)start_time += 1000;               //If the SysTick interrupt is set 1000us have to added
uint32_t PPM_delay_time = start_time - previous_time;           //to get the correct microseconds result

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 > 6000){
  channel=1;}
 else {channel++;}
 previous_time = start_time;
}


void Outputs(){
 
 uint32_t  OPStartT = millis();
 int16_t OPdT = OPStartT - OPFinT;      
if(OPdT >= OPlooptime){   //50Hz update rate
  
 int16_t pitchmap = map(ch2, 1135, 1904, -200, 200); 
 int16_t rollmap  = map(ch1, 1135, 1904, -200, 200);  
 int16_t YawRateTarget  = map(ch4, 1142, 1910, -100, 100); 
float  PitchTarget = pitchmap/10.0;
float  RollTarget = rollmap/10.0;
  
float PitchError = PitchTarget - compangleY;   
float RollError = RollTarget - compangleX;
float YawError = YawRateTarget - YawRate;

float PitchError_Sum = PitchError_Sum + (PitchError * OPdT);    //Integral terms
float RollError_Sum = RollError_Sum + (RollError * OPdT);
float YawError_Sum = YawError_Sum + (YawError * OPdT);

PitchError_Sum = constrain(PitchError_Sum, -Integral_Windup, Integral_Windup);  
RollError_Sum = constrain(RollError_Sum, -Integral_Windup, Integral_Windup);
YawError_Sum = constrain(YawError_Sum, -Integral_Windup_Yaw, Integral_Windup_Yaw);

int16_t PID_Pitch = (kp * PitchError) + (ki * PitchError_Sum) + (kd * (PitchError - Last_PitchError) / OPdT);
int16_t PID_Roll  = (kp * RollError ) + (ki * RollError_Sum ) + (kd * (RollError  - Last_RollError ) / OPdT);
int16_t PID_Yaw   = (kp_y * YawError) + (ki_y * YawError_Sum) + (kd_y * (YawError_Sum - Last_YawError)/OPdT);

PID_Pitch = constrain(PID_Pitch, -gimbal_limit, gimbal_limit);   
PID_Roll  = constrain(PID_Roll,  -gimbal_limit, gimbal_limit);
PID_Yaw   = constrain(PID_Yaw,  -yaw_limit, yaw_limit);

Last_PitchError = PitchError;
Last_RollError = RollError;
Last_YawError = YawError;


  TV_Pitch.writeMicroseconds(1518 + PID_Pitch);      
  TV_Roll.writeMicroseconds(1518 + PID_Roll);
  Motor1.writeMicroseconds(ch3 + PID_Yaw);
  Motor2.writeMicroseconds(ch3 - PID_Yaw);
  OPFinT = OPStartT;    
  } 
}

I tested the code again and commented out big sections to see where the problem was coming from and found that only when the IMU code is running the PPM signal is affected.
This is the IMU code taken from the main script above:

void GetIMU(){
uint32_t IMUStartT = micros();
uint16_t  IMUdT = IMUStartT - IMUFinT;      //dT for each computation of the IMU loop
 
if(IMUdT >= IMUlooptime){
Wire.beginTransmission(MPU_Address);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU_Address,14);
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();
int16_t Temp =    Wire.read()<<8 | Wire.read();
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();

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

float Acc_Angle_X = atan2(Accel_Y, sqrt( pow(Accel_X,2) + pow(Accel_Z,2) )  )*57.30;
float Acc_Angle_Y = atan2(Accel_X, sqrt( pow(Accel_Y,2) + pow(Accel_Z,2) )  )*57.30;

float IMUdt_Const = (IMUdT*0.0000000152579);        //65LSB/d/s, micros, 1/65540000= 0.00001525.. 
float Xdelta = compangleX + (Gyro_X*IMUdt_Const);    // calculates first bit of comp. filter eq. 
compangleX = gain*Xdelta + (1-gain)*Acc_Angle_X;   
float Ydelta = compangleY + (Gyro_Y*IMUdt_Const);    // calculates first bit of comp. filter eq. 
compangleY = gain*Ydelta + (1-gain)*Acc_Angle_Y;

YawRate = (-Gyro_Z/65.54); 

IMUFinT = IMUStartT;    // leave in IF loop, then only computed when If=true. 
  } 
}

I've tried commenting out the atan2 lines as I thought perhaps they are using a lot of processing power and screwing the PPM signal about but didn't have any luck with that.

If anyone knows why the IMU code is causing the PPM signal to be modified, or has any suggestions I really would love to hear them! Ive attached a photo showing one of the 5 PPM signals, and how the signal is being changed from time to time. This only happens when the IMU code is running.

Thanks.

PPM Signal.png
PPM Signal.png1416×950 295 KB

2 
float Acc_Angle_X = atan2(Accel_Y, sqrt( pow(Accel_X,2) + pow(Accel_Z,2) )  )*57.30;

pow() is the absolutely worst possible way to get x * x. It is very slow. And you call is 4 times, uselessly.

You call sqrt() twice, too, when once is plenty. Another slow function.

If anyone knows why the IMU code is causing the PPM signal to be modified

We can't see your code to read the PPM signal.

3

Hi,

Thanks for that I'll change the pow() function, I did however also comment out those lines completely and it made no difference whatsoever so I don't think the problem lies there.

The code for the PPM is in the main code but I've put it below as well. Many thanks.

void PPM_calc() {                   // Reads PPM I/P & calculates PWM signal for up to 7  channels.
uint32_t  systick_counter = SYSTICK_BASE->CNT;                //Read SysTick counter
  if (0b1 & SCB_BASE->ICSR >> 26) {                           //If SysTick interrupt pending flag is set
    int_flag = 1;                                             //Set interrupt flag
    systick_counter = SYSTICK_BASE->CNT;                      //Re-read the SysTick counter
  }
  else int_flag = 0;                                 //SysTick interrupt flag is not set during reading
 uint32_t start_time = (systick_uptime_millis * 1000) + 
    (SYSTICK_RELOAD_VAL + 1 - systick_counter) / CYCLES_PER_MICROSECOND; //Calculate  total microseconds
  
  if (int_flag)start_time += 1000;               //If the SysTick interrupt is set 1000us have to added
uint32_t PPM_delay_time = start_time - previous_time;           //to get the correct microseconds result

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 > 6000){
  channel=1;}
 else {channel++;}
 previous_time = start_time;
}
4

I should probably mention as well this is being run on an STM32, apologies, I don't think i said that earlier.

I think I have narrowed the issue down to the code that reads the registers from the MPU9250.

Wire.beginTransmission(MPU_Address);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU_Address,2);
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();
int16_t Temp =    Wire.read()<<8 | Wire.read();
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();

When these lines are commented out and replaced with constants the PPM signal is solid, maybe a 2uS variation max. But as soon as it starts reading from the registers, whether it be 2 registers or all 14 then you see large (60+uS) variation in the PPM signal for each channel.

I've tried using micros() to count the PPM signal rather then the SYSTICK timer, but then that provides its own issues and doesn't give an accurate result.

Is there a different way to read from the IMU registers as to not interfere with the timing function for the PWM?

Thanks.