Demo entry 6634538

APM_Double

   

Submitted by anonymous on Aug 11, 2017 at 04:19
Language: C++. Code size: 9.7 kB.

#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_Param.h>
#include <AP_Progmem.h>
#include <AP_ADC.h>
#include <AP_InertialSensor.h>
#include <GCS_MAVLink.h>
#include <AP_Notify.h>
#include <DataFlash.h>
#include <AP_GPS.h>
#include <AP_Baro.h>
#include <Filter.h>
#include <AP_AHRS.h>
#include <AP_Compass.h>
#include <AP_Declination.h>
#include <AP_Airspeed.h>
#include <AP_Vehicle.h>
#include <AP_Mission.h>
#include <AP_ADC_AnalogSource.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <PID.h>
//rc_input define
#define RC_THR_MIN   1100
#define RC_YAW_MIN   1097
#define RC_YAW_MAX   1936
#define RC_PIT_MIN   1098
#define RC_PIT_MAX   1937
#define RC_ROL_MIN   1100
#define RC_ROL_MAX   1940

// motor define
#define FRONT_MOTOR  0     // Front Motor
#define BACK_MOTOR   1     // Back  Motor
#define F_RUDDER     10    // Front Rudder   
#define B_RUDDER     9 // back right
//PID define
#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))
PID pids[6];
#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 CONFIG_HAL_BOARD == HAL_BOARD_APM2
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;

AP_InertialSensor_MPU6000 ins;
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);

AP_Compass_HMC5843 compass;

AP_GPS gps;

// choose which AHRS system to use
AP_AHRS_DCM  ahrs(ins, baro, gps);

AP_Baro_HIL barometer;

#define HIGH 1
#define LOW 0

//map function
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;
}

void setup()
{
  //setup rc_out PWM
  for (uint8_t i=0; i<16; i++) {
        hal.rcout->enable_ch(i);
    }
  hal.rcout->set_freq(0xF, 490);
  
//set rc_out_PWM 
//init PID
pids[PID_PITCH_RATE].kP(3.7);
//pids[PID_PITCH_RATE].kI(0.9);
pids[PID_PITCH_RATE].kD(0.14);
pids[PID_PITCH_RATE].imax(50);

pids[PID_ROLL_RATE].kP(3.4);
pids[PID_ROLL_RATE].kI(0.15);
pids[PID_ROLL_RATE].kD(0.03);
pids[PID_ROLL_RATE].imax(50);

pids[PID_YAW_RATE].kP(4.5);
//pids[PID_YAW_RATE].kI(0.23);
pids[PID_YAW_RATE].kD(0.05);
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);

//star ins
  
//Disable barometer to stop it corrupting bushal.gpio->pinMode(40,GPIO_OUTPUT);hal.gpio->write(40, 1);
hal.console->println("AP_InertialSensor startup...");
// we need to stop the barometer from holding the SPI bus
hal.gpio->pinMode(40, GPIO_OUTPUT);
hal.gpio->write(40, HIGH);
// Initialise MPU6050 sensorins.
ins.init(AP_InertialSensor::COLD_START, 
			 AP_InertialSensor::RATE_100HZ);
// Initialise MPU6050's internal sensor fusion (akaDigitalMotionProcessing)
 ins.init_accel();
    ahrs.init();
