void vector_difference_to_euler_angles(fixedpointnum *v1, fixedpointnum *v2, fixedpointnum *euler) {
// take the difference between the two attitudes and return the euler angles between them
// find the axis of rotation and angle between the two downVectors
// the cross products of the two vectors will give us the axis of rotation from one to the other
fixedpointnum axisofrotation[3];
vector_cross_product(v1, v2, axisofrotation);
fixedpointnum axislength=lib_fp_sqrt(normalize_vector(axisofrotation));
// get the angle of rotation between the two vectors
fixedpointnum angle=lib_fp_atan2(axislength, vector_dot_product(v1, v2));
fixedpointnum unitvector[3];
unitvector[0]=0;
unitvector[1]=FIXEDPOINTONE;
unitvector[2]=0;
euler[0]=lib_fp_multiply(vector_dot_product(axisofrotation, unitvector), angle);
unitvector[0]=FIXEDPOINTONE;
unitvector[1]=0;
unitvector[2]=0;
euler[1]=lib_fp_multiply(vector_dot_product(axisofrotation, unitvector), angle);
}
void rotate_vector_by_axis_small_angle(fixedpointnum *v1, fixedpointnum *axisvector, fixedpointnum angle) {
// rotates vector by small angle angle around axis vector.
fixedpointnum angleinradians=lib_fp_multiply(angle, FIXEDPOINTPIOVER180);
fixedpointnum crossproductvector[3];
vector_cross_product(axisvector,v1, crossproductvector);
v1[0]+=lib_fp_multiply(crossproductvector[0],angleinradians);
v1[1]+=lib_fp_multiply(crossproductvector[1],angleinradians);
v1[2]+=lib_fp_multiply(crossproductvector[2],angleinradians);
}
void resetpilotcontrol(void)
{ // called when switching from navigation control to pilot control or when idling on the ground.
// keeps us from accumulating yaw error that we can't correct.
desiredcompassheading = global.currentestimatedeulerattitude[YAWINDEX];
// Same for yaw hold mode
accumulatedyawerror = 0;
// calculate our max rotation rates based on usersettings
highyawrate = lib_fp_multiply(usersettings.maxyawrate, FP_HIGH_RATES_MULTILIER);
highpitchandrollrate = lib_fp_multiply(usersettings.maxpitchandrollrate, FP_HIGH_RATES_MULTILIER);
}
void compute_mix(fixedpointnum throttle, fixedpointnum pid[]) {
#if (AIRCRAFT_CONFIGURATION==QUADX)
global.motor[QUADX_REAR_RIGHT_MOTOR] =throttle-pid[ROLL_INDEX]+pid[PITCH_INDEX]-pid[YAW_INDEX];
global.motor[QUADX_FRONT_RIGHT_MOTOR]=throttle-pid[ROLL_INDEX]-pid[PITCH_INDEX]+pid[YAW_INDEX];
global.motor[QUADX_REAR_LEFT_MOTOR] =throttle+pid[ROLL_INDEX]+pid[PITCH_INDEX]+pid[YAW_INDEX];
global.motor[QUADX_FRONT_LEFT_MOTOR] =throttle+pid[ROLL_INDEX]-pid[PITCH_INDEX]-pid[YAW_INDEX];
#elif (AIRCRAFT_CONFIGURATION==TRI)
global.motor[TRI_REAR_MOTOR]=throttle+lib_fp_multiply(FIXEDPOINT_FOUR_THIRDS,pid[PITCH_INDEX]);
global.motor[TRI_LEFT_MOTOR]=throttle-pid[ROLL_INDEX]-lib_fp_multiply(FIXEDPOINT_TWO_THIRDS,pid[PITCH_INDEX]);
global.motor[TRI_RIGHT_MOTOR]=throttle=pid[ROLL_INDEX]-lib_fp_multiply(FIXEDPOINT_TWO_THIRDS,pid[PITCH_INDEX]);
global.servo[TRI_REAR_SERVO]=SERVO_DIR(TRI_REAR_SERVO)*pid[YAW_INDEX]+SERVO_MID(TRI_REAR_SERVO);
#else
#error "Must define mix for aircraft"
#endif
}
fixedpointnum normalize_vector(fixedpointnum *v) {
fixedpointnum vectorlengthsquared=lib_fp_multiply(v[0],v[0])+lib_fp_multiply(v[1],v[1])+lib_fp_multiply(v[2],v[2]);
// if we are near zero length, choose any unit length vector
if (vectorlengthsquared<10) {
v[0]=FIXEDPOINTONE;
v[1]=v[2]=0;
} else {
fixedpointnum multiplier=lib_fp_invsqrt(vectorlengthsquared);
v[0]=lib_fp_multiply(v[0], multiplier);
v[1]=lib_fp_multiply(v[1], multiplier);
v[2]=lib_fp_multiply(v[2], multiplier);
}
return(vectorlengthsquared);
}
fixedpointnum gpsstringtoangle(char *string)
{ // takes a gps string and converts it to a fixedpointnum angle
// "4807.123" means 48 degrees, 7.123 minutes, south
// how many digits are there before the decimal point?
int index=0;
while (string[index]>='0' && string[index]<='9') ++index;
// convert the minutes part
fixedpointnum minutes=lib_fp_stringtofixedpointnum(&string[index-2]);
string[index-2]='\0';
fixedpointnum degrees=lib_fp_stringtofixedpointnum(string);
return((degrees<<LATLONGEXTRASHIFT)+lib_fp_multiply(minutes,69905L)); // 69905L is (1/60) * 2^16 * 2^6
}
void rotate_vector_by_axis_angle(fixedpointnum *v1, fixedpointnum *axisvector, fixedpointnum angle, fixedpointnum *v2) {
fixedpointnum cosineofangle=lib_fp_cosine(angle);
fixedpointnum sineofangle=lib_fp_sine(angle);
fixedpointnum crossproductvector[3];
vector_cross_product(axisvector,v1, crossproductvector);
fixedpointnum dotproducttimesoneminuscosineofangle=lib_fp_multiply(vector_dot_product(axisvector,v1),FIXEDPOINTONE-cosineofangle);
v2[0]=lib_fp_multiply(v1[0], cosineofangle)+lib_fp_multiply(crossproductvector[0],sineofangle)+lib_fp_multiply(axisvector[0],dotproducttimesoneminuscosineofangle);
v2[1]=lib_fp_multiply(v1[1], cosineofangle)+lib_fp_multiply(crossproductvector[1],sineofangle)+lib_fp_multiply(axisvector[1],dotproducttimesoneminuscosineofangle);
v2[2]=lib_fp_multiply(v1[2], cosineofangle)+lib_fp_multiply(crossproductvector[2],sineofangle)+lib_fp_multiply(axisvector[2],dotproducttimesoneminuscosineofangle);
}
void rotate_vector_with_small_angles(fixedpointnum *v, fixedpointnum rolldeltaangle, fixedpointnum pitchdeltaangle, fixedpointnum yawdeltaangle) {
// rotate the attitude by the delta angles.
// assumes that the delta angles are small angles in degrees and that they are shifted left by TIMESLIVEREXTRASHIFT
fixedpointnum v_tmp_x=v[X_INDEX];
fixedpointnum v_tmp_y=v[Y_INDEX];
fixedpointnum v_tmp_z=v[Z_INDEX];
// remember that our delta angles are shifted left by TIMESLIVEREXTRASHIFT for resolution. Take it out here
v[X_INDEX] += (lib_fp_multiply(rolldeltaangle ,v_tmp_z) - lib_fp_multiply(yawdeltaangle,v_tmp_y))>>(TIMESLIVEREXTRASHIFT);
v[Y_INDEX] += (lib_fp_multiply(pitchdeltaangle,v_tmp_z) + lib_fp_multiply(yawdeltaangle ,v_tmp_x))>>(TIMESLIVEREXTRASHIFT);
v[Z_INDEX] -= (lib_fp_multiply(rolldeltaangle,v_tmp_x) + lib_fp_multiply(pitchdeltaangle ,v_tmp_y))>>(TIMESLIVEREXTRASHIFT);
}
fixedpointnum navigation_getdistanceandbearing(fixedpointnum lat1,fixedpointnum lon1,fixedpointnum lat2,fixedpointnum lon2,fixedpointnum *bearing)
{ // returns fixedpointnum distance in meters and bearing in fixedpointnum degrees from point 1 to point 2
fixedpointnum latdiff=lat2-lat1;
fixedpointnum londiff=lib_fp_multiply(lon2-lon1,lib_fp_cosine(lat1>>LATLONGEXTRASHIFT));
*bearing = FIXEDPOINT90 + lib_fp_atan2(-latdiff, londiff);
if (*bearing >FIXEDPOINT180) *bearing -= FIXEDPOINT360;
// distance is 111319 meters per degree. This factor needs to be shifted 16 to make it a fixedpointnum.
// Since lat and lon are already shifted by 6,
// we will shift by 10 more total. Shift lat and long by 8 and the constant by 2 to get the extra 10.
// The squaring will overflow our fixedpointnum at distances greater than 1000 meters or so, so test for size and shift accordingly
if (lib_fp_abs(latdiff)+lib_fp_abs(londiff)>40000L)
{ // for big distances, don't shift lat and long. Instead shift the constant by 10.
// this will get us to 32 kilometers at which point our fixedpoingnum can't hold a larger distance.
return(lib_fp_multiply(lib_fp_sqrt(lib_fp_multiply(latdiff,latdiff)+lib_fp_multiply(londiff,londiff)),113990656L));
}
else
{
latdiff=latdiff<<8;
londiff=londiff<<8;
return(lib_fp_multiply(lib_fp_sqrt(lib_fp_multiply(latdiff,latdiff)+lib_fp_multiply(londiff,londiff)),445276L));
}
}
void navigation_setangleerror(unsigned char gotnewgpsreading,fixedpointnum *angleerror)
{ // calculate the angle errors between our current attitude and the one we wish to have
// and adjust the angle errors that were passed to us. They have already been set by pilot input.
// For now, we just override any pilot input.
// keep track of the time between good gps readings.
navigation_time_sliver+=global.timesliver;
if (gotnewgpsreading)
{
// unshift our timesliver since we are about to use it. Since we are accumulating time, it may get too large to use while shifted.
navigation_time_sliver=navigation_time_sliver>>TIMESLIVEREXTRASHIFT;
// get the new distance and bearing from our current location to our target position
global.navigation_distance=navigation_getdistanceandbearing(global.gps_current_latitude,global.gps_current_longitude,target_latitude,target_longitude,&global.navigation_bearing);
// split the distance into it's ontrack and crosstrack components
// see the diagram above
fixedpointnum angledifference=global.navigation_bearing-navigation_starttodestbearing;
fixedpointnum crosstrack_distance=lib_fp_multiply(global.navigation_distance,lib_fp_sine(angledifference));
fixedpointnum ontrack_distance=lib_fp_multiply(global.navigation_distance,lib_fp_cosine(angledifference));
// accumulate integrated error for both ontrack and crosstrack
navigation_crosstrack_integrated_error+=lib_fp_multiply(crosstrack_distance,navigation_time_sliver);
navigation_ontrack_integrated_error+=lib_fp_multiply(ontrack_distance,navigation_time_sliver);
lib_fp_constrain(&navigation_crosstrack_integrated_error,-NAVIGATIONINTEGRATEDERRORLIMIT,NAVIGATIONINTEGRATEDERRORLIMIT);
lib_fp_constrain(&navigation_ontrack_integrated_error,-NAVIGATIONINTEGRATEDERRORLIMIT,NAVIGATIONINTEGRATEDERRORLIMIT);
// calculate the ontrack and crosstrack velocities toward our target.
// We want to put the navigation velocity (change in distance to target over time) into a low pass filter but
// we don't want to divide by the time interval to get velocity (divide is expensive) to then turn around and
// multiply by the same time interval. So the following is the same as the lib_fp_lowpassfilter code
// except we eliminate the multiply.
// note: if we use a different time period than FIXEDPOINTONEOVERONE, we need to multiply the distances by the new time period.
fixedpointnum fraction=lib_fp_multiply(navigation_time_sliver,FIXEDPOINTONEOVERONE);
navigation_crosstrack_velocity=(navigation_last_crosstrack_distance-crosstrack_distance+lib_fp_multiply((FIXEDPOINTONE)-fraction,navigation_crosstrack_velocity));
navigation_ontrack_velocity=(navigation_last_ontrack_distance-ontrack_distance+lib_fp_multiply((FIXEDPOINTONE)-fraction,navigation_ontrack_velocity));
navigation_last_crosstrack_distance=crosstrack_distance;
navigation_last_ontrack_distance=ontrack_distance;
// calculate the desired tilt in each direction independently using navigation PID
fixedpointnum crosstracktiltangle=lib_fp_multiply(usersettings.pid_pgain[NAVIGATIONINDEX],crosstrack_distance)
+lib_fp_multiply(usersettings.pid_igain[NAVIGATIONINDEX],navigation_crosstrack_integrated_error)
-lib_fp_multiply(usersettings.pid_dgain[NAVIGATIONINDEX],navigation_crosstrack_velocity);
fixedpointnum ontracktiltangle =lib_fp_multiply(usersettings.pid_pgain[NAVIGATIONINDEX],ontrack_distance)
+lib_fp_multiply(usersettings.pid_igain[NAVIGATIONINDEX],navigation_ontrack_integrated_error)
-lib_fp_multiply(usersettings.pid_dgain[NAVIGATIONINDEX],navigation_ontrack_velocity);
// don't tilt more than MAX_TILT
lib_fp_constrain(&crosstracktiltangle,-MAX_TILT,MAX_TILT);
lib_fp_constrain(&ontracktiltangle,-MAX_TILT,MAX_TILT);
// Translate the ontrack and cross track tilts into pitch and roll tilts.
// Set angledifference equal to the difference between the aircraft's heading (the way it's currently pointing)
// and the angle between waypoints and rotate our tilts by that much.
angledifference=global.currentestimatedeulerattitude[YAWINDEX]-navigation_starttodestbearing;
fixedpointnum sineofangle=lib_fp_sine(angledifference);
fixedpointnum cosineofangle=lib_fp_cosine(angledifference);
navigation_desiredeulerattitude[ROLLINDEX]=lib_fp_multiply(crosstracktiltangle,cosineofangle)-lib_fp_multiply(ontracktiltangle,sineofangle);
navigation_desiredeulerattitude[PITCHINDEX]=lib_fp_multiply(crosstracktiltangle,sineofangle)+lib_fp_multiply(ontracktiltangle,cosineofangle);
// for now, don't rotate the aircraft in the direction of travel. Add this later.
navigation_time_sliver=0;
}
// set the angle error as the difference between where we want to be and where we currently are angle wise.
angleerror[ROLLINDEX]=navigation_desiredeulerattitude[ROLLINDEX]-global.currentestimatedeulerattitude[ROLLINDEX];
angleerror[PITCHINDEX]=navigation_desiredeulerattitude[PITCHINDEX]-global.currentestimatedeulerattitude[PITCHINDEX];
// don't set the yaw. Let the pilot do yaw
// angleerror[YAWINDEX]=navigation_desiredeulerattitude[YAWINDEX]-global.currentestimatedeulerattitude[YAWINDEX];
// don't let the yaw angle error get too large for any one cycle in order to control the maximum yaw rate.
// lib_fp_constrain180(&angleerror[YAWINDEX]);
// lib_fp_constrain(&angleerror[YAWINDEX],-MAXYAWANGLEERROR,MAXYAWANGLEERROR);
}
void getangleerrorfrompilotinput(fixedpointnum * angleerror)
{
// sets the ange errors for roll, pitch, and yaw based on where the pilot has the tx sticks.
fixedpointnum rxrollvalue;
fixedpointnum rxpitchvalue;
// if in headfree mode, rotate the pilot's stick inputs by the angle that is the difference between where we are currently heading and where we were heading when we armed.
if (global.activecheckboxitems & CHECKBOXMASKHEADFREE) {
fixedpointnum angledifference = global.currentestimatedeulerattitude[YAWINDEX] - global.heading_when_armed;
fixedpointnum cosangledifference = lib_fp_cosine(angledifference);
fixedpointnum sinangledifference = lib_fp_sine(angledifference);
rxpitchvalue = lib_fp_multiply(global.rxvalues[PITCHINDEX], cosangledifference) + lib_fp_multiply(global.rxvalues[ROLLINDEX], sinangledifference);
rxrollvalue = lib_fp_multiply(global.rxvalues[ROLLINDEX], cosangledifference) - lib_fp_multiply(global.rxvalues[PITCHINDEX], sinangledifference);
#ifdef INVERTED
rxrollvalue = - rxrollvalue;
#endif
} else {
rxpitchvalue = global.rxvalues[PITCHINDEX];
rxrollvalue = global.rxvalues[ROLLINDEX];
#ifdef INVERTED
rxrollvalue = - rxrollvalue;
#endif
}
// first, calculate level mode values
// how far is our estimated current attitude from our desired attitude?
// desired angle is rxvalue (-1 to 1) times LEVEL_MODE_MAX_TILT
// First, figure out which max angle we are using depending on aux switch settings.
fixedpointnum levelmodemaxangle;
if (global.activecheckboxitems & CHECKBOXMASKHIGHANGLE)
levelmodemaxangle = FP_LEVEL_MODE_MAX_TILT_HIGH_ANGLE;
else
levelmodemaxangle = FP_LEVEL_MODE_MAX_TILT;
// the angle error is how much our current angles differ from our desired angles.
fixedpointnum levelmoderollangleerror = lib_fp_multiply(rxrollvalue, levelmodemaxangle) - global.currentestimatedeulerattitude[ROLLINDEX];
fixedpointnum levelmodepitchangleerror = lib_fp_multiply(rxpitchvalue, levelmodemaxangle) - global.currentestimatedeulerattitude[PITCHINDEX];
// In acro mode, we want the rotation rate to be proportional to the pilot's stick movement. The desired rotation rate is
// the stick movement * a multiplier.
// Fill angleerror with acro values. If we are currently rotating at rate X and
// we want to be rotating at rate Y, then our angle should be off by (Y-X)*timesliver. In theory, we should probably accumulate
// this error over time, but our I term will do that anyway and we are talking about rates, which don't need to be perfect.
// First figure out what max rotation rates we are using depending on our aux switch settings.
fixedpointnum maxyawrate;
fixedpointnum maxpitchandrollrate;
if (global.activecheckboxitems & CHECKBOXMASKHIGHRATES) {
maxyawrate = highyawrate;
maxpitchandrollrate = highpitchandrollrate;
} else {
maxyawrate = usersettings.maxyawrate;
maxpitchandrollrate = usersettings.maxpitchandrollrate;
}
angleerror[ROLLINDEX] = lib_fp_multiply(lib_fp_multiply(rxrollvalue, maxpitchandrollrate) - global.gyrorate[ROLLINDEX], global.timesliver);
angleerror[PITCHINDEX] = lib_fp_multiply(lib_fp_multiply(rxpitchvalue, maxpitchandrollrate) - global.gyrorate[PITCHINDEX], global.timesliver);
// put a low pass filter on the yaw gyro. If we don't do this, things can get jittery.
lib_fp_lowpassfilter(&filteredyawgyrorate, global.gyrorate[YAWINDEX], global.timesliver >> (TIMESLIVEREXTRASHIFT - 3), FIXEDPOINTONEOVERONESIXTYITH, 3);
if(global.activecheckboxitems & CHECKBOXMASKYAWHOLD) {
// Yaw hold: control yaw angle instead of yaw rate by accumulating the yaw errors.
// This is similar to compass mode, but no hardware compass needed.
if(!(global.previousactivecheckboxitems & CHECKBOXMASKYAWHOLD)) {
// This mode was just switched on. Use current position as reference by setting error to zero.
accumulatedyawerror = 0;
}
// Accumulate yaw angle error
accumulatedyawerror += lib_fp_multiply(lib_fp_multiply(global.rxvalues[YAWINDEX], maxyawrate) - filteredyawgyrorate, global.timesliver) >> TIMESLIVEREXTRASHIFT;
// Make sure it does not get too high
lib_fp_constrain180(&accumulatedyawerror);
angleerror[YAWINDEX] = accumulatedyawerror;
} else {
char readgps()
{
while (lib_serial_numcharsavailable(GPS_SERIAL_PORT))
{
char c=lib_serial_getchar(GPS_SERIAL_PORT);
if (c=='$') // start of a new message
{
gpsparameternumber=0;
gpsdataindex=0;
gpschecksum=0;
gpsframetype=0;
}
else
{
if (c==',' || c=='*')
{ // we just finished loading a parameter, interpret it
gpsdata[gpsdataindex]='\0';
if (gpsparameternumber == 0)
{ //frame identification
if (gpsdata[0] == 'G' && gpsdata[1] == 'P' && gpsdata[2] == 'G' && gpsdata[3] == 'G' && gpsdata[4] == 'A') gpsframetype=FRAME_GGA;
else if (gpsdata[0] == 'G' && gpsdata[1] == 'P' && gpsdata[2] == 'R' && gpsdata[3] == 'M' && gpsdata[4] == 'C') gpsframetype = FRAME_RMC;
}
else if (gpsframetype == FRAME_GGA)
{
if (gpsparameternumber == 2)
{
gpslatitudedegrees = gpsstringtoangle(gpsdata);
}
else if (gpsparameternumber == 3)
{
if ( gpsdata[0] == 'S') gpslatitudedegrees=-gpslatitudedegrees;
}
else if (gpsparameternumber == 4)
{
gpslongitudedegrees = gpsstringtoangle(gpsdata);
}
else if (gpsparameternumber == 5)
{
if ( gpsdata[0] == 'W') gpslongitudedegrees=-gpslongitudedegrees;
}
else if (gpsparameternumber == 6)
{
gpsfix = gpsdata[0]-'0';
}
else if (gpsparameternumber == 7)
{
global.gps_num_satelites = lib_fp_stringtolong(gpsdata);
}
else if (gpsparameternumber == 9)
{
global.gps_current_altitude = lib_fp_stringtofixedpointnum(gpsdata);
}
}
else if (gpsframetype == FRAME_RMC)
{
if (gpsparameternumber == 7)
{
// 1 knot = 0.514444444 m / s
global.gps_current_speed=lib_fp_multiply(lib_fp_stringtofixedpointnum(gpsdata),33715L); // 33715L is .514444444 * 2^16
}
else if (gpsparameternumber == 8)
{
// GPS_ground_course = grab_fields(gpsdata,1);
} //ground course deg*10
}
++gpsparameternumber;
gpsdataindex = 0; // get read for the next parameter
if (c == '*') gpsfinalchecksum=gpschecksum;
}
else if (c == '\r' || c == '\n')
{ // end of the message
unsigned char checksum = hextochar(gpsdata[0]);
checksum <<= 4;
checksum += hextochar(gpsdata[1]);
if (checksum != gpsfinalchecksum || gpsframetype!=FRAME_GGA || !gpsfix) return(0);
gpsframetype=0; // so we don't check again
global.gps_current_latitude=gpslatitudedegrees;
global.gps_current_longitude=gpslongitudedegrees;
return(1); // we got a good frame
}
else if (gpsdataindex<GPSDATASIZE)
{
gpsdata[gpsdataindex++]=c;
}
gpschecksum^=c;
}
}
return(0); // no complete data yet
}
char readgps()
{ // returns 1 if new data was read
if (lib_timers_gettimermicroseconds(gpstimer)<50000) return(0); // 20 hz
gpstimer=lib_timers_starttimer();
unsigned char shiftedaddress=I2C_GPS_ADDRESS<<1;
lib_i2c_start_wait(shiftedaddress+I2C_WRITE);
if (lib_i2c_write(I2C_GPS_STATUS_00))
return(0);
lib_i2c_rep_start(shiftedaddress+I2C_READ);
unsigned char gps_status= lib_i2c_readnak();
global.gps_num_satelites = (gps_status & 0xf0) >> 4;
if (gps_status & I2C_GPS_STATUS_3DFIX)
{ //Check is we have a good 3d fix (numsats>5)
long longvalue;
unsigned char *ptr=(unsigned char *)&longvalue;
lib_i2c_rep_start(shiftedaddress+I2C_WRITE);
lib_i2c_write(I2C_GPS_LOCATION);
lib_i2c_rep_start(shiftedaddress+I2C_READ);
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr = lib_i2c_readack();
global.gps_current_latitude=lib_fp_multiply(longvalue,27488L); // convert from 10,000,000 m to fixedpointnum
ptr=(unsigned char *)&longvalue;
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
*ptr = lib_i2c_readnak();
global.gps_current_longitude=lib_fp_multiply(longvalue,27488L); // convert from 10,000,000 m to fixedpointnum
int intvalue;
ptr= (unsigned char *)&intvalue;
lib_i2c_rep_start(shiftedaddress+I2C_WRITE);
lib_i2c_write(I2C_GPS_GROUND_SPEED);
lib_i2c_rep_start(shiftedaddress+I2C_READ);
*ptr++ = lib_i2c_readack();
*ptr++ = lib_i2c_readack();
global.gps_current_speed=intvalue*665L; // convert fro cm/s to fixedpointnum to m/s
ptr= (unsigned char *)&intvalue;
*ptr++ = lib_i2c_readack();
*ptr = lib_i2c_readnak();
global.gps_current_altitude=((fixedpointnum)intvalue)<<FIXEDPOINTSHIFT;
lib_i2c_stop();
return(1);
}
lib_i2c_stop();
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();
}
void vector_cross_product(fixedpointnum *v1, fixedpointnum *v2, fixedpointnum *v3) {
v3[X_INDEX]=lib_fp_multiply(v1[Y_INDEX],v2[Z_INDEX])-lib_fp_multiply(v1[Z_INDEX],v2[Y_INDEX]);
v3[Y_INDEX]=lib_fp_multiply(v1[Z_INDEX],v2[X_INDEX])-lib_fp_multiply(v1[X_INDEX],v2[Z_INDEX]);
v3[Z_INDEX]=lib_fp_multiply(v1[X_INDEX],v2[Y_INDEX])-lib_fp_multiply(v1[Y_INDEX],v2[X_INDEX]);
}
fixedpointnum vector_dot_product(fixedpointnum *v1, fixedpointnum *v2) {
return(lib_fp_multiply(v1[0], v2[0])+lib_fp_multiply(v1[1], v2[1])+lib_fp_multiply(v1[2], v2[2]));
}
// This mode was just switched on. Use current position as reference by setting error to zero.
accumulatedyawerror = 0;
}
// Accumulate yaw angle error
accumulatedyawerror += lib_fp_multiply(lib_fp_multiply(global.rxvalues[YAWINDEX], maxyawrate) - filteredyawgyrorate, global.timesliver) >> TIMESLIVEREXTRASHIFT;
// Make sure it does not get too high
lib_fp_constrain180(&accumulatedyawerror);
angleerror[YAWINDEX] = accumulatedyawerror;
} else {
// Normal mode: control yaw rate
// Calculate yaw angle error since last update based on desired and actual yaw rate.
// TIMESLIVEREXTRASHIFT is not used here, so this value is 256 times higher than expected.
// This is OK because timesliver is very small, making the angle error during this time
// also very small. Using the amplified result the same yaw PID control parameters can be used
// for all modes (normal, compass and yaw hold).
angleerror[YAWINDEX] = lib_fp_multiply(lib_fp_multiply(global.rxvalues[YAWINDEX], maxyawrate) - filteredyawgyrorate, global.timesliver);
}
// handle compass control
if (global.activecheckboxitems & CHECKBOXMASKCOMPASS) {
if (!(global.previousactivecheckboxitems & CHECKBOXMASKCOMPASS)) { // we just switched into compass mode
// reset the angle error to zero so that we don't yaw because of the switch
desiredcompassheading = global.currentestimatedeulerattitude[YAWINDEX];
}
// the compass holds only when right side up with throttle up (not sitting on the ground)
// and the yaw stick is centered. If it is centered, override the pilot's input
if (global.estimateddownvector[ZINDEX] > 0 && global.rxvalues[THROTTLEINDEX] > FPSTICKLOW && global.rxvalues[YAWINDEX] > -YAWCOMPASSRXDEADBAND && global.rxvalues[YAWINDEX] < YAWCOMPASSRXDEADBAND) {
angleerror[YAWINDEX] = desiredcompassheading - global.currentestimatedeulerattitude[YAWINDEX];
lib_fp_constrain180(&angleerror[YAWINDEX]);
} else // the pilot is controlling yaw, so update the desired heading
desiredcompassheading = global.currentestimatedeulerattitude[YAWINDEX];
