Permalink
Browse files

Adding ability to fly with roll/pitch controlling east/north

This changeset overrides the attitude/rate mode controller to use as change between 'QuadRelative' and 'TrueNorth' flight modes. In 'QuadRelative' mode pitching makes the quad go forward. In 'TrueNorth' mode, pitching makes the quad go Northward, regardless of the yaw rotation of the quad. This is achieved by rotating the receiver commands before anything else uses them. The rotation prevents the calibration command from occuring in 'TrueNorth' flight mode.
  • Loading branch information...
1 parent c807112 commit 53192ecdf6ee66ce7ad0d1c7d9726fd2bb85ba92 @eelsirhc eelsirhc committed Aug 7, 2012
Showing with 56 additions and 14 deletions.
  1. +7 −1 AeroQuad/AeroQuad.h
  2. +1 −1 AeroQuad/AeroQuad.ino
  3. +18 −11 AeroQuad/FlightCommandProcessor.h
  4. +30 −1 Libraries/AQ_Receiver/Receiver.h
View
@@ -97,13 +97,19 @@ int testCommand = 1000;
#define RATE_FLIGHT_MODE 0
#define ATTITUDE_FLIGHT_MODE 1
+#define TRUE_NORTH_MODE 2
+#define QUAD_RELATIVE_MODE 3
+
+
#define TASK_100HZ 1
#define TASK_50HZ 2
#define TASK_10HZ 10
#define TASK_1HZ 100
#define THROTTLE_ADJUST_TASK_SPEED TASK_50HZ
-byte flightMode = RATE_FLIGHT_MODE;
+byte flightMode = ATTITUDE_FLIGHT_MODE ; // RATE_FLIGHT_MODE;
+byte quadRelativeMode = QUAD_RELATIVE_MODE;
+
unsigned long frameCounter = 0; // main loop executive frame counter
int minArmedThrottle; // initial value configured by user
unsigned long tenHzTimes[10];
View
@@ -1467,7 +1467,7 @@ void loop () {
fiftyHZpreviousTime = currentTime;
// Reads external pilot commands and performs functions based on stick configuration
- readPilotCommands();
+ readPilotCommands(trueNorthHeading);
#if defined (UseRSSIFaileSafe)
readRSSI();
@@ -31,9 +31,17 @@
* This function is responsible to read receiver
* and process command from the users
*/
-void readPilotCommands() {
+void readPilotCommands(float trueNorthHeading) {
readReceiver();
+ //Check Mode switch for TrueNorth or QuadRelative
+ if (receiverCommand[MODE] > 1500) {
+ quadRelativeMode = QUAD_RELATIVE_MODE;
+ rotateRollPitchToEastNorth(trueNorthHeading);
+ }
+ else {
+ quadRelativeMode = TRUE_NORTH_MODE;
+ }
if (receiverCommand[THROTTLE] < MINCHECK) {
zeroIntegralError();
// Disarm motors (left stick lower left corner)
@@ -88,16 +96,14 @@ void readPilotCommands() {
safetyCheck = ON;
}
}
-
+ //OVERRIDE RATE MODE
// Check Mode switch for Acro or Stable
- if (receiverCommand[MODE] > 1500) {
- flightMode = ATTITUDE_FLIGHT_MODE;
- }
- else {
- flightMode = RATE_FLIGHT_MODE;
- }
-
-
+ //if (receiverCommand[MODE] > 1500) {
+ // flightMode = ATTITUDE_FLIGHT_MODE;
+ //}
+ //else {
+ // flightMode = RATE_FLIGHT_MODE;
+ //}
#if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder
#if defined (UseGPSNavigator)
if ((receiverCommand[AUX1] < 1750) || (receiverCommand[AUX2] < 1750)) {
@@ -230,4 +236,5 @@ void readPilotCommands() {
#endif
}
-#endif // _AQ_FLIGHT_COMMAND_READER_
+#endif // _AQ_FLIGHT_COMMAND_READER_
+
@@ -82,7 +82,8 @@ void initializeReceiverParam(int nbChannel = 6) {
int getRawChannelValue(byte channel);
void readReceiver();
-
+void rotateRollPitchToEastNorth(float trueNorthHeading);
+
void readReceiver()
{
for(byte channel = XAXIS; channel < lastReceiverChannel; channel++) {
@@ -101,6 +102,34 @@ void readReceiver()
for (byte channel = THROTTLE; channel < lastReceiverChannel; channel++) {
receiverCommand[channel] = receiverCommandSmooth[channel];
}
+
+}
+
+void rotateRollPitchToEastNorth(float trueNorthHeading)
+{
+//Take the receiver command for roll and pitch to be directions along the west-east and south-north axes, respectively.
+//To do this, rotate the roll and pitch receiver commands using the stored compass heading.
+//Assuming the magnetometer lines up with the Quad, then Quad-Pitch is aligned with North=0, so to make Quad-Pitch be North-Pitch
+
+//quad pitch
+//If we face north and pitch up, go North (cos(angle))
+//if we face north and roll up, go East (sin(angle))
+
+//quad roll
+//if we face east and pitch up, go North (-sin(angle))
+//if we face east and roll up, go East (cos(angle))
+
+//if (quadRelativeMode == TRUE_NORTH_MODE) {
+ float rollCommand = receiverCommand[XAXIS] - receiverZero[XAXIS];
+ float pitchCommand = receiverCommand[YAXIS] - receiverZero[YAXIS];
+
+ float sinAngle = sin(trueNorthHeading);
+ float cosAngle = cos(trueNorthHeading);
+
+ receiverCommand[XAXIS] = receiverZero[XAXIS] + (-pitchCommand * sinAngle) + rollCommand * cosAngle;
+ receiverCommand[YAXIS] = receiverZero[YAXIS] + pitchCommand * cosAngle + rollCommand * sinAngle;
+//}
+
}

0 comments on commit 53192ec

Please sign in to comment.