Пример #1
0
static void cliSave(char *cmdline)
{
    uartPrint("Saving...");
    writeParams(0);
    uartPrint("\r\nRebooting...");
    delay(10);
    systemReset(false);
}
Пример #2
0
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.
}
Пример #3
0
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);
        }
    }
}
Пример #4
0
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.
}
Пример #5
0
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;
}
Пример #6
0
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);
}
Пример #7
0
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
}
Пример #8
0
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;
	}
}
Пример #9
0
Файл: mw.c Проект: trigrass2/tmr
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;
            }
Пример #10
0
// 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&regulation, 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);
}
Пример #11
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);
}
Пример #12
0
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();
}
Пример #13
0
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
Пример #14
0
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;
  }
}
Пример #15
0
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; // f**k 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);
}
Пример #16
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;
  }
Пример #17
0
// 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; // f**k 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);
}
Пример #18
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;
    }
}
Пример #19
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];
}
Пример #20
0
// 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();
}
Пример #21
0
// 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);
}
Пример #22
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();
}
Пример #23
0
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();
}
Пример #25
0
/*******************************************************************************
**   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