Quellcode

 {autotoc addNumbering=true|addNumberingToc=true}

Der Quellcode unterteilt sich auf verschiedene Dateien:

  • Ardupilot.ino: Hauptprogramm
  • Controller.ino: Regelungssoftware
  • FlightAux.ino: Hilfsfunktionen die das Fliegeverhalten des Zeppelins abstrahieren
  • HMC5883L.cpp: Magnetometer-Bibliothek
  • I2Cdev.cpp: I²C-Bibliothek
  • MPU6050.cpp: Accelerometer-, Gyrometer-Bibliothek
  • MS561101BA.cpp: Barometer-Bibliothek
  • PPMTimer.ino: Regelt die Dekodierung und Kodierung der PPM- und PWM-Signale
  • Sensor.ino: Hochlevelige Sensorfunktionen

 

Ardupilot.ino

// Main Ardupilot File
//
// Author: Andreas Lenz
//

/*************************************************************************************************************************
 *************************************************************************************************************************
 ******************************                           MAIN                        ************************************
 *************************************************************************************************************************
 *************************************************************************************************************************
 */

// Defines
//
#define WDT_TIMEOUT WDTO_2S                          // Timeout 4 sec
#define STARTJMPDDR DDRL
#define STARTJMPPORT PINL
#define STARTJMPPIN PL2
#define GREENLEDDDR DDRL
#define GREENLEDPORT PORTL
#define GREENLEDPIN PL1
#define REDLEDDDR DDRE
#define REDLEDPORT PORTE
#define REDLEDPIN PE6

// Includes
//
#include <Wire.h>
#include <math.h>
#include "avr/wdt.h"

#include "Controller.h"
#include "FlightAux.h"
#include "Sensor.h"

// Global variables
//

// Autopilot variables
char flight_mode = 0;
bool verbose = true;

float angle = 0;


// Function declarations
//

// Control serial input and sets the autopilot according to the input
//
void serialControl (Controller *controller);

// Init the Ardupilot
//
void Ardupilot_init (void);

// Enables the watchdog timer to interrupt when the program runs mad
//
void enableWatchdog (void);



// Main program
//

void setup () {
  
  
  // Variables
  //
  
  // Starting mode
  int started = 0;
  int time_started = 0, time_up = 0;
  int mode = 0;

  // Init ardupilot
  Ardupilot_init ();

  // Initialise sensors
  //Sensor sensor = Sensor();
  //if (!sensor.init ()) { Serial.println ("sensor init failed"); }
  if (!Sensor_init ()) { Serial.println ("sensor init failed"); }
  
  // Initialise controller
  Controller controller;
  controller.init ();
 
  // MAIN LOOP
  //
  //
  while (true) {

    // Check the startjumper
    if (STARTJMPPORT & (1<<STARTJMPPIN)) { 
      started = 1;
    } else {
      started = 0;
    }
    
    // Check flightmode
    mode = getCh(7);
    
    // Check, if the Ardupilot/Zeppelin is started; SWITCH EVERYTHING OFF WHEN ARDUPILOT IS NOT STARTED
    if ( started != 1) {
      
      // LED status green=on, red=on
      GREENLEDPORT |= (1<<GREENLEDPIN);
      REDLEDPORT |= (1<<REDLEDPIN);
      
      // All servos mid
      for (int i=0; i<8; i++) {
        setCh (i, 1500);
      }
      
    }
    
    
    
    
    else { // Jumper is removed: Autopilot is started
      
      // Autopilot mode
      //
      
      // LED status green=on, red=off
      GREENLEDPORT |= (1<<GREENLEDPIN);
      REDLEDPORT &= ~(1<<REDLEDPIN);
      
      // switch to manual mode (1750 ... max) switch down
      if (mode > 1750) {
  
        for (int i=0; i<8; i++) { setCh (i, getCh(i)); }
  
      // switch to save mode: don't move (min ... 1250) switch up
      } else if (mode < 1250) {
      
        // All servos mid
        for (int i=0; i<8; i++) {
          setCh (i, 1500);
        }
  
      // autopilot (1250 ... 1750) switch mid
      } else {
        
        
        //serialControl (&controller);
        //Serial.println(Sensor_getHeight());
        
      } // Autopilot
      
      // Update sensor values
      Sensor_update ();
      
    } // started
      
    // Reset the watchdog: Everything goes well
    wdt_reset ();
    
  }  // Main loop
  
  
  // Disbale interrupt bit
  cli ();
	    
}

