コード例 #1
0
size_t CNetworkHost::Serialize(unsigned char *buf) const
{
	unsigned char *p = buf;

	p += serialize32(p, this->Host);
	p += serialize16(p, this->Port);
	p += serialize16(p, this->PlyNr);
	p += serialize(p, this->PlyName);

	return p - buf;
}
コード例 #2
0
size_t CNetworkSelection::Serialize(unsigned char *buf) const
{
	unsigned char *p = buf;

	p += serialize16(p, this->player);
	p += serialize16(p, uint16_t(this->Units.size()));
	for (size_t i = 0; i != this->Units.size(); ++i) {
		p += serialize16(p, Units[i]);
	}
	return p - buf;
}
コード例 #3
0
static void sendTime(void)
{
    uint32_t seconds = millis() / 1000;
    uint8_t minutes = (seconds / 60) % 60;

    // if we fly for more than an hour, something's wrong anyway
    sendDataHead(ID_HOUR_MINUTE);
    serialize16(minutes << 8);
    sendDataHead(ID_SECOND);
    serialize16(seconds % 60);
}
コード例 #4
0
ファイル: frsky.c プロジェクト: Echelon9/cleanflight
static void sendFuelLevel(void)
{

    sendDataHead(ID_FUEL_LEVEL);

    if (batteryConfig()->batteryCapacity > 0) {
        serialize16((uint16_t)batteryCapacityRemainingPercentage());
    } else {
        amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
        serialize16((uint16_t)constrain(state->mAhDrawn, 0, 0xFFFF));
    }
}
コード例 #5
0
static void sendGpsAltitude(void)
{
    uint16_t altitude = GPS_altitude;
    //Send real GPS altitude only if it's reliable (there's a GPS fix)
    if (!STATE(GPS_FIX)) {
        altitude = 0;
    }
    sendDataHead(ID_GPS_ALTIDUTE_BP);
    serialize16(altitude);
    sendDataHead(ID_GPS_ALTIDUTE_AP);
    serialize16(0);
}
コード例 #6
0
static void sendSpeed(void)
{
    if (!STATE(GPS_FIX)) {
        return;
    }
    //Speed should be sent in knots (GPS speed is in cm/s)
    sendDataHead(ID_GPS_SPEED_BP);
    //convert to knots: 1cm/s = 0.0194384449 knots
    serialize16(GPS_speed * 1944 / 100000);
    sendDataHead(ID_GPS_SPEED_AP);
    serialize16((GPS_speed * 1944 / 100) % 100);
}
コード例 #7
0
static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
    uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
    sendDataHead(ID_RPM);
    if (ARMING_FLAG(ARMED)) {
        throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
        if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
                    throttleForRPM = 0;
        serialize16(throttleForRPM);
    } else {
        serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
    }

}
コード例 #8
0
size_t serialize(unsigned char *buf, const std::vector<unsigned char> &data)
{
	if (buf) {
		if (data.empty()) {
			return serialize16(buf, uint16_t(data.size()));
		}
		buf += serialize16(buf, uint16_t(data.size()));
		memcpy(buf, &data[0], data.size());
		buf += data.size();
		if ((data.size() & 0x03) != 0) {
			memset(buf, 0, data.size() & 0x03);
		}
	}
	return 2 + ((data.size() + 3) & ~0x03); // round up to multiple of 4 for alignment.
}
コード例 #9
0
ファイル: frsky.c プロジェクト: 180jacob/cleanflight
static void sendAccel(void)
{
    for (int i = 0; i < 3; i++) {
        sendDataHead(ID_ACC_X + i);
        serialize16(1000 * (int32_t)accSmooth[i] / acc.acc_1G);
    }
}
コード例 #10
0
/*
 * Send voltage via ID_VOLT
 *
 * NOTE: This sends voltage divided by batteryCellCount. To get the real
 * battery voltage, you need to multiply the value by batteryCellCount.
 */
