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 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 {
