Beispiel #1
0
//Initialize all the peripherals
void init_peripherals(void)
{
	//Hardware modules:
	init_systick_timer();		//SysTick timer
	init_usart1(2000000);		//USART1 (RS-485 #1)
	init_usart6(2000000);		//USART6 (RS-485 #2)
	init_rs485_outputs();
	init_leds();
	init_switches();
	init_dio();					//All inputs by default
	init_adc1();
	init_spi4();				//Plan
	//init_spi5();				//FLASH
	//init_spi6();				//Expansion
	init_i2c1();
	init_imu();
	init_adva_fc_pins();
	init_pwr_out();

	//Software:
	init_master_slave_comm();

	//All RGB LEDs OFF
	LEDR(0); LEDG(0); LEDB(0);

	//Default analog input states:
	set_default_analog();
}
Beispiel #2
0
int main(int argc,char **argv) {
  int i; //iterator
  pid *x,*y,*z; //structs representing 3 pid controllers
  imu_t *imu;
  fd_set net_set;
  double oldmag,magdiff;
  struct timeval timeout;
  int axes[4]={0,0,0,0};
  int x_adjust,y_adjust,z_adjust;
  int sock = init_net();
  register_int();
  x=init_pid(3500,000,000);
  y=init_pid(3400,000,1000);
  z=init_pid(000,000,000);
  timeout.tv_sec=0;
  timeout.tv_usec=REFRESH_TIME;
  for(i=0;i<4;i++) { //initialize PWM modules
    motors[i]=init_pwm(i);
  }
  imu = init_imu();
  update_imu(imu);
  oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
  while(1) {
    ////printf("loop\n");
    FD_ZERO(&net_set);
    FD_SET(sock,&net_set);
    if(select(sock+1,&net_set,(fd_set *) 0,(fd_set *) 0, &timeout)==1) {
      ////printf("reading from the socket\n");
      //read from sock
      if(!update_axis(sock,axes)) {
        break;
      }
    } else {
      ////printf("reading from the imu\n");
      //update imu
      update_imu(imu);
      timeout.tv_sec=0;
      timeout.tv_usec=REFRESH_TIME;
      magdiff=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE-oldmag;
      if(magdiff>300) {
        magdiff-=360;
      }
      if(magdiff<-300) {
        magdiff+=360;
      }
      x_adjust = update_pid(x,0/*axes[1]*.00061*/,imu->mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE);
      y_adjust = update_pid(y,0/*axes[2]*.00061*/,imu->mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE);
      //printf("X axis value: %lf\n",imu->mpu.fusedEuler[VEC3_X]*RAD_TO_DEGREE);
      //printf("Y axis value: %lf\n",imu->mpu.fusedEuler[VEC3_Y]*RAD_TO_DEGREE);
      z_adjust = update_pid(z,axes[3]*.00061,magdiff);
      oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
      set_duty(motors[0],900000+axes[0]*16-z_adjust-y_adjust+x_adjust);
      set_duty(motors[1],900000+axes[0]*16+z_adjust-y_adjust-x_adjust);
      set_duty(motors[2],900000+axes[0]*16-z_adjust+y_adjust-x_adjust);
      set_duty(motors[3],900000+axes[0]*16+z_adjust+y_adjust+x_adjust);
    }
  }
  exit_fxn(0);
}
Beispiel #3
0
/******************************************
 ***************SETUP*********************
 ******************************************/