static void sendVoltage(void)
{
    static uint16_t currentCell = 0;
    uint32_t cellVoltage;
    uint16_t payload;

    /*
     * Format for Voltage Data for single cells is like this:
     *
     *  llll llll cccc hhhh
     *  l: Low voltage bits
     *  h: High voltage bits
     *  c: Cell number (starting at 0)
     *
     * The actual value sent for cell voltage has resolution of 0.002 volts
     * Since vbat has resolution of 0.1 volts it has to be multiplied by 50
     */
    cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);

    // Cell number is at bit 9-12
    payload = (currentCell << 4);

    // Lower voltage bits are at bit 0-8
    payload |= ((cellVoltage & 0x0ff) << 8);

    // Higher voltage bits are at bits 13-15
    payload |= ((cellVoltage & 0xf00) >> 8);

    sendDataHead(ID_VOLT);
    serialize16(payload);

    currentCell++;
    currentCell %= batteryCellCount;
}
コード例 #11
0
ファイル: frsky.c プロジェクト: Echelon9/cleanflight
static void sendAmperage(void)
{
    amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);

    sendDataHead(ID_CURRENT);
    serialize16((uint16_t)(state->amperage / 10));
}
コード例 #12
0
ファイル: frsky.c プロジェクト: phobos-/cleanflight
/*
 * Send voltage with ID_VOLTAGE_AMP
 */
static void sendVoltageAmp(void)
{
    if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
        /*
         * Use new ID 0x39 to send voltage directly in 0.1 volts resolution
         */
        sendDataHead(ID_VOLTAGE_AMP);
        serialize16(vbat);
    } else {
        uint16_t voltage = (vbat * 110) / 21;

        sendDataHead(ID_VOLTAGE_AMP_BP);
        serialize16(voltage / 100);
        sendDataHead(ID_VOLTAGE_AMP_AP);
        serialize16(((voltage % 100) + 5) / 10);
    }
}
コード例 #13
0
static void sendSatalliteSignalQualityAsTemperature2(void)
{
    uint16_t satellite = GPS_numSat;
    if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
        satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
    }
    sendDataHead(ID_TEMPRATURE2);

    if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
        serialize16(satellite);
    } else {
        float tmp = (satellite - 32) / 1.8f;
        //Round the value
        tmp += (tmp < 0) ? -0.5f : 0.5f;
        serialize16(tmp);
    }
}
コード例 #14
0
ファイル: telemetry.c プロジェクト: DuinoPilot/TMR
static void sendAccel(void)
{
    uint8_t i;

    for (i = 0; i < 3; i++) {
        sendDataHead(0x24 + i);
        serialize16(((float)accSmooth[i] / acc_1G) * 1000);
    }
}
コード例 #15
0
static void sendAccel(void)
{
    int i;

    for (i = 0; i < 3; i++) {
        sendDataHead(ID_ACC_X + i);
        serialize16(((float)accSmooth[i] / acc.acc_1G) * 1000);
    }
}
コード例 #16
0
size_t serialize(unsigned char *buf, const std::string &s)
{
	if (buf) {
		buf += serialize16(buf, uint16_t(s.size()));
		memcpy(buf, s.c_str(), s.size());
		buf += s.size();
		if ((s.size() & 0x03) != 0) {
			memset(buf, 0, s.size() & 0x03);
		}
	}
	return 2 + ((s.size() + 3) & ~0x03); // round up to multiple of 4 for alignment.
}
コード例 #17
0
ファイル: frsky.c プロジェクト: FLYFPV/cleanflight
static void sendGPS(void)
{
    int32_t localGPS_coord[2] = {0,0};
    // Don't set dummy GPS data, if we already had a GPS fix
    // it can be usefull to keep last valid coordinates
    static uint8_t gpsFixOccured = 0;

    //Dummy data if no 3D fix, this way we can display heading in Taranis
    if (STATE(GPS_FIX) || gpsFixOccured == 1) {
        localGPS_coord[LAT] = GPS_coord[LAT];
        localGPS_coord[LON] = GPS_coord[LON];
        gpsFixOccured = 1;
    } else {
        // Send dummy GPS Data in order to display compass value
        localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
        localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
    }

    gpsCoordinateDDDMMmmmm_t coordinate;
    GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
    sendDataHead(ID_LATITUDE_BP);
    serialize16(coordinate.dddmm);
    sendDataHead(ID_LATITUDE_AP);
    serialize16(coordinate.mmmm);
    sendDataHead(ID_N_S);
    serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');

    GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
    sendDataHead(ID_LONGITUDE_BP);
    serialize16(coordinate.dddmm);
    sendDataHead(ID_LONGITUDE_AP);
    serialize16(coordinate.mmmm);
    sendDataHead(ID_E_W);
    serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
}
コード例 #18
0
ファイル: net_message.cpp プロジェクト: Wargus/stratagus
size_t serialize(unsigned char *buf, const std::string &s)
{
	if (buf) {
		buf += serialize16(buf, uint16_t(s.size()));
		memcpy(buf, s.c_str(), s.size());
		buf += s.size();
		//Wyrmgus start
		//Andrettin: fix to the multiplayer OOS issue - the fix works, but it would be better if someone reviewed the code! I am leaving the Wyrmgus tags on these changes until the code has been properly reviewed
		/*
		if ((s.size() & 0x03) != 0) {
			memset(buf, 0, s.size() & 0x03);
		}
		*/
		//Wyrmgus end
	}
	//Wyrmgus start
//	return 2 + ((s.size() + 3) & ~0x03); // round up to multiple of 4 for alignment.
	return 2 + (s.size() + 3);
	//Wyrmgus end
}
コード例 #19
0
ファイル: telemetry.c プロジェクト: DuinoPilot/TMR
static void sendGPS(void)
{
    sendDataHead(0x13);
    serialize16(abs(GPS_coord[LAT]) / 100000);
    sendDataHead(0x13 + 8);
    serialize16((abs(GPS_coord[LAT]) / 100000) % 10000);

    sendDataHead(0x1B + 8);
    serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');

    sendDataHead(0x12);
    serialize16(abs(GPS_coord[LON]) / 100000);
    sendDataHead(0x12 + 8);
    serialize16((abs(GPS_coord[LON]) / 100000) % 10000);
    sendDataHead(0x1A + 8);
    serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
}
コード例 #20
0
static void sendGPS(void)
{
    sendDataHead(ID_LATITUDE_BP);
    serialize16(abs(GPS_coord[LAT]) / 100000);
    sendDataHead(ID_LATITUDE_AP);
    serialize16((abs(GPS_coord[LAT]) / 10) % 10000);

    sendDataHead(ID_N_S);
    serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');

    sendDataHead(ID_LONGITUDE_BP);
    serialize16(abs(GPS_coord[LON]) / 100000);
    sendDataHead(ID_LONGITUDE_AP);
    serialize16((abs(GPS_coord[LON]) / 10) % 10000);
    sendDataHead(ID_E_W);
    serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
}
コード例 #21
0
/*
 * Send voltage via ID_VOLT
 *
 * NOTE: This sends voltage divided by batteryCellCount. To get the real
 * battery voltage, you need to multiply the value by batteryCellCount.
 */
