static void cliFeature(char *cmdline)
{
uint32_t i;
uint32_t len;
uint32_t mask;
len = strlen(cmdline);
mask = featureMask();
if (len == 0) {
cliPrint("Enabled features: ");
for (i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("%s ", featureNames[i]);
}
cliPrint("\r\n");
} else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available features: ");
for (i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
printf("%s ", featureNames[i]);
}
cliPrint("\r\n");
return;
} else {
bool remove = false;
if (cmdline[0] == '-') {
// remove feature
remove = true;
cmdline++; // skip over -
len--;
}
for (i = 0; ; i++) {
if (featureNames[i] == NULL) {
cliPrint("Invalid feature name...\r\n");
break;
}
if (strncasecmp(cmdline, featureNames[i], len) == 0) {
if (remove) {
featureClear(1 << i);
cliPrint("Disabled ");
} else {
featureSet(1 << i);
cliPrint("Enabled ");
}
printf("%s\r\n", featureNames[i]);
break;
}
}
}
}
static void cliDump(char *cmdline)
{
(void)cmdline;
unsigned int i, channel;
char buf[16];
float thr, roll, pitch, yaw;
uint32_t mask;
const clivalue_t *setval;
cliVersion(NULL);
printf("Current Config: Copy everything below here...\r\n");
// print out aux switches
cliAux("");
// print out current motor mix
printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
// print custom mix if exists
if (mcfg.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f)
break;
thr = mcfg.customMixer[i].throttle;
roll = mcfg.customMixer[i].roll;
pitch = mcfg.customMixer[i].pitch;
yaw = mcfg.customMixer[i].yaw;
printf("cmix %d", i + 1);
if (thr < 0)
printf(" ");
printf("%s", ftoa(thr, buf));
if (roll < 0)
printf(" ");
printf("%s", ftoa(roll, buf));
if (pitch < 0)
printf(" ");
printf("%s", ftoa(pitch, buf));
if (yaw < 0)
printf(" ");
printf("%s\r\n", ftoa(yaw, buf));
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
}
// print custom servo mixer if exists
if (mcfg.customServoMixer[0].rate != 0) {
for (i = 0; i < MAX_SERVO_RULES; i++) {
if (mcfg.customServoMixer[i].rate == 0)
break;
printf("smix %d ", i + 1);
printf("%d ", mcfg.customServoMixer[i].targetChannel + 1);
printf("%d ", mcfg.customServoMixer[i].fromChannel + 1);
printf("%d ", mcfg.customServoMixer[i].rate);
printf("%d ", mcfg.customServoMixer[i].speed);
printf("%d ", mcfg.customServoMixer[i].min);
printf("%d ", mcfg.customServoMixer[i].max);
printf("%d\r\n", mcfg.customServoMixer[i].box);
}
printf("smix %d 0 0 0 0\r\n", i + 1);
}
// print servo directions
for (i = 0; i < MAX_SERVOS; i++)
for (channel = 0; channel < INPUT_ITEMS; channel++)
if (cfg.servoConf[i].direction & (1 << channel))
printf("smix direction %d %d -1\r\n", i + 1 , channel + 1);
// print servo config
for (i = 0; i < MAX_SERVOS; i++)
printf("servo %d %d %d %d %d\r\n", i + 1, cfg.servoConf[i].min, cfg.servoConf[i].middle, cfg.servoConf[i].max, cfg.servoConf[i].rate);
// print enabled features
mask = featureMask();
for (i = 0; ; i++) { // disable all feature first
if (featureNames[i] == NULL)
break;
printf("feature -%s\r\n", featureNames[i]);
}
for (i = 0; ; i++) { // reenable what we want.
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("feature %s\r\n", featureNames[i]);
}
// print RC MAPPING
for (i = 0; i < mcfg.rc_channel_count; i++)
buf[mcfg.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0';
printf("map %s\r\n", buf);
// print settings
for (i = 0; i < VALUE_COUNT; i++) {
setval = &valueTable[i];
printf("set %s = ", valueTable[i].name);
cliPrintVar(setval, 0);
cliPrint("\r\n");
}
}
static void cliDump(char *cmdline)
{
int i;
char buf[16];
float thr, roll, pitch, yaw;
uint32_t mask;
const clivalue_t *setval;
printf("Current Config: Copy everything below here...\r\n");
// print out aux switches
cliAux("");
// print out current motor mix
printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
// print custom mix if exists
if (mcfg.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f)
break;
thr = mcfg.customMixer[i].throttle;
roll = mcfg.customMixer[i].roll;
pitch = mcfg.customMixer[i].pitch;
yaw = mcfg.customMixer[i].yaw;
printf("cmix %d", i + 1);
if (thr < 0)
printf(" ");
printf("%s", ftoa(thr, buf));
if (roll < 0)
printf(" ");
printf("%s", ftoa(roll, buf));
if (pitch < 0)
printf(" ");
printf("%s", ftoa(pitch, buf));
if (yaw < 0)
printf(" ");
printf("%s\r\n", ftoa(yaw, buf));
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
}
// print enabled features
mask = featureMask();
for (i = 0; ; i++) { // disable all feature first
if (featureNames[i] == NULL)
break;
printf("feature -%s\r\n", featureNames[i]);
}
for (i = 0; ; i++) { // reenable what we want.
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("feature %s\r\n", featureNames[i]);
}
// print RC MAPPING
for (i = 0; i < 8; i++)
buf[mcfg.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0';
printf("map %s\r\n", buf);
// print settings
for (i = 0; i < VALUE_COUNT; i++) {
setval = &valueTable[i];
printf("set %s = ", valueTable[i].name);
cliPrintVar(setval, 0);
cliPrint("\r\n");
}
}
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();
}
static void cliDump(char *cmdline)
{
unsigned int i;
char buf[17];
float thr, roll, pitch, yaw;
uint32_t mask;
uint8_t dumpMask = DUMP_ALL;
if (strcasecmp(cmdline, "master") == 0) {
dumpMask = DUMP_MASTER; // only
}
if (strcasecmp(cmdline, "profile") == 0) {
dumpMask = DUMP_PROFILE; // only
}
if (dumpMask & DUMP_MASTER) {
printf("\r\n# dump master\r\n");
printf("\r\n# mixer\r\n");
printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
if (masterConfig.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
if (masterConfig.customMixer[i].throttle == 0.0f)
break;
thr = masterConfig.customMixer[i].throttle;
roll = masterConfig.customMixer[i].roll;
pitch = masterConfig.customMixer[i].pitch;
yaw = masterConfig.customMixer[i].yaw;
printf("cmix %d", i + 1);
if (thr < 0)
printf(" ");
printf("%s", ftoa(thr, buf));
if (roll < 0)
printf(" ");
printf("%s", ftoa(roll, buf));
if (pitch < 0)
printf(" ");
printf("%s", ftoa(pitch, buf));
if (yaw < 0)
printf(" ");
printf("%s\r\n", ftoa(yaw, buf));
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
}
printf("\r\n\r\n# feature\r\n");
mask = featureMask();
for (i = 0; ; i++) { // disable all feature first
if (featureNames[i] == NULL)
break;
printf("feature -%s\r\n", featureNames[i]);
}
for (i = 0; ; i++) { // reenable what we want.
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("feature %s\r\n", featureNames[i]);
}
printf("\r\n\r\n# map\r\n");
for (i = 0; i < rxRuntimeConfig.channelCount; i++)
buf[i] = '-';
buf[i] = '\0';
for (i = 0; i < 12; i++)
if (masterConfig.rxConfig.rcmap[i] != -1)
buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
printf("map %s\r\n", buf);
printSectionBreak();
dumpValues(MASTER_VALUE);
}
if (dumpMask & DUMP_PROFILE) {
printf("\r\n# dump profile\r\n");
printf("\r\n# aux\r\n");
cliAux("");
printSectionBreak();
dumpValues(PROFILE_VALUE);
}
}