void setup()
{

	Serial.begin(115200);

	GCS.init(var_info);

	Thread::load_tasks(tasks);

//	GCS.wait_gcs_connect();

	init_radio();

	init_imu();

	motorsQuad.init();

	leds.negate_state(R);

}
Beispiel #4
0
// It all starts here:
int main(void) {
    // start with default user settings in case there's nothing in eeprom
    default_user_settings();
    // try to load settings from eeprom
    read_user_settings_from_eeprom();
   
    // set our LED as a digital output
    lib_digitalio_initpin(LED1_OUTPUT,DIGITALOUTPUT);

    //initialize the libraries that require initialization
    lib_timers_init();
    lib_i2c_init();

    // pause a moment before initializing everything. To make sure everything is powered up
    lib_timers_delaymilliseconds(100);
   
    // initialize all other modules
    init_rx();
    init_outputs();
    serial_init();
    init_gyro();
    init_acc();
    init_baro();
    init_compass();
    init_gps();
    init_imu();
   
    // set the default i2c speed to 400 KHz.  If a device needs to slow it down, it can, but it should set it back.
    lib_i2c_setclockspeed(I2C_400_KHZ);

    // initialize state
    global.state.armed=0;
    global.state.calibratingCompass=0;
    global.state.calibratingAccAndGyro=0;
    global.state.navigationMode=NAVIGATION_MODE_OFF;
    global.failsafeTimer=lib_timers_starttimer();

    // run loop
    for(;;) {
      // check to see what switches are activated
      check_checkbox_items();
      
      // check for config program activity
      serial_check_for_action();   
      
      calculate_timesliver();
      
      // run the imu to estimate the current attitude of the aircraft
      imu_calculate_estimated_attitude();

      // arm and disarm via rx aux switches
      if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // see if we want to change armed modes
          if (!global.state.armed) {
             if (global.activeCheckboxItems & CHECKBOX_MASK_ARM) {
                 global.state.armed=1;
                #if (GPS_TYPE!=NO_GPS)
                 navigation_set_home_to_current_location();
                #endif
                 global.home.heading=global.currentEstimatedEulerAttitude[YAW_INDEX];
                 global.home.location.altitude=global.baroRawAltitude;
             }
          } else if (!(global.activeCheckboxItems & CHECKBOX_MASK_ARM)) global.state.armed=0;
      }

      #if (GPS_TYPE!=NO_GPS)
      // turn on or off navigation when appropriate
      if (global.state.navigationMode==NAVIGATION_MODE_OFF) {
          if (global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME) { // return to home switch turned on
              navigation_set_destination(global.home.location.latitude,global.home.location.longitude);
              global.state.navigationMode=NAVIGATION_MODE_RETURN_TO_HOME;
          } else if (global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD) { // position hold turned on
              navigation_set_destination(global.gps.currentLatitude,global.gps.currentLongitude);
              global.state.navigationMode=NAVIGATION_MODE_POSITION_HOLD;
          }
      } else { // we are currently navigating
          // turn off navigation if desired
          if ((global.state.navigationMode==NAVIGATION_MODE_RETURN_TO_HOME && !(global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME)) || (global.state.navigationMode==NAVIGATION_MODE_POSITION_HOLD && !(global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD))) {
              global.state.navigationMode=NAVIGATION_MODE_OFF;
            
              // we will be turning control back over to the pilot.
              reset_pilot_control();
          }
      }
        #endif
      
       // read the receiver
       read_rx();
      
       // turn on the LED when we are stable and the gps has 5 satelites or more
      #if (GPS_TYPE==NO_GPS)
       lib_digitalio_setoutput(LED1_OUTPUT, (global.state.stable==0)==LED1_ON);
      #else
       lib_digitalio_setoutput(LED1_OUTPUT, (!(global.state.stable && global.gps.numSatelites>=5))==LED1_ON);
      #endif
      
       // get the angle error.  Angle error is the difference between our current attitude and our desired attitude.
       // It can be set by navigation, or by the pilot, etc.
       fixedpointnum angleError[3];
      
       // let the pilot control the aircraft.
       get_angle_error_from_pilot_input(angleError);
      
#if (GPS_TYPE!=NO_GPS)
       // read the gps
       unsigned char gotNewGpsReading=read_gps();

       // if we are navigating, use navigation to determine our desired attitude (tilt angles)
       if (global.state.navigationMode!=NAVIGATION_MODE_OFF) { // we are navigating
           navigation_set_angle_error(gotNewGpsReading,angleError);
       }
#endif

       if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) {
           // We are probably on the ground. Don't accumnulate error when we can't correct it
           reset_pilot_control();
         
           // bleed off integrated error by averaging in a value of zero
           lib_fp_lowpassfilter(&global.integratedAngleError[ROLL_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
           lib_fp_lowpassfilter(&global.integratedAngleError[PITCH_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
           lib_fp_lowpassfilter(&global.integratedAngleError[YAW_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
       }

#ifndef NO_AUTOTUNE
       // let autotune adjust the angle error if the pilot has autotune turned on
       if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
           if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE)) {
               autotune(angleError,AUTOTUNE_STARTING); // tell autotune that we just started autotuning
           } else {
               autotune(angleError,AUTOTUNE_TUNING); // tell autotune that we are in the middle of autotuning
           }
       } else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
           autotune(angleError,AUTOTUNE_STOPPING); // tell autotune that we just stopped autotuning
       }
#endif

        // This gets reset every loop cycle
        // keep a flag to indicate whether we shoud apply altitude hold.  The pilot can turn it on or
        // uncrashability/autopilot mode can turn it on.
        global.state.altitudeHold=0;
        
        if (global.activeCheckboxItems & CHECKBOX_MASK_ALTHOLD) {
            global.state.altitudeHold=1;
            if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_ALTHOLD)) {
                // we just turned on alt hold.  Remember our current alt. as our target
                global.altitudeHoldDesiredAltitude=global.altitude;
                global.integratedAltitudeError=0;
            }
        }
        
        fixedpointnum throttleOutput;
        