static void sendVoltage(void)
{
    static uint16_t currentCell = 0;
    uint16_t cellNumber;
    uint32_t cellVoltage;
    uint16_t payload;

    /*
     * Note: F**k the pdf. Format for Voltage Data for single cells is like this:
     *
     *  llll llll cccc hhhh
     *  l: Low voltage bits
     *  h: High voltage bits
     *  c: Cell number (starting at 0)
     */
    cellVoltage = vbat / batteryCellCount;

    // Map to 12 bit range
    cellVoltage = (cellVoltage * 2100) / 42;

    cellNumber = currentCell % batteryCellCount;

    // Cell number is at bit 9-12
    payload = (cellNumber << 4);

    // Lower voltage bits are at bit 0-8
    payload |= ((cellVoltage & 0x0ff) << 8);

    // Higher voltage bits are at bits 13-15
    payload |= ((cellVoltage & 0xf00) >> 8);

    sendDataHead(ID_VOLT);
    serialize16(payload);

    currentCell++;
    currentCell %= batteryCellCount;
}
コード例 #22
0
static void sendLatLong(int32_t coord[2])
{
    gpsCoordinateDDDMMmmmm_t coordinate;
    GPStoDDDMM_MMMM(coord[LAT], &coordinate);
    sendDataHead(ID_LATITUDE_BP);
    serialize16(coordinate.dddmm);
    sendDataHead(ID_LATITUDE_AP);
    serialize16(coordinate.mmmm);
    sendDataHead(ID_N_S);
    serialize16(coord[LAT] < 0 ? 'S' : 'N');

    GPStoDDDMM_MMMM(coord[LON], &coordinate);
    sendDataHead(ID_LONGITUDE_BP);
    serialize16(coordinate.dddmm);
    sendDataHead(ID_LONGITUDE_AP);
    serialize16(coordinate.mmmm);
    sendDataHead(ID_E_W);
    serialize16(coord[LON] < 0 ? 'W' : 'E');
}
コード例 #23
0
static void sendAmperage(void)
{
    sendDataHead(ID_CURRENT);
    serialize16((uint16_t)(amperage / 10));
}
コード例 #24
0
ファイル: serial.c プロジェクト: fiendie/baseflight
static void evaluateCommand(void)
{
    uint32_t i, j, tmp, junk;
#ifdef GPS
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;
#endif
    const char *build = __DATE__;

    switch (currentPortState->cmdMSP) {
        case MSP_SET_RAW_RC:
            for (i = 0; i < 8; i++)
                rcData[i] = read16();
            headSerialReply(0);
            mspFrameRecieve();
            break;
        case MSP_SET_ACC_TRIM:
            cfg.angleTrim[PITCH] = read16();
            cfg.angleTrim[ROLL]  = read16();
            headSerialReply(0);
            break;
#ifdef 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:
            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 < numberBoxItems; i++)
                cfg.activate[availableBoxes[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:
            tmp = read16();
            // sanity check
            if (tmp < 1600 && tmp > 1400)
                mcfg.midrc = tmp;
            mcfg.minthrottle = read16();
            mcfg.maxthrottle = read16();
            mcfg.mincommand = read16();
            cfg.failsafe_throttle = read16();
            mcfg.gps_type = read8();
            mcfg.gps_baudrate = read8();
            mcfg.gps_ubx_sbas = read8();
            mcfg.multiwiicurrentoutput = read8();
            mcfg.rssi_aux_channel = read8();
            read8();
            cfg.mag_declination = read16() * 10;
            mcfg.vbatscale = read8();           // actual vbatscale as intended
            mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
            mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
            mcfg.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
            headSerialReply(0);
            break;
        case MSP_SET_MOTOR:
            for (i = 0; i < 8; i++)
                motor_disarmed[i] = read16();
            headSerialReply(0);
            break;
        case MSP_SELECT_SETTING:
            if (!f.ARMED) {
                mcfg.current_profile = read8();
                if (mcfg.current_profile > 2)
                    mcfg.current_profile = 0;
                // this writes new profile index and re-reads it
                writeEEPROM(0, false);
            }
            headSerialReply(0);
            break;
        case MSP_SET_HEAD:
            magHold = read16();
            headSerialReply(0);
            break;
        case MSP_IDENT:
            headSerialReply(7);
            serialize8(VERSION);                    // multiwii version
            serialize8(mcfg.mixerConfiguration);    // type of multicopter
            serialize8(MSP_VERSION);                // MultiWii Serial Protocol Version
            serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | CAP_FW_FLAPS); // "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);
            // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
            // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
            // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
            junk = 0;
            tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                  f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                  rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                  f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                  f.PASSTHRU_MODE << BOXPASSTHRU |
                  rcOptions[BOXBEEPERON] << BOXBEEPERON |
                  rcOptions[BOXLEDMAX] << BOXLEDMAX |
                  rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                  rcOptions[BOXVARIO] << BOXVARIO |
                  rcOptions[BOXCALIB] << BOXCALIB |
                  rcOptions[BOXGOV] << BOXGOV |
                  rcOptions[BOXOSD] << BOXOSD |
                  rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
                  rcOptions[BOXSERVO1] << BOXSERVO1 |
                  rcOptions[BOXSERVO2] << BOXSERVO2 |
                  rcOptions[BOXSERVO3] << BOXSERVO3 |
                  f.ARMED << BOXARM;
            for (i = 0; i < numberBoxItems; i++) {
                int flag = (tmp & (1 << availableBoxes[i]));
                if (flag)
                    junk |= 1 << i;
            }
            serialize32(junk);
            serialize8(mcfg.current_profile);
            break;
        case MSP_RAW_IMU:
            headSerialReply(18);
            // Retarded hack until multiwiidorks start using real units for sensor data
            if (acc_1G > 1024) {
                for (i = 0; i < 3; i++)
                    serialize16(accSmooth[i] / 8);
            } else {
                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:
            s_struct((uint8_t *)&servo, 16);
            break;
        case MSP_SERVO_CONF:
            headSerialReply(56);
            for (i = 0; i < MAX_SERVOS; i++) {
                serialize16(cfg.servoConf[i].min);
                serialize16(cfg.servoConf[i].max);
                serialize16(cfg.servoConf[i].middle);
                serialize8(getOldServoConfig(i));
            }
            break;
        case MSP_SET_SERVO_CONF:
            headSerialReply(0);
            for (i = 0; i < MAX_SERVOS; i++) {
                cfg.servoConf[i].min = read16();
                cfg.servoConf[i].max = read16();
                cfg.servoConf[i].middle = read16();
                tmp = read8();
                // trash old servo direction
                cfg.servoConf[i].rate = tmp & 0xfc;

                // store old style servo direction depending on current mixer configuration
                switch (mcfg.mixerConfiguration) {
                    case MULTITYPE_BI:
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_PITCH);

                        storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW);

                        break;
                    case MULTITYPE_TRI:
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_YAW);

                        break;
                    case MULTITYPE_FLYING_WING:
                        storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);

                        storeOldServoConfig(i, tmp, 3, 2, INPUT_ROLL);
                        storeOldServoConfig(i, tmp, 4, 2, INPUT_ROLL);

                        break;
                    case MULTITYPE_DUALCOPTER:
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL);

                        break;
                    case MULTITYPE_SINGLECOPTER:
                        storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL);
                        storeOldServoConfig(i, tmp, 6, 1, INPUT_ROLL);

                        storeOldServoConfig(i, tmp, 3, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 6, 2, INPUT_YAW);

                        break;
                }
            }
            break;
        case MSP_MOTOR:
            s_struct((uint8_t *)motor, 16);
            break;
        case MSP_RC:
            headSerialReply(16);
            for (i = 0; i < 8; i++)
                serialize16(rcData[i]);
            break;