void loop () {}

// Function definitions
//


// init the ardupilot
//
void Ardupilot_init () {
  
  // Enable watchdog
  enableWatchdog ();
  
  // Initialise timers that are needed to read the sum PPM and write the single PWM channels
  Init_PWM1();			// OUT 2&3 
  Init_PWM3();			// OUT 6&7
  Init_PWM5();			// OUT 0&1
  Init_PPM_PWM4();		// OUT 4&5 + PPM IN
  
  // Init start jumper
  STARTJMPDDR &= ~(1<<STARTJMPPIN);
  
  // Init status LEDs
  GREENLEDDDR |= (1<<GREENLEDPIN);
  REDLEDDDR |= (1<<REDLEDPIN);
  
  // Initialise serial port
  Serial.begin(9600);
  
  // Initialise the I²C
  Wire.begin();
  
}


// enables the watchdog timer to interrupt when the program runs mad
//
// timeout: time in milliseconds after the interrupt is thrown
//
void enableWatchdog (void) {
  
  // reset the watchdog timer
  wdt_reset();
  
  // Disable interrupt
  cli();
  
  // Set the interrput flag
  MCUSR = (1<<WDRF);

  // This must be done to set the WDTCSR
  WDTCSR = (1<<WDCE) | (1<<WDE);
  
  // Enable the watchdog
  WDTCSR = (1<<WDE) | (1<<WDIE) | WDT_TIMEOUT;
  
  sei();
  
}


// Watchdog interrupt
//
ISR(WDT_vect) {

  // Disable any interrupt
  cli ();
  
  // Send a crash message
  // TODO: send crash message over CAN
  Serial.println ("system crashed, restarting...");
  
}


// Control serial input and sets the autopilot according to the input
//
void serialControl (Controller *controller) {
  
  // Variables
  int num_characters = 0;
  char serial_in = 0;
  float h_target;
  
  // Check serial input
  if (Serial.available () > 0)
    serial_in = Serial.read ();
    
  
  // Switch to the desired mode
  //
  // '0': do nothing
  // 'r': reset controller
  // 'k': keep height
  // 's': set height
  // 'm': main motor
  // 'n': switch to non verbal mode
  // 'v': switch to verbal mode
  switch (serial_in) {
  
  // '0': do nothing
  case '0':
  
    flight_mode = 0;
    
    break;
    
  // 'r': reset controller
  case 'r':
    
    controller->reset ();
  
    break;
    
  // 'k': keep height
  case 'k':
  
    h_target = Sensor_getHeight ();
    flight_mode = serial_in;
  
    break;
  
  // 's': set height
  case 's':
    
    num_characters = 0;
    h_target = 0;
    
    // Read 3 characters
    while (num_characters<3) {
            
      if (Serial.available () > 0) {
        h_target += (Serial.read () - '0') * pow(10, 2-num_characters);
        num_characters++;
      }
      
    }
    
    controller->setTargetHeight (h_target);
    flight_mode = serial_in;
    
    break;
  
  // 'm': main motor
  case 'm':
  
    num_characters = 0;
    angle = 0;
    flight_mode = serial_in;
    
    // Read 2 characters angle
    while (num_characters<3) {
      if (Serial.available () > 0) {
        angle += (Serial.read () - '0') * pow(10, 2-num_characters);
        num_characters++;
      }
    }

    break;
 
  // 'n': switch to non verbal mode
  case 'n':
  
    verbose = false;
    
    break;
    
  // 'v': switch to verbal mode
  case 'v':
  
    verbose = true;
    
    break;
            
  }
  // reset serial input
  serial_in = 0;
  
  
  // Launch the desired mode
  //
  switch (flight_mode) {
  
  case 'm':
    //FlightAux_setMainMotorAngle (angle);
    break;  
  
  case 'k':
  case 's':
    
    // Control the height
    controller->heightControl (verbose);
    
    break;
  
  case '0':
  default:
    
    // All servos mid
    for (int i=0; i<8; i++) {
      setCh (i, 1500);
    }
  
    break;
   
    
  }

}

