latest update: March 25, 2015
-- updated code
-- added new box option called "PID TUNE" so you can enable/disable the tuning with your AUX channel.
let's start with headers first
config.h - add a new config definition
Code: Select all
/************************* INFLIGHT ROLL & PITCH PID Calibration *****************************/
#define INFLIGHT_PID_TUNING
before this line
types.h - add new box for the GUI
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
BOXPIDTUNE,
#endif
after this line
note: it's important to have the new BOX before the CHECKBOXITEMS
add this line
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
uint8_t PIDTUNE_MODE :1;
#endif
after this line
Code: Select all
#if GPS
uint8_t GPS_FIX :1 ;
uint8_t GPS_FIX_HOME :1 ;
uint8_t GPS_BARO_MODE : 1; // This flag is used when GPS controls baro mode instead of user (it will replace rcOptions[BARO]
uint8_t GPS_head_set: 1; // it is 1 if the navigation engine got commands to control heading (SET_POI or SET_HEAD) CLEAR_HEAD will zero it
uint8_t LAND_COMPLETED: 1;
uint8_t LAND_IN_PROGRESS: 1;
#endif
then the CPP files. let's start with
Protocol.cpp
add this line
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
if (f.PIDTUNE_MODE) tmp |= 1 << BOXPIDTUNE;
#endif
after this code
Code: Select all
#if defined(OSD_SWITCH)
if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
then MultiWii.cpp
add this line - that should define the box name
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
"PID TUNE;"
#endif
after this code
add this line
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
22, //"PID CALIB;"
#endif
after this code
Code: Select all
#if GPS
20, //"MISSION;"
21, //"LAND;"
#endif
then this line
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
uint8_t isin_inflight_calib = 0;
uint16_t old_AUX1_val = 0;
uint16_t new_pid = 0;
uint16_t new_i = 0;
uint16_t aux_pos = 0;
#endif
after this line
Code: Select all
int32_t AltHold; // in cm
int16_t sonarAlt;
int16_t BaroPID = 0;
int16_t errorAltitudeI = 0;
then let's add new method
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
/* --------------------------------------------------------------------------------- */
/* ----------------- EXPERIMENTAL INFLIGHT ROLL + PITCH PID TUNING ----------------- */
/* --------------------------------------------------------------------------------- */
void go_inflight_pid_tuning() {
debug[3] = conf.activate[BOXARM];
if (f.PIDTUNE_MODE)
{
//if (f.ARMED && f.ANGLE_MODE)
if (f.ARMED)
{
/*
AUX3 - you must use your Tx's pot
AUX2 - use your Tx's 3 way switch
*/
uint16_t potauxval = rcData[AUX3];
uint16_t pidsel = rcData[AUX2];
// flag that we're in calibration
isin_inflight_calib = 1;
// set the old AUX3 value once
// MAKE SURE BEFORE ARMING, THE AUX3 IS AT THE LOWEST POINT!!
if (old_AUX1_val == 0) old_AUX1_val = potauxval;
new_pid = 0;
new_i = 0;
aux_pos = 0;
if (potauxval < old_AUX1_val) { // make sure new_i is at always 0
new_i = 0;
}
else {
// get the difference from the new AUX3 value and old AUX3 value;
new_i = (potauxval - old_AUX1_val);
}
debug[0] = old_AUX1_val; //old_AUX1_val;
debug[1] = potauxval; //new_p;
debug[2] = new_i; //rcData[AUX1 + 2];
// set safe values
if (new_i < 0) {
new_i = 0;
}
else if (new_i > 1000) {
new_i = 1000;
}
debug[3] = new_i; //rcData[AUX1 + 2];
// check for 3 way switch position
aux_pos >>= 2;
if (pidsel > MINCHECK) aux_pos |= 0x20; // check for MIN
if (pidsel < MAXCHECK) aux_pos |= 0x40; // check for MAX
new_pid = new_i;
if (aux_pos == AUX2_LO)
{
new_pid = new_pid / 5;
if (new_pid > 200) new_pid = 200;
conf.pid[ROLL].P8 = new_pid + 1;
conf.pid[PITCH].P8 = new_pid + 1;
}
else if (aux_pos == AUX2_CE)
{
new_pid = new_pid / 4;
if (new_pid > 250) new_pid = 250;
conf.pid[ROLL].I8 = new_pid + 1;
conf.pid[PITCH].I8 = new_pid + 1;
}
else if (aux_pos == AUX2_HI)
{
new_pid = new_pid / 10;
if (new_pid > 100) new_pid = 100;
conf.pid[ROLL].D8 = new_pid + 1;
conf.pid[PITCH].D8 = new_pid + 1;
}
}
}
if (!f.ARMED && !f.PIDTUNE_MODE) {
if (isin_inflight_calib == 1) {
isin_inflight_calib = 0;
writeParams(1);
}
}
}
#endif
before the go_disarm() method
then this line
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
if (rcOptions[BOXPIDTUNE]) {
f.PIDTUNE_MODE = 1;
}
else {
f.PIDTUNE_MODE = 0;
}
#endif
after this code
Code: Select all
#if ACC
if (rcOptions[BOXANGLE] || (failsafeCnt > 5 * FAILSAFE_DELAY)) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ANGLE_MODE = 1;
}
}
else {
if (f.ANGLE_MODE) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
}
f.ANGLE_MODE = 0;
}
if (rcOptions[BOXHORIZON]) {
f.ANGLE_MODE = 0;
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
}
else {
if (f.HORIZON_MODE) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0;
}
f.HORIZON_MODE = 0;
}
#endif
and finally this line
Code: Select all
#if defined(INFLIGHT_PID_TUNING)
// call inflight pid tuning
go_inflight_pid_tuning();
#endif
after this code
Code: Select all
#if defined(GPS_SIMULATOR)
SerialWrite(2, 0xa5);
SerialWrite16(2, nav[LAT] + rcCommand[PITCH]);
SerialWrite16(2, nav[LON] + rcCommand[ROLL]);
SerialWrite16(2, (nav[LAT] + rcCommand[PITCH]) - (nav[LON] + rcCommand[ROLL])); //check
#endif
#endif //GPS
let me know if you have some trouble implementing it.
