int main (int argc, char **argv)
{
int file;
int16_t temp;
uint8_t data[2] = {0};
Triplet a_bias = {0}, g_bias = {0}, m_bias = {0};
FTriplet m_scale;
int opt, option_index, help = 0, option_dump = 0;
OptionMode option_mode = OPTION_MODE_ANGLES;
float declination = 0.0;
while ((opt = getopt_long(argc, argv, "d:hm:u",
long_options, &option_index )) != -1) {
switch (opt) {
case 'd' :
declination = atof (optarg);
break;
case 'm' :
if (strcmp (optarg, "sensor") == 0)
option_mode = OPTION_MODE_SENSOR;
else if (strcmp (optarg, "angles") == 0)
option_mode = OPTION_MODE_ANGLES;
else
help = 1;
break;
case 'u' :
option_dump = 1;
break;
default:
help = 1;
break;
}
}
if (help || argv[optind] != NULL) {
printf ("%s [--mode <sensor|angles>] [--dump]\n", argv[0]);
return 0;
}
if (!read_bias_files (&a_bias, &g_bias, &m_bias, &m_scale))
return 1;
file = init_device (I2C_DEV_NAME);
if (file == 0)
return 1;
if (option_dump) {
dump_config_registers(file);
printf ("\n");
}
init_gyro(file, GYRO_SCALE_245DPS);
init_mag(file, MAG_SCALE_2GS);
init_acc(file, ACCEL_SCALE_2G);
// temperature is a 12-bit value: cut out 4 highest bits
read_bytes (file, XM_ADDRESS, OUT_TEMP_L_XM, &data[0], 2);
temp = (((data[1] & 0x0f) << 8) | data[0]);
printf ("Temperature: %d\n", temp);
printf ("Temperature: %d\n", temp);
if (option_mode == OPTION_MODE_SENSOR)
printf (" Gyroscope (deg/s) | Magnetometer (mGs) | Accelerometer (mG)\n");
else
printf (" Rotations (mag + acc):\n");
while (1) {
FTriplet gyro, mag, acc, angles1;
usleep (500000);
read_gyro (file, g_bias, GYRO_SCALE_245DPS, &gyro);
read_mag (file, m_bias, m_scale, MAG_SCALE_2GS, &mag);
read_acc (file, a_bias, ACCEL_SCALE_2G, &acc);
if (option_mode == OPTION_MODE_SENSOR) {
printf ("gyro: %4.0f %4.0f %4.0f | ", gyro.x, gyro.y, gyro.z);
printf ("mag: %4.0f %4.0f %4.0f | ", mag.x*1000, mag.y*1000, mag.z*1000);
printf ("acc: %4.0f %4.0f %5.0f\n", acc.x*1000, acc.y*1000, acc.z*1000);
} else {
calculate_simple_angles (mag, acc, declination, &angles1);
printf ("pitch: %4.0f, roll: %4.0f, yaw: %4.0f\n",
angles1.x, angles1.y, angles1.z);
}
}
return 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();
}
/*************************************************************************************************
Main
**************************************************************************************************/
void main(void)
{
HAL_BOARD_INIT();
UART_init();
U0CSR &= ~0x04;
ENABLE_RX();
/*setup sensors*/
sensors_init();
sensor_int_init();
/* Setup LED's */
P1DIR |= BV(0);
P0DIR |= BV(4);
P1_0 = 0;
start_gyro(); //start the gyro
init_acc(); //start the accelerometer
start_mag();
start_baro();
//zero_mag();
EA = 1;
SLEEPCMD |= 0x02; //pm 2
uint8 flag;
uint8 IDbyte;
uint8 baro_stage = 1;
while(1){
if (RXin)
{
//If it is a cal data request
if ( active_sensors & BV(4) )
{
flag = 0x03;
flush_data(&flag);
baro_read_cal();
//End of line char
flag = 0x00;
flush_byte(&flag);
} else if ( active_sensors > 0 ) {
// P0_4 = 1;
/**************************************************************************/
/* Read and transmit sensor data */
flag = 0x04;
flush_byte(&flag);
flush_byte(&active_sensors);
/*---------------------------------------------------------------------*/
//Read accelerometer and gyro
if ( active_sensors & BV(0) )
{
IDbyte = BV(0);
flush_data(&IDbyte);
while( !( acc_int_status() ) ); //wait for interrupt
read_acc(); //read the accelerometer
while( !(gyro_int_status() & BV(0) ) ); //wait for interrupt
read_gyro();
}
/*---------------------------------------------------------------------*/
//Read Magnetometer
//start_interrupts(MAG_INT);
if ( active_sensors & BV(1) )
{
IDbyte = BV(1);
flush_data(&IDbyte);
//PCON |= 1;
while( !(mag_status() & 0x08 ) );
read_mag();
// mag_sleep(TRUE);
}
/*---------------------------------------------------------------------*/
//Barometer
//uint16 delay_ticks;
uint8 baro_res = 2;
if ( active_sensors & BV(2) )
{
IDbyte = BV(2);
flush_data(&IDbyte);
if (active_sensors & 0x40)
{
//delay_ticks = 0xFA00;
baro_capture_press(baro_res);
//while(delay_ticks--);
baro_read_press(TRUE);
//delay_ticks = 0xFA00;
baro_capture_temp();
//while(delay_ticks--);
baro_read_temp(TRUE);
}else{
uint8 nullbyte = 3;
switch (baro_stage)
{
case 1 :
baro_capture_press(baro_res);
baro_read_press(FALSE);
baro_read_temp(FALSE);
baro_stage++;
break;
case 2 :
baro_read_press(TRUE);
baro_read_temp(FALSE);
baro_stage++;
break;
case 3 :
baro_capture_temp();
baro_read_press(FALSE);
baro_read_temp(FALSE);
baro_stage++;
break;
case 4 :
baro_read_press(FALSE);
baro_read_temp(TRUE);
baro_stage = 1;
break;
}
}
//baro_shutdown();
}
/*---------------------------------------------------------------------*/
//Humidity
if ( active_sensors & BV(3) )
{
IDbyte = BV(3);
flush_data(&IDbyte);
humid_init();
humid_read_humidity(TRUE);
}
/*---------------------------------------------------------------------*/
//End of line char
flag = 0x00;
flush_byte(&flag);
}
if ( !(active_sensors & BV(5)) ) //if autopoll is off
{
P0_4 = 1;
RXin = 0; //clear the RX flag
}else{
P0_4 = 0;
}
}
IEN0 |= 0x04;
U0CSR &= ~0x04;
}
}