Controller.ino

// Controller Ardupilot File
//
// Author: Andreas Lenz
//

/*************************************************************************************************************************
 *************************************************************************************************************************
 ******************************                        CONTROLLER                     ************************************
 *************************************************************************************************************************
 *************************************************************************************************************************
 */

#include "Controller.h"


// Constructor
//
Controller::Controller () {
  
}

// Initialise the controller; Note: the Sensors should be initialised before the controller is initialised!
//
void Controller::init () {
  
  // Get the global sensor
  //this->sensor = sensor;
  
  // Create FlightAux class
  flightAux = new FlightAux;
  
  // Get height and heading of the zeppelin
  float height = Sensor_getHeight ();
  float heading = Sensor_getCompassAngle ();
  
  // Calculate discrete coefficients
  d0 = (2*P+I*Ts)*(2+N*Ts)+4*D*N;
  d1 = (-2*P+I*Ts)*(2+N*Ts)+(2*P+I*Ts)*(-2+N*Ts)-8*D*N;
  d2 = (-2*P+I*Ts)*(-2+N*Ts)+4*D*N;
  e0 = 4+2*N*Ts;
  e1 = -8;
  e2 = 4-2*N*Ts;
  
  // Init control variables
  for (int i=0; i<3; i++) {
    h_x[i] = height;
    h_w[i] = height;
    h_y[i] = 0;
    r_x[i] = heading;
    r_w[i] = heading;
    r_y[i] = 0;
  }
  
}

// Reset the controller variables
//
void Controller::reset (void) {
  
  // Get height and heading of the zeppelin
  float height = Sensor_getHeight ();
  float heading = Sensor_getCompassAngle ();
  
  // Reset control variables
  for (int i=0; i<3; i++) {
    h_x[i] = height;
    h_w[i] = height;
    h_y[i] = 0;
    r_x[i] = heading;
    r_w[i] = heading;
    r_y[i] = 0;
  }
  
  
}


// Set the target height
//
// height: the height that shall be reached
//
void Controller::setTargetHeight (float height) {
  
  h_target = height;
  
}


// Control the height of the zeppelin
//
// height: desired height
//
void Controller::heightControl (bool verbose_only) {
  
  // Update control variables
  for (int i=0; i<2; i++) {
    h_x[i] = h_x[i+1];
    h_w[i] = h_w[i+1];
    h_y[i] = h_y[i+1];
  }
  
  h_x[2] = Sensor_getHeight ();
  h_w[2] = h_target;
  
  // Calculate new value (timediscrete model/transfer function)
  h_y[2] = (-e1*h_y[1]-e2*h_y[0]+d0*(h_w[2]-h_x[2])+d1*(h_w[1]-h_x[1])+d2*(h_w[0]-h_x[0]))/e0;
  if (h_y[2] > 1.0) { h_y[2] = 1.0; }
  if (h_y[2] < -1.0) { h_y[2] = -1.0; }
  
  if (verbose_only) {
    Serial.print("acc= ");
    Serial.print(h_y[2]);
    Serial.print(", height= ");
    Serial.print(h_x[2]);
    Serial.print(", desired height= ");
    Serial.println(h_w[2]);
  } else {
    flightAux->setMainMotor (180+(h_y[2]>0 ? -1 : 1)*90, abs(h_y[2]));
  }
  
  
}