hal.scheduler->suspend_timer_procs();  // stop bus collisions
/*ins.dmp_init();                        
ins.push_gyro_offsets_to_dmp();
*/
//display_offsets_and_scaling();
hal.scheduler->resume_timer_procs();
if( compass.init() ) {
        hal.console->printf("Enabling compass\n");
        ahrs.set_compass(&compass);
    } else {
        hal.console->printf("No compass detected\n");
    }
    gps.init(NULL);
}
void loop()
{ //pid yaw control
  static float yaw_target = 0;  
  
/*
 RCin_part
  */
  uint16_t channels[8],channels1[8];//set 8 recieve channels
  uint16_t rcthr, rcyaw, rcpit, rcroll;  //set rc input thr yaw pit roll
  
  hal.rcin->read(channels,8);

  rcthr =  map(channels[2], RC_YAW_MIN, 1940, RC_YAW_MIN, 1750);
  rcyaw = map(channels[3], RC_YAW_MIN, RC_YAW_MAX, -180, 180);
  rcpit = map(channels[1], RC_PIT_MIN, RC_PIT_MAX, -45, 45);
  rcroll = map(channels[0], RC_ROL_MIN, RC_ROL_MAX, -45, 45);
 
  //ahrs start
   static uint16_t counter;
    static uint32_t last_t, last_print, last_compass;
    uint32_t now = hal.scheduler->micros();
    float heading = 0;

    if (last_t == 0) {
        last_t = now;
        return;
    }
    last_t = now;
    if (now - last_compass > 100*1000UL && compass.read()) 
        {
        heading = compass.calculate_heading(ahrs.get_dcm_matrix());
        // read compass at 10Hz
        last_compass = now;
        #if WITH_GPS
        g_gps->update();
        #endif
        }
    ahrs.update();
    counter++;
//
  
if(!ins.wait_for_sample(100)){hal.console->println("INS WRONG!!!!");}
  
  //INS part
  // Ask MPU6050 for orientation
  
  //ins.update();
  ahrs.update();
  //Vector3f gyro = ins.get_gyro_offsets();
  float roll,pitch,yaw;  
 // ins.quaternion.to_euler(&roll, &pitch, &yaw);
  roll   =  ToDeg(ahrs.roll);
  pitch  =  ToDeg(ahrs.pitch);
  yaw    =  ToDeg(ahrs.yaw);
//  hal.console->printf_P(PSTR(" roll:%f  r_cos:%f  r_sin:%f \n"),    roll,cos_roll,sin_roll);
//  hal.console->printf_P(PSTR(" pitch:%f  p_cos:%f  p_sin:%f \n"),  pitch,cos_pitch,sin_pitch);
//  hal.console->printf_P(PSTR(" yaw:%f  y_cos:%f  y_sin:%f \n\n"),   yaw, cos_yaw,sin_yaw);
  
// Ask MPU6050 for gyro data
  Vector3f gyro123= ins.get_gyro();
  float  gyroPitch = ToDeg(gyro123.y);
  float  gyroRoll = ToDeg(gyro123.x);
  float   gyroYaw = ToDeg(gyro123.z);

// Do the magic
   if (now - last_print >= 100000 /* 100ms : 10hz */) 
   {
        Vector3f drift  = ahrs.get_gyro_drift();

        last_print = now;
        counter = 0;
    }
// Do the magic
  if(rcthr > RC_THR_MIN + 100) {  // Throttle raised, turn on stablisation.
// Stablise PIDS
float pitch_stab_output_earth = constrain_float(pids[PID_PITCH_STAB].get_pid(float((int)rcpit)-pitch, 1), -250, 250); 

float roll_stab_output_earth = constrain_float(pids[PID_ROLL_STAB].get_pid(float((int)rcroll)-roll, 1), -250, 250);

float yaw_stab_output_earth = constrain_float(pids[PID_YAW_STAB].get_pid(wrap_180(float((int)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_earth =float((int)rcyaw);
    yaw_target = yaw;   // remember this yaw for when pilot stops
}
    
        //from earth-frame rates to body-frame rates
    float roll_stab_output = roll_stab_output_earth - ahrs.sin_pitch() * yaw_stab_output_earth;
    float pitch_stab_output = ahrs.cos_roll()  * pitch_stab_output_earth + ahrs.sin_roll() * ahrs.cos_pitch() * yaw_stab_output_earth;
    float yaw_stab_output = -ahrs.sin_roll() * pitch_stab_output_earth + ahrs.cos_pitch() * ahrs.cos_roll() * yaw_stab_output_earth;
    
    // rate PIDS
    long pitch_output =  (long) constrain_float(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500,500);  
    long roll_output =  (long) constrain_float(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);  
    long yaw_output =  (long) constrain_float(pids[PID_YAW_RATE].get_pid(float((int) yaw_stab_output)- gyroYaw, 1), -500, 500);  
              
    // mix pid outputs and send to the motors.
      hal.rcout->write(FRONT_MOTOR,   (long) constrain_float(rcthr + pitch_output,1000,2000));
       hal.rcout->write(BACK_MOTOR,   (long) constrain_float(rcthr - pitch_output,1000,2000));
       hal.rcout->write(F_RUDDER,     (long) constrain_float(1440  - roll_output + yaw_output ,1000,2000));
       hal.rcout->write(B_RUDDER,     (long) constrain_float(1442  + roll_output + yaw_output ,1000,2000));

       hal.console->printf_P(PSTR("[Output]===> pitch = %d, roll = %d, yaw = %d, throttle = %d. <rcpit:%d  pitch:%f>\n"),pitch_output, roll_output, yaw_output, rcthr, rcpit,pitch);
                    
  } else {
    // motors off
    hal.rcout->write(FRONT_MOTOR, 1000 );
    hal.rcout->write(BACK_MOTOR,  1000 );
    hal.rcout->write(F_RUDDER,    1000 );
    hal.rcout->write(B_RUDDER,    1000 );
      
    // reset yaw target so we maintain this on takeoff
    yaw_target = yaw;
    }
    // reset PID integrals whilst on the ground
    hal.console->printf_P(PSTR("rcthr:%d\n"),rcthr);
    for(int i=0; i<8; i++){
     channels1[i]=hal.rcout->read(i);

  }


  hal.console->printf_P(
                PSTR("INDIVIDUAL READ THR%d  YAW%d  PIT%d ROLL%d    "),            
                       rcthr, rcyaw, rcpit, rcroll);
//                       
//  int valid=8;                  
//  hal.console->printf_P(
//                      PSTR("multi  output  read %d: %d %d %d %d \r\n"),
//                      (int) valid, channels1[0], channels1[1], channels1[2], channels1[3]);

            
          //     hal.scheduler->delay(100);
             
  
}

//a part show the date from INS 
//void display_offsets_and_scaling()
//{
//    Vector3f accel_offsets = ins.get_accel_offsets();
//    Vector3f accel_scale = ins.get_accel_scale();
//    Vector3f gyro_offsets = ins.get_gyro_offsets();
//
//    // display results
//    hal.console->printf_P(
//            PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
//                    accel_offsets.x,
//                    accel_offsets.y,
//                    accel_offsets.z);
//    hal.console->printf_P(
//            PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
//                    accel_scale.x,
//                    accel_scale.y,
//                    accel_scale.z);
//    hal.console->printf_P(
//            PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
//                    gyro_offsets.x,
//                    gyro_offsets.y,
//                    gyro_offsets.z);
//                    
//                 
//}



//
AP_HAL_MAIN();

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).