#ifdef 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);
            break;
        case MSP_COMP_GPS:
            headSerialReply(5);
            serialize16(GPS_distanceToHome);
            serialize16(GPS_directionToHome);
            serialize8(GPS_update & 1);
            break;
#endif
        case MSP_ATTITUDE:
            headSerialReply(6);
            for (i = 0; i < 2; i++)
                serialize16(angle[i]);
            serialize16(heading);
            break;
        case MSP_ALTITUDE:
            headSerialReply(6);
            serialize32(EstAlt);
            serialize16(vario);
            break;
        case MSP_ANALOG:
            headSerialReply(7);
            serialize8((uint8_t)constrain(vbat, 0, 255));
            serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery
            serialize16(rssi);
            if (mcfg.multiwiicurrentoutput)
                serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
            else
                serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
            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_PIDNAMES:
            headSerialReply(sizeof(pidnames) - 1);
            serializeNames(pidnames);
            break;
        case MSP_BOX:
            headSerialReply(2 * numberBoxItems);
            for (i = 0; i < numberBoxItems; i++)
                serialize16(cfg.activate[availableBoxes[i]]);
            break;
        case MSP_BOXNAMES:
            // headSerialReply(sizeof(boxnames) - 1);
            serializeBoxNamesReply();
            break;
        case MSP_BOXIDS:
            headSerialReply(numberBoxItems);
            for (i = 0; i < numberBoxItems; i++) {
                for  (j = 0; j < CHECKBOXITEMS; j++) {
                    if (boxes[j].permanentId == availableBoxes[i])
                        serialize8(boxes[j].permanentId);
                }
            }
            break;
        case MSP_MISC:
            headSerialReply(2 * 6 + 4 + 2 + 4);
            serialize16(mcfg.midrc);
            serialize16(mcfg.minthrottle);
            serialize16(mcfg.maxthrottle);
            serialize16(mcfg.mincommand);
            serialize16(cfg.failsafe_throttle);
            serialize8(mcfg.gps_type);
            serialize8(mcfg.gps_baudrate);
            serialize8(mcfg.gps_ubx_sbas);
            serialize8(mcfg.multiwiicurrentoutput);
            serialize8(mcfg.rssi_aux_channel);
            serialize8(0);
            serialize16(cfg.mag_declination / 10); // TODO check this shit
            serialize8(mcfg.vbatscale);
            serialize8(mcfg.vbatmincellvoltage);
            serialize8(mcfg.vbatmaxcellvoltage);
            serialize8(mcfg.vbatwarningcellvoltage);
            break;
        case MSP_MOTOR_PINS:
            headSerialReply(8);
            for (i = 0; i < 8; i++)
                serialize8(i + 1);
            break;