// Set the target heading
//
// heading: the heading that shall be reached
//
void Controller::setTargetHeading (float heading) {
  
  r_target = heading;
  
}

// Control the rotation of the zeppelin
void Controller::rotationControl (bool verbose_only) {
  
  // Update control variables
  for (int i=0; i<2; i++) {
    r_x[i] = r_x[i+1];
    r_w[i] = r_w[i+1];
    r_y[i] = r_y[i+1];
  }
  
  r_x[2] = Sensor_getCompassAngle ();
  r_w[2] = r_target;
  
  // calculate new value (timediscrete model/transfer function)
  r_y[2] = (-e1*r_y[1]-e2*r_y[0]+d0*(r_w[2]-r_x[2])+d1*(r_w[1]-r_x[1])+d2*(r_w[0]-r_x[0]))/e0;
  if (r_y[2] > 1.0) { r_y[2] = 1.0; }
  if (r_y[2] < -1.0) { r_y[2] = -1.0; }
  
  if (verbose_only) {
    Serial.print("acc= ");
    Serial.print(r_y[2]);
    Serial.print(", rot= ");
    Serial.print(r_x[2]);
    Serial.print(", desired rot= ");
    Serial.println(r_w[2]);
  } else {
   flightAux->rotate (r_y[2]);
  }
  
}


// Fly a special heading and height to a target
//
// height: desired target height
// heading: the heading to fly
// distance: horizontal distance to the target
//
void Controller::flyToTarget (float height, float heading, float distance) {
  
  // Variables
  h_target = height;
  r_target = heading;
  
  // Check, whether we are far from the target
  if (distance >= CONTROLLER_FLYNEAR) {
    
    // Simply go fullspeed in the desired direction
    flightAux->setMainMotor (180-atan2(height-Sensor_getHeight(), distance), 1.0);
    
  }

}

// Init the selftuning PID controller
//
// Kp: Proportional constant
// Ki: Integral constant
// Kd: Differential constant
// lambda: Selftuning constant
//
void Controller::initPID (float Kp, float Ki, float Kd, float lambda) {
  
  // Set constants
  this->Kp = Kp;
  this->Ki = Ki;
  this->Kd = Kd;
  this->lambda = lambda;
  
  // Set initial time
  lT = millis ();
  lE = 0;            // last error
  iE = 0;            // integral error
  
}


// Run the selftuning PID controller
//
// e: Current error between actual value and desired value
// Return: Throttle/Output of the PID controller
//
float Controller::selfTuningPID (float e) {

  // Calculate time difference
  float dT = millis() - lT;
  lT = millis();
  
  // Prevent too big time delays // startup problems
  if (dT > CONTROLLER_MAXDT) { dT = CONTROLLER_MAXDT; }
    
  // Integrate error
  iE = iE + e*dT;
  
  // Differential error
  float dE = (e-lE)/dT;
  
  // Calculate new controller coefficients
  Kp = Kp + e*e*lambda*dT;
  Ki = Ki + e*iE*lambda*dT;
  Kd = Kd + e*dE*lambda*dT;
  
  // Calculate output
  float out = Kp*e + Ki*iE + Kd*dE;

  // Update error
  lE = e;
  
  // Return output
  return out;

}

FlightAux.ino

// FlightAux Ardupilot File
//
// Author: Andreas Lenz
//

/*************************************************************************************************************************
 *************************************************************************************************************************
 ******************************                         PPMTIMER                      ************************************
 *************************************************************************************************************************
 *************************************************************************************************************************
 */



#include "FlightAux.h"


// Constructor
//
FlightAux::FlightAux () {
  
}

