MIXING in Flying Mode (Mix table)
MIXING in Flying Mode (Mix table)
Hello,
I'm facing a difficulty here. I can't find a way to mix Roll and Pitch for a Flying Wing.
Trying the following table long and large in Output.pde I had to add an external Vtail mixer in order to get a correct mix. Without it I had to reverse the AIL channel on the radio, and I want to let it in normal as it also used to command the motors. Here is the table I used, If anyone has an idea of a missing line on that table... Tku;)
ROLL+PITCH-mid - Mix Roll in Pitch, Roll normal
mid-ROLL+PITCH - Mix Roll in Pitch, inverse Roll
mid-PITCH+ROLL - Mix Pitch in Roll, inverse Pitch
PITCH-ROLL+mid - Mix Pitch in inversed Roll, Pitch normal
ROLL-PITCH+mid - Mix Pitch in Roll, inverse Pitch
mid+ROLL-PITCH - Mix Pitch in Roll, Pitch normal
I'm facing a difficulty here. I can't find a way to mix Roll and Pitch for a Flying Wing.
Trying the following table long and large in Output.pde I had to add an external Vtail mixer in order to get a correct mix. Without it I had to reverse the AIL channel on the radio, and I want to let it in normal as it also used to command the motors. Here is the table I used, If anyone has an idea of a missing line on that table... Tku;)
ROLL+PITCH-mid - Mix Roll in Pitch, Roll normal
mid-ROLL+PITCH - Mix Roll in Pitch, inverse Roll
mid-PITCH+ROLL - Mix Pitch in Roll, inverse Pitch
PITCH-ROLL+mid - Mix Pitch in inversed Roll, Pitch normal
ROLL-PITCH+mid - Mix Pitch in Roll, inverse Pitch
mid+ROLL-PITCH - Mix Pitch in Roll, Pitch normal
Re: MIXING in Flying Mode (Mix table)
Hi Cob,
mid is always the same value. :=15000 (set in config to center servos)
To find out the direction of channels.
Start with
mid+PITCH check if direction is right/wrong
mid+ROLL check if direction is right/wrong
Then do the full mix when you know what effects what.
/Patrik
mid is always the same value. :=15000 (set in config to center servos)
To find out the direction of channels.
Start with
mid+PITCH check if direction is right/wrong
mid+ROLL check if direction is right/wrong
Then do the full mix when you know what effects what.
/Patrik
Re: MIXING in Flying Mode (Mix table)
value should be 1500
1500 is center in PWM world of 1000-2000
1500 is center in PWM world of 1000-2000
Re: MIXING in Flying Mode (Mix table)
Sorry slight difference on 15000 and 1500....;O)
Re: MIXING in Flying Mode (Mix table)
Thanks, I know mid value is 1500, the point here is I'm probably missing one element in the mixing table, ie one formula as I can't get the correct mixing whith any of the element in the mix table above. I always come to Pitch going in the correct direction but Roll is inverted whatever the calculation.
I have Roll+Pitch-1500 on servo[1] and 1500-Pitch+Roll on servo [2]. This was working on the right direction for Pitch axis but with reverse on Roll output. I mean the plane will go to left when I ask to go to the right on the radio. I never have been able to reverse Roll while keeping Pitch output as it is. I tried any and all possibility of the table above... I'm probably missing one formula or two ? Or perhaps Serial sum PPM order of channel should be changed for channel Roll and Pitch ? Roll and Pitch channel n°1 and n°2 coming in the MWii should be the reversed : Pitch=1 and Roll=2 ?? The channel line in config.h is currently set as:
#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL
with is correct for the radio I have.
Anyhow, it works with an external Vtail mixer. But it seems dumb thing considering having an AVR which can be set to do it.
I have Roll+Pitch-1500 on servo[1] and 1500-Pitch+Roll on servo [2]. This was working on the right direction for Pitch axis but with reverse on Roll output. I mean the plane will go to left when I ask to go to the right on the radio. I never have been able to reverse Roll while keeping Pitch output as it is. I tried any and all possibility of the table above... I'm probably missing one formula or two ? Or perhaps Serial sum PPM order of channel should be changed for channel Roll and Pitch ? Roll and Pitch channel n°1 and n°2 coming in the MWii should be the reversed : Pitch=1 and Roll=2 ?? The channel line in config.h is currently set as:
#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL
with is correct for the radio I have.
Anyhow, it works with an external Vtail mixer. But it seems dumb thing considering having an AVR which can be set to do it.
Re: MIXING in Flying Mode (Mix table)
A few question to make it clearer...
Is Controler compensating correct with gyros?
Is RadioInputs in right directions?
Also check the manual to your radio to se the channelorder.
God to know before thinking..
/Patrik
Is Controler compensating correct with gyros?
Is RadioInputs in right directions?
Also check the manual to your radio to se the channelorder.
God to know before thinking..
/Patrik
Re: MIXING in Flying Mode (Mix table)
A mod on last dev 714.
Added possibility to invert throw in soft.
Add in config.h
Channelorder is same as in #define SERIAL_SUM_PPM
Change 1 to -1 will invert throw on channel.
added some code in RX.pde
Alex. something to incude?.
/Patrik
Added possibility to invert throw in soft.
Add in config.h
Code: Select all
// Invert RcChannels
int InvertRC[] = { 1,1,1,1,1,1,1,1 }; // -1 to Invert RcChannels.
Channelorder is same as in #define SERIAL_SUM_PPM
Change 1 to -1 will invert throw on channel.
added some code in RX.pde
Code: Select all
void computeRC() {
static uint8_t rc4ValuesIndex = 0;
uint8_t chan,a;
rc4ValuesIndex++;
for (chan = 0; chan < 8; chan++) {
rcData4Values[chan][rc4ValuesIndex%4] = readRawRC(chan);
rcDataMean[chan] = 0;
for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a];
rcDataMean[chan]= (rcDataMean[chan]+2)/4;
if ( rcDataMean[chan] < rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2;
if ( rcDataMean[chan] > rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2;
}
// Invert RcChannels
//=====================================================================
for (chan = 0; chan < 8; chan++) {
rcData[chan] = MIDRC +((MIDRC-rcData[chan])*InvertRC[chan]);
}
//=====================================================================
}
Alex. something to incude?.
/Patrik
Re: MIXING in Flying Mode (Mix table)
Yes thanks PatrikE. What you say is correct. But, radio channels are in the correct order as ROLL-PITCH-THROTTLE-YAW-AUX1-AUX2-AUX3-AUX4. Anyway this radio makes it possible to assign channels in any desired order (Hitec Aurora)
The serial summ PPM converter is giving its output in the same order and sequence.
The config.h is uncommented on #define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL
When we speak about inverting there is confusion possible as to invert the signal itself, meaning working on up trigger or down trigger of the signal (refering the signal on ground or +V) which can be obtained with a simple transistor and 2 resistors. This is not the problem here.
The Controller is compensating perfectly right on the correct direction on all axis.
If I reverse the signal on ROLL channel from the radio, it means that 2000ms will be 1000ms. In that case everything controlled by that channel will be inverted.
The controller will compensate correctly but any radio given order will be inverse. If it is addressed to servo control it is ok as this is what I want. But not on the motors in TRI config. As I'm using TRI config and switches to Flying Mode config using AUX2 in order to change flight configuration. (It is a VTOL project.) see: viewtopic.php?f=7&t=497
Thank you for the code anyway it'll be usefull for those who can't assign channels on simple radios. But I don't want to change the channel order, just finding out the way to mix adequately ROLL and PITCH so as to have a Vtail or FlyingWing type mixing. The default code on FlyingWing servo command in Output.pde is:
servo[1] = constrain(1500 + axisPID[PITCH] - axisPID[ROLL], 1020, 2000); //LEFT the direction of the 2 servo can be changed here: invert the sign before axisPID
servo[2] = constrain(1500 + axisPID[PITCH] + axisPID[ROLL], 1020, 2000); //RIGHT
which mixes ROLL inside PITCH. Which is correct when you have two servos vertically mounted side by side.
But this is depending on the orientation of servos on the wing. If servos are flat mounted under the wing or on extrados, if they are both flat mounted top facing top or rear facing rear will change the needed mixes.
So far the code I modified for a Wing having two servos mounted on extrados each facing the other is:
servo[1] = constrain( rcData[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[2] = constrain(TILT_ROLL_MIDDLE + (1500-rcData[ROLL]), TILT_ROLL_MIN, TILT_ROLL_MAX); //is easier to understand than 3000-rcData(ROLL) which is the way to invert a servo.
which is inverting the ROLL in all orders given by the Controller to that servo ... but without mixing
So as I tried all possibilities in the Mix Table above without succes, going round and round, I currently use an external mixer. If anyone has an idea. Would be nice to have a full mixing table (like the one I wrote on first post but probably incomplete) in the GUI which could allow any radio type, any inverse, mixing config to be selected from the guy letting the radio without any mixes or reverse, just the channels to be set in the correct order
Thank you for caring, withtou you all and Alex this would not be here in question!
The serial summ PPM converter is giving its output in the same order and sequence.
The config.h is uncommented on #define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL
When we speak about inverting there is confusion possible as to invert the signal itself, meaning working on up trigger or down trigger of the signal (refering the signal on ground or +V) which can be obtained with a simple transistor and 2 resistors. This is not the problem here.
The Controller is compensating perfectly right on the correct direction on all axis.
If I reverse the signal on ROLL channel from the radio, it means that 2000ms will be 1000ms. In that case everything controlled by that channel will be inverted.
The controller will compensate correctly but any radio given order will be inverse. If it is addressed to servo control it is ok as this is what I want. But not on the motors in TRI config. As I'm using TRI config and switches to Flying Mode config using AUX2 in order to change flight configuration. (It is a VTOL project.) see: viewtopic.php?f=7&t=497
Thank you for the code anyway it'll be usefull for those who can't assign channels on simple radios. But I don't want to change the channel order, just finding out the way to mix adequately ROLL and PITCH so as to have a Vtail or FlyingWing type mixing. The default code on FlyingWing servo command in Output.pde is:
servo[1] = constrain(1500 + axisPID[PITCH] - axisPID[ROLL], 1020, 2000); //LEFT the direction of the 2 servo can be changed here: invert the sign before axisPID
servo[2] = constrain(1500 + axisPID[PITCH] + axisPID[ROLL], 1020, 2000); //RIGHT
which mixes ROLL inside PITCH. Which is correct when you have two servos vertically mounted side by side.
But this is depending on the orientation of servos on the wing. If servos are flat mounted under the wing or on extrados, if they are both flat mounted top facing top or rear facing rear will change the needed mixes.
So far the code I modified for a Wing having two servos mounted on extrados each facing the other is:
servo[1] = constrain( rcData[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[2] = constrain(TILT_ROLL_MIDDLE + (1500-rcData[ROLL]), TILT_ROLL_MIN, TILT_ROLL_MAX); //is easier to understand than 3000-rcData(ROLL) which is the way to invert a servo.
which is inverting the ROLL in all orders given by the Controller to that servo ... but without mixing
So as I tried all possibilities in the Mix Table above without succes, going round and round, I currently use an external mixer. If anyone has an idea. Would be nice to have a full mixing table (like the one I wrote on first post but probably incomplete) in the GUI which could allow any radio type, any inverse, mixing config to be selected from the guy letting the radio without any mixes or reverse, just the channels to be set in the correct order
Thank you for caring, withtou you all and Alex this would not be here in question!
Re: MIXING in Flying Mode (Mix table)
OK
It's actually 2 different machines in one you are making!..
Then you probably must have 2 sets of axisPID also.
one axisPID for Muklticopter
and a DeltaxisPID for plane.
It's this part who makes all PID calc
You need to make a new var to hold DeltaxisPID[axis]
and reverse rcCommand[axis] as needed.
That's the only thing i can think of.
/Patrik
It's actually 2 different machines in one you are making!..
Then you probably must have 2 sets of axisPID also.
one axisPID for Muklticopter
and a DeltaxisPID for plane.
It's this part who makes all PID calc
Code: Select all
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if (accMode == 1 && axis<2 ) { //LEVEL MODE
errorAngle = rcCommand[axis] - angle[axis]; //rcCommand can reach 500*5 (rcRate=5) ; 500*5+1800 = 4300: 16 bits is ok here
PTerm = (int32_t)errorAngle*P8[PIDLEVEL]/100 ; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
errorAngleI[axis] += errorAngle; //16 bits is ok here
errorAngleI[axis] = constrain(errorAngleI[axis],-10000,+10000); //WindUp //16 bits is ok here
ITerm = (int32_t)errorAngleI[axis]*I8[PIDLEVEL]/4000; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
} else { //ACRO MODE or YAW axis
error = (int32_t)rcCommand[axis]*10*8/P8[axis] - gyroData[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
PTerm = rcCommand[axis];
errorGyroI[axis] += error; //16 bits is ok here
errorGyroI[axis] = constrain(errorGyroI[axis],-16000,+16000); //WindUp //16 bits is ok here
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITerm = (int32_t)errorGyroI[axis]*I8[axis]/1000/8; //32 bits is needed for calculation: 16000*I8 16 bits is ok for result
}
PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; //32 bits is needed for calculation 16 bits is ok for result
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
DTerm = (int32_t)(delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/8; //32 bits is needed for calculation (800+800+800)*50 = 120000 16 bits is ok for result
delta2[axis] = delta1[axis];
delta1[axis] = delta;
lastGyro[axis] = gyroData[axis];
axisPID[axis] = PTerm + ITerm - DTerm;
}
You need to make a new var to hold DeltaxisPID[axis]
and reverse rcCommand[axis] as needed.
That's the only thing i can think of.
/Patrik
Re: MIXING in Flying Mode (Mix table)
Ooooh Thank you I think it is the solution!
In order to separate the mixes used for Tri or Plane mode.
I'll try it and let you know.
Thanks again!
In order to separate the mixes used for Tri or Plane mode.
I'll try it and let you know.
Thanks again!
Re: MIXING in Flying Mode (Mix table)
I found a way to make it in a easier way...
It's possible to reverse the channels when you change flightmode.
Using AUX1
In config.h
in RX.pde
It looks right in the gui.
/Patrik
It's possible to reverse the channels when you change flightmode.
Using AUX1
In config.h
Code: Select all
// Invert RcChannels
#define MULTI_MODE // Enable reversing channels in air..
int InvertRC[] = { 1,1,-1,-1,-1,1,1,1 }; // 1 will reverse RcChannels if AUX >1500
in RX.pde
Code: Select all
void computeRC() {
static uint8_t rc4ValuesIndex = 0;
uint8_t chan,a;
rc4ValuesIndex++;
for (chan = 0; chan < 8; chan++) {
rcData4Values[chan][rc4ValuesIndex%4] = readRawRC(chan);
rcDataMean[chan] = 0;
for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a];
rcDataMean[chan]= (rcDataMean[chan]+2)/4;
if ( rcDataMean[chan] < rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2;
if ( rcDataMean[chan] > rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2;
}
#ifdef MULTI_MODE
//Reverse RcChannels
if(rcData[AUX1]>1500){
for (chan = 0; chan < 8; chan++) { rcData[chan] = MIDRC +((MIDRC-rcData[chan])*InvertRC[chan]); }
}
#endif
}
It looks right in the gui.
/Patrik
Re: MIXING in Flying Mode (Mix table)
Wouahou!!
Even better! I must confess I was scared by your previous solution. I went trough it, read it again and again, compared with the original code...
I was falling back on the external mixer.
I'll implement these change and let you know.
I tested today and it works great, I'm ready to test it in real on TRI config first. I still have a problem of CG not coincident on the two config: good CG for flying as a plane, way too heavy on the front for a TRI config. The two front motors are lifting 70% of the lot. I reduced the size of the Yaw propeller and I'm thinking about a sliding case under the plane to carry the Lipos and moving the CG back and forth following AUX2 orders tilting the front motors. But simpler is allways better and I just need to buy a set of varied propellers sizes.
Thanks
Even better! I must confess I was scared by your previous solution. I went trough it, read it again and again, compared with the original code...
I was falling back on the external mixer.
I'll implement these change and let you know.
I tested today and it works great, I'm ready to test it in real on TRI config first. I still have a problem of CG not coincident on the two config: good CG for flying as a plane, way too heavy on the front for a TRI config. The two front motors are lifting 70% of the lot. I reduced the size of the Yaw propeller and I'm thinking about a sliding case under the plane to carry the Lipos and moving the CG back and forth following AUX2 orders tilting the front motors. But simpler is allways better and I just need to buy a set of varied propellers sizes.
Thanks
Re: MIXING in Flying Mode (Mix table)
So I implemented your code and...it works!!!!
I just changed a little so +1 values gives normal rcChanel command and -1 gives the reverse.
instead of
it looks so simple when written down
I think you could propose it to Alex to be implemented as a way to allows reverse channels on cheap or old radios. It doesn't use lots of memory and is useful.
I just changed a little so +1 values gives normal rcChanel command and -1 gives the reverse.
Code: Select all
for (chan = 0; chan < 8; chan++) { rcData[chan] = MIDRC +((rcData[chan]-MIDRC)*InvertRC[chan]);
instead of
Code: Select all
for (chan = 0; chan < 8; chan++) { rcData[chan] = MIDRC +((MIDRC-rcData[chan])*InvertRC[chan]);
it looks so simple when written down
I think you could propose it to Alex to be implemented as a way to allows reverse channels on cheap or old radios. It doesn't use lots of memory and is useful.