#ifdef GPS
        case MSP_WP:
            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:
            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
                nav_mode = NAV_MODE_WP;
                GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
            }
            headSerialReply(0);
            break;
#endif /* GPS */
        case MSP_RESET_CONF:
            if (!f.ARMED)
                checkFirstTime(true);
            headSerialReply(0);
            break;
        case MSP_ACC_CALIBRATION:
            if (!f.ARMED)
                calibratingA = CALIBRATING_ACC_CYCLES;
            headSerialReply(0);
            break;
        case MSP_MAG_CALIBRATION:
            if (!f.ARMED)
                f.CALIBRATE_MAG = 1;
            headSerialReply(0);
            break;
        case MSP_EEPROM_WRITE:
            if (f.ARMED) {
                headSerialError(0);
            } else {
                writeEEPROM(0, true);
                headSerialReply(0);
            }
            break;
        case MSP_DEBUG:
            headSerialReply(8);
            // make use of this crap, output some useful QA statistics
            debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
            for (i = 0; i < 4; i++)
                serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
            break;

        // Additional commands that are not compatible with MultiWii
        case MSP_ACC_TRIM:
            headSerialReply(4);
            serialize16(cfg.angleTrim[PITCH]);
            serialize16(cfg.angleTrim[ROLL]);
            break;
        case MSP_UID:
            headSerialReply(12);
            serialize32(U_ID_0);
            serialize32(U_ID_1);
            serialize32(U_ID_2);
            break;