// set the main motor angle
//
// angle: angle of the motor: between 0 and FLIGHTAUX_MAXANGLE or 180 and 180+FLIGHTAUX_MAXANGLE
//        0° forward, 90° top, 180° backward, 270° bot
//
int FlightAux::setMainMotorAngle (float angle) {
  
  // Variables
  int bwd, chRot;
  
  // Check boundaries
  if ((angle>=0 && angle<=FLIGHTAUX_MAXANGLE) || (angle>=180 && angle<=180+FLIGHTAUX_MAXANGLE)) {
    
    // Update the angle
    mainMotorAngle = angle;
    
    // Backward = -1, Forward = 1 // calculate pwm signal
    bwd = angle>=180 ? -1 : 1;
    chRot = FLIGHTAUX_VERTICAL - (bwd == 1 ? angle : angle-180)/90*FLIGHTAUX_QUARTER;
    
    // output the pwm signal
    setCh (FLIGHTAUX_ROTMOTORPWMCH, chRot);
    
    return 0;
    
  }
  else return -1;

  
}

// set the main motor speed 
//
// speed: speed of the motor: between 0 and 1
//
int FlightAux::setMainMotorSpeed (float speed) {
  
  // Variables
  int bwd, chSpeed;
  
  // Check boundaries
  if ((speed>=0.0) && (speed<=1.0)) {
  
    // Backward = -1, Forward = 1 // calculate pwm signal
    bwd = mainMotorAngle>180 ? -1 : 1;
    chSpeed = 1500 + 400*speed*bwd;
    
    // output pwm signal
    setCh (FLIGHTAUX_SPEEDPWMCH1, chSpeed);
    setCh (FLIGHTAUX_SPEEDPWMCH2, chSpeed);
    
    
    return 0;
    
  }
  else return -1;

  
}


// set the main motor speed and angle
//
// angle: angle of the motor: between 0 and FLIGHTAUX_MAXANGLE or 180 and 180+FLIGHTAUX_MAXANGLE
//        0° forward, 90° top, 180° backward, 270° bot
// speed: speed of the motor: between 0 and 1
//
int FlightAux::setMainMotor (float angle, float speed) {
  
  // Variables
  int bwd, chSpeed, chRot;
  
  // Check boundaries
  if (((angle>=0 && angle<=FLIGHTAUX_MAXANGLE) || (angle>=180 && angle<=180+FLIGHTAUX_MAXANGLE)) && (speed>=0.0 && speed<=1.0)) {
    
    // Update the angle
    mainMotorAngle = angle;
    
    // Backward = -1, Forward = 1
    bwd = angle>180 ? -1 : 1;
    
    chSpeed = 1500 + 400*speed*bwd;
    chRot = FLIGHTAUX_VERTICAL - (bwd == 1 ? angle : angle-180)/90*FLIGHTAUX_QUARTER;
    
    
    setCh (FLIGHTAUX_SPEEDPWMCH1, chSpeed);
    setCh (FLIGHTAUX_SPEEDPWMCH2, chSpeed);
    setCh (FLIGHTAUX_ROTMOTORPWMCH, chRot);
    
    return 0;
    
  }
  else return -1;

  
}


// rotate the zeppelin; should be done while standing
//
// speed: speed to rotate (right < 0, left > 0, abs(speed) must be <1)
//
void FlightAux::rotate (float speed) {
  
  if ((speed>-1) && (speed<1)) {
    
    setCh (FLIGHTAUX_ROT, 1500+400*speed);
    
  }
  else setCh (FLIGHTAUX_ROT, 1500);
  
  
}


// yaw the zeppelin while flying
//
// yaw: angle of the rear oar (left > 0, right<0, abs(angle) must be <90)
//
void FlightAux::yaw (float angle) {
  
}

PPMTimer.ino

// PPMtimer Ardupilot File
//
// Author: Andreas Lenz
//

/*************************************************************************************************************************
 *************************************************************************************************************************
 ******************************                         PPMTIMER                      ************************************
 *************************************************************************************************************************
 *************************************************************************************************************************
 */
 

