Page 1 of 1

MultiWii v1.7 discussion

Posted: Wed Jul 06, 2011 8:45 pm
by captaingeek
Specifically for discussing v 1.7:

1.7 list of differences with the 1.6

ALL: BARO is more precise, but still not perfect (1m to 2m amplitude). Note it's a code issue, not a baro component issue.

ALL: RC channels AUX2, CAM1 CAM2 added. only relevant for PPM SUM stream or MEGA boards.
On the 328p with a standard receiver, only the first 5 channels are recognized.
CAM1 and CAM2 controls are not yet implemented.

ALL: AUX1 and AUX2 switches are fully customizable via a 3 state position.
We can activate/deactivate individually level mode (ACC), baro or compass (mag).
It's a generic approach which lets other possibilities to control things in the future.
With this principle, it's possible to activate permanently the options you want even if you have only 4 channels
(replace the FORCE LEVEL option)

ALL: CAM triger option:
a servo can be connected on digital pin A2 (pro mini) to activate the trigger of a camera
the option can be activated or deactivated via the button configuration panel AUX1/AUX2
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo
#define CAM_SERVO_LOW 1020 // the position of LOW state servo
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms
the PIN on arduino mega is also A2, but this is not the final PIN mapping

ALL CAM stab (servo tilt) button option:
the servo tilt option can be activated or deactivated via the button configuration panel AUX1/AUX2

ALL: BI COPTER is now a avatar style implementation.
on a pro mini: 9 motor left, 10 motor right, 11 servo left, 3 servo right
on mega: 3 motor left, 5 motor right, 6 servo left, 2 servo right

ALL: addition of Y4 (not tested)
on a pro mini: motors 9,10,11,3
on mega: motors 3,5,6,2
REAR1 , FRONT R , REAR2 , FRONT L

ALL: addition of HEX6X (not tested)
on a pro mini: motors 9,10,11,3,6,5
on mega: motors 3,5,6,2,7,8
REAR_R , FRONT_R , REAR_L , FRONT_L , RIGHT , LEFT

ALL: new level mode
The level mode is completely redesigned with a coherent independent trim
There a now a PI control loop for level mode based on angle estimation.
The old auto level strength value was a sort of P only control loop.
With the new code, the I term allows to refine the remaining angular error for a better angle positioning accuracy.
With an RC rate = 1, the angle at full stick is around 45deg => a flip should never happen in this mode.

MAIN SOFT: software trim for stable mode
It is now possible to adjust the trim of the level mode to match the same TX trim used for the acro mode.
1) disarm the motors
2) full throttle (must be >1900)
3) full PITCH forward/backward and full ROLL left/right (2 axis possibilities) will trim the level mode according to
the neutral angle you want to change. The status LED will blink to confirm each ticks.

MAIN SOFT: new calibration procedure
The ACC calibration differs now from the gyro calibration.
gyro calibration: it's still done at each power on. It's also possible manually as before: min throttle+full pitch
backward+full yaw left.
acc calibration: motor disarmed, full throttle up, full pitch backward+full yaw left.

MAIN SOFT: it was in 1.6 but not mentioned. it's possible to arm/disarm motors
either via min throttle + full yaw stick or full roll stick.

MAIN SOFT: anti yaw jump modification for multi with 4 motors or more

MAIN SOFT: servo range for tricopter
#define TRI_YAW_CONSTRAINT_MIN 1020
#define TRI_YAW_CONSTRAINT_MAX 2000

MAIN SOFT: thanks to Ciskje, integration of L3G4200D gyro (this Gyro is present in PIPO ISU)

MAIN SOFT: HMC5883 compass integration

MAIN SOFT: compass lock now works
the option can be activated or deactivated via the button configuration panel AUX1/AUX2
the lock is activated 1s after the stick release
the lock is maintained only if the YAW stick in centred +/- 50
In some case, especially with the 5883, the mag must be calibrated otherwise, the direction is not good.
The calibration procedure is not yet implemented.

MAIN SOFT: there was a bug on servo tilt (it came back to zero at around 45deg due to a variable overflow)

MAIN SOFT: thanks to Syberian, a bug was corrected to prevent ACC correction just before hovering.
It could explain some odd flips when using autolevel at the beginning

MAIN SOFT: thanks to ziss_dm, a bug was corrected about the ITG3200: there was a variable
overflow which occured only for high PITCH variation.

MAIN SOFT: thanks to mis_b (MIS), integration of failsafe code.
Failsafe detect absence of RC pulse on THROTTLE channel (standard PPM receiver)
If the pulse is OFF the failsafe procedure is initiated.

After configurable FAILSAVE_DELAY time of pulse absence,
the level mode is switched to ON (if ACC or nunchuk is available),
PITCH, ROLL and YAW is centered and THROTTLE is set to FAILSAVE_THROTTLE value.
This value is dependent from your configuration, AUW and some other params.