#ifdef GPS
        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]);
            }
            // Poll new SVINFO from GPS
            gpsPollSvinfo();
            break;
        case MSP_GPSDEBUGINFO:
            headSerialReply(16);
            if (sensors(SENSOR_GPS)) {
                serialize32(GPS_update_rate[1] - GPS_update_rate[0]);
                serialize32(GPS_svinfo_rate[1] - GPS_svinfo_rate[0]);
            } else {
                serialize32(0);
                serialize32(0);
            }
            serialize32(GPS_HorizontalAcc);
            serialize32(GPS_VerticalAcc);
            break;
#endif /* GPS */

        case MSP_SET_CONFIG:
            headSerialReply(0);
            mcfg.mixerConfiguration = read8(); // multitype
            featureClearAll();
            featureSet(read32()); // features bitmap
            mcfg.serialrx_type = read8(); // serialrx_type
            mcfg.board_align_roll = read16(); // board_align_roll
            mcfg.board_align_pitch = read16(); // board_align_pitch
            mcfg.board_align_yaw = read16(); // board_align_yaw
            mcfg.currentscale = read16();
            mcfg.currentoffset = read16();
            /// ???
            break;
        case MSP_CONFIG:
            headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4);
            serialize8(mcfg.mixerConfiguration);
            serialize32(featureMask());
            serialize8(mcfg.serialrx_type);
            serialize16(mcfg.board_align_roll);
            serialize16(mcfg.board_align_pitch);
            serialize16(mcfg.board_align_yaw);
            serialize16(mcfg.currentscale);
            serialize16(mcfg.currentoffset);
            /// ???
            break;

        case MSP_RCMAP:
            headSerialReply(MAX_INPUTS); // TODO fix this
            for (i = 0; i < MAX_INPUTS; i++)
                serialize8(mcfg.rcmap[i]);
            break;
        case MSP_SET_RCMAP:
            headSerialReply(0);
            for (i = 0; i < MAX_INPUTS; i++)
                mcfg.rcmap[i] = read8();
            break;

        case MSP_REBOOT:
            headSerialReply(0);
            pendReboot = true;
            break;

        case MSP_BUILDINFO:
            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
            break;

        default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
            headSerialError(0);
            break;
    }
    tailSerialReply();
}
コード例 #25
0
/*
 * Send vertical speed for opentx. ID_VERT_SPEED
 * Unit is cm/s
 */