// Includes
//
#include <avr/io.h>
#include <avr/interrupt.h>

// Variables needed to decode the ppm signal
//
volatile unsigned int PPMtimer_Start_Pulse;
volatile unsigned int PPMtimer_Stop_Pulse;
volatile unsigned int PPMtimer_Pulse_Width;

volatile char PPMtimer_Counter;
volatile int PWMtimer_RAW[8] = {3000, 3000, 3000, 3000, 3000, 3000, 3000, 3000};


// set a servo channel to the submitted value
//
// ch: channel to set
// pwm: value (between 1000 and 2000)
//
void setCh (char ch, int pwm) {
  
	if (pwm < 1050) { pwm = 1050; }
	if (pwm > 1950) { pwm = 1950; }
	pwm *= 2;

	switch(ch) {
		case 0:  OCR5B = pwm; break;  // channel 0
		case 1:  OCR5C = pwm; break;  // channel 1
		case 2:  OCR1B = pwm; break;  // channel 2
		case 3:  OCR1C = pwm; break;  // channel 3
		case 4:  OCR4C = pwm; break;  // channel 4
		case 5:  OCR4B = pwm; break;  // channel 5
		case 6:  OCR3C = pwm; break;  // channel 6
		case 7:  OCR3B = pwm; break;  // channel 7
	}

}


// read a value from the specified channel
//
// ch: channel to read from
// return: channel value
//
int getCh(char ch) {
	return (PWMtimer_RAW[ch])/2; 
}


// initialise timer1 (channel 2&3)
//
void Init_PWM1(void)
{

	// set the two pwm pins to output
	DDRB |= (1 << PB6) | (1 << PB7);

	// timer settings
	TCCR1A = (1<<WGM11) | (1<<COM1B1) | (1<<COM1C1);				// clear OC1B and OC1C at compare match
	TCCR1B = (1<<WGM13) | (1<<WGM12) | (1<<CS11);					// Prescaler set to 8, that give us a resolution of 0.5µs
	OCR1B = 3000;													// OUT2
	OCR1C = 3000;													// OUT3
	ICR1 = 39999;													// 50hz frequence 16000000Hz / 8 / (39999+1) = 50Hz

}


// initialise timer3 (channel 6&7)
//
void Init_PWM3(void)
{

	// set the two pwm pins to output
	DDRE |= (1 << PE4) | (1 << PE5);

	// timer settings
	TCCR3A = (1<<WGM31) | (1<<COM3B1) | (1<<COM3C1);				// clear OC3B and OC3C at compare match
	TCCR3B = (1<<WGM33) | (1<<WGM32) | (1<<CS31);					// Prescaler set to 8, that give us a resolution of 0.5µs
	OCR3B = 3000;													// OUT7
	OCR3C = 3000;													// OUT6
	ICR3 = 39999;													// 50hz frequence 16000000hz / 8 / (39999+1) = 50Hz

}


// initialise timer5 (channel 0&1)
//
void Init_PWM5(void)
{

	// set the two pwm pins to output
	DDRL |= (1 << PL4) | (1 << PL5);

	// timer settings
	TCCR5A = (1<<WGM51) | (1<<COM5B1) | (1<<COM5C1);				// clear OC3B and OC3C at compare match
	TCCR5B = (1<<WGM53) | (1<<WGM52) | (1<<CS51);					// Prescaler set to 8, that give us a resolution of 0.5µs
	OCR5B = 3000;													// OUT0
	OCR5C = 3000;													// OUT1
	ICR5 = 39999;													// 50hz frequence 16000000hz / 8 / (39999+1) = 50Hz

}

