static void cliSave(char *cmdline)
{
uartPrint("Saving...");
writeParams(0);
uartPrint("\r\nRebooting...");
delay(10);
systemReset(false);
}
void update_constants() {
#if defined (FAILSAFE)
conf.failsafe_throttle = FAILSAFE_THROTTLE;
#endif
conf.minthrottle = MINTHROTTLE;
#if MAG
conf.mag_declination = (int16_t)(MAG_DECLINATION * 10);
#endif
writeParams(0); // this will also (p)reset checkNewConf with the current version number again.
}
void Mag_getADC(void)
{
static uint32_t t, tCal = 0;
static int16_t magZeroTempMin[3];
static int16_t magZeroTempMax[3];
uint32_t axis;
if ((int32_t)(currentTime - t) < 0)
return; //each read is spaced by 100ms
t = currentTime + 100000;
// Read mag sensor
Mag_getRawADC();
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
if (f.CALIBRATE_MAG) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
cfg.magZero[axis] = 0;
magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis];
}
f.CALIBRATE_MAG = 0;
}
if (magInit) { // we apply offset only once mag calibration is done
magADC[ROLL] -= cfg.magZero[ROLL];
magADC[PITCH] -= cfg.magZero[PITCH];
magADC[YAW] -= cfg.magZero[YAW];
}
if (tCal != 0) {
if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE;
for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin[axis])
magZeroTempMin[axis] = magADC[axis];
if (magADC[axis] > magZeroTempMax[axis])
magZeroTempMax[axis] = magADC[axis];
}
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++)
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
writeParams(1);
}
}
}
void update_constants() {
#if defined(GYRO_SMOOTHING)
{
uint8_t s[3] = GYRO_SMOOTHING;
for(uint8_t i=0;i<3;i++) conf.Smoothing[i] = s[i];
}
#endif
#if defined (FAILSAFE)
conf.failsafe_throttle = FAILSAFE_THROTTLE;
#endif
#ifdef VBAT
conf.vbatscale = VBATSCALE;
conf.vbatlevel_warn1 = VBATLEVEL_WARN1;
conf.vbatlevel_warn2 = VBATLEVEL_WARN2;
conf.vbatlevel_crit = VBATLEVEL_CRIT;
#endif
#ifdef POWERMETER
conf.pint2ma = PINT2mA;
#endif
#ifdef POWERMETER_HARD
conf.psensornull = PSENSORNULL;
#endif
#ifdef MMGYRO
conf.mmgyro = MMGYRO;
#endif
#if defined(ARMEDTIMEWARNING)
conf.armedtimewarning = ARMEDTIMEWARNING;
#endif
conf.minthrottle = MINTHROTTLE;
#if MAG
conf.mag_declination = (int16_t)(MAG_DECLINATION * 10);
#endif
#ifdef GOVERNOR_P
conf.governorP = GOVERNOR_P;
conf.governorD = GOVERNOR_D;
#endif
#ifdef YAW_COLL_PRECOMP
conf.yawCollPrecomp = YAW_COLL_PRECOMP;
conf.yawCollPrecompDeadband = YAW_COLL_PRECOMP_DEADBAND;
#endif
#if defined(MY_PRIVATE_DEFAULTS)
#include MY_PRIVATE_DEFAULTS
#endif
#if GPS
loadGPSdefaults();
#endif
writeParams(0); // this will also (p)reset checkNewConf with the current version number again.
}
Element* Bank::writeModule( ModuleData* data )
{
Element* element = new Element( "Module" );
element->SetAttribute( "id", data->id_ );
element->SetAttribute( "label", data->label_ );
element->SetAttribute( "catalog", data->catalog_ );
element->SetAttribute( "poly", data->polyphony_ );
element->SetAttribute( "x", data->xPos_ );
element->SetAttribute( "y", data->yPos_ );
if( data->collapsed_ )
element->SetAttribute( "collapsed", data->collapsed_ );
writeParams( element, data );
writeLinks( element, data );
return element;
}
static void Acc_Calibrate(void) // Removed Sphere Algo, wasn't really better, sorry.
{
uint16_t i;
Acc_500Hz_AVG(cfg.accZero, ACCdiscardcnt); // Discard some values for warmup abuse accZero[3]
Acc_500Hz_AVG(cfg.accZero, ACCavgcount);
cfg.sens_1G = 16384; // preset 2^14 that is the 16G Scale of MPU
for (i = 0; i < 9; i++) // Eval Bitresolution of ACC. BitScale is recognized here
{
if((float)abs((int16_t)cfg.accZero[2]) > ((float)cfg.sens_1G * 0.85f)) break;
else cfg.sens_1G >>= 1;
}
cfg.accZero[2] -= cfg.sens_1G;
for (i = 0; i < 3; i++) cfg.ShakyGyroZero[i] = gyroZero[i];
for (i = 0; i < 2; i++) cfg.angleTrim[i] = 0.0f;
cfg.ShakyDataAvail = 1;
cfg.acc_calibrated = 1;
writeParams(1);
systemReset(false);
}
void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
if (ks.solve(maxTime))
{
std::ofstream out(outputFile.c_str());
oc::PathControl path(ks.getSolutionPath());
path.interpolate();
if (!path.check())
OMPL_ERROR("Path is invalid");
writeParams(out);
path.printAsMatrix(out);
if (!ks.haveExactSolutionPath())
OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
ks.getProblemDefinition()->getSolutionDifference());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
#if 0
// Get the planner data, save the ship's (x,y) coordinates to one file and
// the edge information to another file. This can be used for debugging
// purposes; plotting the tree of states might give you some idea of
// a planner's strategy.
ob::PlannerData pd(ks.getSpaceInformation());
ks.getPlannerData(pd);
std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
double* coords;
unsigned numVerts = pd.numVertices();
std::vector<unsigned int> edgeList;
for (unsigned int i = 0; i < numVerts; ++i)
{
coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
vertexFile << coords[0] << ' ' << coords[1] << '\n';
pd.getEdges(i, edgeList);
for (unsigned int j = 0; j < edgeList.size(); ++j)
edgeFile << i << ' ' << edgeList[j] << '\n';
}
#endif
}
void evaluateCommand(uint8_t c) {
uint32_t i, tmp = 0, junk;
uint8_t zczxczxczxc = 0;
const char *build = __DATE__;
switch (c) {
// adding this message as a comment will return an error status for MSP_PRIVATE (end of switch), allowing third party tools to distinguish the implementation of this message
//case MSP_PRIVATE:
// headSerialError();tailSerialReply(); // we don't have any custom msp currently, so tell the gui we do not use that
// break;
#if defined(CLEANFLIGHT)
case MSP_API_VERSION:
headSerialReply(1 + API_VERSION_LENGTH);
serialize8(MSP_PROTOCOL_VERSION);
serialize8(API_VERSION_MAJOR);
serialize8(API_VERSION_MINOR);
tailSerialReply();
USE_CLEANFLIGHT_REPLIES = 1;
break;
case MSP_FC_VARIANT:
headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
serialize8(flightControllerIdentifier[i]);
}
tailSerialReply();
break;
case MSP_FC_VERSION:
headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);
serialize8(FC_VERSION_MAJOR);
serialize8(FC_VERSION_MINOR);
serialize8(FC_VERSION_PATCH_LEVEL);
tailSerialReply();
break;
case MSP_BOARD_INFO:
headSerialReply(
BOARD_IDENTIFIER_LENGTH +
BOARD_HARDWARE_REVISION_LENGTH
);
for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
serialize8(boardIdentifier[i]);
}
//#ifdef NAZE
// serialize16(hardwareRevision);
//#else
serialize16(0); // No other build targets currently have hardware revision detection.
//#endif
tailSerialReply();
break;
case MSP_BF_BUILD_INFO: // BASEFLIGHT
headSerialReply(11 + 4 + 4);
for (i = 0; i < 11; i++)
serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
serialize32(0); // future exp
serialize32(0); // future exp
tailSerialReply();
case MSP_BUILD_INFO: // CLEANFLIGHT
headSerialReply(11 + 4 + 4);
for (i = 0; i < 11; i++)
serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
serialize32(0); // future exp
serialize32(0); // future exp
/*headSerialReply(
BUILD_DATE_LENGTH +
BUILD_TIME_LENGTH +
GIT_SHORT_REVISION_LENGTH
);
for (i = 0; i < BUILD_DATE_LENGTH; i++) {
serialize8(buildDate[i]);
}
for (i = 0; i < BUILD_TIME_LENGTH; i++) {
serialize8(buildTime[i]);
}
for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
serialize8(shortGitRevision[i]);
}*/
tailSerialReply();
break;
#endif
case MSP_SUPRESS_DATA_FROM_RX:
supress_data_from_rx = read8();
headSerialReply(1);
serialize8((uint8_t)supress_data_from_rx);
tailSerialReply();
break;
case MSP_SET_RAW_RC:
s_struct_w((uint8_t*) &rcSerial, 16);
rcSerialCount = 50; // 1s transition
break;
case MSP_SET_PID:
mspAck();
s_struct_w((uint8_t*) &conf.pid[0].P8, 3 * PIDITEMS);
break;
case MSP_SET_BOX:
mspAck();
#if EXTAUX
s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*4);
#else
s_struct_w((uint8_t*) &conf.activate[0], CHECKBOXITEMS * 2);
#endif
break;
case MSP_SET_RC_TUNING:
mspAck();
s_struct_w((uint8_t*) &conf.rcRate8, 7);
break;
#if !defined(DISABLE_SETTINGS_TAB)
case MSP_SET_MISC:
struct {
uint16_t a, b, c, d, e, f;
uint32_t g;
uint16_t h;
uint8_t i, j, k, l;
} set_misc;
mspAck();
s_struct_w((uint8_t*) &set_misc, 22);
#if defined(POWERMETER)
conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
#endif
conf.minthrottle = set_misc.b;
#ifdef FAILSAFE
conf.failsafe_throttle = set_misc.e;
#endif
#if MAG
conf.mag_declination = set_misc.h;
#endif
#if defined(VBAT)
conf.vbatscale = set_misc.i;
conf.vbatlevel_warn1 = set_misc.j;
conf.vbatlevel_warn2 = set_misc.k;
conf.vbatlevel_crit = set_misc.l;
#endif
break;
case MSP_MISC:
struct {
#if defined(CLEANFLIGHT)
uint16_t a, b, c, d, e;
uint8_t f;
uint8_t g;
uint8_t h;
uint8_t i;
uint8_t j;
uint8_t k;
uint16_t l;
uint8_t m, n, o, p;
#else
uint16_t a, b, c, d, e, f;
uint32_t g;
uint16_t h;
uint8_t i, j, k, l;
#endif
} misc;
misc.a = intPowerTrigger1;
misc.b = conf.minthrottle;
misc.c = MAXTHROTTLE;
misc.d = MINCOMMAND;
#ifdef FAILSAFE
misc.e = conf.failsafe_throttle;
#else
misc.e = 0;
#endif
#if defined(CLEANFLIGHT)
#if GPS
//serialize8(masterConfig.gpsConfig.provider); // gps_type
//serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
//serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
#else
//f serialize8(0); // gps_type
//g serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
//h serialize8(0); // gps_ubx_sbas
misc.f = 0;
misc.g = 1;
misc.h = 0;
#endif
// i serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
// j serialize8(masterConfig.rxConfig.rssi_channel);
// k serialize8(0);
misc.i = 0;
misc.j = 0;
misc.k = 0;
#if MAG
misc.l = conf.mag_declination;
#else
misc.l = 0;
#endif
#ifdef VBAT
misc.m = conf.vbatscale;
misc.n = conf.vbatlevel_warn1;
misc.o = conf.vbatlevel_warn2;
misc.p = conf.vbatlevel_crit;
#else
misc.m = 0; misc.n = 0; misc.o = 0; misc.p = 0;
#endif
s_struct((uint8_t*) &misc, 32);
#else
#ifdef LOG_PERMANENT
misc.f = plog.arm;
misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
#else
misc.f = 0; misc.g = 0;
#endif
#if MAG
misc.h = conf.mag_declination;
#else
misc.h = 0;
#endif
#ifdef VBAT
misc.i = conf.vbatscale;
misc.j = conf.vbatlevel_warn1;
misc.k = conf.vbatlevel_warn2;
misc.l = conf.vbatlevel_crit;
#else
misc.i = 0; misc.j = 0; misc.k = 0; misc.l = 0;
#endif
s_struct((uint8_t*) &misc, 22);
#endif
break;
#endif
//#if defined (DYNBALANCE)
case MSP_SET_MOTOR:
mspAck();
s_struct_w((uint8_t*)&motor_disarmed,16);
break;
//#endif
#ifdef MULTIPLE_CONFIGURATION_PROFILES
case MSP_SELECT_SETTING:
if (!f.ARMED) {
global_conf.currentSet = read8();
if (global_conf.currentSet > 2) global_conf.currentSet = 0;
writeGlobalSet(0);
readEEPROM();
}
mspAck();
break;
#endif
case MSP_SET_HEAD:
mspAck();
s_struct_w((uint8_t*) &magHold, 2);
break;
case MSP_IDENT:
struct {
uint8_t v, t, msp_v;
uint32_t cap;
} id;
id.v = VERSION;
id.t = MULTITYPE;
id.msp_v = MSP_VERSION;
id.cap = (0 + BIND_CAPABLE) | DYNBAL << 2 | FLAP << 3 | NAVCAP << 4 | EXTAUX << 5 | ((uint32_t) NAVI_VERSION << 28); //Navi version is stored in the upper four bits;
s_struct((uint8_t*) &id, 7);
break;
case MSP_STATUS:
struct {
uint16_t cycleTime, i2c_errors_count, sensor;
uint32_t flag;
uint8_t set;
} st;
st.cycleTime = cycleTime;
st.i2c_errors_count = i2c_errors_count;
st.sensor = ACC | BARO << 1 | MAG << 2 | GPS << 3 | SONAR << 4;
#if ACC
if (f.ANGLE_MODE) tmp |= 1 << BOXANGLE;
if (f.HORIZON_MODE) tmp |= 1 << BOXHORIZON;
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
if (f.BARO_MODE) tmp |= 1 << BOXBARO;
#endif
if (f.MAG_MODE) tmp |= 1 << BOXMAG;
#if !defined(FIXEDWING)
#if defined(HEADFREE)
if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
#endif
#if defined(CAMTRIG)
if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
#endif
#if GPS
switch (f.GPS_mode) {
case GPS_MODE_HOLD:
tmp |= 1 << BOXGPSHOLD;
break;
case GPS_MODE_RTH:
tmp |= 1 << BOXGPSHOME;
break;
case GPS_MODE_NAV:
tmp |= 1 << BOXGPSNAV;
break;
}
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
#endif
#if defined(BUZZER)
if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
#endif
#if defined(LED_FLASHER)
if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
#endif
#if defined(LANDING_LIGHTS_DDR)
if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
#endif
#if defined(VARIOMETER)
if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if (rcOptions[BOXCALIB]) tmp |= 1 << BOXCALIB;
#endif
#if defined(GOVERNOR_P)
if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
#endif
#if defined(OSD_SWITCH)
if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
#if defined(INFLIGHT_PID_TUNING)
if (f.PIDTUNE_MODE) tmp |= 1 << BOXPIDTUNE;
#endif
#if SONAR
if (f.SONAR_MODE) tmp |= 1 << BOXSONAR;
#endif
if (f.ARMED) tmp |= 1 << BOXARM;
st.flag = tmp;
st.set = global_conf.currentSet;
s_struct((uint8_t*) &st, 11);
break;
case MSP_RAW_IMU:
//#if defined(DYNBALANCE)
for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
//#endif
s_struct((uint8_t*) &imu, 18);
break;
case MSP_SERVO:
s_struct((uint8_t*) &servo, 16);
break;
case MSP_SERVO_CONF:
s_struct((uint8_t*) &conf.servoConf[0].min, 56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56
break;
case MSP_SET_SERVO_CONF:
mspAck();
s_struct_w((uint8_t*) &conf.servoConf[0].min, 56);
break;
case MSP_MOTOR:
s_struct((uint8_t*) &motor, 16);
break;
case MSP_ACC_TRIM:
headSerialReply(4);
s_struct_partial((uint8_t*) &conf.angleTrim[PITCH], 2);
s_struct_partial((uint8_t*) &conf.angleTrim[ROLL], 2);
tailSerialReply();
break;
case MSP_SET_ACC_TRIM:
mspAck();
s_struct_w((uint8_t*) &conf.angleTrim[PITCH], 2);
s_struct_w((uint8_t*) &conf.angleTrim[ROLL], 2);
break;
case MSP_RC:
s_struct((uint8_t*) &rcData, RC_CHANS * 2);
break;
#if GPS
case MSP_SET_RAW_GPS:
struct {
uint8_t a, b;
int32_t c, d;
int16_t e;
uint16_t f;
} set_set_raw_gps;
mspAck();
s_struct_w((uint8_t*) &set_set_raw_gps, 14);
f.GPS_FIX = set_set_raw_gps.a;
GPS_numSat = set_set_raw_gps.b;
GPS_coord[LAT] = set_set_raw_gps.c;
GPS_coord[LON] = set_set_raw_gps.d;
GPS_altitude = set_set_raw_gps.e;
GPS_speed = set_set_raw_gps.f;
GPS_update |= 2; // New data signalisation to GPS functions
break;
case MSP_RAW_GPS:
struct {
uint8_t a, b;
int32_t c, d;
int16_t e;
uint16_t f, g;
} msp_raw_gps;
msp_raw_gps.a = f.GPS_FIX;
msp_raw_gps.b = GPS_numSat;
msp_raw_gps.c = GPS_coord[LAT];
msp_raw_gps.d = GPS_coord[LON];
msp_raw_gps.e = GPS_altitude;
msp_raw_gps.f = GPS_speed;
msp_raw_gps.g = GPS_ground_course;
s_struct((uint8_t*) &msp_raw_gps, 16);
break;
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
}
tailSerialReply();
break;
case MSP_COMP_GPS:
struct {
uint16_t a;
int16_t b;
uint8_t c;
} msp_comp_gps;
msp_comp_gps.a = GPS_distanceToHome;
msp_comp_gps.b = GPS_directionToHome;
msp_comp_gps.c = GPS_update & 1;
s_struct((uint8_t*) &msp_comp_gps, 5);
break;
#if defined(USE_MSP_WP)
case MSP_SET_NAV_CONFIG:
mspAck();
s_struct_w((uint8_t*)&GPS_conf,sizeof(GPS_conf));
break;
case MSP_NAV_CONFIG:
s_struct((uint8_t*)&GPS_conf,sizeof(GPS_conf));
break;
case MSP_NAV_STATUS: // to move to struct transmission
headSerialReply(7);
serialize8(f.GPS_mode);
serialize8(NAV_state);
serialize8(mission_step.action);
serialize8(mission_step.number);
serialize8(NAV_error);
serialize16( (int16_t)(target_bearing/100));
//serialize16(magHold);
tailSerialReply();
break;
case MSP_WP: // to move to struct transmission
{
uint8_t wp_no;
uint8_t flag;
bool success;
wp_no = read8(); //get the wp number
headSerialReply(21);
if (wp_no == 0) { //Get HOME coordinates
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
flag = MISSION_FLAG_HOME;
}
if (wp_no == 255) { //Get poshold coordinates
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
flag = MISSION_FLAG_HOLD;
}
if ((wp_no>0) && (wp_no<255)) {
if (NAV_state == NAV_STATE_NONE) {
success = recallWP(wp_no);
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(mission_step.pos[LAT]);
serialize32(mission_step.pos[LON]);
if (success == true) flag = mission_step.flag;
else flag = MISSION_FLAG_CRC_ERROR; //CRC error
} else {
serialize8(wp_no);
serialize8(0);
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
flag = MISSION_FLAG_NAV_IN_PROG;
}
}
serialize32(mission_step.altitude);
serialize16(mission_step.parameter1);
serialize16(mission_step.parameter2);
serialize16(mission_step.parameter3);
serialize8(flag);
tailSerialReply();
}
break;
case MSP_SET_WP: // to move to struct transmission
{
uint8_t wp_no = read8(); //Get the step number
if (NAV_state == NAV_STATE_HOLD_INFINIT && wp_no == 255) { //Special case - during stable poshold we allow change the hold position
mission_step.number = wp_no;
mission_step.action = MISSION_HOLD_UNLIM;
uint8_t temp = read8();
mission_step.pos[LAT] = read32();
mission_step.pos[LON] = read32();
mission_step.altitude = read32();
mission_step.parameter1 = read16();
mission_step.parameter2 = read16();
mission_step.parameter3 = read16();
mission_step.flag = read8();
if (mission_step.altitude != 0) set_new_altitude(mission_step.altitude);
GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
if ((wp_distance/100) >= GPS_conf.safe_wp_distance) NAV_state = NAV_STATE_NONE;
else NAV_state = NAV_STATE_WP_ENROUTE;
break;
}
if (NAV_state == NAV_STATE_NONE) { // The Nav state is not zero, so navigation is in progress, silently ignore SET_WP command)
mission_step.number = wp_no;
mission_step.action = read8();
mission_step.pos[LAT] = read32();
mission_step.pos[LON] = read32();
mission_step.altitude = read32();
mission_step.parameter1 = read16();
mission_step.parameter2 = read16();
mission_step.parameter3 = read16();
mission_step.flag = read8();
//It's not sure, that we want to do poshold change via mission planner so perhaps the next if is deletable
/*
if (mission_step.number == 255) //Set up new hold position via mission planner, It must set the action to MISSION_HOLD_INFINIT
{
if (mission_step.altitude !=0) set_new_altitude(mission_step.altitude); //Set the altitude
GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
NAV_state = NAV_STATE_WP_ENROUTE; //Go to that position, then it will switch to poshold unlimited when reached
}
*/
if (mission_step.number == 0) { //Set new Home position
GPS_home[LAT] = mission_step.pos[LAT];
GPS_home[LON] = mission_step.pos[LON];
}
if (mission_step.number >0 && mission_step.number<255) //Not home and not poshold, we are free to store it in the eprom
if (mission_step.number <= getMaxWPNumber()) // Do not thrash the EEPROM with invalid wp number
storeWP();
mspAck();
}
}
break;
#endif //USE_MSP_WP
#endif //GPS
case MSP_ATTITUDE:
s_struct((uint8_t*) &att, 6);
break;
case MSP_ALTITUDE:
s_struct((uint8_t*) &alt, 6);
break;
case MSP_ANALOG:
s_struct((uint8_t*) &analog, 7);
break;
case MSP_RC_TUNING:
s_struct((uint8_t*) &conf.rcRate8, 7);
break;
case MSP_PID:
s_struct((uint8_t*) &conf.pid[0].P8, 3 * PIDITEMS);
break;
case MSP_PIDNAMES:
serializeNames(pidnames);
break;
#if defined(CLEANFLIGHT)
case MSP_PID_CONTROLLER:
headSerialReply(1);
//s_struct((uint8_t *) PID_CONTROLLER, 7);
serialize8(PID_CONTROLLER - 1);
tailSerialReply();
break;
#endif
case MSP_BOX:
#if EXTAUX
s_struct((uint8_t*)&conf.activate[0],4*CHECKBOXITEMS);
#else
s_struct((uint8_t*) &conf.activate[0], 2 * CHECKBOXITEMS);
#endif
break;
case MSP_BOXNAMES:
serializeNames(boxnames);
break;
case MSP_BOXIDS:
headSerialReply(CHECKBOXITEMS);
for (uint8_t i = 0; i < CHECKBOXITEMS; i++)
serialize8(pgm_read_byte(&(boxids[i])));
tailSerialReply();
break;
case MSP_MOTOR_PINS:
s_struct((uint8_t*) &PWM_PIN, 8);
break;
case MSP_RESET_CONF:
if (!f.ARMED) LoadDefaults();
mspAck();
break;
case MSP_ACC_CALIBRATION:
if (!f.ARMED) calibratingA = 512;
mspAck();
break;
#if GYRO
case MSP_GYRO_CALIBRATION:
if (!f.ARMED) calibratingG = 512;
mspAck();
break;
#endif
#if MAG
case MSP_MAG_CALIBRATION:
if (!f.ARMED) f.CALIBRATE_MAG = 1;
mspAck();
break;
#endif
#if defined(SPEK_BIND)
case MSP_BIND:
spekBind();
mspAck();
break;
#endif
case MSP_EEPROM_WRITE:
writeParams(0);
mspAck();
break;
case MSP_DEBUG:
s_struct((uint8_t*) &debug, 8);
break;
#if defined(CLEANFLIGHT)
case MSP_BF_CONFIG:
// baseflight
//headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2);
headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2);
serialize8((uint8_t) MULTITYPE); // QUADX
// features
serialize32(1 << 4 | 1 << 9 | 1 << 2); // MOTOR_STOP, FEATURE_SONAR, FEATURE_INFLIGHT_ACC_CAL
//serialize32((uint32_t)0); // MOTOR_STOP, FEATURE_SONAR
// rx provider
serialize8((uint8_t) 0);
// board alignment
serialize16((uint16_t) 0);
serialize16((uint16_t) 0);
serialize16((uint16_t) 0);
// battery
serialize16((uint16_t) 0);
serialize16((uint16_t) 0);
// motor_pwn_rate
serialize16((uint16_t) 0);
// pitch and roll rate
// serialize8((uint8_t) s_struct((uint8_t*) &, 7););
serialize8((uint8_t) conf.rollPitchRate);
serialize8((uint8_t) conf.rollPitchRate);
/* ? baseflight
serialize16(mcfg.currentscale);
serialize16(mcfg.currentoffset);
serialize16(mcfg.motor_pwm_rate);
serialize8(cfg.rollPitchRate[0]);
serialize8(cfg.rollPitchRate[1]);
*/
// battery
/*serialize16((uint16_t) 0);
serialize16((uint16_t) 0);*/
tailSerialReply();
break;
case MSP_CF_SERIAL_CONFIG:
headSerialReply(
((sizeof(uint8_t) +sizeof(uint16_t) +(sizeof(uint8_t) * 4)) * 4)
);
for (i = 0; i < 4; i++) {
serialize8(0);
serialize16(0);
serialize8(0);
serialize8(0);
serialize8(0);
serialize8(0);
}
tailSerialReply();
break;
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
tailSerialReply();
break;
#endif
#ifdef DEBUGMSG
case MSP_DEBUGMSG:
{
uint8_t size = debugmsg_available();
if (size > 16) size = 16;
headSerialReply(size);
debugmsg_serialize(size);
tailSerialReply();
}
break;
#endif
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(); tailSerialReply();
break;
}
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta, deltaSum;
int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static int16_t delta1[3], delta2[3];
static int16_t errorGyroI[3] = { 0, 0, 0 };
static int16_t errorAngleI[2] = { 0, 0 };
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t loopTime;
uint16_t auxState = 0;
int16_t prop;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
computeRC();
if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
rcTime = currentTime + 20000;
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
computeRC();
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++)
rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = cfg.failsafe_throttle;
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
}
if (rcData[THROTTLE] < cfg.mincheck) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && !f.ARMED) {
if (rcDelayCommand == 20) {
calibratingG = 1000;
if (feature(FEATURE_GPS))
GPS_reset_home_position();
}
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (!f.ARMED && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
if (rcDelayCommand == 20) {
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
} else {
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
if (AccInflightCalibrationArmed) {
toggleBeep = 2;
} else {
toggleBeep = 3;
}
}
}
} else if (cfg.activate[BOXARM] > 0) {
if (rcOptions[BOXARM] && f.OK_TO_ARM) {
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt == 0
f.ARMED = 1;
headFreeModeHold = heading;
} else if (f.ARMED)
f.ARMED = 0;
rcDelayCommand = 0;
} else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && f.ARMED) {
if (rcDelayCommand == 20)
f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ((rcData[YAW] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > cfg.maxcheck && !f.ARMED) {
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20)
calibratingA = 400;
rcDelayCommand++;
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20)
f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > cfg.maxcheck) {
cfg.angleTrim[PITCH] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[PITCH] < cfg.mincheck) {
cfg.angleTrim[PITCH] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] > cfg.maxcheck) {
cfg.angleTrim[ROLL] += 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else if (rcData[ROLL] < cfg.mincheck) {
cfg.angleTrim[ROLL] -= 2;
writeParams(1);
#ifdef LEDRING
if (feature(FEATURE_LED_RING))
ledringBlink();
#endif
} else {
rcDelayCommand = 0;
}
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
InflightcalibratingA = 50;
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
}
for(i = 0; i < 4; i++)
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
for(i = 0; i < CHECKBOXITEMS; i++)
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
f.ANGLE_MODE = 1;
}
} else {
f.ANGLE_MODE = 0; // failsave support
}
if (rcOptions[BOXHORIZON]) {
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
} else {
f.HORIZON_MODE = 0;
}
if ((rcOptions[BOXARM]) == 0)
f.OK_TO_ARM = 1;
if (f.ANGLE_MODE || f.HORIZON_MODE) {
LED1_ON;
} else {
LED1_OFF;
}
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID = 0;
}
} else
f.BARO_MODE = 0;
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else
f.MAG_MODE = 0;
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
}
#endif
if (sensors(SENSOR_GPS)) {
if (f.GPS_FIX && GPS_numSat >= 5) {
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
}
if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1;
} else {
f.PASSTHRU_MODE = 0;
}
if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
f.HEADFREE_MODE = 0;
}
} else { // not in rc loop
static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
switch (taskOrder++ % 4) {
case 0:
#ifdef MAG
if (sensors(SENSOR_MAG))
Mag_getADC();
#endif
break;
case 1:
#ifdef BARO
if (sensors(SENSOR_BARO))
Baro_update();
#endif
break;
case 2:
#ifdef BARO
if (sensors(SENSOR_BARO))
getEstimatedAltitude();
#endif
break;
case 3:
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
Sonar_update();
debug[2] = sonarAlt;
}
#endif
break;
default:
taskOrder = 0;
break;
}
}
currentTime = micros();
if (cfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + cfg.looptime;
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
#ifdef MPU6050_DMP
mpu6050DmpLoop();
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= -180)
dif += 360;
if (dif >= +180)
dif -= 360;
if (f.SMALL_ANGLES_25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
}
#endif
if (sensors(SENSOR_GPS)) {
// Check that we really need to navigate ?
if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME) {
// If not. Reset nav loops and all nav related parameters
GPS_reset_nav();
} else {
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
}
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
#ifdef LEVEL_PDF
PTermACC = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
#else
PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
// Default settings
static void resetConf(void)
{
uint8_t i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { 0, 0, 0 } };
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
// featureSet(FEATURE_VBAT);
featureSet(FEATURE_PPM);
// featureSet(FEATURE_FAILSAFE);
// featureSet(FEATURE_LCD);
// featureSet(FEATURE_GPS);
// featureSet(FEATURE_PASS); // Just pass Throttlechannel
// featureSet(FEATURE_SONAR);
cfg.P8[ROLL] = 35; // 40
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 30;
cfg.P8[PITCH] = 35; // 40
cfg.I8[PITCH] = 30;
cfg.D8[PITCH] = 30;
cfg.P8[YAW] = 60; // 70
cfg.I8[YAW] = 45;
cfg.P8[PIDALT] = 100;
cfg.I8[PIDALT] = 30;
cfg.D8[PIDALT] = 80;
cfg.P8[PIDPOS] = 10; // FIND YOUR VALUE
cfg.I8[PIDPOS] = 40; // USED
cfg.P8[PIDPOSR] = 70; // FIND YOUR VALUE // Controls the speed part with my PH logic
cfg.D8[PIDPOSR] = 100; // FIND YOUR VALUE // Controls the speed part with my PH logic
cfg.P8[PIDNAVR] = 15; // 14 More ?
cfg.I8[PIDNAVR] = 0; // NAV_I * 100; // Scaling/Purpose unchanged
cfg.D8[PIDNAVR] = 0; // NAV_D * 1000; // Scaling/Purpose unchanged
// cfg.P8[PIDPOS] = 11; // APM PH Stock values
// cfg.I8[PIDPOS] = 0;
// cfg.D8[PIDPOS] = 0;
// cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
// cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
// cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
// cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
// cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
// cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 70; // 70
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 50;
cfg.P8[PIDMAG] = 80; // cfg.P8[PIDVEL] = 0;// cfg.I8[PIDVEL] = 0;// cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 100;
cfg.rcExpo8 = 80; // cfg.rollPitchRate = 0;// cfg.yawRate = 0;// cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
memcpy(&cfg.align, default_align, sizeof(cfg.align));
cfg.mag_dec = 113; // Crashpilot //cfg.acc_hdw = ACC_DEFAULT;// default/autodetect
cfg.mag_time = 1; // (1-6) Calibration time in minutes
cfg.mag_gain = 0; // 0(default) = 1.9 GAUSS ; 1 = 2.5 GAUSS (problematic copters, will reduce 20% resolution)
cfg.acc_hdw = 2; // Crashpilot MPU6050
cfg.acc_lpfhz = 10.0f; // [0.x-100Hz] LPF for angle/horizon 0.536f resembles somehow the orig mwii factor
cfg.acc_altlpfhz = 15; // [1-100Hz] LPF for althold
cfg.acc_gpslpfhz = 30; // [1-100Hz] LPF for GPS ins stuff
cfg.looptime = 3000;
cfg.mainpidctrl = 0; // 0 = OriginalMwiiPid pimped by me, 1 = New mwii controller (experimental, float pimped + pt1)
cfg.maincuthz = 12; // [1-100Hz] Cuf Off Frequency for D term of main Pid controller
cfg.gpscuthz = 45; // [1-100Hz] Cuf Off Frequency for D term of GPS Pid controller
cfg.gy_gcmpf = 700; // (10-1000) 400 default. Now 1000. The higher, the more weight gets the gyro and the lower is the correction with Acc data.
cfg.gy_mcmpf = 200; // (10-2000) 200 default for 10Hz. Now higher. Gyro/Magnetometer Complement.
cfg.gy_smrll = 0;
cfg.gy_smptc = 0;
cfg.gy_smyw = 0; // In Tricopter mode a "1" will enable a moving average filter, anything higher will also enable a lowpassfilter
cfg.gy_lpf = 42; // Values for MPU 6050/3050: 256, 188, 98, 42, 20, 10, (HZ) For L3G4200D: 93, 78, 54, 32
cfg.gy_stdev = 5;
// Baro
cfg.accz_vcf = 0.985f; // Crashpilot: Value for complementary filter accz and barovelocity
cfg.accz_acf = 0.960f; // Crashpilot: Value for complementary filter accz and altitude
cfg.bar_lag = 0.3f; // Lag of Baro/Althold stuff in general, makes stop in hightchange snappier
cfg.bar_dscl = 0.7f; // Scale downmovement down (because copter drops faster than rising)
cfg.bar_dbg = 0; // Crashpilot: 1 = Debug Barovalues //cfg.baro_noise_lpf = 0.6f;// Crashpilot: Not used anymore//cfg.baro_cf = 0.985f;// Crashpilot: Not used anymore
// Autoland
cfg.al_barolr = 50; // [10 - 200cm/s] Baro Landingrate
cfg.al_snrlr = 50; // [10 - 200cm/s] Sonar Landingrate - You can specify different landingfactor here on sonar contact, because sonar land maybe too fast when snr_cf is high
cfg.al_debounce = 5; // (0-20%) 0 Disables. Defines a Throttlelimiter on Autoland. Percentage defines the maximum deviation of assumed hoverthrottle during Autoland
cfg.al_tobaro = 2000; // Timeout in ms (100 - 5000) before shutoff on autoland. "esc_nfly" must be undershot for that timeperiod
cfg.al_tosnr = 1000; // Timeout in ms (100 - 5000) If sonar aided land is wanted (snr_land = 1) you can choose a different timeout here
// Autostart
cfg.as_lnchr = 200; // [50 - 250 no dimension DEFAULT:200] Autostart initial launchrate to get off the ground. When as_stdev is exceeded, as_clmbr takes over
cfg.as_clmbr = 100; // [50 - 250cm/s DEFAULT:100] Autostart climbrate in cm/s after liftoff! Autostart Rate in cm/s will be lowered when approaching targethight.
cfg.as_trgt = 0; // [0 - 255m DEFAULT:0 (0 = Disable)] Autostart Targethight in m Note: use 2m or more
cfg.as_stdev = 10; // [5 - 20 no dimension DEFAULT:10] This is the std. deviation of the variometer when a liftoff is assumed. The higher the more unsensitive.
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
cfg.power_adc_channel = 0;
// Radio
parseRcChannels("AETR1234");
cfg.rc_db = 20; // Crashpilot: A little deadband will not harm our crappy RC
cfg.rc_dbyw = 20; // Crashpilot: A little deadband will not harm our crappy RC
cfg.rc_dbah = 50; // Crashpilot: A little deadband will not harm our crappy RC
cfg.rc_dbgps = 5; // Additional Deadband for all GPS functions;
cfg.devorssi = 0; // Will take the last channel for RSSI value, so add one to rc_auxch, don't use that auxchannel unless you want it to trigger something
// Note Spektrum or Graupner will override that setting to 0.
cfg.rssicut = 0; // [0-80%][0 Disables] Below that percentage rssi will show zero.
// cfg.spektrum_hires = 0;
cfg.rc_minchk = 1100;
cfg.rc_mid = 1500;
cfg.rc_maxchk = 1900;
cfg.rc_lowlat = 1; // [0 - 1] Default 1. 1 = lower latency, 0 = normal latency/more filtering.
cfg.rc_rllrm = 0; // disable arm/disarm on roll left/right
cfg.rc_auxch = 4; // [4 - 6] cGiesen: Default = 4, then like the standard! Crashpilot: Limited to 6 aux for safety
cfg.rc_killt = 0; // Time in ms when your arm switch becomes a Killswitch, 0 disables the Killswitch, can not be used together with FEATURE_INFLIGHT_ACC_CAL
cfg.rc_flpsp = 0; // [0-3] When enabled(1) and upside down in acro or horizon mode throttle is reduced (see readme)
cfg.rc_motor = 0; // [0-2] Behaviour when thr < rc_minchk: 0= minthrottle no regulation, 1= minthrottle®ulation, 2= Motorstop
// Motor/ESC/Servo
cfg.esc_gain = 0; // [0Disables - 32] Gain for esc to reduce delay 16 = Gain of 1 that would double the initial response(limited to +500) Only helps unflashed ESC.
// cfg.esc_min = 1150; // ORIG
cfg.esc_min = 1100;
cfg.esc_max = 1950;
cfg.esc_moff = 1000;
cfg.esc_nfly = 1300; // This is the absolute throttle that kicks off the "has landed timer" if it is too low cfg.rc_minchk + 5% is taken. Also baselinethr for Autostart, also plausibility check for initial Failsafethrottle
// cfg.esc_nfly = 0; // This is the absolute throttle that kicks off the "has landed timer" if it is too low cfg.rc_minchk + 5% is taken.
cfg.esc_pwm = 400;
cfg.srv_pwm = 50;
cfg.pass_mot = 0; // Crashpilot: Only used with feature pass. If 0 = all Motors, otherwise specific Motor
// servos
cfg.tri_ydir = 1;
cfg.tri_ymid = 1500;
cfg.tri_ymin = 1020;
cfg.tri_ymax = 2000;
cfg.tri_ydel = 0; // [0-1000ms] Tri Yaw Arm delay: Time in ms after wich the yaw servo after arming will engage (useful with "yaw arm"). 0 disables Yawservo always active.
// flying wing
cfg.wing_left_min = 1020;
cfg.wing_left_mid = 1500;
cfg.wing_left_max = 2000;
cfg.wing_right_min = 1020;
cfg.wing_right_mid = 1500;
cfg.wing_right_max = 2000;
cfg.pitch_direction_l = 1;
cfg.pitch_direction_r = -1;
cfg.roll_direction_l = 1;
cfg.roll_direction_r = 1;
// gimbal
cfg.gbl_pgn = 10;
cfg.gbl_rgn = 10;
cfg.gbl_flg = GIMBAL_NORMAL;
cfg.gbl_pmn = 1020;
cfg.gbl_pmx = 2000;
cfg.gbl_pmd = 1500;
cfg.gbl_rmn = 1020;
cfg.gbl_rmx = 2000;
cfg.gbl_rmd = 1500;
// gps/nav
cfg.gps_type = 1; // GPS_NMEA = 0, GPS_UBLOX = 1, GPS_MTK16 = 2, GPS_MTK19 = 3, GPS_UBLOX_DUMB = 4
cfg.gps_baudrate = 115200; //38400; // Changed 8/6/13 to 115200;
cfg.gps_ins_vel = 0.6f; // Crashpilot GPS INS The LOWER the value the closer to gps speed // Dont go to high here
cfg.gps_lag = 2000; // GPS Lag in ms
cfg.gps_ph_minsat = 6; // Minimal Satcount for PH, PH on RTL is still done with 5Sats or more
cfg.gps_expo = 20; // 1 - 99 % defines the actual Expo applied for GPS
cfg.gps_ph_settlespeed = 10; // 1 - 200 cm/s PH settlespeed in cm/s
cfg.gps_ph_brakemaxangle = 15; // 1 - 45 Degree Maximal Overspeedbrake
cfg.gps_ph_minbrakepercent = 50; // 1 - 99% minimal percent of "brakemaxangle" left over for braking. Example brakemaxangle = 6 so 50 Percent is 3..
cfg.gps_ph_brkacc = 40; // [1 - 500] Is the assumed negative braking acceleration in cm/(s*s) of copter. Value is positive though. It will be a timeout. The lower the Value the longe the Timeout
cfg.gps_maxangle = 35; // 10 - 45 Degree Maximal over all GPS bank angle
cfg.gps_wp_radius = 200;
// cfg.rtl_mnh = 20; // (0 - 200m) Minimal RTL hight in m, 0 disables feature
cfg.rtl_mnh = 0; // (0 - 200m) Minimal RTL hight in m, 0 disables feature
cfg.rtl_cr = 80; // [10 - 200cm/s] When rtl_mnh is defined this is the climbrate in cm/s
cfg.rtl_mnd = 0; // 0 Disables. Minimal distance for RTL in m, otherwise it will just autoland, prevent Failsafe jump in your face, when arming copter and turning off TX
cfg.gps_rtl_flyaway = 0; // [0 - 100m] 0 Disables. If during RTL the distance increases beyond this value (in meters relative to RTL activation point), something is wrong, autoland
cfg.gps_yaw = 30; // Thats the MAG P during GPS functions, substitute for "cfg.P8[PIDMAG]"
cfg.nav_rtl_lastturn = 1; // 1 = when copter gets to home position it rotates it's head to takeoff direction independend of nav_controls_heading
cfg.nav_tail_first = 0; // 1 = Copter comes back with ass first (only works with nav_controls_heading = 1)
cfg.nav_controls_heading = 0; // 1 = Nav controls YAW during WP ONLY
// cfg.nav_controls_heading = 1; // 1 = Nav controls YAW during WP ONLY
cfg.nav_speed_min = 100; // 10 - 200 cm/s don't set higher than nav_speed_max! That dumbness is not covered.
cfg.nav_speed_max = 350; // 50 - 2000 cm/s don't set lower than nav_speed_min! That dumbness is not covered.
cfg.nav_approachdiv = 3; // 2 - 10 This is the divisor for approach speed for wp_distance. Example: 400cm / 3 = 133cm/s if below nav_speed_min it will be adjusted
cfg.nav_tiltcomp = 30; // 0 - 100 (20 TestDefault) Only arducopter really knows. Dfault was 54. This is some kind of a hack of them to reach actual nav_speed_max. 54 was Dfault, 0 disables
cfg.nav_ctrkgain = 0.5f; // 0 - 10.0 (0.5 TestDefault) (Floatvariable) That is the "Crosstrackgain" APM Dfault is "1". "0" disables
// Failsafe Variables
cfg.fs_delay = 10; // in 0.1s (10 = 1sec)
cfg.fs_ofdel = 200; // in 0.1s (200 = 20sec)
cfg.fs_rcthr = 1200; // decent Dfault which should always be below hover throttle for people.
cfg.fs_ddplt = 0; // EXPERIMENTAL Time in sec when FS is engaged after idle on THR/YAW/ROLL/PITCH, 0 disables max 250
cfg.fs_jstph = 0; // Does just PH&Autoland an not RTL, use this in difficult areas with many obstacles to avoid RTL crash into something
cfg.fs_nosnr = 1; // When snr_land is set to 1, it is possible to ignore that on Failsafe, because FS over a tree could turn off copter
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
cfg.tele_prot = 0; // Protocol ONLY used when Armed including Baudchange if necessary. 0 (Dfault)=Keep Multiwii @CurrentUSB Baud, 1=Frsky @9600Baud, 2=Mavlink @CurrentUSB Baud, 3=Mavlink @57KBaud (like stock minimOSD wants it)
// LED Stuff
cfg.LED_invert = 0; // Crashpilot: Inversion of LED 0&1 Partly implemented because Bootup is not affected
cfg.LED_Type = 1; // 1=MWCRGB / 2=MONO_LED / 3=LEDRing
cfg.LED_Pinout = 1; // rc6
cfg.LED_ControlChannel = 8; // AUX4 (Channel 8)
cfg.LED_Armed = 0; // 0 = Show LED only if armed, 1 = always show LED
cfg.LED_Pattern1 = 1300; // 32bit bit pattern to have flickering led patterns / the pattern for MWCRGB 1000-2000
cfg.LED_Pattern2 = 1800; // 32bit bit pattern to have flickering led patterns / the pattern for MWCRGB 1000-2000
cfg.LED_Pattern3 = 1900; // 32bit bit pattern to have flickering led patterns / the pattern for MWCRGB 1000-2000
cfg.LED_Toggle_Delay1 = 0x08; // slow down LED_Pattern
cfg.LED_Toggle_Delay2 = 0x08; // slow down LED_Pattern
cfg.LED_Toggle_Delay3 = 0x08; // slow down LED_Pattern
// SONAR
// SOME INFO ON SONAR:
// PWM56 are 5V resistant, RC78 only tolerate 3.3V(!!) so add a 1K Ohms resistor!!!
// Note: You will never see the maximum possible sonar range in a copter, so go for the half of it (or less?)
//
// Connection possibilities depending on Receivertype:
// PPSUM: RC78 possible, PWM56 possible (on max. quadcopters, see below)
// Normal RX: Just Connection on Motorchannel 5&6 (PWM56) is possible.
// The PWM56 sonar connection option is only available in setups with max motors 4, otherwise sonar is not initialized.
//
// HC-SR04:
// Operation Voltage: 5V (!! Use PWM56 or 1K resistor !!)
// Range: 2cm - 400cm
// Angle: 15 Degrees (Test out for yourself: cfg.snr_tilt = X)
//
// Maxbotix in general
// Operation Voltage: (some 2.5V)3.3V - 5V ((!! Use PWM56 or resistor with 5V !!)
// Only wire the Maxbotics for PWM output (more precise anyway), not the analog etc. modes, just wire echopin (normally pin 2)
// Range: 20cm(!) - 765cm (some >1000cm), MaxTiltAngle is not specified, depending on Model
// Tested on MB1200 XL-MaxSonar-EZ0
//
// I2C sonar in general (by mj666)
// If operation voltage of the sonar sensor is 5 Volt (NAZE I2C is 3.3 Volt), take care they do not have pull up resistors connected to 5 Volt.
// Outputs are always open drain so there is no risk kill something only signals may be critical so keep wires short as possible.
// Maxbotix I2CXL series operates with 3.3 and 5 Volt but 5Volt are preferred for best performance and stability.
//
// Devantech Ltd. (SRF02, SRF235, SRF08, SRF10):
// Type; Range; Cycletime; Angle; Comment
// SRF02; 16 to 600cm; 65ms; 55 degree; automatic calibration, minimum rage can be read from sensor (not implemented)
// SRF235; 10 - 1200cm; 10ms; 15 degree; angle is may be to small for the use case
// SRF08; 3 - 600cm; 65ms; 55 degree; range, gain an cycletime can be adjusted, multiple echos are measured (both not implemented)
// SRF10; 6 - 600cm; 65ms; 72 degree; range, gain an cycletime can be adjusted (not implemented)
// be sure to adjust settings accordingly, no additional test are done.
// more details at: http://www.robot-electronics.co.uk/index.html
//
// Maxbotix I2CXL (MB1202, MB1212, MB1222, MB1232, MB1242)
// I"CXL Series of sensors only differentiated by the beam pattern and sensibility. Maxbotix is recommending the MX1242 for quadcopter applications. The interface is always the same
// NOTE: Maxbotix Sonars only operate with lower I2C speed, so the speed is changed on the fly during Maxbotix readout.
// Thanks must go to mj666 for implementing that!
// GENERAL WARNING: DON'T SET snr_min TOO LOW, OTHERWISE THE WRONG SONARVALUE WILL BE TAKEN AS REAL MEASUREMENT!!
// I implemented some checks to prevent that user error, but still keep that in mind.
// Min/Max are checked and changed if they are too stupid for your sonar. So if you suddenly see other values, thats not an eeprom error or so.
// MAXBOTICS: SET snr_min to at least 30! I check this in sensors and change the value, if needed.
// NOTE: I limited Maxbotics to 7 meters in the code, knowing that some types will do >10m, if you have one of them 7m is still the limit for you.
// HC-SR04: SET snr_min to at least 10 ! I check this in sensors and change the value, if needed.
// DaddyWalross Sonar: I DON'T KNOW! But it uses HC-SR04 so i apply the same limits (10cm-400cm) to its output
// Sonar minimal hight must be higher (including temperature difference) than the physical lower limit of the sensor to do a proximity alert
// NOTE: Sonar is def. not a must - have. But nice to have.
cfg.snr_type = 3; // 0=PWM56 HC-SR04, 1=RC78 HC-SR04, 2=I2C(DaddyWalross), 3=MBPWM56, 4=MBRC78, 5=I2C(SRFxx), 6=I2C (MX12x2)
cfg.snr_min = 30; // Valid Sonar minimal range in cm (10 - 200) see warning above
cfg.snr_max = 200; // Valid Sonar maximal range in cm (50 - 700)
cfg.snr_dbg = 0; // 1 Sends Sonardata (within defined range and tilt) to debug[0] and tiltvalue to debug[1], debug[0] will be -1 if out of range/tilt. debug[2] contains raw sonaralt, like before
cfg.snr_tilt = 18; // Somehow copter tiltrange in degrees (Not exactly but good enough. Value * 0.9 = realtilt) in wich Sonar is possible
cfg.snr_cf = 0.5f; // The bigger, the more Sonarinfluence, makes switch between Baro/Sonar smoother and defines baroinfluence when sonarcontact. 1.0f just takes Sonar, if contact (otherwise baro)
cfg.snr_diff = 0; // 0 disables that check. Range (0-200) Maximal allowed difference in cm between sonar readouts (100ms rate and snr_diff = 50 means max 5m/s)
cfg.snr_land = 1; // Aided Sonar - landing, by setting upper throttle limit to current throttle. - Beware of Trees!! Can be disabled for Failsafe with fs_nosnr = 1
cfg.FDUsedDatasets = 0; // Default no Datasets stored
cfg.stat_clear = 1; // This will clear the stats between flights, or you can set to 0 and treasue overallstats, but you have to write manually eeprom or have logging enabled
cfg.sens_1G = 1; // Just feed a dummy "1" to avoid div by zero
ClearStats();
for (i = 0; i < MAX_MOTORS; i++) cfg.customMixer[i].throttle = 0.0f;// custom mixer. clear by Dfaults.
writeParams(0);
}
static void Mag_Calibration(void) // Called from XHz loop normally....
{
#define MAGmaxcount 500 // Take 500 samples at 10Hz rate i.e 50Sec
#define MAGerror 10000
#define MAGdiscardcnt 50
#define sflsdelta 0.0f
#define maxiterations 100
float xyz[3], x_sumplain = 0.0f, x_sumsq = 0.0f, x_sumcube = 0.0f, y_sumplain = 0.0f, y_sumsq = 0.0f;
float y_sumcube = 0.0f, z_sumplain = 0.0f, z_sumsq = 0.0f, z_sumcube = 0.0f, xy_sum = 0.0f;
float xz_sum = 0.0f, yz_sum = 0.0f, x2y_sum = 0.0f, x2z_sum = 0.0f, y2x_sum = 0.0f, y2z_sum = 0.0f;
float z2x_sum = 0.0f, z2y_sum = 0.0f, x2, y2, z2, x_sum, x_sum2, x_sum3, y_sum, y_sum2, y_sum3, z_sum;
float z_sum2, z_sum3, XY, XZ, YZ, X2Y, X2Z, Y2X, Y2Z, Z2X, Z2Y, F0, F1, F2, F3, F4, A, B, C, A2, B2;
float C2, QS, QB, Rsq, Q0, Q1, Q2, aA, aB, aC, nA, nB, nC, dA, dB, dC, fltsize = (float)MAGmaxcount;
uint16_t i, gathercnt = (uint16_t)cfg.mag_time * 5;
LD0_OFF(); // Green LED OFF
LD1_ON(); // Red LED ON
Mag_42Hz_AVG(xyz, MAGdiscardcnt); // Discard some
for (i = 0; i < MAGmaxcount; i++) // Gather up Mag Data. Freeze FC. Adjust Mag readout JUST by GAIN/SCALE
{
Mag_42Hz_AVG(xyz, gathercnt);
x2 = xyz[0] * xyz[0]; // http://imaginaryz.blogspot.de/2011/04/least-squares-fit-sphere-to-3d-data.html
y2 = xyz[1] * xyz[1];
z2 = xyz[2] * xyz[2];
x_sumplain += xyz[0];
x_sumsq += x2;
x_sumcube += x2 * xyz[0];
y_sumplain += xyz[1];
y_sumsq += y2;
y_sumcube += y2 * xyz[1];
z_sumplain += xyz[2];
z_sumsq += z2;
z_sumcube += z2 * xyz[2];
xy_sum += xyz[0] * xyz[1];
xz_sum += xyz[0] * xyz[2];
yz_sum += xyz[1] * xyz[2];
x2y_sum += x2 * xyz[1];
x2z_sum += x2 * xyz[2];
y2x_sum += y2 * xyz[0];
y2z_sum += y2 * xyz[2];
z2x_sum += z2 * xyz[0];
z2y_sum += z2 * xyz[1];
LED0_TOGGLE
LED1_TOGGLE
}
x_sum = x_sumplain / fltsize;
x_sum2 = x_sumsq / fltsize;
x_sum3 = x_sumcube / fltsize;
y_sum = y_sumplain / fltsize;
y_sum2 = y_sumsq / fltsize;
y_sum3 = y_sumcube / fltsize;
z_sum = z_sumplain / fltsize;
z_sum2 = z_sumsq / fltsize;
z_sum3 = z_sumcube / fltsize;
XY = xy_sum / fltsize;
XZ = xz_sum / fltsize;
YZ = yz_sum / fltsize;
X2Y = x2y_sum / fltsize;
X2Z = x2z_sum / fltsize;
Y2X = y2x_sum / fltsize;
Y2Z = y2z_sum / fltsize;
Z2X = z2x_sum / fltsize;
Z2Y = z2y_sum / fltsize;
F0 = x_sum2 + y_sum2 + z_sum2;
F1 = 0.5f * F0;
F2 = -8.0f * (x_sum3 + Y2X + Z2X);
F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
F4 = -8.0f * (X2Z + Y2Z + z_sum3);
A = x_sum;
B = y_sum;
C = z_sum;
A2 = A * A;
B2 = B * B;
C2 = C * C;
QS = A2 + B2 + C2;
QB = -2.0f * QS;
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * (QS - Rsq + QB + F0);
i = 0;
while (i < maxiterations)
{
i++;
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
if ((dA * dA + dB * dB + dC * dC) <= sflsdelta) break;
A = nA;
B = nB;
C = nC;
A2 = A * A;
B2 = B * B;
C2 = C * C;
QS = A2 + B2 + C2;
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * (QS - Rsq + QB + F0);
}
cfg.magZero[0] = A;
cfg.magZero[1] = B;
cfg.magZero[2] = C;
cfg.mag_calibrated = 1;
for (i = 0; i < 3; i++)
{
if (fabs(cfg.magZero[i]) > MAGerror)
{
cfg.mag_calibrated = 0; // Supress GPS functions & Guicrazymag
cfg.magZero[i] = 0;
}
}
writeParams(1); // Calibration done, save whatever result
systemReset(false);
}
static void evaluateCommand(void)
{
uint32_t i;
uint8_t wp_no;
switch (cmdMSP) {
case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++)
rcData[i] = read16();
headSerialReply(0);
break;
case MSP_SET_RAW_GPS:
f.GPS_FIX = read8();
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0);
break;
case MSP_SET_PID:
for (i = 0; i < PIDITEMS; i++) {
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
headSerialReply(0);
break;
case MSP_SET_BOX:
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = read16();
headSerialReply(0);
break;
case MSP_SET_RC_TUNING:
cfg.rcRate8 = read8();
cfg.rcExpo8 = read8();
cfg.rollPitchRate = read8();
cfg.yawRate = read8();
cfg.dynThrPID = read8();
cfg.thrMid8 = read8();
cfg.thrExpo8 = read8();
headSerialReply(0);
break;
case MSP_SET_MISC:
headSerialReply(0);
break;
case MSP_IDENT:
headSerialReply(7);
serialize8(VERSION); // multiwii version
serialize8(cfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability"
break;
case MSP_STATUS:
headSerialReply(10);
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3);
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
break;
case MSP_RAW_IMU:
headSerialReply(18);
for (i = 0; i < 3; i++)
serialize16(accSmooth[i]);
for (i = 0; i < 3; i++)
serialize16(gyroData[i]);
for (i = 0; i < 3; i++)
serialize16(magADC[i]);
break;
case MSP_SERVO:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(servo[i]);
break;
case MSP_MOTOR:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(motor[i]);
break;
case MSP_RC:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(rcData[i]);
break;
case MSP_RAW_GPS:
headSerialReply(14);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
break;
case MSP_ATTITUDE:
headSerialReply(8);
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize16(heading);
serialize16(headFreeModeHold);
break;
case MSP_ALTITUDE:
headSerialReply(4);
serialize32(EstAlt);
break;
case MSP_BAT:
headSerialReply(3);
serialize8(vbat);
serialize16(0); // power meter trash
break;
case MSP_RC_TUNING:
headSerialReply(7);
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
serialize8(cfg.thrMid8);
serialize8(cfg.thrExpo8);
break;
case MSP_PID:
headSerialReply(3 * PIDITEMS);
for (i = 0; i < PIDITEMS; i++) {
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
break;
case MSP_BOX:
headSerialReply(2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++)
serialize16(cfg.activate[i]);
break;
case MSP_BOXNAMES:
headSerialReply(sizeof(boxnames) - 1);
serializeNames(boxnames);
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_MISC:
headSerialReply(2);
serialize16(0); // intPowerTrigger1
break;
case MSP_MOTOR_PINS:
headSerialReply(8);
for (i = 0; i < 8; i++)
serialize8(i + 1);
break;
case MSP_WP:
wp_no = read8(); // get the wp number
headSerialReply(12);
if (wp_no == 0) {
serialize8(0); // wp0
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
} else if (wp_no == 16) {
serialize8(16); // wp16
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
break;
case MSP_RESET_CONF:
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
calibratingA = 400;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
case MSP_EEPROM_WRITE:
writeParams(0);
headSerialReply(0);
break;
case MSP_DEBUG:
headSerialReply(8);
for (i = 0; i < 4; i++)
serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
int main(){
int status = EXIT_SUCCESS;
// read in from parameter file, params.in
if( EXIT_SUCCESS != (status = readParams()))
return status;
// Create arrays for data
double r[N]; // radial position
double ff[N]; // test function
double FF; // integral
// Intialize r and ff (begign for ff)
if(EXIT_SUCCESS != (status = initialize(r,ff)))
return status;
// print Parameters we'll use
if(EXIT_SUCCESS != (status = writeParams()))
return status;
/*
// ---------------------------------------- integral tests ...
double real,perDiff;
// test function f(r) = r
for( int j = 0 ; j < N ; j++ )
ff[j] = r[j]; // f(r) = r
FF = simpsonInt(ff);
real = .5*(rMax*rMax - rMin*rMin);
perDiff = (FF-real)/(real)*100.0;
fprintf(stderr,"f(x) = x\n\t>>> percent diff: %g\n",perDiff);
// test function f(r) = -(r-1)(r-3)
for( int j = 0 ; j < N ; j++ )
ff[j] = -(r[j]-1.0)*(r[j]-3.0);
FF = simpsonInt(ff);
real = -1.0/3.0*(pow(rMax,3)-pow(rMin,3))
+2.0/1.0*(pow(rMax,2)-pow(rMin,2))
-3.0/1.0*(pow(rMax,1)-pow(rMin,1));
perDiff = (FF-real)/(real)*100.0;
fprintf(stderr,"f(x) = -(x-1)(x-3)\n\t>>> percent diff: %g\n",perDiff);
// test function f(r) = -r(r-1)(r-2)(r-3)
for( int j = 0 ; j < N ; j++ )
ff[j] = -1.0*r[j]*(r[j]-1.0)*(r[j]-2.0)*(r[j]-3.0);
FF = simpsonInt(ff);
real = -1.0/5.0*(pow(rMax,5)-pow(rMin,5))
+3.0/2.0*(pow(rMax,4)-pow(rMin,4))
-11./3.0*(pow(rMax,3)-pow(rMin,3))
+3.00000*(pow(rMax,2)-pow(rMin,2));
perDiff = (FF-real)/(real)*100.0;
fprintf(stderr,"f(x) = -x(x-1)(x-2)(x-3)\n\t>>> percent diff: %g\n",perDiff);
// test torque profile:
for( int j = 0 ; j < N ; j++ )
ff[j] = tidalTorque(r[j],a,h(r[j]));
FF = simpsonInt(ff);
fprintf(stderr,"TORQUE PROFILE:\n\t>>> %g\n",FF);
*/
/*
// Test new Bode for torque profile
for( int j=0;j<N;j++){
ff[j] = 1.0;
}
FF = bodeInterpIntWithTorque(r,ff);
fprintf(stderr,"TORQUE PROFILE:\n\t>>> %g\n",FF);
*/
// Test Gaussian Quadrature version:
// fprintf(stderr,"\t\t>> Result of 16-point Gauss Quadrature scheme: %g\n",gaussIntWithTorque(16));
// fprintf(stderr,"\t\t>> Result of 48-point Gauss Quadrature scheme: %g\n",gaussIntWithTorque(48));
// fprintf(stderr,"\t\t>> Result of 96-point Gauss Quadrature scheme: %g\n",gaussIntWithTorque(96));
// fprintf(stderr,"\t\t>> Result of 100-point Gauss Quadrature scheme: %g\n",gaussIntWithTorque(100));
// test speed of this thing:
for( int i = 0 ; i < 1000000 ; i++ )
gaussIntWithTorque(96);
// print last file:
if(EXIT_SUCCESS != (status = writeStandard(-1,r,ff,a,0.0)))
return status;
return status;
} // end main
void evaluateCommand() {
uint32_t tmp=0;
switch(cmdMSP[CURRENTPORT]) {
case MSP_PRIVATE:
//headSerialError();tailSerialReply(); // we don't have any custom msp currently, so tell the gui we do not use that
break;
case MSP_SET_RAW_RC:
s_struct_w((uint8_t*)&rcSerial,16);
rcSerialCount = 50; // 1s transition
break;
case MSP_SET_PID:
s_struct_w((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
break;
case MSP_SET_BOX:
#if EXTAUX
s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*4);
#else
s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*2);
#endif
break;
case MSP_SET_RC_TUNING:
s_struct_w((uint8_t*)&conf.rcRate8,7);
break;
#if !defined(DISABLE_SETTINGS_TAB)
case MSP_SET_MISC:
struct {
uint16_t a,b,c,d,e,f;
uint32_t g;
uint16_t h;
uint8_t i,j,k,l;
} set_misc;
s_struct_w((uint8_t*)&set_misc,22);
#if defined(POWERMETER)
conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
#endif
conf.minthrottle = set_misc.b;
#ifdef FAILSAFE
conf.failsafe_throttle = set_misc.e;
#endif
#if MAG
conf.mag_declination = set_misc.h;
#endif
#if defined(VBAT)
conf.vbatscale = set_misc.i;
conf.vbatlevel_warn1 = set_misc.j;
conf.vbatlevel_warn2 = set_misc.k;
conf.vbatlevel_crit = set_misc.l;
#endif
break;
case MSP_MISC:
struct {
uint16_t a,b,c,d,e,f;
uint32_t g;
uint16_t h;
uint8_t i,j,k,l;
} misc;
misc.a = intPowerTrigger1;
misc.b = conf.minthrottle;
misc.c = MAXTHROTTLE;
misc.d = MINCOMMAND;
#ifdef FAILSAFE
misc.e = conf.failsafe_throttle;
#else
misc.e = 0;
#endif
#ifdef LOG_PERMANENT
misc.f = plog.arm;
misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
#else
misc.f = 0; misc.g =0;
#endif
#if MAG
misc.h = conf.mag_declination;
#else
misc.h = 0;
#endif
#ifdef VBAT
misc.i = conf.vbatscale;
misc.j = conf.vbatlevel_warn1;
misc.k = conf.vbatlevel_warn2;
misc.l = conf.vbatlevel_crit;
#else
misc.i = 0;misc.j = 0;misc.k = 0;misc.l = 0;
#endif
s_struct((uint8_t*)&misc,22);
break;
#endif
#if defined (DYNBALANCE)
case MSP_SET_MOTOR:
s_struct_w((uint8_t*)&motor,16);
break;
#endif
#ifdef MULTIPLE_CONFIGURATION_PROFILES
case MSP_SELECT_SETTING:
if(!f.ARMED) {
global_conf.currentSet = read8();
if(global_conf.currentSet>2) global_conf.currentSet = 0;
writeGlobalSet(0);
readEEPROM();
}
//headSerialReply(0);tailSerialReply();
break;
#endif
case MSP_SET_HEAD:
s_struct_w((uint8_t*)&magHold,2);
break;
case MSP_IDENT:
struct {
uint8_t v,t,msp_v;
uint32_t cap;
} id;
id.v = VERSION;
id.t = MULTITYPE;
id.msp_v = MSP_VERSION;
id.cap = capability|DYNBAL<<2|FLAP<<3|NAVCAP<<4|EXTAUX<<5|((uint32_t)NAVI_VERSION<<28); //Navi version is stored in the upper four bits;
s_struct((uint8_t*)&id,7);
break;
case MSP_STATUS:
struct {
uint16_t cycleTime,i2c_errors_count,sensor;
uint32_t flag;
uint8_t set;
} st;
st.cycleTime = cycleTime;
st.i2c_errors_count = i2c_errors_count;
st.sensor = ACC|BARO<<1|MAG<<2|GPS<<3|SONAR<<4;
#if ACC
if(f.ANGLE_MODE) tmp |= 1<<BOXANGLE;
if(f.HORIZON_MODE) tmp |= 1<<BOXHORIZON;
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
if(f.BARO_MODE) tmp |= 1<<BOXBARO;
#endif
if(f.MAG_MODE) tmp |= 1<<BOXMAG;
#if !defined(FIXEDWING)
#if defined(HEADFREE)
if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
#endif
#if defined(CAMTRIG)
if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
#endif
#if GPS
switch (f.GPS_mode) {
case GPS_MODE_HOLD:
tmp |= 1<<BOXGPSHOLD;
break;
case GPS_MODE_RTH:
tmp |= 1<<BOXGPSHOME;
break;
case GPS_MODE_NAV:
tmp |= 1<<BOXGPSNAV;
break;
}
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
#endif
#if defined(BUZZER)
if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
#endif
#if defined(LED_FLASHER)
if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
#endif
#if defined(LANDING_LIGHTS_DDR)
if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
#endif
#if defined(VARIOMETER)
if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if(rcOptions[BOXCALIB]) tmp |= 1<<BOXCALIB;
#endif
#if defined(GOVERNOR_P)
if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
#endif
#if defined(OSD_SWITCH)
if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
if(f.ARMED) tmp |= 1<<BOXARM;
st.flag = tmp;
st.set = global_conf.currentSet;
s_struct((uint8_t*)&st,11);
break;
case MSP_RAW_IMU:
#if defined(DYNBALANCE)
for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
#endif
s_struct((uint8_t*)&imu,18);
break;
case MSP_SERVO:
s_struct((uint8_t*)&servo,16);
break;
case MSP_SERVO_CONF:
s_struct((uint8_t*)&conf.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56
break;
case MSP_SET_SERVO_CONF:
s_struct_w((uint8_t*)&conf.servoConf[0].min,56);
break;
case MSP_MOTOR:
s_struct((uint8_t*)&motor,16);
break;
case MSP_ACC_TRIM:
s_struct((uint8_t*)&conf.angleTrim[0],4);
break;
case MSP_SET_ACC_TRIM:
s_struct_w((uint8_t*)&conf.angleTrim[0],4);
break;
case MSP_RC:
s_struct((uint8_t*)&rcData,RC_CHANS*2);
break;
#if GPS
case MSP_SET_RAW_GPS:
struct {
uint8_t a,b;
int32_t c,d;
int16_t e;
uint16_t f;
} set_set_raw_gps;
s_struct_w((uint8_t*)&set_set_raw_gps,14);
f.GPS_FIX = set_set_raw_gps.a;
GPS_numSat = set_set_raw_gps.b;
GPS_coord[LAT] = set_set_raw_gps.c;
GPS_coord[LON] = set_set_raw_gps.d;
GPS_altitude = set_set_raw_gps.e;
GPS_speed = set_set_raw_gps.f;
GPS_update |= 2; // New data signalisation to GPS functions
break;
case MSP_RAW_GPS:
struct {
uint8_t a,b;
int32_t c,d;
int16_t e;
uint16_t f,g;
} msp_raw_gps;
msp_raw_gps.a = f.GPS_FIX;
msp_raw_gps.b = GPS_numSat;
msp_raw_gps.c = GPS_coord[LAT];
msp_raw_gps.d = GPS_coord[LON];
msp_raw_gps.e = GPS_altitude;
msp_raw_gps.f = GPS_speed;
msp_raw_gps.g = GPS_ground_course;
s_struct((uint8_t*)&msp_raw_gps,16);
break;
case MSP_COMP_GPS:
struct {
uint16_t a;
int16_t b;
uint8_t c;
} msp_comp_gps;
msp_comp_gps.a = GPS_distanceToHome;
msp_comp_gps.b = GPS_directionToHome;
msp_comp_gps.c = GPS_update & 1;
s_struct((uint8_t*)&msp_comp_gps,5);
break;
#if defined(USE_MSP_WP)
case MSP_SET_NAV_CONFIG:
s_struct_w((uint8_t*)&GPS_conf,sizeof(GPS_conf));
break;
case MSP_NAV_CONFIG:
s_struct((uint8_t*)&GPS_conf,sizeof(GPS_conf));
break;
case MSP_NAV_STATUS: // to move to struct transmission
headSerialReply(7);
serialize8(f.GPS_mode);
serialize8(NAV_state);
serialize8(mission_step.action);
serialize8(mission_step.number);
serialize8(NAV_error);
serialize16( (int16_t)(target_bearing/100));
//serialize16(magHold);
tailSerialReply();
break;
case MSP_WP: // to move to struct transmission
{
uint8_t wp_no;
uint8_t flag;
bool success;
wp_no = read8(); //get the wp number
headSerialReply(21);
if (wp_no == 0) { //Get HOME coordinates
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
flag = MISSION_FLAG_HOME;
}
if (wp_no == 255) { //Get poshold coordinates
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
flag = MISSION_FLAG_HOLD;
}
if ((wp_no>0) && (wp_no<255)) {
if (NAV_state == NAV_STATE_NONE) {
success = recallWP(wp_no);
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(mission_step.pos[LAT]);
serialize32(mission_step.pos[LON]);
if (success == true) flag = mission_step.flag;
else flag = MISSION_FLAG_CRC_ERROR; //CRC error
} else {
serialize8(wp_no);
serialize8(0);
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
flag = MISSION_FLAG_NAV_IN_PROG;
}
}
serialize32(mission_step.altitude);
serialize16(mission_step.parameter1);
serialize16(mission_step.parameter2);
serialize16(mission_step.parameter3);
serialize8(flag);
tailSerialReply();
}
break;
case MSP_SET_WP: // to move to struct transmission
//TODO: add I2C_gps handling
{
uint8_t wp_no = read8(); //Get the step number
if (NAV_state == NAV_STATE_HOLD_INFINIT && wp_no == 255) { //Special case - during stable poshold we allow change the hold position
mission_step.number = wp_no;
mission_step.action = MISSION_HOLD_UNLIM;
uint8_t temp = read8();
mission_step.pos[LAT] = read32();
mission_step.pos[LON] = read32();
mission_step.altitude = read32();
mission_step.parameter1 = read16();
mission_step.parameter2 = read16();
mission_step.parameter3 = read16();
mission_step.flag = read8();
if (mission_step.altitude != 0) set_new_altitude(mission_step.altitude);
GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
if ((wp_distance/100) >= GPS_conf.safe_wp_distance) NAV_state = NAV_STATE_NONE;
else NAV_state = NAV_STATE_WP_ENROUTE;
break;
}
if (NAV_state == NAV_STATE_NONE) { // The Nav state is not zero, so navigation is in progress, silently ignore SET_WP command)
mission_step.number = wp_no;
mission_step.action = read8();
mission_step.pos[LAT] = read32();
mission_step.pos[LON] = read32();
mission_step.altitude = read32();
mission_step.parameter1 = read16();
mission_step.parameter2 = read16();
mission_step.parameter3 = read16();
mission_step.flag = read8();
//It's not sure, that we want to do poshold change via mission planner so perhaps the next if is deletable
/*
if (mission_step.number == 255) //Set up new hold position via mission planner, It must set the action to MISSION_HOLD_INFINIT
{
if (mission_step.altitude !=0) set_new_altitude(mission_step.altitude); //Set the altitude
GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
NAV_state = NAV_STATE_WP_ENROUTE; //Go to that position, then it will switch to poshold unlimited when reached
}
*/
if (mission_step.number == 0) { //Set new Home position
GPS_home[LAT] = mission_step.pos[LAT];
GPS_home[LON] = mission_step.pos[LON];
}
if (mission_step.number >0 && mission_step.number<255) //Not home and not poshold, we are free to store it in the eprom
if (mission_step.number <= getMaxWPNumber()) // Do not thrash the EEPROM with invalid wp number
storeWP();
//headSerialReply(0);tailSerialReply();
}
}
break;
#endif //USE_MSP_WP
#endif //GPS
case MSP_ATTITUDE:
s_struct((uint8_t*)&att,6);
break;
case MSP_ALTITUDE:
s_struct((uint8_t*)&alt,6);
break;
case MSP_ANALOG:
s_struct((uint8_t*)&analog,7);
break;
case MSP_RC_TUNING:
s_struct((uint8_t*)&conf.rcRate8,7);
break;
case MSP_PID:
s_struct((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
break;
case MSP_PIDNAMES:
serializeNames(pidnames);
break;
case MSP_BOX:
#if EXTAUX
s_struct((uint8_t*)&conf.activate[0],4*CHECKBOXITEMS);
#else
s_struct((uint8_t*)&conf.activate[0],2*CHECKBOXITEMS);
#endif
break;
case MSP_BOXNAMES:
serializeNames(boxnames);
break;
case MSP_BOXIDS:
headSerialReply(CHECKBOXITEMS);
for(uint8_t i=0;i<CHECKBOXITEMS;i++)
serialize8(pgm_read_byte(&(boxids[i])));
tailSerialReply();
break;
case MSP_MOTOR_PINS:
s_struct((uint8_t*)&PWM_PIN,8);
break;
case MSP_RESET_CONF:
if(!f.ARMED) LoadDefaults();
//headSerialReply(0);tailSerialReply();
break;
case MSP_ACC_CALIBRATION:
if(!f.ARMED) calibratingA=512;
//headSerialReply(0);tailSerialReply();
break;
case MSP_MAG_CALIBRATION:
if(!f.ARMED) f.CALIBRATE_MAG = 1;
//headSerialReply(0);tailSerialReply();
break;
#if defined(SPEK_BIND)
case MSP_BIND:
spekBind();
//headSerialReply(0);tailSerialReply();
break;
#endif
case MSP_EEPROM_WRITE:
writeParams(0);
//headSerialReply(0);tailSerialReply();
break;
case MSP_DEBUG:
s_struct((uint8_t*)&debug,8);
break;
#ifdef DEBUGMSG
case MSP_DEBUGMSG:
{
uint8_t size = debugmsg_available();
if (size > 16) size = 16;
headSerialReply(size);
debugmsg_serialize(size);
tailSerialReply();
}
break;
#endif
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError();tailSerialReply();
break;
}
}
void checkFirstTime(bool reset)
{
uint8_t test_val, i;
test_val = *(uint8_t *) FLASH_WRITE_ADDR;
if (!reset && test_val == checkNewConf)
return;
// Default settings
cfg.version = checkNewConf;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
featureSet(FEATURE_VBAT);
cfg.looptime = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
cfg.P8[PITCH] = 40;
cfg.I8[PITCH] = 30;
cfg.D8[PITCH] = 23;
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 16;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 70;
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 20;
cfg.P8[PIDMAG] = 40;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 90;
cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
cfg.thrExpo8 = 0;
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = 0;
cfg.angleTrim[0] = 0;
cfg.angleTrim[1] = 0;
cfg.accZero[0] = 0;
cfg.accZero[1] = 0;
cfg.accZero[2] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
cfg.accz_deadband = 50;
cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42;
cfg.mpu6050_scale = 1; // fuck invensense
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
cfg.baro_cf = 0.985f;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
// Radio
parseRcChannels("AETR1234");
cfg.deadband = 0;
cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
cfg.spektrum_hires = 0;
cfg.midrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
// Motor/ESC/Servo
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
cfg.mincommand = 1000;
cfg.motor_pwm_rate = 400;
cfg.servo_pwm_rate = 50;
// servos
cfg.yaw_direction = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;
// flying wing
cfg.wing_left_min = 1020;
cfg.wing_left_mid = 1500;
cfg.wing_left_max = 2000;
cfg.wing_right_min = 1020;
cfg.wing_right_mid = 1500;
cfg.wing_right_max = 2000;
cfg.pitch_direction_l = 1;
cfg.pitch_direction_r = -1;
cfg.roll_direction_l = 1;
cfg.roll_direction_r = 1;
// gimbal
cfg.gimbal_pitch_gain = 10;
cfg.gimbal_roll_gain = 10;
cfg.gimbal_flags = GIMBAL_NORMAL;
cfg.gimbal_pitch_min = 1020;
cfg.gimbal_pitch_max = 2000;
cfg.gimbal_pitch_mid = 1500;
cfg.gimbal_roll_min = 1020;
cfg.gimbal_roll_max = 2000;
cfg.gimbal_roll_mid = 1500;
// gps/nav stuff
cfg.gps_type = GPS_NMEA;
cfg.gps_baudrate = 115200;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
cfg.nav_slew_rate = 30;
cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
// custom mixer. clear by defaults.
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
writeParams(0);
}
/*!
* @brief Initializes the whole system and runs the desired application
*
* This is the main function of the project. It calls initialization functions
* of the MCU and the sensors. In the infinite loop it repeatedly checks
* the USART module read buffer and Streams sensor data periodically (100 ms) via USART.
*
*/
int main(void)
{
/********************* Initialize global variables **********************/
bmf055_input_state = USART_INPUT_STATE_PRINT_DATA;
/************************* Initializations ******************************/
/*Initialize SAMD20 MCU*/
system_init();
/*Initialize clock module of SAMD20 MCU - Internal RC clock*/
//clock_initialize(); // done via conf_clocks.h --> ASF
/*SPI master for communicating with sensors*/
spi_initialize();
/*eeprom emulator for configuration storage */
eeprom_emulator_initialize();
/*Initialize timers */
tc_initialize();
/*Initialize UART for communication with PC*/
usart_initialize();
/*Enable the system interrupts*/
system_interrupt_enable_global();/* All interrupts have a priority of level 0 which is the highest. */
/* Initialize the sensors */
bmf055_sensors_initialize();
readEEPROM();
checkFirstTime(0);
//readEEPROM();
configureReceiver();
initSensors();
previousTime = micros();
calibratingG = 400;
f.SMALL_ANGLES_25=1; // important for gyro only conf
if(conf.copterType == 0){//0=Bi,1=Tri,2=QUADP,3=QUADX,4=Y4,5=Y6,6=H6P,7=H6X,8=Vtail4
MULTITYPE = 4;
NUMBER_MOTOR = 2;
}
if(conf.copterType == 1){
MULTITYPE = 1;
NUMBER_MOTOR = 3;
}
if(conf.copterType == 2){
MULTITYPE = 2;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 3){
MULTITYPE = 3;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 4){
MULTITYPE = 9;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 5){
MULTITYPE = 6;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 6){
MULTITYPE = 7;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 7){
MULTITYPE = 10;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 8){
MULTITYPE = 17;
NUMBER_MOTOR = 4;
}
initOutput();
/************************** Infinite Loop *******************************/
while (true)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t beepon = 0;
uint8_t axis,i;
int16_t error,errorAngle;
int16_t delta,deltaSum;
int16_t PTerm=0,ITerm=0,PTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0,DTerm=0;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorGyroI[3] = {0,0,0};
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static uint32_t BeepTime = 0;
static uint8_t stickarmed = 0;
//static int16_t initialThrottleHold;
if(!rcOptions[BOXARM] && stickarmed == 0 && f.ARMED == 0){
if(rcData[YAW]<conf.MINCHECK && rcData[ROLL]>conf.MAXCHECK){
conf.calibState=1;
writeParams(1);
while(true){
//blinkLED(10,30,1);
}
}
}
while(SetupMode == 1){
checkSetup();
}
if(conf.RxType == 1 || conf.RxType == 2){
if (rcFrameComplete) computeRC();
}
if(!rcOptions[BOXARM] && stickarmed == 0) {
f.ARMED = 0;
}
if (currentTime > rcTime ) { // 50Hz
rcTime = currentTime + 20000;
if(failsave < 250)failsave++;
debug[0] = failsave;
if(conf.RxType != 1 && conf.RxType != 2){
computeRC();
}
if ((rcData[THROTTLE] < conf.MINCHECK && s3D == 0) || (rcData[THROTTLE] > (1500-conf.MIDDLEDEADBAND) && rcData[THROTTLE] < (1500+conf.MIDDLEDEADBAND) && s3D == 1 && f.ARMED == 0)) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK && !f.ARMED) {
if (rcDelayCommand == 20 && failsave < 20) {
calibratingG=400;
}
}else if (rcData[YAW] > conf.MAXCHECK && rcData[PITCH] > conf.MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
previousTime = micros();
}
}else if (conf.activate[BOXARM] > 0) {
if ( rcOptions[BOXARM] && f.OK_TO_ARM && good_calib) {
f.ARMED = 1;
stickarmed = 0;
} else if (f.ARMED) f.ARMED = 0;
rcDelayCommand = 0;
} else if ( (rcData[YAW] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[YAW] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
if (rcDelayCommand == 20 && good_calib) {
f.ARMED = 1;
stickarmed = 1;
}
} else if ( (rcData[ROLL] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[ROLL] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
if (rcDelayCommand == 20 && good_calib) {
f.ARMED = 1;
stickarmed = 1;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > conf.MAXCHECK && !f.ARMED) {
if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20) calibratingA=400;
rcDelayCommand++;
} else if (rcData[PITCH] > conf.MAXCHECK) {
conf.angleTrim[PITCH]+=2;writeParams(1);
} else if (rcData[PITCH] < conf.MINCHECK) {
conf.angleTrim[PITCH]-=2;writeParams(1);
} else if (rcData[ROLL] > conf.MAXCHECK) {
conf.angleTrim[ROLL]+=2;writeParams(1);
} else if (rcData[ROLL] < conf.MINCHECK) {
conf.angleTrim[ROLL]-=2;writeParams(1);
} else {
rcDelayCommand = 0;
}
}
uint16_t auxState = 0;
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0;
if(failsave > 200 && f.ARMED){
rcOptions[BOXACC] = 1;
s3D = 0;
rcData[THROTTLE] = 1190;
rcCommand[THROTTLE] = 1190;
}
if (rcOptions[BOXACC] && s3D == 0) {
// bumpless transfer to Level mode
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1;
}
} else {
// failsafe support
f.ACC_MODE = 0;
}
if (rcOptions[BOXBEEP]) {
f.FSBEEP = 1;
if(currentTime > BeepTime){
BeepTime = currentTime + 50000;
if(beepon == 0){
if(conf.RxType == 0){
//digitalWrite(A2,HIGH);
}else{
//digitalWrite(8,HIGH);
}
beepon = 1;
}else{
if(conf.RxType == 0){
//digitalWrite(A2,LOW);
}else{
//digitalWrite(8,LOW);
}
beepon = 0;
}
}
} else {
f.FSBEEP = 0;
if(conf.RxType == 0){
//digitalWrite(A2,LOW);
}else{
//digitalWrite(8,LOW);
}
}
if (rcOptions[BOXHORIZON] && s3D == 0) {
// bumpless transfer to Horizon mode
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
} else {
f.HORIZON_MODE = 0;
}
if (rcOptions[BOX3D] && conf.F3D == 1) {
if(f.ARMED == 0 && s3D == 0){
s3D = 1;
f.ACC_MODE = 0;
f.HORIZON_MODE = 0;
}
} else if(f.ARMED == 0){
s3D = 0;
}
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
}
computeIMU();
int16_t prop;
if (f.HORIZON_MODE)
prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500]
if (f.ACC_MODE){
if(Zadd > 0)Zadd--;
if(Zadd < 0)Zadd++;
}else{
Zadd = 0;
}
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if ((f.ACC_MODE || f.HORIZON_MODE) && axis<2 ) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTermACC = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
#else
PTermACC = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
}
if ( !f.ACC_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
if (abs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if ( f.HORIZON_MODE && axis<2) {
PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500;
ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500;
} else {
if ( f.ACC_MODE && axis<2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
if (abs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}
// Default settings
static void resetConf(void)
{
int i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
//cfg.mixerConfiguration = MULTITYPE_QUADX;
cfg.mixerConfiguration = MULTITYPE_AIRPLANE;
featureClearAll();
//featureSet(FEATURE_VBAT); // Enable Vbat monitoring
//featureSet(FEATURE_PPM); // Enable CPPM input
cfg.looptime = 0;
cfg.P8[ROLL] = 20;
//cfg.I8[ROLL] = 30;
//cfg.D8[ROLL] = 23;
cfg.I8[ROLL] = 0;
cfg.D8[ROLL] = 0;
cfg.P8[PITCH] = 20;
//cfg.I8[PITCH] = 30;
//cfg.D8[PITCH] = 23;
cfg.I8[PITCH] = 0;
cfg.D8[PITCH] = 0;
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 0;
//cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 0;
cfg.I8[PIDALT] = 0;
cfg.D8[PIDALT] = 0;
cfg.P8[PIDPOS] = 0; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 0; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 0; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 0; // POSHOLD_RATE_D * 1000;
cfg.P8[PIDNAVR] = 0; // NAV_P * 10;
cfg.I8[PIDNAVR] = 0; // NAV_I * 100;
cfg.D8[PIDNAVR] = 0; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 20;
cfg.I8[PIDLEVEL] = 0;
cfg.D8[PIDLEVEL] = 100;
//cfg.P8[PIDLEVEL] = 70;
//cfg.I8[PIDLEVEL] = 10;
//cfg.D8[PIDLEVEL] = 20;
cfg.P8[PIDMAG] = 40;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 100;
cfg.rcExpo8 = 0;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
cfg.thrExpo8 = 0;
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = 0;
cfg.angleTrim[0] = 0;
cfg.angleTrim[1] = 0;
cfg.accZero[0] = 0;
cfg.accZero[1] = 0;
cfg.accZero[2] = 0;
// Magnetic declination: format is [sign]dddmm (degreesminutes) default is zero.
// +12deg 31min = 1231 Sydney, Australia
// -6deg 37min = -637 Southern Japan
// cfg.mag_declination = 0;
cfg.mag_declination = 1231;
memcpy(&cfg.align, default_align, sizeof(cfg.align));
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
cfg.accz_deadband = 50;
cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42;
cfg.mpu6050_scale = 1; // fuck invensense
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
cfg.baro_cf = 0.985f;
cfg.moron_threshold = 32;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
// cfg.power_adc_channel = 0;
// Radio
parseRcChannels("AETR1234");
//parseRcChannels("TAER1234"); // Debug - JR
cfg.deadband = 0;
cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
cfg.spektrum_hires = 0;
for (i = 0; i < 8; i++) // RC stick centers
{
cfg.midrc[i] = 1500;
}
cfg.defaultrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
// Motor/ESC/Servo
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
cfg.mincommand = 1000;
cfg.motor_pwm_rate = 400;
cfg.servo_pwm_rate = 50;
// servos
cfg.yaw_direction = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;
// gimbal
cfg.gimbal_pitch_gain = 10;
cfg.gimbal_roll_gain = 10;
cfg.gimbal_flags = GIMBAL_NORMAL;
cfg.gimbal_pitch_min = 1020;
cfg.gimbal_pitch_max = 2000;
cfg.gimbal_pitch_mid = 1500;
cfg.gimbal_roll_min = 1020;
cfg.gimbal_roll_max = 2000;
cfg.gimbal_roll_mid = 1500;
// gps/nav stuff
cfg.gps_type = GPS_NMEA;
cfg.gps_baudrate = 115200;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
cfg.nav_slew_rate = 30;
cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
// Aeroplane stuff
cfg.flapmode = ADV_FLAP; // Switch for flaperon mode?
cfg.flapchan = AUX2; // RC channel number for simple flaps)
cfg.aileron2 = AUX1; // RC channel number for second aileron
cfg.flapspeed = 10; // Desired rate of change of flaps
cfg.flapstep = 3; // Steps for each flap movement
cfg.DynPIDchan = THROTTLE; // Dynamic PID source channel
cfg.DynPIDbreakpoint = 1500; // Dynamic PID breakpoint
cfg.rollPIDpol = 1;
cfg.pitchPIDpol = 1;
cfg.yawPIDpol = 1;
for (i = 0; i < 8; i++) // Servo limits
{
cfg.servoendpoint_low[i] = 1000;
cfg.servoendpoint_high[i] = 2000;
cfg.servoreverse[i] = 1;
cfg.servotrim[i] = 1500; // Set servo trim to default
}
// custom mixer. clear by defaults.
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
writeParams(0);
}
void updateCommands(void)
{
uint8_t i;
static uint8_t commandDelay;
if(!featureGet(FEATURE_SPEKTRUM) || spektrumFrameComplete())
computeRC();
// Ground Routines
if(rcData[THROTTLE] < cfg.minCheck) {
zeroPIDs(); // Stops integrators from exploding on the ground
if(cfg.auxActivate[OPT_ARM] > 0) {
if(auxOptions[OPT_ARM] && mode.OK_TO_ARM) { // AUX Arming
mode.ARMED = 1;
headfreeReference = stateData.heading;
} else if(mode.ARMED){ // AUX Disarming
mode.ARMED = 0;
}
} else if(rcData[YAW] > cfg.maxCheck && !mode.ARMED) { // Stick Arming
if(commandDelay++ == 20) {
mode.ARMED = 1;
headfreeReference = stateData.heading;
}
} else if(rcData[YAW] < cfg.minCheck && mode.ARMED) { // Stick Disarming
if(commandDelay++ == 20) {
mode.ARMED = 0;
}
} else if(rcData[YAW] < cfg.minCheck && rcData[PITCH] > cfg.minCheck && !mode.ARMED) {
if(commandDelay++ == 20) {
computeGyroRTBias();
//pulseMotors(3);
// GPS Reset
}
} else {
commandDelay = 0;
}
} else if(rcData[THROTTLE] > cfg.maxCheck && !mode.ARMED) {
if(rcData[YAW] > cfg.maxCheck && rcData[PITCH] < cfg.minCheck) {
if(commandDelay++ == 20) {
magCalibration();
}
} else if(rcData[YAW] < cfg.minCheck && rcData[PITCH] < cfg.minCheck) {
if(commandDelay++ == 20) {
accelCalibration();
pulseMotors(3);
}
} else if (rcData[PITCH] > cfg.maxCheck) {
cfg.angleTrim[PITCH] += 0.01;
writeParams();
} else if (rcData[PITCH] < cfg.minCheck) {
cfg.angleTrim[PITCH] -= 0.01;
writeParams();
} else if (rcData[ROLL] > cfg.maxCheck) {
cfg.angleTrim[ROLL] += 0.01;
writeParams();
} else if (rcData[ROLL] < cfg.minCheck) {
cfg.angleTrim[ROLL] -= 0.01;
writeParams();
} else {
commandDelay = 0;
}
}
// Failsafe
if(featureGet(FEATURE_FAILSAFE)) {
if(failsafeCnt > cfg.failsafeOnDelay && mode.ARMED) {
// Stabilise and set Throttle to failsafe level
for(i = 0; i < 3; ++i) {
rcData[i] = cfg.midCommand;
}
rcData[THROTTLE] = cfg.failsafeThrottle;
mode.FAILSAFE = 1;
if(failsafeCnt > cfg.failsafeOffDelay + cfg.failsafeOnDelay) {
// Disarm
mode.ARMED = 0;
// you will have to switch off first to rearm
mode.OK_TO_ARM = 0;
}
if(failsafeCnt > cfg.failsafeOnDelay && !mode.ARMED) {
mode.ARMED = 0;
// you will have to switch off first to rearm
mode.OK_TO_ARM = 0;
}
++failsafeCnt;
} else {
mode.FAILSAFE = 0;
}
}
// Aux Options
uint16_t auxOptionMask = 0;
for(i = 0; i < AUX_CHANNELS; ++i) {
auxOptionMask |= (rcData[AUX1 + i] < cfg.minCheck) << (3 * i) |
(rcData[AUX1 + i] > cfg.minCheck && rcData[i] < cfg.minCheck) << (3 * i + 1) |
(rcData[AUX1 + i] > cfg.maxCheck) << (3 * i + 2);
}
for(i = 0; i < AUX_OPTIONS; ++i) {
auxOptions[i] = (auxOptionMask & cfg.auxActivate[i]) > 0;
}
if(auxOptions[OPT_ARM] == 0) {
mode.OK_TO_ARM = 1;
}
// Passthrough
if(auxOptions[OPT_PASSTHRU]) {
mode.PASSTHRU_MODE = 1;
} else {
mode.PASSTHRU_MODE = 0;
}
// Level - TODO Add failsafe and ACC check
if(auxOptions[OPT_LEVEL] || mode.FAILSAFE) { //
if(!mode.LEVEL_MODE) {
zeroPID(&pids[ROLL_LEVEL_PID]);
zeroPID(&pids[PITCH_LEVEL_PID]);
mode.LEVEL_MODE = 1;
}
} else {
mode.LEVEL_MODE = 0;
}
// Heading
if(auxOptions[OPT_HEADING]) {
if(!mode.HEADING_MODE) {
mode.HEADING_MODE = 1;
headingHold = stateData.heading;
}
} else {
mode.HEADING_MODE = 0;
}
// Headfree
if(auxOptions[OPT_HEADFREE]) {
if(!mode.HEADFREE_MODE) {
mode.HEADFREE_MODE = 1;
headfreeReference = stateData.heading;
}
} else {
mode.HEADFREE_MODE = 0;
}
if(auxOptions[OPT_HEADFREE_REF]) {
headfreeReference = stateData.heading;
}
// GPS GOES HERE
uint8_t axis;
uint16_t tmp, tmp2;
// Apply deadbands, rates and expo
for (axis = 0; axis < 3; axis++) {
lastCommandInDetent[axis] = commandInDetent[axis];
tmp = min(abs(rcData[axis] - cfg.midCommand), 500);
if (tmp > cfg.deadBand[axis]) {
tmp -= cfg.deadBand[axis];
commandInDetent[axis] = false;
} else {
tmp = 0;
commandInDetent[axis] = true;
}
if(axis != 2) { // Roll and Pitch
tmp2 = tmp / 100;
command[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
} else { // Yaw
command[axis] = tmp;
}
if (rcData[axis] < cfg.midCommand)
command[axis] = -command[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.minCheck, 2000);
tmp = (uint32_t) (tmp - cfg.minCheck) * 1000 / (2000 - cfg.minCheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
command[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
// This will force a reset
if(fabs(command[THROTTLE] - altitudeThrottleHold) > THROTTLE_HOLD_DEADBAND)
mode.ALTITUDE_MODE = 0;
// Altitude TODO Add barometer check
if(auxOptions[OPT_ALTITUDE]) {
if(!mode.ALTITUDE_MODE) {
mode.ALTITUDE_MODE = 1;
altitudeThrottleHold = command[THROTTLE];
altitudeHold = stateData.altitude;
zeroPID(&pids[ALTITUDE_PID]);
}
} else {
mode.ALTITUDE_MODE = 0;
}
}
static void ACC_Common(void)
{
static int32_t a[3];
int axis;
if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == 400)
a[axis] = 0;
// Sum up 400 readings
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
cfg.accZero[axis] = 0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
cfg.accZero[ROLL] = a[ROLL] / 400;
cfg.accZero[PITCH] = a[PITCH] / 400;
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM
}
calibratingA--;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t angleTrim_saved[2] = { 0, 0 };
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = cfg.accZero[ROLL];
accZero_saved[PITCH] = cfg.accZero[PITCH];
accZero_saved[YAW] = cfg.accZero[YAW];
angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
}
if (InflightcalibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
cfg.accZero[axis] = 0;
}
// all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
toggleBeep = 2; // buzzer for indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred
cfg.accZero[ROLL] = accZero_saved[ROLL];
cfg.accZero[PITCH] = accZero_saved[PITCH];
cfg.accZero[YAW] = accZero_saved[YAW];
cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1) { // the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
cfg.accZero[ROLL] = b[ROLL] / 50;
cfg.accZero[PITCH] = b[PITCH] / 50;
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM
}
}
accADC[ROLL] -= cfg.accZero[ROLL];
accADC[PITCH] -= cfg.accZero[PITCH];
accADC[YAW] -= cfg.accZero[YAW];
}
// Default settings
static void resetConf(void)
{
int32_t i;
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
// Default settings
cfg.version = EEPROM_CONF_VERSION;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
featureSet(FEATURE_VBAT);
featureSet(FEATURE_PPM);
parseRcChannels("AETR1234");
// Motor/ESC
cfg.escPwmRate = 400;
cfg.servoPwmRate = 50;
// Failsafe
cfg.failsafeOnDelay = 50; // Number of command loops (50Hz) until failsafe kicks in
cfg.failsafeOffDelay = 20000; // Number of command loops until failsafe stops
cfg.failsafeThrottle = 1200;
cfg.commandRate = 90;
cfg.commandExpo = 65;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
//cfg.dynThrPID = 0;
cfg.throttleMid = 50;
cfg.throttleExpo = 0;
// Command Settings
for(i = 0; i < AUX_OPTIONS; ++i)
cfg.auxActivate[i] = 0;
cfg.minCommand = 1000;
cfg.midCommand = 1500;
cfg.maxCommand = 2000;
cfg.minCheck = 1100;
cfg.maxCheck = 1900;
cfg.minThrottle = 1150;
cfg.maxThrottle = 1850;
cfg.spektrumHiRes = false;
cfg.deadBand[ROLL] = 12;
cfg.deadBand[PITCH] = 12;
cfg.deadBand[YAW] = 12;
// Servos
// Tricopter
cfg.yawDirection = 1;
cfg.triYawServoMin = 1000;
cfg.triYawServoMid = 1500;
cfg.triYawServoMax = 2000;
// Bicopter
cfg.biLeftServoMin = 1000;
cfg.biLeftServoMid = 1500;
cfg.biLeftServoMax = 2000;
cfg.biRightServoMin = 1000;
cfg.biRightServoMid = 1500;
cfg.biRightServoMax = 2000;
// Flying wing
cfg.wingLeftMin = 1020;
cfg.wingLeftMid = 1500;
cfg.wingLeftMax = 2000;
cfg.wingRightMin = 1020;
cfg.wingRightMid = 1500;
cfg.wingRightMax = 2000;
cfg.pitchDirectionLeft = 1;
cfg.pitchDirectionRight = -1;
cfg.rollDirectionLeft = 1;
cfg.rollDirectionRight = 1;
cfg.gimbalFlags = GIMBAL_NORMAL;
cfg.gimbalSmoothFactor = 0.95f;
cfg.gimbalRollServoMin = 1000;
cfg.gimbalRollServoMid = 1500;
cfg.gimbalRollServoMax = 2000;
cfg.gimbalRollServoGain = 1.0f;
cfg.gimbalPitchServoMin = 1000;
cfg.gimbalPitchServoMid = 1500;
cfg.gimbalPitchServoMax = 2000;
cfg.gimbalPitchServoGain = 1.0f;
// PIDs
cfg.pids[ROLL_RATE_PID].p = 100.0f;
cfg.pids[ROLL_RATE_PID].i = 0.0f;
cfg.pids[ROLL_RATE_PID].d = 0.0f;
cfg.pids[ROLL_RATE_PID].iLim = 100.0f; // PWMs
cfg.pids[PITCH_RATE_PID].p = 100.0f;
cfg.pids[PITCH_RATE_PID].i = 0.0f;
cfg.pids[PITCH_RATE_PID].d = 0.0f;
cfg.pids[PITCH_RATE_PID].iLim = 100.0f; // PWMs
cfg.pids[YAW_RATE_PID].p = 200.0f;
cfg.pids[YAW_RATE_PID].i = 0.0f;
cfg.pids[YAW_RATE_PID].d = 0.0f;
cfg.pids[YAW_RATE_PID].iLim = 100.0f; // PWMs
cfg.pids[ROLL_LEVEL_PID].p = 2.0f;
cfg.pids[ROLL_LEVEL_PID].i = 0.0f;
cfg.pids[ROLL_LEVEL_PID].d = 0.0f;
cfg.pids[ROLL_LEVEL_PID].iLim = 0.5f; // radians/sec
cfg.pids[PITCH_LEVEL_PID].p = 2.0f;
cfg.pids[PITCH_LEVEL_PID].i = 0.0f;
cfg.pids[PITCH_LEVEL_PID].d = 0.0f;
cfg.pids[PITCH_LEVEL_PID].iLim = 0.5f; // radians/sec
cfg.pids[HEADING_PID].p = 1.5f;
cfg.pids[HEADING_PID].i = 0.0f;
cfg.pids[HEADING_PID].d = 0.0f;
cfg.pids[HEADING_PID].iLim = 0.5f; // radians/sec
cfg.pids[ALTITUDE_PID].p = 20.0f;
cfg.pids[ALTITUDE_PID].i = 17.0f;
cfg.pids[ALTITUDE_PID].d = 7.0f;
cfg.pids[ALTITUDE_PID].iLim = 30000.0f; // 0.1 m
cfg.angleTrim[ROLL] = 0.0f;
cfg.angleTrim[PITCH] = 0.0f;
cfg.accelLPF = false;
cfg.accelSmoothFactor = 0.75f;
cfg.accelCalibrated = false;
cfg.accelBias[XAXIS] = 0;
cfg.accelBias[YAXIS] = 0;
cfg.accelBias[ZAXIS] = 0;
cfg.gyroBiasOnStartup = false;
cfg.gyroSmoothFactor = 0.95f;
cfg.gyroTCBiasSlope[ROLL] = 0.0f;
cfg.gyroTCBiasSlope[PITCH] = 0.0f;
cfg.gyroTCBiasSlope[YAW] = 0.0f;
cfg.gyroTCBiasIntercept[ROLL] = 0.0f;
cfg.gyroTCBiasIntercept[PITCH] = 0.0f;
cfg.gyroTCBiasIntercept[YAW] = 0.0f;
cfg.magCalibrated = false;
cfg.magBias[ROLL] = 0;
cfg.magBias[PITCH] = 0;
cfg.magBias[YAW] = 0;
cfg.mpu6050Scale = false; // Shitty hack
// For Mahony AHRS
cfg.accelKp = 2.0f;
cfg.accelKi = 0.01f;
cfg.magKp = 1.0f;
cfg.magKi = 0.01f;
cfg.magDriftCompensation = false;
// Get your magnetic decliniation from here : http://magnetic-declination.com/
// For example, -6deg 37min, = -6.37 Japan, format is [sign]ddd.mm (degreesminutes)
cfg.magDeclination = 10.59f;
cfg.batScale = 11.0f;
cfg.batMinCellVoltage = 3.3f;
cfg.batMaxCellVoltage = 4.2f;
cfg.startupDelay = 1000;
// custom mixer. clear by defaults.
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
writeParams();
}
// Default settings
static void resetConf(void)
{
uint8_t i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
// featureSet(FEATURE_VBAT);
featureSet(FEATURE_PPM);
// featureSet(FEATURE_FAILSAFE);
// featureSet(FEATURE_LCD);
// featureSet(FEATURE_GPS);
// featureSet(FEATURE_PASS); // Just pass Throttlechannel
// featureSet(FEATURE_SONAR);
cfg.P8[ROLL] = 35; // 40
cfg.I8[ROLL] = 20;
cfg.D8[ROLL] = 30;
cfg.P8[PITCH] = 35; // 40
cfg.I8[PITCH] = 20;
cfg.D8[PITCH] = 30;
cfg.P8[YAW] = 60; // 70
cfg.I8[YAW] = 45;
cfg.P8[PIDALT] = 100;
cfg.I8[PIDALT] = 30;
cfg.D8[PIDALT] = 80;
cfg.P8[PIDPOS] = 10; // FIND YOUR VALUE
cfg.I8[PIDPOS] = 0; // NOT USED
cfg.D8[PIDPOS] = 0; // NOT USED
cfg.P8[PIDPOSR] = 50; // FIND YOUR VALUE // Controls the speed part with my PH logic
cfg.I8[PIDPOSR] = 0; // 25; // DANGER "I" may lead to circeling // Controls the speed part with my PH logic
cfg.D8[PIDPOSR] = 40; // FIND YOUR VALUE // Controls the speed part with my PH logic
cfg.P8[PIDNAVR] = 15; // 14 More ?
cfg.I8[PIDNAVR] = 0; // NAV_I * 100; // Scaling/Purpose unchanged
cfg.D8[PIDNAVR] = 0; // NAV_D * 1000; // Scaling/Purpose unchanged
// cfg.P8[PIDPOS] = 11; // APM PH Stock values
// cfg.I8[PIDPOS] = 0;
// cfg.D8[PIDPOS] = 0;
// cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
// cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
// cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
// cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
// cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
// cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 60; // 70
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 50;
cfg.P8[PIDMAG] = 80; // cfg.P8[PIDVEL] = 0;// cfg.I8[PIDVEL] = 0;// cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 100;
cfg.rcExpo8 = 80; // cfg.rollPitchRate = 0;// cfg.yawRate = 0;// cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
// cfg.thrExpo8 = 0;//for (i = 0; i < CHECKBOXITEMS; i++)//cfg.activate[i] = 0;
// cfg.angleTrim[0] = 0;// cfg.angleTrim[1] = 0;// cfg.accZero[0] = 0;// cfg.accZero[1] = 0;
// cfg.accZero[2] = 0;// cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
memcpy(&cfg.align, default_align, sizeof(cfg.align));
// cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.mag_declination = 107; // Crashpilot //cfg.acc_hardware = ACC_DEFAULT;// default/autodetect
cfg.mag_oldcalib = 0; // 1 = old hard iron calibration // 0 = extended calibration (better)
cfg.mag_oldctime = 1; // 1 - 5 Time in MINUTES for old calibration. Use this together with mag_oldcalib = 1 if you have a monster of a copter
cfg.acc_hardware = 2; // Crashpilot MPU6050
cfg.acc_lpf_factor = 100; // changed 27.11.2012
cfg.acc_ins_lpf = 10; // General LPF for all INS stuff
cfg.looptime = 3000; // changed 27.11.2012 // cfg.acc_lpf_factor = 4;
cfg.mainpidctrl = 1; // 1 = OriginalMwiiPid pimped by me, 2 = New mwii controller (experimental, float pimped + pt1)
cfg.mainpt1cut = 15; // (0-50Hz) 0 Disables pt1element. Cuf Off Frequency for pt1 element D term in Hz of main Pid controller
cfg.newpidimax = 256; // [10-65000) 256 Default. Imax of new Pidcontroller
cfg.gpspt1cut = 10; // (1-50Hz) Cuf Off Frequency for D term in Hz of GPS Pid controller
cfg.gyro_cmpf_factor = 1000; // (10-1000) 400 default. Now 1000. The higher, the more weight gets the gyro and the lower is the correction with Acc data.
cfg.gyro_cmpfm_factor = 1000; // (10-2000) 200 default for 10Hz. Now 1000 for 70Hz seems ok. Gyro/Magnetometer Complement. Greater Value means more filter on mag/delay
cfg.gyro_lpf = 42; // Possible values 256 188 98 42 20 10 (HZ)
// Baro
cfg.accz_vel_cf = 0.985f; // Crashpilot: Value for complementary filter accz and barovelocity
cfg.accz_alt_cf = 0.940f; // Crashpilot: Value for complementary filter accz and altitude
cfg.baro_lag = 0.3f; // Lag of Baro/Althold stuff in general, makes stop in hightchange snappier
cfg.barodownscale = 0.7f; // Scale downmovement down (because copter drops faster than rising)
// Autoland
cfg.al_barolr = 75; // Temporary value "64" increase to increase Landingspeed
cfg.al_snrlr = 75; // You can specify different landingfactor here on sonar contact, because sonar land maybe too fast when snr_cf is high
cfg.al_lndthr = 0; // This is the absolute throttle that kicks off the "has landed timer" if it is too low cfg.minthrottle is taken.
cfg.al_debounce = 5; // (0-20%) 0 Disables. Defines a Throttlelimiter on Autoland. Percentage defines the maximum deviation of assumed hoverthrottle during Autoland
cfg.al_tobaro = 2000; // Timeout in ms (100 - 5000) before shutoff on autoland. "al_lndthr" must be undershot for that timeperiod
cfg.al_tosnr = 1000; // Timeout in ms (100 - 5000) If sonar aided land is wanted (snr_land = 1) you can choose a different timeout here
cfg.baro_debug = 0; // Crashpilot: 1 = Debug Barovalues //cfg.baro_noise_lpf = 0.6f;// Crashpilot: Not used anymore//cfg.baro_cf = 0.985f;// Crashpilot: Not used anymore
cfg.moron_threshold = 32;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
cfg.power_adc_channel = 0;
// Radio
parseRcChannels("AETR1234");
cfg.deadband = 15; // Crashpilot: A little deadband will not harm our crappy RC
cfg.yawdeadband = 15; // Crashpilot: A little deadband will not harm our crappy RC
cfg.alt_hold_throttle_neutral = 50; // Crashpilot: A little deadband will not harm our crappy RC
cfg.gps_adddb = 5; // Additional Deadband for all GPS functions;
// cfg.spektrum_hires = 0;
cfg.midrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
cfg.auxChannels = 4; // [4 - 10] cGiesen: Default = 4, then like the standard!
cfg.killswitchtime = 0; // Time in ms when your arm switch becomes a Killswitch, 0 disables the Killswitch, can not be used together with FEATURE_INFLIGHT_ACC_CAL
// Motor/ESC/Servo
cfg.minthrottle = 1150; // ORIG
// cfg.minthrottle = 1100;
cfg.maxthrottle = 1950;
cfg.passmotor = 0; // Crashpilot: Only used with feature pass. If 0 = all Motors, otherwise specific Motor
cfg.mincommand = 1000;
cfg.motor_pwm_rate = 400;
cfg.servo_pwm_rate = 50;
// servos
cfg.yaw_direction = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;
// flying wing
cfg.wing_left_min = 1020;
cfg.wing_left_mid = 1500;
cfg.wing_left_max = 2000;
cfg.wing_right_min = 1020;
cfg.wing_right_mid = 1500;
cfg.wing_right_max = 2000;
cfg.pitch_direction_l = 1;
cfg.pitch_direction_r = -1;
cfg.roll_direction_l = 1;
cfg.roll_direction_r = 1;
// gimbal
cfg.gimbal_pitch_gain = 10;
cfg.gimbal_roll_gain = 10;
cfg.gimbal_flags = GIMBAL_NORMAL;
cfg.gimbal_pitch_min = 1020;
cfg.gimbal_pitch_max = 2000;
cfg.gimbal_pitch_mid = 1500;
cfg.gimbal_roll_min = 1020;
cfg.gimbal_roll_max = 2000;
cfg.gimbal_roll_mid = 1500;
// gps/nav
cfg.gps_type = 1; // GPS_NMEA = 0, GPS_UBLOX = 1, GPS_MTK16 = 2, GPS_MTK19 = 3, GPS_UBLOX_DUMB = 4
cfg.gps_baudrate = 115200; //38400; // Changed 8/6/13 to 115200;
// cfg.gps_baudrate = 38400; //38400; // Changed 8/6/13 to 115200;
// cfg.gps_ins_vel = 0.72f; // Crashpilot GPS INS The LOWER the value the closer to gps speed // Dont go to high here
cfg.gps_ins_vel = 0.6f; // Crashpilot GPS INS The LOWER the value the closer to gps speed // Dont go to high here
cfg.gps_ins_mdl = 1; // NOTE: KEEP THIS TO "1" FOR NOW because other models work like shit currently. GPS ins model. 1 = Based on lat/lon, 2 = based on Groundcourse & speed,(3 = based on ublx velned deleted)
cfg.gps_lag = 2000; // GPS Lag in ms
cfg.gps_phase = 0; // +- 30 Degree Make a phaseshift of GPS output for whatever reason you might want that (frametype etc)
cfg.gps_ph_minsat = 6; // Minimal Satcount for PH, PH on RTL is still done with 5Sats or more
cfg.gps_ph_settlespeed = 10; // 1 - 200 cm/s PH settlespeed in cm/s
cfg.gps_ph_brakemaxangle = 10; // 1 - 45 Degree Maximal 5 Degree Overspeedbrake
cfg.gps_ph_minbrakepercent = 50; // 1 - 99% minimal percent of "brakemaxangle" left over for braking. Example brakemaxangle = 6 so 50 Percent is 3..
cfg.gps_ph_brkacc = 40; // [1 - 500] Is the assumed negative braking acceleration in cm/(s*s) of copter. Value is positive though. It will be a timeout. The lower the Value the longe the Timeout
cfg.gps_ph_abstub = 100; // 0 - 1000cm (300 Dfault, 0 disables) Defines the "bath tub" around current absolute PH Position, where PosP is diminished, reaction gets harder on tubs edge and then goes on linear
cfg.gps_maxangle = 25; // 10 - 45 Degree Maximal over all GPS bank angle
cfg.gps_wp_radius = 150;
// cfg.gps_rtl_minhight = 20; // (0 - 200) Minimal RTL hight in m, 0 disables feature
cfg.gps_rtl_minhight = 0; // (0 - 200) Minimal RTL hight in m, 0 disables feature
cfg.gps_rtl_mindist = 0; // 0 Disables. Minimal distance for RTL in m, otherwise it will just autoland, prevent Failsafe jump in your face, when arming copter and turning off TX
cfg.gps_rtl_flyaway = 0; // 0 Disables. If during RTL the distance increases beyond this value (in meters relative to RTL activation point), something is wrong, autoland
cfg.gps_yaw = 30; // Thats the MAG P during GPS functions, substitute for "cfg.P8[PIDMAG]"
cfg.nav_rtl_lastturn = 1; // 1 = when copter gets to home position it rotates it's head to takeoff direction independend of nav_controls_heading
// cfg.nav_slew_rate = 30; // was 30 and 50 before
cfg.nav_slew_rate = 20; // was 30 and 50 before
cfg.nav_tail_first = 0; // 1 = Copter comes back with ass first (only works with nav_controls_heading = 1)
cfg.nav_controls_heading = 0; // 1 = Nav controls YAW during WP ONLY
// cfg.nav_controls_heading = 1; // 1 = Nav controls YAW during WP ONLY
cfg.nav_speed_min = 100; // 10 - 200 cm/s don't set higher than nav_speed_max! That dumbness is not covered.
cfg.nav_speed_max = 350; // 50 - 2000 cm/s don't set lower than nav_speed_min! That dumbness is not covered.
cfg.nav_approachdiv = 3; // 2 - 10 This is the divisor for approach speed for wp_distance. Example: 400cm / 3 = 133cm/s if below nav_speed_min it will be adjusted
cfg.nav_tiltcomp = 20; // 0 - 100 (20 TestDefault) Only arducopter really knows. Dfault was 54. This is some kind of a hack of them to reach actual nav_speed_max. 54 was Dfault, 0 disables
cfg.nav_ctrkgain = 0.5f; // 0 - 10.0 (0.5 TestDefault) (Floatvariable) That is the "Crosstrackgain" APM Dfault is "1". "0" disables
// Failsafe Variables
cfg.failsafe_delay = 10; // in 0.1s (10 = 1sec)
cfg.failsafe_off_delay = 200; // in 0.1s (200 = 20sec)
cfg.failsafe_throttle = 1200; // decent Dfault which should always be below hover throttle for people.
cfg.failsafe_deadpilot = 0; // DONT USE, EXPERIMENTAL Time in sec when FS is engaged after idle on THR/YAW/ROLL/PITCH, 0 disables max 250
cfg.failsafe_justph = 0; // Does just PH&Autoland an not RTL, use this in difficult areas with many obstacles to avoid RTL crash into something
cfg.failsafe_ignoreSNR = 1; // When snr_land is set to 1, it is possible to ignore that on Failsafe, because FS over a tree could turn off copter
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
cfg.tele_prot = 0; // Protocol ONLY used when Armed including Baudchange if necessary. 0 (Dfault)=Keep Multiwii @CurrentUSB Baud, 1=Frsky @9600Baud, 2=Mavlink @CurrentUSB Baud, 3=Mavlink @57KBaud (like stock minimOSD wants it)
// LED Stuff
cfg.LED_invert = 0; // Crashpilot: Inversion of LED 0&1 Partly implemented because Bootup is not affected
cfg.LED_Type = 1; // 1=MWCRGB / 2=MONO_LED / 3=LEDRing
cfg.LED_Pinout = 1; // rc6
cfg.LED_ControlChannel = 8; // AUX4 (Channel 8)
cfg.LED_Armed = 0; // 0 = Show LED only if armed, 1 = always show LED
cfg.LED_Pattern1 = 1300; // 32bit bit pattern to have flickering led patterns / the pattern for MWCRGB 1000-2000
cfg.LED_Pattern2 = 1800; // 32bit bit pattern to have flickering led patterns / the pattern for MWCRGB 1000-2000
cfg.LED_Pattern3 = 1900; // 32bit bit pattern to have flickering led patterns / the pattern for MWCRGB 1000-2000
cfg.LED_Toggle_Delay1 = 0x08; // slow down LED_Pattern
cfg.LED_Toggle_Delay2 = 0x08; // slow down LED_Pattern
cfg.LED_Toggle_Delay3 = 0x08; // slow down LED_Pattern
// SONAR
// SOME INFO ON SONAR:
// PWM56 are 5V resistant, RC78 only tolerate 3.3V(!!) so add a 1K Ohms resistor!!!
// Note: You will never see the maximum possible sonar range in a copter, so go for the half of it (or less?)
//
// Connection possibilities depending on Receivertype:
// PPSUM: RC78 possible, PWM56 possible (on max. quadcopters, see below)
// Normal RX: Just Connection on Motorchannel 5&6 (PWM56) is possible.
// The PWM56 sonar connection option is only available in setups with max motors 4, otherwise sonar is not initialized.
//
// HC-SR04:
// Operation Voltage: 5V (!! Use PWM56 or 1K resistor !!)
// Range: 2cm - 400cm
// Angle: 15 Degrees (Test out for yourself: cfg.snr_tilt = X)
//
// Maxbotics in general
// Operation Voltage: (some 2.5V)3.3V - 5V ((!! Use PWM56 or resistor with 5V !!)
// Only wire the Maxbotics for PWM output (more precise anyway), not the analog etc. modes, just wire echopin (normally pin 2)
// Range: 20cm(!) - 765cm (some >1000cm), MaxTiltAngle is not specified, depending on Model
// Tested on MB1200 XL-MaxSonar-EZ0
//
// GENERAL WARNING: DON'T SET snr_min TOO LOW, OTHERWISE THE WRONG SONARVALUE WILL BE TAKEN AS REAL MEASUREMENT!!
// I implemented some checks to prevent that user error, but still keep that in mind.
// Min/Max are checked and changed if they are too stupid for your sonar. So if you suddenly see other values, thats not an eeprom error or so.
// MAXBOTICS: SET snr_min to at least 25! I check this in sensors and change the value, if needed.
// NOTE: I limited Maxbotics to 7 meters in the code, knowing that some types will do >10m, if you have one of them 7m is still the limit for you.
// HC-SR04: SET snr_min to at least 5 ! I check this in sensors and change the value, if needed.
// DaddyWalross Sonar: I DON'T KNOW! But it uses HC-SR04 so i apply the same limits (5cm-400cm) to its output
// NOTE: Sonar is def. not a must - have. But nice to have.
cfg.snr_type = 3; // 0 = PWM56 HC-SR04, 1 = RC78 HC-SR04, 2 = I2C (DaddyWalross), 3 = MBPWM56, 4 = MBRC78
cfg.snr_min = 25; // Valid Sonar minimal range in cm (5-200) see warning above
cfg.snr_max = 200; // Valid Sonar maximal range in cm (50-700)
cfg.snr_debug = 0; // 1 Sends Sonardata (within defined range and tilt) to debug[0] and tiltvalue to debug[1], debug[0] will be -1 if out of range/tilt. debug[2] contains raw sonaralt, like before
cfg.snr_tilt = 18; // Somehow copter tiltrange in degrees (Not exactly but good enough. Value * 0.9 = realtilt) in wich Sonar is possible
cfg.snr_cf = 0.7f; // The bigger, the more Sonarinfluence, makes switch between Baro/Sonar smoother and defines baroinfluence when sonarcontact. 1.0f just takes Sonar, if contact (otherwise baro)
cfg.snr_diff = 0; // 0 disables that check. Range (0-200) Maximal allowed difference in cm between sonar readouts (100ms rate and snr_diff = 50 means max 5m/s)
cfg.snr_land = 1; // Aided Sonar - landing, by setting upper throttle limit to current throttle. - Beware of Trees!! Can be disabled for Failsafe with failsafe_ignoreSNR = 1
// LOGGING
cfg.floppy_mode = FD_MODE_GPSLOGGER; // Usagemode of free Space. 1 = GPS Logger
cfg.FDUsedDatasets = 0; // Default no Datasets stored
cfg.stat_clear = 1; // This will clear the stats between flights, or you can set to 0 and treasue overallstats, but you have to write manually eeprom or have logging enabled
cfg.sens_1G = 1; // Just feed a dummy "1" to avoid div by zero
ClearStats();
// custom mixer. clear by Dfaults.
for (i = 0; i < MAX_MOTORS; i++) cfg.customMixer[i].throttle = 0.0f;
writeParams(0);
}
static void evaluateCommand(void)
{
uint32_t i, tmpu32 = 0;
uint8_t wp_no;
switch (cmdMSP)
{
case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++)
rcDataSAVE[i] = read16();
headSerialReply(0);
break;
case MSP_SET_RAW_GPS:
f.GPS_FIX = read8();
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0);
break;
case MSP_SET_PID:
for (i = 0; i < PIDITEMS; i++)
{
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
headSerialReply(0);
break;
case MSP_SET_BOX:
for (i = 0; i < CHECKBOXITEMS; i++)
{
tmpu32 = 0;
if (cfg.rc_auxch > 4) tmpu32 = read32();
else tmpu32 = read16();
cfg.activate[i] = tmpu32;
}
headSerialReply(0);
break;
case MSP_SET_RC_TUNING:
cfg.rcRate8 = read8();
cfg.rcExpo8 = read8();
cfg.rollPitchRate = read8();
cfg.yawRate = read8();
cfg.dynThrPID = read8();
cfg.thrMid8 = read8();
cfg.thrExpo8 = read8();
headSerialReply(0);
break;
case MSP_SET_MISC:
headSerialReply(0);
break;
case MSP_IDENT:
headSerialReply(7);
serialize8(VERSION); // multiwii version
serialize8(cfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability"
break;
case MSP_STATUS:
headSerialReply(11);
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) |
sensors(SENSOR_BARO) << 1 |
sensors(SENSOR_MAG) << 2 |
sensors(SENSOR_GPS) << 3 |
sensors(SENSOR_SONAR) << 4);
serialize32(f.ANGLE_MODE << BOXANGLE |
f.HORIZON_MODE << BOXHORIZON |
f.BARO_MODE << BOXBARO |
f.MAG_MODE << BOXMAG |
f.ARMED << BOXARM |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB |
f.GPS_HOME_MODE << BOXGPSHOME |
f.GPS_HOLD_MODE << BOXGPSHOLD |
f.GPS_LOG_MODE << BOXGPSLOG |
f.HEADFREE_MODE << BOXHEADFREE |
f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON |
rcOptions[BOXHEADADJ] << BOXHEADADJ |
rcOptions[BOXOSD] << BOXOSD);
serialize8(0);
break;
case MSP_RAW_IMU:
headSerialReply(18);
if (!feature(FEATURE_PASS)) // Just Do the normal stuff
{
for (i = 0; i < 3; i++) serialize16((int16_t)accSmooth[i]);
for (i = 0; i < 3; i++) serialize16((int16_t)gyroData[i]);
for (i = 0; i < 3; i++) serialize16((int16_t)magADCfloat[i]);
}
else // Just serialize unfiltered AccZ for Balancing
{
for (i = 0; i < 2; i++) serialize16(0);
serialize16((int16_t)accADC[YAW] - (uint16_t)acc_1G); // Put accz into the middle
for (i = 0; i < 3; i++) serialize16(0);
for (i = 0; i < 3; i++) serialize16(0);
}
break;
case MSP_SERVO:
headSerialReply(16);
for (i = 0; i < 8; i++) serialize16(servo[i]);
break;
case MSP_MOTOR:
headSerialReply(16);
for (i = 0; i < 8; i++) serialize16(motor[i]);
break;
case MSP_RC:
headSerialReply((cfg.rc_auxch + 4) * 2); // headSerialReply(16);
for (i = 0; i < cfg.rc_auxch + 4; i++) // for (i = 0; i < 8; i++)
serialize16(rcDataSAVE[i]);
break;
case MSP_RAW_GPS:
headSerialReply(14);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
break;
case MSP_ATTITUDE:
headSerialReply(8);
for (i = 0; i < 2; i++) serialize16((int16_t)angle[i]);
serialize16((int16_t)heading);
serialize16((int16_t)headFreeModeHold);
break;
case MSP_ALTITUDE:
headSerialReply(6);
if (feature(FEATURE_PASS))
{
serialize32(0);
serialize16(0);
}
else
{
serialize32((int32_t)EstAlt);
serialize16((int16_t)vario);
}
break;
case MSP_BAT:
headSerialReply(5);
serialize8(vbat);
serialize16(0); // power meter stuff
serialize16((uint16_t)rssi << 2); // Upscale 255 to 1020, so 100% (1023) is hard to achieve, who cares?
break;
case MSP_RC_TUNING:
headSerialReply(7);
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
serialize8(cfg.thrMid8);
serialize8(cfg.thrExpo8);
break;
case MSP_PID:
headSerialReply(3 * PIDITEMS);
for (i = 0; i < PIDITEMS; i++)
{
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
break;
case MSP_BOX:
if (cfg.rc_auxch > 4) headSerialReply(4 * CHECKBOXITEMS);
else headSerialReply(2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++)
{
tmpu32 = cfg.activate[i];
if (cfg.rc_auxch > 4) serialize32(tmpu32);
else serialize16((int16_t)tmpu32);
}
break;
case MSP_BOXNAMES:
headSerialReply(sizeof(boxnames) - 1);
serializeNames(boxnames);
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_MISC:
headSerialReply(2);
serialize16(0); // intPowerTrigger1
break;
case MSP_MOTOR_PINS:
headSerialReply(8);
for (i = 0; i < 8; i++) serialize8(i + 1);
break;
case MSP_WP:
wp_no = read8(); // get the wp number
headSerialReply(12);
if (wp_no == 0)
{
serialize8(0); // wp0
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
else if (wp_no == 16)
{
serialize8(16); // wp16
serialize32(GPS_WP[LAT]);
serialize32(GPS_WP[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
break;
case MSP_RESET_CONF:
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
calibratingA = true;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
case MSP_EEPROM_WRITE:
writeParams(0);
headSerialReply(0);
break;
case MSP_DEBUG:
headSerialReply(8);
for (i = 0; i < 4; i++) serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
static void evaluateCommand(void)
{
uint32_t i;
uint8_t wp_no;
switch (cmdMSP)
{
case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++)
rcDataSAVE[i] = read16();
headSerialReply(0);
break;
case MSP_SET_ACC_TRIM:
cfg.angleTrim[PITCH] = read16();
cfg.angleTrim[ROLL] = read16();
headSerialReply(0);
break;
case MSP_SET_RAW_GPS:
f.GPS_FIX = read8();
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0);
break;
case MSP_SET_PID:
for (i = 0; i < PIDITEMS; i++)
{
cfg.P8[i] = read8();
cfg.I8[i] = read8();
cfg.D8[i] = read8();
}
headSerialReply(0);
break;
case MSP_SET_BOX:
for (i = 0; i < CHECKBOXITEMS; i++)
{
if (cfg.auxChannels > 4) cfg.activate[i] = read32();
else cfg.activate[i] = read16();
}
headSerialReply(0);
break;
case MSP_SET_RC_TUNING:
cfg.rcRate8 = read8();
cfg.rcExpo8 = read8();
cfg.rollPitchRate = read8();
cfg.yawRate = read8();
cfg.dynThrPID = read8();
cfg.thrMid8 = read8();
cfg.thrExpo8 = read8();
headSerialReply(0);
break;
case MSP_SET_MISC:
headSerialReply(0);
break;
case MSP_IDENT:
headSerialReply(7);
serialize8(VERSION); // multiwii version
serialize8(cfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability"
break;
case MSP_STATUS:
headSerialReply(10);
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
break;
case MSP_RAW_IMU:
headSerialReply(18);
if (!feature(FEATURE_PASS)) // Just Do the normal stuff
{
for (i = 0; i < 3; i++) serialize16(accSmooth[i]);
for (i = 0; i < 3; i++) serialize16(gyroData[i]);
for (i = 0; i < 3; i++) serialize16((int16_t)magADCfloat[i]);
}
else // Just serialize unfiltered AccZ for Balancing
{
for (i = 0; i < 2; i++) serialize16(0);
serialize16(accADC[YAW] - acc_1G); // Put accz into the middle
for (i = 0; i < 3; i++) serialize16(0);
for (i = 0; i < 3; i++) serialize16(0);
}
break;
case MSP_SERVO:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(servo[i]);
break;
case MSP_MOTOR:
headSerialReply(16);
for (i = 0; i < 8; i++)
serialize16(motor[i]);
break;
case MSP_RC:
headSerialReply((cfg.auxChannels + 4) * 2); // headSerialReply(16);
for (i = 0; i < cfg.auxChannels + 4; i++) // for (i = 0; i < 8; i++)
serialize16(rcDataSAVE[i]);
break;
case MSP_RAW_GPS:
headSerialReply(14);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
if (cfg.gps_debug == 1)
{
serialize32(Real_GPS_coord[LAT]);
serialize32(Real_GPS_coord[LON]);
}
else
{
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
}
serialize16(GPS_altitude);
serialize16(GPS_speed);
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
break;
case MSP_ATTITUDE:
headSerialReply(8);
for (i = 0; i < 2; i++)
serialize16(angle[i]);
serialize16((int16_t)heading);
serialize16((int16_t)headFreeModeHold);
break;
case MSP_ALTITUDE:
headSerialReply(4);
if (feature(FEATURE_PASS))
serialize32(0);
else
serialize32(EstAlt);
break;
case MSP_BAT:
headSerialReply(3);
serialize8(vbat);
serialize16(0); // power meter trash
break;
case MSP_RC_TUNING:
headSerialReply(7);
serialize8(cfg.rcRate8);
serialize8(cfg.rcExpo8);
serialize8(cfg.rollPitchRate);
serialize8(cfg.yawRate);
serialize8(cfg.dynThrPID);
serialize8(cfg.thrMid8);
serialize8(cfg.thrExpo8);
break;
case MSP_PID:
headSerialReply(3 * PIDITEMS);
for (i = 0; i < PIDITEMS; i++)
{
serialize8(cfg.P8[i]);
serialize8(cfg.I8[i]);
serialize8(cfg.D8[i]);
}
break;
case MSP_BOX:
if (cfg.auxChannels > 4) headSerialReply(4 * CHECKBOXITEMS);
else headSerialReply(2 * CHECKBOXITEMS);
for (i = 0; i < CHECKBOXITEMS; i++)
{
if (cfg.auxChannels > 4) serialize32(cfg.activate[i]);
else serialize16(cfg.activate[i]);
}
break;
case MSP_BOXNAMES:
headSerialReply(sizeof(boxnames) - 1);
serializeNames(boxnames);
break;
case MSP_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_MISC:
headSerialReply(2);
serialize16(0); // intPowerTrigger1
break;
case MSP_MOTOR_PINS:
headSerialReply(8);
for (i = 0; i < 8; i++)
serialize8(i + 1);
break;
case MSP_WP:
wp_no = read8(); // get the wp number
headSerialReply(12);
if (wp_no == 0)
{
serialize8(0); // wp0
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
else if (wp_no == 16)
{
serialize8(16); // wp16
serialize32(GPS_WP[LAT]);
serialize32(GPS_WP[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
break;
case MSP_RESET_CONF:
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
calibratingA = 400;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
case MSP_EEPROM_WRITE:
writeParams(0);
headSerialReply(0);
break;
case MSP_ACC_TRIM:
headSerialReply(4);
serialize16(cfg.angleTrim[PITCH]);
serialize16(cfg.angleTrim[ROLL]);
break;
case MSP_DEBUG:
headSerialReply(8);
for (i = 0; i < 4; i++) serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
// new @Johannes based of MultiWii V2.21
case MSP_SERVO_CONF: // @Johannes:
//s_struct((uint8_t*)&cfg.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56
headSerialReply(56);
for(i=0;i<8;i++) { // new servostructure Johannes:
serialize16(cfg.servoConf[i].min);
serialize16(cfg.servoConf[i].max);
serialize16(cfg.servoConf[i].middle);
serialize8(cfg.servoConf[i].rate);
}
break;
case MSP_SET_SERVO_CONF: // @Johannes:
//s_struct_w((uint8_t*)&cfg.servoConf[0].min,56); doesn't work Johannes
headSerialReply(0);
for(i=0;i<8;i++) { // new servostructure Johannes:
cfg.servoConf[i].min = read16();
cfg.servoConf[i].max = read16();
cfg.servoConf[i].middle = read16();
cfg.servoConf[i].rate = read8();
}
break;
// Additional commands that are not compatible with MultiWii
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
case MSP_TEMPERATURE:
headSerialReply(2);
serialize16(telemTemperature1);
break;
case MSP_SONAR:
headSerialReply(2);
serialize16(sonarAlt);
break;
case MSP_FIRMWARE:
headSerialReply(sizeof(FIRMWARE) - 1);
serializeNames(FIRMWARE);
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
void evaluateCommand() {
uint32_t tmp=0;
switch(cmdMSP[CURRENTPORT]) {
case MSP_SET_RAW_RC:
s_struct_w((uint8_t*)&rcSerial,16);
rcSerialCount = 1000; // 1s transition
break;
#if GPS
case MSP_SET_RAW_GPS:
f.GPS_FIX = read8();
GPS_numSat = read8();
GPS_coord[LAT] = read32();
GPS_coord[LON] = read32();
GPS_altitude = read16();
GPS_speed = read16();
GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0);
break;
#endif
case MSP_SET_PID:
s_struct_w((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
break;
case MSP_SET_BOX:
s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*2);
break;
case MSP_SET_RC_TUNING:
s_struct_w((uint8_t*)&conf.rcRate8,7);
break;
#if !defined(DISABLE_SETTINGS_TAB)
case MSP_SET_MISC:
struct {
uint16_t a,b,c,d,e,f;
uint32_t g;
uint16_t h;
uint8_t i,j,k,l;
} set_misc;
s_struct_w((uint8_t*)&set_misc,22);
#if defined(POWERMETER)
conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
#endif
conf.minthrottle = set_misc.b;
#ifdef FAILSAFE
conf.failsafe_throttle = set_misc.e;
#endif
#if MAG
conf.mag_declination = set_misc.h;
#endif
#if defined(VBAT)
conf.vbatscale = set_misc.i;
conf.vbatlevel_warn1 = set_misc.j;
conf.vbatlevel_warn2 = set_misc.k;
conf.vbatlevel_crit = set_misc.l;
#endif
break;
case MSP_MISC:
struct {
uint16_t a,b,c,d,e,f;
uint32_t g;
uint16_t h;
uint8_t i,j,k,l;
} misc;
misc.a = intPowerTrigger1;
misc.b = conf.minthrottle;
misc.c = MAXTHROTTLE;
misc.d = MINCOMMAND;
#ifdef FAILSAFE
misc.e = conf.failsafe_throttle;
#else
misc.e = 0;
#endif
#ifdef LOG_PERMANENT
misc.f = plog.arm;
misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
#else
misc.f = 0; misc.g =0;
#endif
#if MAG
misc.h = conf.mag_declination;
#else
misc.h = 0;
#endif
#ifdef VBAT
misc.i = conf.vbatscale;
misc.j = conf.vbatlevel_warn1;
misc.k = conf.vbatlevel_warn2;
misc.l = conf.vbatlevel_crit;
#else
misc.i = 0;misc.j = 0;misc.k = 0;misc.l = 0;
#endif
s_struct((uint8_t*)&misc,22);
break;
#endif
#if defined (DYNBALANCE)
case MSP_SET_MOTOR:
s_struct_w((uint8_t*)&motor,16);
break;
#endif
#ifdef MULTIPLE_CONFIGURATION_PROFILES
case MSP_SELECT_SETTING:
if(!f.ARMED) {
global_conf.currentSet = read8();
if(global_conf.currentSet>2) global_conf.currentSet = 0;
writeGlobalSet(0);
readEEPROM();
}
headSerialReply(0);
break;
#endif
case MSP_SET_HEAD:
s_struct_w((uint8_t*)&magHold,2);
break;
case MSP_IDENT:
struct {
uint8_t v,t,msp_v;
uint32_t cap;
} id;
id.v = VERSION;
id.t = MULTITYPE;
id.msp_v = MSP_VERSION;
id.cap = capability|DYNBAL<<2|FLAP<<3;
s_struct((uint8_t*)&id,7);
break;
case MSP_STATUS:
struct {
uint16_t cycleTime,i2c_errors_count,sensor;
uint32_t flag;
uint8_t set;
} st;
st.cycleTime = cycleTime;
st.i2c_errors_count = i2c_errors_count;
st.sensor = ACC|BARO<<1|MAG<<2|GPS<<3|SONAR<<4;
#if ACC
if(f.ANGLE_MODE) tmp |= 1<<BOXANGLE;
if(f.HORIZON_MODE) tmp |= 1<<BOXHORIZON;
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
if(f.BARO_MODE) tmp |= 1<<BOXBARO;
#endif
#if MAG
if(f.MAG_MODE) tmp |= 1<<BOXMAG;
#if !defined(FIXEDWING)
if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
#endif
#if defined(CAMTRIG)
if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
#endif
#if GPS
if(f.GPS_HOME_MODE) tmp |= 1<<BOXGPSHOME;
if(f.GPS_HOLD_MODE) tmp |= 1<<BOXGPSHOLD;
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
#endif
#if defined(BUZZER)
if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
#endif
#if defined(LED_FLASHER)
if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
#endif
#if defined(LANDING_LIGHTS_DDR)
if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
#endif
#if defined(VARIOMETER)
if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if(rcOptions[BOXCALIB]) tmp |= 1<<BOXCALIB;
#endif
#if defined(GOVERNOR_P)
if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
#endif
#if defined(OSD_SWITCH)
if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
if(f.ARMED) tmp |= 1<<BOXARM;
st.flag = tmp;
st.set = global_conf.currentSet;
s_struct((uint8_t*)&st,11);
break;
case MSP_RAW_IMU:
#if defined(DYNBALANCE)
for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
#endif
s_struct((uint8_t*)&imu,18);
break;
case MSP_SERVO:
s_struct((uint8_t*)&servo,16);
break;
case MSP_SERVO_CONF:
s_struct((uint8_t*)&conf.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56
break;
case MSP_SET_SERVO_CONF:
s_struct_w((uint8_t*)&conf.servoConf[0].min,56);
break;
case MSP_MOTOR:
s_struct((uint8_t*)&motor,16);
break;
case MSP_RC:
s_struct((uint8_t*)&rcData,RC_CHANS*2);
break;
#if GPS
case MSP_RAW_GPS:
headSerialReply(16);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
serialize16(GPS_ground_course); // added since r1172
break;
case MSP_COMP_GPS:
headSerialReply(5);
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize8(GPS_update & 1);
break;
#endif
case MSP_ATTITUDE:
s_struct((uint8_t*)&att,6);
break;
case MSP_ALTITUDE:
s_struct((uint8_t*)&alt,6);
break;
case MSP_ANALOG:
s_struct((uint8_t*)&analog,7);
break;
case MSP_RC_TUNING:
s_struct((uint8_t*)&conf.rcRate8,7);
break;
case MSP_PID:
s_struct((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
break;
case MSP_PIDNAMES:
serializeNames(pidnames);
break;
case MSP_BOX:
s_struct((uint8_t*)&conf.activate[0],2*CHECKBOXITEMS);
break;
case MSP_BOXNAMES:
serializeNames(boxnames);
break;
case MSP_BOXIDS:
headSerialReply(CHECKBOXITEMS);
for(uint8_t i=0;i<CHECKBOXITEMS;i++) {
serialize8(pgm_read_byte(&(boxids[i])));
}
break;
case MSP_MOTOR_PINS:
s_struct((uint8_t*)&PWM_PIN,8);
break;
#if defined(USE_MSP_WP)
case MSP_WP:
{
int32_t lat = 0,lon = 0;
uint8_t wp_no = read8(); //get the wp number
headSerialReply(18);
if (wp_no == 0) {
lat = GPS_home[LAT];
lon = GPS_home[LON];
} else if (wp_no == 16) {
lat = GPS_hold[LAT];
lon = GPS_hold[LON];
}
serialize8(wp_no);
serialize32(lat);
serialize32(lon);
serialize32(AltHold); //altitude (cm) will come here -- temporary implementation to test feature with apps
serialize16(0); //heading will come here (deg)
serialize16(0); //time to stay (ms) will come here
serialize8(0); //nav flag will come here
}
break;
case MSP_SET_WP:
{
int32_t lat = 0,lon = 0,alt = 0;
uint8_t wp_no = read8(); //get the wp number
lat = read32();
lon = read32();
alt = read32(); // to set altitude (cm)
read16(); // future: to set heading (deg)
read16(); // future: to set time to stay (ms)
read8(); // future: to set nav flag
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
f.GPS_FIX_HOME = 1;
if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps
} else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
GPS_hold[LAT] = lat;
GPS_hold[LON] = lon;
if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps
#if !defined(I2C_GPS)
nav_mode = NAV_MODE_WP;
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
#endif
}
}
headSerialReply(0);
break;
#endif
case MSP_RESET_CONF:
if(!f.ARMED) LoadDefaults();
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
if(!f.ARMED) calibratingA=512;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
if(!f.ARMED) f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
#if defined(SPEK_BIND)
case MSP_BIND:
spekBind();
headSerialReply(0);
break;
#endif
case MSP_EEPROM_WRITE:
writeParams(0);
headSerialReply(0);
break;
case MSP_DEBUG:
s_struct((uint8_t*)&debug,8);
break;
#ifdef DEBUGMSG
case MSP_DEBUGMSG:
{
uint8_t size = debugmsg_available();
if (size > 16) size = 16;
headSerialReply(size);
debugmsg_serialize(size);
}
break;
#endif
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void)
{
/* Basic chip initialization is taken care of in SystemInit() called
* from the startup code. SystemInit() and chip settings are defined
* in the CMSIS system_<part family>.c file.
*/
setup();
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t axis,i;
int16_t delta,deltaSum;
int16_t PTerm,ITerm,DTerm;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
int16_t error,errorAngle =0;
#if PIDcontroller ==1
static int16_t errorGyroI[3] = {0,0,0};
#elif PIDcontroller == 2
int32_t AngleRateTmp = 0, RateError = 0, AngleITerm = 0;
static int32_t errorGyroI[3] = {0,0,0};
#endif
#define RC_FREQ 50
while(1)
{
#ifdef LCD_TELEMETRY_STEP
static char telemetryStepSequence [] = LCD_TELEMETRY_STEP;
static uint8_t telemetryStepIndex = 0;
#endif
#if defined(SPEKTRUM)
if (rcFrameComplete) computeRC();
#endif
#if defined(OPENLRSv2MULTI)
Read_OpenLRS_RC();
#endif
if ((currentTime > rcTime) || rcTime > (currentTime + 20000)) { // 50Hz + additional check to ensure rcTime can reach currentTime
rcTime = currentTime + 20000;
computeRC();
//reset watchdog timer in RC loop to ensure user is not locked out of control
feedWatchDog();
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5*FAILSAVE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level
for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
rcData[THROTTLE] = FAILSAVE_THROTTLE;
if (failsafeCnt > 5*(FAILSAVE_DELAY+FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeEvents++;
}
if ( failsafeCnt > (5*FAILSAVE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
}
failsafeCnt++;
#endif
// end of failsave routine - next change is made with RcOptions setting
if (rcData[THROTTLE] < MINCHECK) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
calibratingG=400;
#if GPS
GPS_reset_home_position();
#endif
}
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
#ifdef TRI
servo[5] = 1500; // we center the yaw servo in conf mode
writeServos();
#endif
#ifdef FLYING_WING
servo[0] = conf.wing_left_mid;
servo[1] = conf.wing_right_mid;
writeServos();
#endif
#ifdef AIRPLANE
for(i = 4; i<7 ;i++) servo[i] = 1500;
writeServos();
#endif
#if defined(LCD_CONF)
configurationLoop(); // beginning LCD configuration
#endif
previousTime = micros();
}
}
#if defined(INFLIGHT_ACC_CALIBRATION)
else if (!f.ARMED && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK){
if (rcDelayCommand == 20){
if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}else{
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
#if defined(BUZZER)
if (AccInflightCalibrationArmed){
toggleBeep = 2;
} else {
toggleBeep = 3;
}
#endif
}
}
}
#endif
else if (conf.activate[BOXARM] > 0) {
if ( rcOptions[BOXARM] && f.OK_TO_ARM
#if defined(FAILSAFE)
&& failsafeCnt <= 1
#endif
) {
f.ARMED = 1;
headFreeModeHold = heading;
} else if (f.ARMED) f.ARMED = 0;
rcDelayCommand = 0;
#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
} else if ( (rcData[YAW] < MINCHECK ) && f.ARMED) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[YAW] > MAXCHECK ) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
#endif
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
} else if ( (rcData[ROLL] < MINCHECK) && f.ARMED) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[ROLL] > MAXCHECK) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) {
if (rcDelayCommand == 20) {
f.ARMED = 1;
headFreeModeHold = heading;
}
#endif
#ifdef LCD_TELEMETRY_AUTO
} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
if (telemetry_auto) {
telemetry_auto = 0;
telemetry = 0;
} else
telemetry_auto = 1;
}
#endif
#ifdef LCD_TELEMETRY_STEP
} else if (rcData[ROLL] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)];
LCDclear(); // make sure to clear away remnants
}
#endif
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > MAXCHECK && !f.ARMED) {
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20) calibratingA=400;
rcDelayCommand++;
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=right, pitch=min
if (rcDelayCommand == 20) f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++;
} else if (rcData[PITCH] > MAXCHECK) {
conf.angleTrim[PITCH]+=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[PITCH] < MINCHECK) {
conf.angleTrim[PITCH]-=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] > MAXCHECK) {
conf.angleTrim[ROLL]+=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else if (rcData[ROLL] < MINCHECK) {
conf.angleTrim[ROLL]-=2;writeParams(1);
#if defined(LED_RING)
blinkLedRing();
#endif
} else {
rcDelayCommand = 0;
}
}
#if defined(LED_FLASHER)
led_flasher_autoselect_sequence();
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){
InflightcalibratingA = 50;
}
}else if(AccInflightCalibrationMeasurementDone && !f.ARMED){
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
#endif
uint16_t auxState = 0;
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if (( rcOptions[BOXACC] || (failsafeCnt > 5*FAILSAVE_DELAY) ) && ACC ) {
// bumpless transfer to Level mode
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1;
}
} else {
// failsafe support
f.ACC_MODE = 0;
}
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
//if (f.ACC_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0;
BaroPID=0;
}
} else {
f.BARO_MODE = 0;
}
#endif
#if MAG
if (rcOptions[BOXMAG]) {
if (!f.MAG_MODE) {
f.MAG_MODE = 1;
magHold = heading;
}
} else {
f.MAG_MODE = 0;
}
if (rcOptions[BOXHEADFREE]) {
if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1;
}
} else {
f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
#endif
#if GPS
#if defined(I2C_NAV)
static uint8_t GPSNavReset = 1;
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (!rcOptions[BOXGPSHOME] && !rcOptions[BOXGPSHOLD] )
{ //Both boxes are unselected
if (GPSNavReset == 0 ) {
GPSNavReset = 1;
GPS_reset_nav();
}
}
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPSNavReset = 0;
GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {
f.GPS_HOLD_MODE = 1;
GPSNavReset = 0;
GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
#endif
#if defined(GPS_SERIAL) || defined(TINY_GPS) || defined(GPS_FROM_OSD)||defined(I2C_GPS)
if (f.GPS_FIX && GPS_numSat >= 5 ) {
if (rcOptions[BOXGPSHOME]) {
if (!f.GPS_HOME_MODE) {
f.GPS_HOME_MODE = 1;
GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
}
if (rcOptions[BOXGPSHOLD]) {
if (!f.GPS_HOLD_MODE) {
f.GPS_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
} else {
f.GPS_HOLD_MODE = 0;
}
}
#endif
#endif
#ifdef FLOW
if(rcOptions[BOXFLOWHOLD])
{
if (!f.FLOW_HOLD_MODE)
{
f.FLOW_HOLD_MODE = 1;
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
flow_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
}
//debug[1] = 1;
}
else
{
f.FLOW_HOLD_MODE = 0;
//debug[1] = 0;
}
#endif
if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
else {f.PASSTHRU_MODE = 0;}
#ifdef FIXEDWING
f.HEADFREE_MODE = 0;
#endif
} else { // not in rc loop
static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
if(taskOrder>4) taskOrder-=5;
switch (taskOrder) {
case 0:
taskOrder++;
#if MAG
if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something
#endif
case 1:
taskOrder++;
#if BARO
if (Baro_update() != 0 ) break;
#endif
case 2:
taskOrder++;
#if BARO
if (getEstimatedAltitude() !=0) break;
#endif
case 3:
taskOrder++;
if(flowUpdate() !=0) break;
#if GPS
if(GPS_Enable) GPS_NewData();
break;
#endif
case 4:
taskOrder++;
#if SONAR
#if !defined(MAXSONAR_PWM) && !defined(FLOW)
Sonar_update();
#endif
//debug[2] = sonarAlt;
#endif
#ifdef LANDING_LIGHTS_DDR
auto_switch_landing_lights();
#endif
#ifdef VARIOMETER
if (f.VARIO_MODE) vario_signaling();
#endif
break;
}
}
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
//delayMs(500);
//if(cycleTime > 3150)
// debug[0]++;
#if MAG
if (fabs(rcCommand[YAW]) <70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]/30; // 18 deg
} else magHold = heading;
#endif
#if BARO
if (f.BARO_MODE) {
if (fabs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
#endif
#if GPS
if ( (!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME ) {
GPS_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
float sin_yaw_y = sinf(heading*0.0174532925f);
float cos_yaw_x = cosf(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
}
//debug[2] = GPS_angle[ROLL];
//debug[3] = GPS_angle[PITCH];
#endif
#ifdef FLOW
if (!f.FLOW_HOLD_MODE) {
flow_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters
GPS_angle[ROLL] = 0;
GPS_angle[PITCH] = 0;
} else {
float sin_yaw_y = sinf(heading*0.0174532925f);
float cos_yaw_x = cosf(heading*0.0174532925f);
#if defined(NAV_SLEW_RATE)
nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
#else
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
#endif
}
//debug[2] = GPS_angle[ROLL];
//debug[3] = GPS_angle[PITCH];
#endif
#if PIDcontroller == 1
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if (f.ACC_MODE && axis<2 ) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2*rcCommand[axis] + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTerm = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
#else
PTerm = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTerm = constrain(PTerm,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITerm = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
} else { //ACRO MODE or YAW axis
if (fabs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTerm = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here
if (fabs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if (fabs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (fabs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