#ifndef NO_AUTOPILOT
        // autopilot is available
        if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
            if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT)) {
                // let autopilot know to transition to the starting state
                autopilot(AUTOPILOT_STARTING);
            } else {
                // autopilot normal run state
                autopilot(AUTOPILOT_RUNNING);
            }
        } else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
            // tell autopilot that we just stopped autotuning
            autopilot(AUTOPILOT_STOPPING);
        } else {
            // get the pilot's throttle component
            // convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
            throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
        }
#else

       // get the pilot's throttle component
       // convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
       throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
#endif

#ifndef NO_UNCRASHABLE
        uncrashable(gotNewGpsReading,angleError,&throttleOutput);
#endif
        
#if (BAROMETER_TYPE!=NO_BAROMETER)
       // check for altitude hold and adjust the throttle output accordingly
       if (global.state.altitudeHold) {
           global.integratedAltitudeError+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,global.timesliver);
           lib_fp_constrain(&global.integratedAltitudeError,-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // don't let the integrated error get too high
         
           // do pid for the altitude hold and add it to the throttle output
           throttleOutput+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,settings.pid_pgain[ALTITUDE_INDEX])-lib_fp_multiply(global.altitudeVelocity,settings.pid_dgain[ALTITUDE_INDEX])+lib_fp_multiply(global.integratedAltitudeError,settings.pid_igain[ALTITUDE_INDEX]);
       }
#endif

#ifndef NO_AUTOTHROTTLE
       if ((global.activeCheckboxItems & CHECKBOX_MASK_AUTOTHROTTLE) || global.state.altitudeHold) {
           if (global.estimatedDownVector[Z_INDEX]>FIXEDPOINTCONSTANT(.3)) {
               // Divide the throttle by the throttleOutput by the z component of the down vector
               // This is probaly the slow way, but it's a way to do fixed point division
               fixedpointnum recriprocal=lib_fp_invsqrt(global.estimatedDownVector[Z_INDEX]);
               recriprocal=lib_fp_multiply(recriprocal,recriprocal);
         
               throttleOutput=lib_fp_multiply(throttleOutput-AUTOTHROTTLE_DEAD_AREA,recriprocal)+AUTOTHROTTLE_DEAD_AREA;
           }
       }
#endif

#ifndef NO_FAILSAFE
       // if we don't hear from the receiver for over a second, try to land safely
       if (lib_timers_gettimermicroseconds(global.failsafeTimer)>1000000L) {
           throttleOutput=FPFAILSAFEMOTOROUTPUT;

           // make sure we are level!
           angleError[ROLL_INDEX]=-global.currentEstimatedEulerAttitude[ROLL_INDEX];
           angleError[PITCH_INDEX]=-global.currentEstimatedEulerAttitude[PITCH_INDEX];
       }