Next, afrer configurable FAILSAVE_OFF_DELAY time,
the copter is disarmed, and motors are stopped.
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time,
after the small quard time the RC control is returned to normal.

The fail-safe can be deactivated via #define

MAIN SOFT: thanks to ziss_dm, nunchuk alone integration (incompatible with WMP and seen as a standalone I2C ACC)

MAIN SOFT: bug correction the mag indication was reversed

MAIN SOFT: small bug correction in angle calculation (conversion rad->1/10 deg)

MAIN SOFT: added a small delay after ITG3200 init to have a better calibration

MAIN SOFT: thanks to zara,
vbat code simplification + low pass filter.
vbat activation via #define.

MAIN SOFT: reduce the PPM sum recognition delay between 2 sequences (5ms->3ms).
might be the source of frsky PPM sum problem

GUI: 3D copter attitude visualization

GUI: ACC calibration. there is now a calibrate button to calibrate the ACC directly from the GUI
(it resets the soft trim)

GUI: thanks to Eberhard, better display of serial ports on Mac OS X and Linux

GUI: the memory leak bug GUI is corrected

GUI and LCD: D is now positive (to avoid confusion in explanations)

LCD: all parameters are now customizable via the LCD thanks to the work initiated by Shirka.
This first one is still P for both ROLL&PITCH as it is the most used.

LCD: implementation of TextStar LCD tanks to Hamburger & gtrick90 code

OSD: implementation of MIS code thanks to Rurek
thanks to Rurek and mis_b, optimisation of OSD code Serial write is driven by a switchable interrupt,
minimising delay to transmit data

SITE: http://www.multiwii.com was updated accordingly.

SPECIAL NOTE ABOUT THE PULL-UPS:
- they are now enable by default
- WARNING if you use I2C devices that don't support 5V.

- If you use a WMP alone: enable it in soft
- If you use a WMP + NK : enable it in soft
- If you use a WMP + BMP020 (5V friendly thanks to its LLC): enable it in soft
- If you use a WMP + (LLC + I2C devices): enable it in soft
- If you use a WMP + direct I2C bus connection I2C devices (not 5V friendly): disable it in soft and use external
pull-ups on 3.3V. note that most breakout boards are built with pullups already available.


Config:

/*
MultiWiiCopter by Alexandre Dubus
www.multiwii.com
April 2011 V1.7
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
any later version. see <http://www.gnu.org/licenses/>
*/

/*******************************/
/****CONFIGURABLE PARAMETERS****/
/*******************************/

/* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)
This is the minimum value that allow motors to run at a idle speed */
//#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A
#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
//#define MINTHROTTLE 1190

/* The type of multicopter */
//#define GIMBAL
//#define BI
#define TRI
//#define QUADP
//#define QUADX
//#define Y4
//#define Y6
//#define HEX6
//#define HEX6X
//#define FLYING_WING //experimental

#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction
//#define YAW_DIRECTION -1

#define I2C_SPEED 100000L //100kHz normal mode, this value must be used for a genuine WMP
//#define I2C_SPEED 400000L //400kHz fast mode, it works only with some WMP clones

#define PROMINI //Arduino type
//#define MEGA

//enable internal I2C pull ups
#define INTERNAL_I2C_PULLUPS

//****** advanced users settings *************

/* Failsave settings - added by MIS
Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results.
This value is depended from your configuration, AUW and some other params.
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/
#define FAILSAFE // Alex: comment this line if you want to deactivate the failsafe function
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example
#define FAILSAVE_THR0TTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case


/* The following lines apply only for a pitch/roll tilt stabilization system
It is not compatible with Y6 or HEX6 or HEX6X
Uncomment the first line to activate it */
//#define SERVO_TILT
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000
#define TILT_PITCH_MIDDLE 1500 //servo neutral value
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement
#define TILT_ROLL_MIN 1020
#define TILT_ROLL_MAX 2000
#define TILT_ROLL_MIDDLE 1500
#define TILT_ROLL_PROP 10

/* I2C gyroscope */
//#define ITG3200
//#define L3G4200D

/* I2C accelerometer */
//#define ADXL345
//#define BMA020
//#define BMA180
//#define NUNCHACK // if you want to use the nunckuk as a standalone I2C ACC without WMP

/* I2C barometer */
//#define BMP085

/* I2C magnetometer */
//#define HMC5843
//#define HMC5883

/* ADC accelerometer */ // for 5DOF from sparkfun, uses analog PIN A1/A2/A3
//#define ADCACC

/* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2
IF YOUR RECEIVER IS NOT CONCERNED, DON'T UNCOMMENT ANYTHING. Note this is mandatory for a Y6 setup
Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */
//#define SERIAL_SUM_PPM PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,CAMPITCH,CAMROLL //For Graupner/Spektrum
//#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL //For Robe/Hitec/Futaba
//#define SERIAL_SUM_PPM PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL //For some Hitec/Sanwa/Others

