From 9c35ef93b3978455fa8f10300dbfd70a17e55e08 Mon Sep 17 00:00:00 2001 From: Black-Thunder Date: Sat, 7 Jun 2014 22:45:58 +0200 Subject: [PATCH] - fixed throttle bug when ALTPANIC is activated: throttle was fixed to last value and didn't respond to user command unless the corresponding switch for altitude/velocity hold was set to off position; now throttle directly reacts again to user command when ALTPANIC is activated - corrected spelling - corrected initial values of altitudeHoldBump and altitudeHoldPanicStickMovement - fixed some compiler warnings with SBUS receivers --- AeroQuad/AeroQuad.h | 6 +++--- AeroQuad/AltitudeControlProcessor.h | 26 ++++++++++++-------------- AeroQuad/FlightCommandProcessor.h | 10 +++++----- AeroQuad/UserConfiguration.h | 4 ++-- Libraries/AQ_Receiver/Receiver_AQ32.h | 8 ++++---- 5 files changed, 26 insertions(+), 28 deletions(-) diff --git a/AeroQuad/AeroQuad.h b/AeroQuad/AeroQuad.h index b768f12b..cca07dfd 100644 --- a/AeroQuad/AeroQuad.h +++ b/AeroQuad/AeroQuad.h @@ -170,11 +170,11 @@ void reportVehicleState(); #define VELOCITY_HOLD_STATE 2 #define ALTPANIC 3 byte altitudeHoldState = OFF; // ON, OFF or ALTPANIC - int altitudeHoldBump = 90; - int altitudeHoldPanicStickMovement = 250; + int altitudeHoldBump = 30; + int altitudeHoldPanicStickMovement = 300; int altitudeHoldThrottle = 1000; boolean isAltitudeHoldInitialized = false; - boolean isVelocityHoldInitialisez = false; + boolean isVelocityHoldInitialized = false; float zVelocity = 0.0; float estimatedAltitude = 0.0; diff --git a/AeroQuad/AltitudeControlProcessor.h b/AeroQuad/AltitudeControlProcessor.h index 2a89af58..8d4559cb 100644 --- a/AeroQuad/AltitudeControlProcessor.h +++ b/AeroQuad/AltitudeControlProcessor.h @@ -69,24 +69,22 @@ void processVelocityHold() void processAltitudeControl() { - if (altitudeHoldState == ALTPANIC) { - return; - } - - if (altitudeHoldState == ALTITUDE_HOLD_STATE || altitudeHoldState == ALTITUDE_HOLD_STATE) { + if (altitudeHoldState == ALTITUDE_HOLD_STATE || altitudeHoldState == VELOCITY_HOLD_STATE) { if (abs(altitudeHoldThrottle - receiverCommand[receiverChannelMap[THROTTLE]]) > altitudeHoldPanicStickMovement) { altitudeHoldState = ALTPANIC; // too rapid of stick movement so PANIC out of ALTHOLD - return; + } + + if (altitudeHoldState == ALTITUDE_HOLD_STATE) { + processAltitudeHold(); + } + else if (altitudeHoldState == VELOCITY_HOLD_STATE){ + processVelocityHold(); + } + else { // ALTPANIC + throttle = receiverCommand[receiverChannelMap[THROTTLE]]; } } - - if (altitudeHoldState == ALTITUDE_HOLD_STATE) { - processAltitudeHold(); - } - else if (altitudeHoldState == VELOCITY_HOLD_STATE){ - processVelocityHold(); - } - else { + else { // OFF or ALTPANIC throttle = receiverCommand[receiverChannelMap[THROTTLE]]; } } diff --git a/AeroQuad/FlightCommandProcessor.h b/AeroQuad/FlightCommandProcessor.h index b43e0596..20435c6e 100644 --- a/AeroQuad/FlightCommandProcessor.h +++ b/AeroQuad/FlightCommandProcessor.h @@ -63,12 +63,12 @@ if (isVelocityHoldStateEnabledByUser()) { if (altitudeHoldState != ALTPANIC ) { // check for special condition with manditory override of Altitude hold - if (!isVelocityHoldInitialisez) { + if (!isVelocityHoldInitialized) { baroAltitudeToHoldTarget = estimatedAltitude;//getBaroAltitude(); PID[BARO_ALTITUDE_HOLD_PID_IDX].lastError = baroAltitudeToHoldTarget; altitudeHoldThrottle = receiverCommand[receiverChannelMap[THROTTLE]]; isAltitudeHoldInitialized = false; - isVelocityHoldInitialisez = true; + isVelocityHoldInitialized = true; } altitudeHoldState = VELOCITY_HOLD_STATE; } @@ -80,14 +80,14 @@ PID[BARO_ALTITUDE_HOLD_PID_IDX].lastError = baroAltitudeToHoldTarget; altitudeHoldThrottle = receiverCommand[receiverChannelMap[THROTTLE]]; isAltitudeHoldInitialized = true; - isVelocityHoldInitialisez = false; + isVelocityHoldInitialized = false; } altitudeHoldState = ALTITUDE_HOLD_STATE; } } else { isAltitudeHoldInitialized = false; - isVelocityHoldInitialisez = false; + isVelocityHoldInitialized = false; altitudeHoldState = OFF; } } @@ -96,7 +96,7 @@ #if defined (AutoLanding) void processAutoLandingStateFromReceiverCommand() { - if (receiverCommand[AUX3] < 1750) { + if (receiverCommand[receiverChannelMap[AUX3]] < 1750) { if (altitudeHoldState != ALTPANIC ) { // check for special condition with manditory override of Altitude hold if (isAutoLandingInitialized) { autoLandingState = BARO_AUTO_DESCENT_STATE; diff --git a/AeroQuad/UserConfiguration.h b/AeroQuad/UserConfiguration.h index 9c1fd517..bba6282a 100644 --- a/AeroQuad/UserConfiguration.h +++ b/AeroQuad/UserConfiguration.h @@ -40,8 +40,8 @@ //#define MWCProEz30 // MWC Prop Ez3.0 from ready to fly quad -> http://witespyquad.gostorego.com/flight-controllers/multiwii-pro-ez3-0-flight-controller-w-gps-option.html // STM32 processor -//#define AeroQuadSTM32 -#define Naze32 +#define AeroQuadSTM32 +//#define Naze32 /**************************************************************************** diff --git a/Libraries/AQ_Receiver/Receiver_AQ32.h b/Libraries/AQ_Receiver/Receiver_AQ32.h index 08f750ff..f374da9e 100644 --- a/Libraries/AQ_Receiver/Receiver_AQ32.h +++ b/Libraries/AQ_Receiver/Receiver_AQ32.h @@ -383,7 +383,7 @@ void readSBUS() if (val != SBUS_ENDBYTE) { //out of sync incorrect end byte int shiftIndex = 0; - for(int i=1;i<=sbusIndex;i++){ // start at array pos 2 because we already know the byte at pos 1 is a syncByte + for(unsigned int i=1;i<=sbusIndex;i++){ // start at array pos 2 because we already know the byte at pos 1 is a syncByte if(sbus[i] == SBUS_SYNCBYTE){ shiftIndex = i; break; //we have the location of the next SYNCBYTE @@ -393,7 +393,7 @@ void readSBUS() if(shiftIndex != 0) { //the start of a packet was found in the middle of the bad packet //shift everything by the value of -shiftIndex - for(int i=0;i<=sbusPacketLength-shiftIndex;i++){ + for(unsigned int i=0;i<=sbusPacketLength-shiftIndex;i++){ sbus[i] = sbus[i+shiftIndex]; } @@ -417,8 +417,8 @@ void readSBUS() rawChannelValue[AUX1] = ((sbus[7]>>7 | sbus[8]<<1 | sbus[9]<<9) & 0x07FF); rawChannelValue[AUX2] = ((sbus[9]>>2 | sbus[10]<<6) & 0x07FF); rawChannelValue[AUX3] = ((sbus[10]>>5 | sbus[11]<<3) & 0x07FF); - rawChannelValue[AUX4] = ((sbus[12] | sbus[13]<<8) & 0x07FF); - rawChannelValue[AUX5] = ((sbus[13]>>3 | sbus[14]<<5) & 0x07FF); + //rawChannelValue[AUX4] = ((sbus[12] | sbus[13]<<8) & 0x07FF); + //rawChannelValue[AUX5] = ((sbus[13]>>3 | sbus[14]<<5) & 0x07FF); //rawChannelValue[AUX6] = ((sbus[14]>>6 | sbus[15]<<2|sbus[16]<<10) & 0x07FF); //rawChannelValue[AUX7] = ((sbus[16]>>1 | sbus[17]<<7) & 0x07FF);