#endif
        
       // calculate output values.  Output values will range from 0 to 1.0
       // calculate pid outputs based on our angleErrors as inputs
       fixedpointnum pidOutput[3];
      
       // Gain Scheduling essentialy modifies the gains depending on
       // throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle,
       // 1.0 when at mid throttle, and .5 when at zero throttle.  This helps
       // eliminate the wobbles when decending at low throttle.
       fixedpointnum gainSchedulingMultiplier=lib_fp_multiply(throttleOutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE;
      
       for (int x=0;x<3;++x) {
           global.integratedAngleError[x]+=lib_fp_multiply(angleError[x],global.timesliver);
         
           // don't let the integrated error get too high (windup)
           lib_fp_constrain(&global.integratedAngleError[x],-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT);
         
           // do the attitude pid
           pidOutput[x]=lib_fp_multiply(angleError[x],settings.pid_pgain[x])-lib_fp_multiply(global.gyrorate[x],settings.pid_dgain[x])+(lib_fp_multiply(global.integratedAngleError[x],settings.pid_igain[x])>>4);
            
           // add gain scheduling.
           pidOutput[x]=lib_fp_multiply(gainSchedulingMultiplier,pidOutput[x]);
       }

       lib_fp_constrain(&throttleOutput,0,FIXEDPOINTONE); // Keep throttle output between 0 and 1

       compute_mix(throttleOutput, pidOutput); // aircraft type dependent mixes
       
#if (NUM_SERVOS>0)
       // do not update servos during unarmed calibration of sensors which are sensitive to vibration
       if (global.state.armed || (!global.state.calibratingAccAndGyro)) write_servo_outputs();
#endif
       
       write_motor_outputs();
   }
 virtual void init_sensors(uint16_t& acc1G, float& gyro_scale, int boardVersion, const std::function<void(void)>& imu_updated_callback) override 
 {
     imu_updated_callback_ = imu_updated_callback;
     init_imu(acc1G, gyro_scale, boardVersion);
 }
Beispiel #6
0
void setup(){
	init_ros();
	init_imu();
	init_controller();
	init_encoder();
}
Beispiel #7
0
//Initialize and enables all the peripherals
void init_peripherals(void)
{
	//Motor control variables & peripherals:
	init_motor();
	
	//Init Control:
	init_ctrl_data_structure();
	
	//Timebases:
	init_tb_timers();
	
	//UART 2 - RS-485
	init_rs485();
	
	//Analog, expansion port:
	init_analog();
	
	//Clutch:
	init_clutch();	
	
	//Enable Global Interrupts
    CyGlobalIntEnable; 
	
	//I2C1 (internal, potentiometers, Safety-CoP & IMU)
	init_i2c1();
	
	//Peripherals that depend on I2C:
	#ifdef USE_I2C_INT	
		
		//MPU-6500 IMU:
		#ifdef USE_IMU
		init_imu();
		CyDelay(25);
		init_imu();
		CyDelay(25);
		init_imu();
		CyDelay(25);
		#endif	//USE_IMU
		
		//Strain amplifier:
		#ifdef USE_STRAIN
		init_strain();
		#endif	//USE_STRAIN
		
	#endif	//USE_I2C_INT	
	
	//I2C2 (external)	
	#ifdef USE_I2C_EXT
	
	//Enable pull-ups:
	I2C_OPT_PU_Write(1);
		
	//I2C2 peripheral:
	init_i2c2();
	
	//Set RGB LED - Starts Green
	i2c_write_minm_rgb(SET_RGB, 0, 255, 0);
	
	#endif //USE_I2C_EXT
	
	//Magnetic encoder:
	init_as5047();
	
	// First DieTemp reading is always inaccurate -- throw out the first one
	#ifdef USE_DIETEMP	
	DieTemp_1_GetTemp(&temp);
	#endif
	
	//USB CDC
	#ifdef USE_USB	
	init_usb();
	#endif	//USE_USB
}