/* interleaving delay in micro seconds between 2 readings WMP/NK in a WMP+NK config
if the ACC calibration time is very long (20 or 30s), try to increase this delay up to 4000
it is relevent only for a conf with NK */
#define INTERLEAVING_DELAY 3000

/* for V BAT monitoring
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
with R1=33k and R2=51k
vbat = [0;1023]*16/VBATSCALE */
#define VBAT // comment this line to suppress the vbat code
#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage
#define VBATLEVEL1_3S 107 // 10,7V
#define VBATLEVEL2_3S 103 // 10,3V
#define VBATLEVEL3_3S 99 // 9.9V

/* when there is an error on I2C bus, we neutralize the values during a short time. expressed in microseconds
it is relevent only for a conf with at least a WMP */
#define NEUTRALIZE_DELAY 100000

/* this is the value for the ESCs when thay are not armed
in some cases, this value must be lowered down to 900 for some specific ESCs */
#define MINCOMMAND 1000

/* this is the maximum value for the ESCs at full power
this value can be increased up to 2000 */
#define MAXTHROTTLE 1850

/* This is the speed of the serial interface. 115200 kbit/s is the best option for a USB connection.*/
#define SERIAL_COM_SPEED 115200

/* In order to save space, it's possibile to desactivate the LCD configuration functions
comment this line only if you don't plan to used a LCD */
#define LCD_CONF

/* to use Cat's whisker TEXTSTAR LCD, uncomment following line.
Pleae note this display needs a full 4 wire connection to (+5V, Gnd, RXD, TXD )
Configure display as follows: 115K baud, and TTL levels for RXD and TXD, terminal mode
NO rx / tx line reconfiguration, use natural pins */
//#define LCD_TEXTSTAR

/* motors will not spin when the throttle command is in low position
this is an alternative method to stop immediately the motors */
//#define MOTOR_STOP

/* some radios have not a neutral point centered on 1500. can be changed here */
#define MIDRC 1500

/* experimental
camera trigger function : activated via AUX1 UP, servo output=A2 */
//#define CAMTRIG
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo
#define CAM_SERVO_LOW 1020 // the position of LOW state servo
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms

/* you can change the tricopter servo travel here */
#define TRI_YAW_CONSTRAINT_MIN 1020
#define TRI_YAW_CONSTRAINT_MAX 2000
#define TRI_YAW_MIDDLE 1500

//****** end of advanced users settings *************

Re: MultiWii v1.7 discussion

Posted: Wed Jul 06, 2011 8:54 pm
by captaingeek
lots of really cool new stuff to play with namely the autolevel mode, software trimming of autolevel mode, and failsafe. I've began testing and have been quite surpirsed how well everything is working! Well done!

Re: MultiWii v1.7 discussion

Posted: Thu Jul 07, 2011 10:49 pm
by cob
Hello, I've found on version Pre7 MultiWii_dev20110705 the following lines in SENSORS.PDE // ACC common part, lines 180-181-182 the following code:
accZero[ROLL] = (a[ROLL])/399;
accZero[PITCH] = (a[PITCH])/399;
accZero[YAW] = (a[YAW])/399-acc_1G; // for nunchuk 200=1G
writeParams(); // write accZero in EEPROM
on version MultiWii_dev20110622 it was:
accZero[ROLL] = (a[ROLL]+200)/399;
accZero[PITCH] = (a[PITCH]+200)/399;
accZero[YAW] = (a[YAW]+200)/399-acc_1G; // for nunchuk 200=1G
writeParams(); // write accZero in EEPROM

something disapeared there ? the 200 digital value output for Nunchuk at 1G ?
I searched trough all the codes if any Var would be assigned the value of 1G for Acc... knowing that acc_1G is 1G value at 25° (sin(25)*1G, is it correct ?
If possible to clear up that point.
thanks for your good work, it's amazing!
Regards

Re: MultiWii v1.7 discussion

Posted: Thu Jul 07, 2011 11:15 pm
by Alexinparis
Hi,

200 here has nothing to do with acc_1G.
It was used to improve accuracy of math integer division.
It is not so important here.

for instance, (x+5)/10 is more accurate than x/10 for integer division.

acc_1G at 25 deg angle was removed in the last dev.
acc_1G is the value of ACCZ when the multi is perfectly flat.

Re: MultiWii v1.7 discussion

Posted: Fri Jul 08, 2011 9:33 am
by cob
Hi Alex,
Thank you for the info, this clear up a few things.
As I'm using an ADXL335 (coming from a WiiMote, clone Nck received being crap...MMA7660 and MMA6661) connected on A1-A2-A3 I was seeking a configuration for that specific gyro. The 335 works on 3.3V so analog values are out of scale on a 5V MiniPro.

Re: MultiWii v1.7 discussion

Posted: Fri Jul 08, 2011 6:23 pm
by captaingeek
A suggestion on the failsafe code - once signal is lost, after the heli lands, can you blip the motors? This way you can find your copter if it comes down out in the woods by hearing.