Hello, I’m trying to do an autonomous flight indoors and I decided to program my APM 2.8 with a custom code using the AP library, when I test the sensor to measure Pitch Roll and Yaw, Pitch and Roll work fine, but Yaw starts to climb gradually to 180 and then immediately becomes -180 and starts to rise to 0 and then repeats, but if I move the drone the sensor perceives it, I explain, if we say goes in 3.2 degrees and move the drone in Yaw about 45 degrees the sensor shows me 48.2, then goes to 49 and so on, is there any way to fix this? I thank you for an answer, I put down the code and a capture of the serial monitor when Yaw is increasing while the drone is still.
// Libraries
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_Param.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h> // RC Channel Library
#include <AP_Motors.h>
#include <AP_Curve.h>
#include <AP_ADC.h>
#include <AP_InertialSensor.h>
#include <AP_Buffer.h>
#include <Filter.h>
#include <AP_Baro.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR.h>
#include <PID.h>
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
static uint32_t timer;
#define MOTOR_FL 2 // Front left
#define MOTOR_FR 0 // Front right
#define MOTOR_BL 1 // back left
#define MOTOR_BR 3 // back right
RC_Channel rc1(0), rc2(1), rc3(2), rc4(3);
AP_MotorsQuad motors(&rc1, &rc2, &rc3, &rc4);
AP_InertialSensor_MPU6000 ins;
// Radio min/max values for each stick for my radio (worked out at beginning of article)
#define RC_THR_MIN 1070
#define RC_THR_MAX 2000
#define RC_YAW_MIN 1068
#define RC_YAW_MAX 1915
#define RC_PIT_MIN 1077
#define RC_PIT_MAX 1915
#define RC_ROL_MIN 1090
#define RC_ROL_MAX 1913
long map(long x, long in_min, long in_max, long out_min, long out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))
PID pids[7];
#define PID_PITCH_RATE 0
#define PID_ROLL_RATE 1
#define PID_PITCH_STAB 2
#define PID_ROLL_STAB 3
#define PID_YAW_RATE 4
#define PID_YAW_STAB 5
#define PID_THR 6
float con=0,yawan=0,yawn=0,con2=0,alt=0,sum=0,prom=0,res=0,YawNew=0,YawBe=0;
// setup
void setup()
{
// motor initialisation
motors.set_update_rate(400);
motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_min_throttle(130);
motors.set_max_throttle(850);
motors.Init(); // initialise motors
if (rc3.radio_min == 0) {
// cope with AP_Param not being loaded
rc3.radio_min = 1000;
}
if (rc3.radio_max == 0) {
// cope with AP_Param not being loaded
rc3.radio_max = 2000;
}
motors.enable();
motors.output_min();
pids[PID_PITCH_RATE].kP(2);
pids[PID_PITCH_RATE].kI(0.5);
pids[PID_PITCH_RATE].kD(1);
pids[PID_PITCH_RATE].imax(50);
pids[PID_ROLL_RATE].kP(2);
pids[PID_ROLL_RATE].kI(0.5);
pids[PID_ROLL_RATE].kD(1);
pids[PID_ROLL_RATE].imax(50);
pids[PID_YAW_RATE].kP(2.7);
pids[PID_YAW_RATE].kI(1);
pids[PID_YAW_RATE].imax(50);
pids[PID_PITCH_STAB].kP(4.5);
pids[PID_ROLL_STAB].kP(4.5);
pids[PID_YAW_STAB].kP(10);
pids[PID_THR].kP(10);
// Turn off Barometer to avoid bus collisions
hal.gpio->pinMode(63, GPIO_OUTPUT);
hal.gpio->write(63, 1);
baro.init();
baro.calibrate();
// Turn on MPU6050 - quad must be kept still as gyros will calibrate
ins.init(AP_InertialSensor::COLD_START,
AP_InertialSensor::RATE_100HZ,
NULL);
// initialise sensor fusion on MPU6050 chip (aka DigitalMotionProcessing/DMP)
hal.scheduler->suspend_timer_procs(); // stop bus collisions
ins.dmp_init();
hal.scheduler->resume_timer_procs();
for(int i=0; i<=500; i++){
ins.update();
float roll,pitch,yaw;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll) ;
pitch = ToDeg(pitch) ;
yawn = ToDeg(yaw);
res=yawn-yawan;
sum=sum+res+0.0007;
yawan=yawn;
}
prom=(sum/500);//+0.0014;
con=yawn;
}
// loop
void loop()
{
baro.read();
alt=baro.get_altitude()*100;
static float yaw_target = 0;
// Wait until new orientation data (normally 5ms max)
while (ins.num_samples_available() == 0);
long rcthr, rcyaw, rcpit, rcroll; // Variables to store radio in
rcyaw = 0;
rcpit = 0;
rcroll = 0;
// Ask MPU6050 for orientation
ins.update();
float roll,pitch,yaw;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll) ;
pitch = ToDeg(pitch) ;
yawn = ToDeg(yaw);
hal.console->printf_P(
PSTR("P:%4.8f \n"),yawn);
if(con2==0){
if(con<100){
con=con+prom;//0.00607;
}else{
con=con+prom+0.00018;//0.00607;
}
yaw = yawn-con;
if(con>=180){
con2=1;
}
}
if(con2==1){
con=con-(prom+0.0001);//0.00608;
yaw = yawn+con;
if(con<=0){
con2=0;
}
}
YawNew=yaw;
res=YawNew-YawBe;
if(res>90 || res<-90){
yaw=YawBe;
}else{
yaw=YawNew;
YawBe=YawNew;
}
/*hal.console->printf_P(
PSTR("P:%4.2f R:%4.2f Y:%4.2f A:%4.2f\n"),
yaw,
con,
yawn,
alt);
*/
// Ask MPU6050 for gyro data
Vector3f gyro = ins.get_gyro();
float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);
rcthr =constrain(pids[PID_THR].get_pid(170 - alt, 1), RC_THR_MIN + 150, RC_THR_MAX);
// Do the magic
if(rcthr > RC_THR_MIN + 50) { // Throttle raised, turn on stablisation.
// Stablise PIDS
float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);
// is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
if(abs(rcyaw ) > 5) {
yaw_stab_output = rcyaw;
yaw_target = yaw; // remember this yaw for when pilot stops
}
// rate PIDS
long pitch_output = (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
long roll_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
long yaw_output = (long) constrain(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);
// mix pid outputs and send to the motors.
hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);
} else {
// motors off
hal.rcout->write(MOTOR_FL, 1000);
hal.rcout->write(MOTOR_BL, 1000);
hal.rcout->write(MOTOR_FR, 1000);
hal.rcout->write(MOTOR_BR, 1000);
// reset yaw target so we maintain this on takeoff
yaw_target = yaw;
// reset PID integrals whilst on the ground
for(int i=0; i<6; i++)
pids[i].reset_I();
}
}
AP_HAL_MAIN();
