Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Some bug fixes #2

Open
wants to merge 1 commit into
base: KennyV3.3Devlopment
Choose a base branch
from
Open

Some bug fixes #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions AeroQuad/AeroQuad.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
26 changes: 12 additions & 14 deletions AeroQuad/AltitudeControlProcessor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]];
}
}
Expand Down
10 changes: 5 additions & 5 deletions AeroQuad/FlightCommandProcessor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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;
}
}
Expand All @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions AeroQuad/UserConfiguration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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


/****************************************************************************
Expand Down
8 changes: 4 additions & 4 deletions Libraries/AQ_Receiver/Receiver_AQ32.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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];
}

Expand All @@ -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);

Expand Down