static void sendVario(void)
{
    sendDataHead(ID_VERT_SPEED);
    serialize16(vario);
}
コード例 #26
0
ファイル: serial.c プロジェクト: jef79m/baseflight
static void evaluateCommand(void)
{
    uint32_t i, tmp, junk;
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;

    switch (cmdMSP) {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcData[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 < numberBoxItems; i++)
            cfg.activate[availableBoxes[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:
        read16(); // powerfailmeter
        mcfg.minthrottle = read16();
        read32(); // mcfg.maxthrottle, mcfg.mincommand
        cfg.failsafe_throttle = read16();
        read16();
        read32();
        cfg.mag_declination = read16() * 10;
        mcfg.vbatscale = read8();           // actual vbatscale as intended
        mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
        mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
        read8();                            // vbatlevel_crit (unused)
        headSerialReply(0);
        break;
    case MSP_SET_MOTOR:
        for (i = 0; i < 8; i++)
            motor_disarmed[i] = read16();
        break;
    case MSP_SELECT_SETTING:
        if (!f.ARMED) {
            mcfg.current_profile = read8();
            if (mcfg.current_profile > 2)
                mcfg.current_profile = 0;
            // this writes new profile index and re-reads it
            writeEEPROM(0, false);
        }
        headSerialReply(0);
        break;
    case MSP_SET_HEAD:
        magHold = read16();
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(mcfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0));        // "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);
        // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
        // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
        // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
        junk = 0;
        tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                    f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                    f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON |
                    rcOptions[BOXLEDMAX] << BOXLEDMAX |
                    rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                    rcOptions[BOXVARIO] << BOXVARIO |
                    rcOptions[BOXCALIB] << BOXCALIB |
                    rcOptions[BOXGOV] << BOXGOV |
                    rcOptions[BOXOSD] << BOXOSD |
                    f.ARMED << BOXARM;
        for (i = 0; i < numberBoxItems; i++) {
            int flag = (tmp & (1 << availableBoxes[i]));
            if (flag)
                junk |= 1 << i;
        }
        serialize32(junk);
        serialize8(mcfg.current_profile);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        // Retarded hack until multiwiidorks start using real units for sensor data
        if (acc_1G > 1024) {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i] / 8);
        } else {
            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:
        s_struct((uint8_t *)&servo, 16);
        break;
    case MSP_SERVO_CONF:
        headSerialReply(56);
        for (i = 0; i < MAX_SERVOS; i++) {
            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:
        headSerialReply(0);
        for (i = 0; i < MAX_SERVOS; i++) {
            cfg.servoConf[i].min = read16();
            cfg.servoConf[i].max = read16();
            cfg.servoConf[i].middle = read16();
            cfg.servoConf[i].rate = read8();
        }
        break;
    case MSP_MOTOR:
        s_struct((uint8_t *)motor, 16);
        break;
    case MSP_RC:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(rcData[i]);
        break;
    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);
        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(6);
        serialize32(EstAlt);
        serialize16(vario);
        break;
    case MSP_ANALOG:
        headSerialReply(5);
        serialize8(vbat);
        serialize16(0); // power meter trash
        serialize16(rssi);
        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_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_BOX:
        headSerialReply(2 * numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize16(cfg.activate[availableBoxes[i]]);
        break;
    case MSP_BOXNAMES:
        // headSerialReply(sizeof(boxnames) - 1);
        serializeBoxNamesReply();
        break;
    case MSP_BOXIDS:
        headSerialReply(numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize8(availableBoxes[i]);
        break;
    case MSP_MISC:
        headSerialReply(2 * 6 + 4 + 2 + 4);
        serialize16(0); // intPowerTrigger1 (aka useless trash)
        serialize16(mcfg.minthrottle);
        serialize16(mcfg.maxthrottle);
        serialize16(mcfg.mincommand);
        serialize16(cfg.failsafe_throttle);
        serialize16(0); // plog useless shit
        serialize32(0); // plog useless shit
        serialize16(cfg.mag_declination / 10); // TODO check this shit
        serialize8(mcfg.vbatscale);
        serialize8(mcfg.vbatmincellvoltage);
        serialize8(mcfg.vbatmaxcellvoltage);
        serialize8(0);
        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(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:
        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
            nav_mode = NAV_MODE_WP;
            GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
        }
        headSerialReply(0);
        break;
    case MSP_RESET_CONF:
        if (!f.ARMED)
            checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        if (!f.ARMED)
            calibratingA = CALIBRATING_ACC_CYCLES;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        if (!f.ARMED)
            f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeEEPROM(0, true);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        // make use of this crap, output some useful QA statistics
        debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
        for (i = 0; i < 4; i++)
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

    // Additional commands that are not compatible with MultiWii
    case MSP_ACC_TRIM:
        headSerialReply(4);
        serialize16(cfg.angleTrim[PITCH]);
        serialize16(cfg.angleTrim[ROLL]);
        break;
    case MSP_UID:
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        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]);
            }
        break;
    default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
コード例 #27
0
static void sendTemperature1(void)
{
    sendDataHead(ID_TEMPRATURE1);
    serialize16((int16_t)telemTemperature1 / 10);
}
コード例 #28
0
ファイル: msp.cpp プロジェクト: f8industries/hackflight
void MSP::update(bool armed)
{
    static bool pendReboot;

    // pendReboot will be set for flashing
    this->_board->checkReboot(pendReboot);

    while (this->_board->serialAvailableBytes()) {

        uint8_t c = this->_board->serialReadByte();

        if (portState.c_state == IDLE) {
            portState.c_state = (c == '$') ? HEADER_START : IDLE;
            if (portState.c_state == IDLE && !armed) {
                if (c == '#')
                    ;
                else if (c == CONFIG_REBOOT_CHARACTER) 
                    this->_board->reboot();
            }
        } else if (portState.c_state == HEADER_START) {
            portState.c_state = (c == 'M') ? HEADER_M : IDLE;
        } else if (portState.c_state == HEADER_M) {
            portState.c_state = (c == '<') ? HEADER_ARROW : IDLE;
        } else if (portState.c_state == HEADER_ARROW) {
            if (c > INBUF_SIZE) {       // now we are expecting the payload size
                portState.c_state = IDLE;
                continue;
            }
            portState.dataSize = c;
            portState.offset = 0;
            portState.checksum = 0;
            portState.indRX = 0;
            portState.checksum ^= c;
            portState.c_state = HEADER_SIZE;      // the command is to follow
        } else if (portState.c_state == HEADER_SIZE) {
            portState.cmdMSP = c;
            portState.checksum ^= c;
            portState.c_state = HEADER_CMD;
        } else if (portState.c_state == HEADER_CMD && 
                portState.offset < portState.dataSize) {
            portState.checksum ^= c;
            portState.inBuf[portState.offset++] = c;
        } else if (portState.c_state == HEADER_CMD && portState.offset >= portState.dataSize) {

            if (portState.checksum == c) {        // compare calculated and transferred checksum

                switch (portState.cmdMSP) {

                    case MSP_SET_RAW_RC:
                        for (uint8_t i = 0; i < 8; i++)
                            this->_rc->data[i] = read16();
                        headSerialReply(0);
                        break;

                    case MSP_SET_MOTOR:
                        for (uint8_t i = 0; i < 4; i++)
                            this->_mixer->motorsDisarmed[i] = read16();
                        headSerialReply(0);
                        break;

                    case MSP_RC:
                        headSerialReply(16);
                        for (uint8_t i = 0; i < 8; i++)
                            serialize16(this->_rc->data[i]);
                        break;

                    case MSP_ATTITUDE:
                        headSerialReply(6);
                        for (uint8_t i = 0; i < 3; i++)
                            serialize16(this->_imu->angle[i]);
                        break;

                    case MSP_BARO_SONAR_RAW:
                        //headSerialReply(8);
                        //serialize32(baroPressure);
                        //serialize32(sonarDistance);
                        break;

                    case MSP_ALTITUDE:
                        //headSerialReply(6);
                        //serialize32(estAlt);
                        //serialize16(vario);
                        break;

                    case MSP_REBOOT:
                        headSerialReply(0);
                        pendReboot = true;
                        break;

                    // don't know how to handle the (valid) message, indicate error MSP $M!
                    default:                   
                        headSerialError(0);
                        break;
                }
                tailSerialReply();
            }
            portState.c_state = IDLE;
        }
    }
}
コード例 #29
0
ファイル: telemetry.c プロジェクト: DuinoPilot/TMR
static void sendTemperature1(void)
{
    sendDataHead(0x02);
    serialize16(telemTemperature1 / 10);
}
コード例 #30
0
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();
}