// intialise timer4 (channel 4&5 + ppm input)
//
void Init_PPM_PWM4(void)
{

	// set the two pwm pins to output and the ppm pin to input
	DDRH |= (1 << PH4) | (1 << PH5);
	DDRH &= ~(1 << PL0);

	// timer settings
	TCCR4A = (1<<WGM40) | (1<<WGM41) | (1<<COM4C1) | (1<<COM4B1) | (1<<COM4A1);  
	TCCR4B = (1<<WGM43) | (1<<WGM42) | (1<<CS41) | (1<<ICES4);		// Prescaler set to 8, that give us a resolution of 0.5µs, interrupt capture enabled
	OCR4A = 39999; 
	OCR4B = 3000;													// OUT5
	OCR4C = 3000;													// OUT4
 
	TIMSK4 |= (1<<ICIE4);											// Timer interrupt mask
	sei();															// Interrupt enable
  
}


// Interrupt routine
//
ISR (TIMER4_CAPT_vect)
{

	// rising edge
	if (((1<<ICES4)&TCCR4B) >= 0x01) { 

  		if(PPMtimer_Start_Pulse>PPMtimer_Stop_Pulse) {					//Checking if the Stop Pulse overflow the register, if yes i normalize it.

			PPMtimer_Stop_Pulse+=40000;						//Nomarlizing the stop pulse.
		}

		PPMtimer_Pulse_Width=PPMtimer_Stop_Pulse-PPMtimer_Start_Pulse + 600;		//Calculating pulse 
		
		if (PPMtimer_Pulse_Width>5600) {						//Verify if this is the sync pulse

			PPMtimer_Counter=0;							//If yes restart the counter
		}
		else {

			PWMtimer_RAW[PPMtimer_Counter]=PPMtimer_Pulse_Width;			//Saving pulse. 
			PPMtimer_Counter++; 
		}

		PPMtimer_Start_Pulse=ICR4;
		TCCR4B &=(~(1<<ICES4));								//Changing edge detector. 
	}

	// falling edge
	else
	{
		PPMtimer_Stop_Pulse=ICR4;							//Capturing time stop of the drop edge
		TCCR4B |= (1<<ICES4);								//Changing edge detector. 

	}
  
}

Sensor.ino

// Sensor Ardupilot File
//
// Author: Andreas Lenz
//

/*************************************************************************************************************************
 *************************************************************************************************************************
 ******************************                          SENSOR                       ************************************
 *************************************************************************************************************************
 *************************************************************************************************************************
 */


#include "Sensor.h"


// initialise the HMC5883L compass
//
bool Sensor_init (void) {
  
  // Variables
  float T = 0;
  
  // Wait a bit
  delay (100);
  
  // Standard values
  Sensor_seapress = 1013.25;
  
  // Enable the bypass to use the magnetometer
  Sensor_acc.initialize();
  Sensor_acc.setI2CBypassEnabled(true);
  if (!Sensor_acc.testConnection()) return false;
  
  // Initialize compass
  Sensor_compass.initialize();
  if (!Sensor_compass.testConnection()) return false;
  Sensor_compass.setSampleAveraging(HMC5883L_AVERAGING_8);
  Sensor_compass.setGain(HMC5883L_GAIN_1370);
  
  // Initialise barometer
  Sensor_baro.init (MS561101BA_ADDR_CSB_LOW);
  Sensor_baro.readPROM ();
  
  // Reset watchdog
  wdt_reset ();
  
  // Wait for sensors to be launched correctly
  delay(500);
  
  // Read some sensor values
  //
  
  // Barometer
  for (int i=0, safe=0; i<SENSOR_AVGSAMPLES && safe<SENSOR_MAXATTEMPT;safe++) {
    if (Sensor_baroAvg[i] = Sensor_baro.getPressure (MS561101BA_OSR_4096)) {
      i++;
    }
    wdt_reset ();
    delay(10);
  }
  
  for (int i=0; i<SENSOR_MAXATTEMPT && !T; i++) { T = Sensor_baro.getTemperature (MS561101BA_OSR_4096); wdt_reset(); }
  if (T) { Sensor_T = T; }
  
  return true;
  
}

// determine the orientation of the zeppelin
//
// return: angle to north in degrees
//
float Sensor_getCompassAngle (void) {
  
  return Sensor_heading;
  
}


// Determine the height of the zeppelin
//
// Return: The height of the zeppelin
//
float Sensor_getHeight () {
  
  // calculate the average
  float avg = 0;
  for (int i=0; i<SENSOR_AVGSAMPLES; i++) {
    avg += Sensor_baroAvg[i];
  }
  avg /= SENSOR_AVGSAMPLES;
  
  // return the height
  return ((pow((Sensor_seapress / avg), 1/5.257) - 1.0) * (Sensor_T + 273.15)) / 0.0065;
  
}


// Get the Temperature
//
// Return: Temperature in °C
float Sensor_getTemperature () {
  
  return Sensor_T;
  
}

// Update sensor values: Used to keep sensor values and not to reload them everytime you need them
//
void Sensor_update (void) {
  
  // BAROMETER
  //
  
  // Variables
  float T = 0, p = 0;
  
  // Switch barometer mode
  // We have to switch between pressure and temperature mode to have enogh time between two measures
  if (Sensor_baroMode == 0) {
    
    // Reset mode
    Sensor_baroMode = 1;

    // If the measure was succesfull
    if (p = Sensor_baro.getPressure (MS561101BA_OSR_4096)) {
      if (++Sensor_baroCount >= SENSOR_AVGSAMPLES) { Sensor_baroCount = 0; }
      Sensor_baroAvg[Sensor_baroCount] = p;
    }
    
  } else {
    if (T = Sensor_baro.getTemperature (MS561101BA_OSR_4096) ) {Sensor_T = T; Sensor_baroMode = 0;}
  }
  
  
  // COMPASS
  //

  // Variables
  int cx, cy, cz, ax, ay, az, gx, gy, gz;
  
  // get raw values from the sensor
  Sensor_compass.getHeading (&cx, &cy, &cz);
  Sensor_acc.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
  
  ax = (ax/16.384) + Sensor_accOffset[0];
  ay = (ay/16.384) + Sensor_accOffset[1];
  az = (az/16.384) + Sensor_accOffset[2];
  
  // add an offset that is produced by the sensor
  cx += Sensor_magOffset[0];
  cy += Sensor_magOffset[1];
  cz += Sensor_magOffset[2];
  
  // calculate angle from magnetic vectors
  Sensor_heading = atan2 (cz, cx)*180.0/PI;
  
  return;
  
}


// Calibrate the barometer to seapress
//
// height: current height of the sensor
//
void Sensor_calibBaro (float h) {
  
  float p = 0, pAvg = 0, T = 0, TAvg = 0;
  int i = 0, safe = 0;
  
  // Determine the pressure
  for (i=0, safe=0; safe<SENSOR_CALIBATTEMTPS; safe++) {
    p = Sensor_baro.getPressure (MS561101BA_OSR_4096);
    if (p) { pAvg+=p; i++; }
    delay(10);
  }
  // At least one measure was succesfull
  if (i > 0) {
    
    // calculate the mean value
    pAvg /= i;
  
    // Determine the temperature
    for (i=0, safe=0; safe<SENSOR_CALIBATTEMTPS; safe++) {
      T = Sensor_baro.getTemperature (MS561101BA_OSR_4096);
      if (T) { TAvg+=T; i++; }
      delay(10);
    }
    // At least one measure was succesfull
    if (i > 0) {
    
      TAvg /= i;
  
      // Determine the seapress
      Sensor_seapress = pAvg / pow ( 1.0 - 0.0065*h / (TAvg + 273.15 + 0.0065*h), 5.257 );
      
    } else { Sensor_seapress = 1013.25; }
    
  } else { Sensor_seapress = 1013.25; }
  
}

 

 

Anhänge:
Diese Datei herunterladen (Ardupilot.zip)Sourcecode[Vollständiger Sourcecode des Autopiloten]%2013-%01-%17 %0:%Jan%+01:00