void formatCallback(const rospy_tutorials::Floats::ConstPtr& raw_msg)
{
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
if(ros::ok()){
if(start){
prev_output = raw_msg->data;
start = false;
}
ros::Time now = ros::Time::now();
sensor_msgs::Imu imu_msg;
nav_msgs::Odometry alt_msg;
geometry_msgs::Vector3Stamped mag_msg;
imu_msg.header.stamp = now;
mag_msg.header.stamp = now;
alt_msg.header.stamp = now;
imu_msg.header.frame_id = "imu";
alt_msg.header.frame_id = "odom";
alt_msg.child_frame_id = "base_link";
mag_msg.header.frame_id = "imu";
imu_msg.angular_velocity.x = ((raw_msg->data[0]-GXOFFSET)/GYROGAIN)*(71.0/4068.0);
imu_msg.angular_velocity.y = ((raw_msg->data[1]-GYOFFSET)/GYROGAIN)*(71.0/4068.0);
imu_msg.angular_velocity.z = ((raw_msg->data[2]-GZOFFSET)/GYROGAIN)*(71.0/4068.0);
imu_msg.linear_acceleration.x = (9.81*(raw_msg->data[3]-AXOFFSET))/ACCELGAIN;
imu_msg.linear_acceleration.y = (9.81*(raw_msg->data[4]-AYOFFSET))/ACCELGAIN;
imu_msg.linear_acceleration.z = (9.81*(raw_msg->data[5]-AZOFFSET))/ACCELGAIN;
uncalib_mag[0] = raw_msg->data[6];
uncalib_mag[1] = raw_msg->data[7];
uncalib_mag[2] = raw_msg->data[8];
magCalibration(uncalib_mag, mag_msg);
temperature = raw_msg->data[9];
alt_msg.pose.pose.position.z = raw_msg->data[10] - alt_offset; //TODO remove the tf broadcaster used for test without GPS
transform.setOrigin(tf::Vector3(0.0,0.0,raw_msg->data[10]));
q.setRPY(0,0,0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/odom", "/base_link"));
fillRowMajor(linear_acc_covar, linear_acc_xvar, linear_acc_yvar, linear_acc_zvar);
fillRowMajor(angular_vel_covar, angular_vel_xvar, angular_vel_yvar, angular_vel_zvar);
imu_msg.angular_velocity_covariance = angular_vel_covar;
imu_msg.linear_acceleration_covariance = linear_acc_covar;
//imu_msg.orientation_covariance[0] = -1;
imu_pub_.publish(imu_msg);
mag_pub_.publish(mag_msg);
alt_pub_.publish(alt_msg);
prev_output = raw_msg->data;
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPrint("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliRead();
cliPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("\n");
cliPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPrintF("\n");
cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPrint("42 Hz\n");
break;
}
cliPrint("Magnetic Variation: ");
if (eepromConfig.magVar >= 0.0f)
cliPrintF("E%6.4f\n\n", eepromConfig.magVar * R2D);
else
cliPrintF("W%6.4f\n\n", -eepromConfig.magVar * R2D);
if (eepromConfig.verticalVelocityHoldOnly)
cliPrint("Vertical Velocity Hold Only\n\n");
else
cliPrint("Vertical Velocity and Altitude Hold\n\n");
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883)
eepromConfig.externalHMC5883 = false;
else
eepromConfig.externalHMC5883 = true;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Magnetic Variation
eepromConfig.magVar = readFloatCLI() * D2R;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPrint(" 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint("'f' Toggle External MS5611 State 'M' Set Mag Variation (+ East, - West) MMagVar\n");
cliPrint("'v' Toggle Vertical Velocity Hold Only\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n\n");
validQuery = false;
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasSlope[XAXIS],
sensorConfig.accelTCBiasSlope[YAXIS],
sensorConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasIntercept[XAXIS],
sensorConfig.accelTCBiasIntercept[YAXIS],
sensorConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasSlope[ROLL ],
sensorConfig.gyroTCBiasSlope[PITCH],
sensorConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasIntercept[ROLL ],
sensorConfig.gyroTCBiasIntercept[PITCH],
sensorConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.magBias[XAXIS],
sensorConfig.magBias[YAXIS],
sensorConfig.magBias[ZAXIS]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", sensorConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", sensorConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", sensorConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", sensorConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", sensorConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", sensorConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", sensorConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(sensorConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(sensorConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n\n");
break;
default:
cliPortPrint("Normal\n\n");
}
if (sensorConfig.gpsVelocityHoldOnly)
cliPortPrint("GPS Velocity Hold Only\n\n");
else
cliPortPrint("GPS Velocity and Position Hold\n\n");
if (sensorConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", sensorConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", sensorConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", sensorConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", sensorConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", sensorConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", sensorConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle GPS Velocity Hold Only
if (sensorConfig.gpsVelocityHoldOnly)
sensorConfig.gpsVelocityHoldOnly = false;
else
sensorConfig.gpsVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (sensorConfig.verticalVelocityHoldOnly)
sensorConfig.verticalVelocityHoldOnly = false;
else
sensorConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, sensorConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
sensorConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
sensorConfig.KpAcc = readFloatCLI();
sensorConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
sensorConfig.KpMag = readFloatCLI();
sensorConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
sensorConfig.compFilterA = readFloatCLI();
sensorConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Sensor Orientation
sensorConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Set Voltage Monitor Trip Points
sensorConfig.batteryLow = readFloatCLI();
sensorConfig.batteryVeryLow = readFloatCLI();
sensorConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = sensorConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = sensorConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = sensorConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
sensorConfig.voltageMonitorScale = readFloatCLI();
sensorConfig.voltageMonitorBias = readFloatCLI();
sensorConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write Sensor EEPROM Parameters
cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n");
writeSensorEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPortPrint("'b' MPU6000 Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPortPrint(" 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint(" 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle GPS Velocity Hold Only 'M' Set Voltage Monitor Trip Points Mlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPrint("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliRead();
cliPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("\nAccel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS],
eepromConfig.accelScaleFactor[YAXIS],
eepromConfig.accelScaleFactor[ZAXIS]);
cliPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS],
eepromConfig.accelBias[YAXIS],
eepromConfig.accelBias[ZAXIS]);
cliPrintF("Gyro TC Bias Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro TC Bias Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPrintF("hdot est/h est Comp Fil B: %9.4f\n\n", eepromConfig.compFilterB);
validQuery = false;
break;
///////////////////////////
case 'b': // Accel Calibration
accelCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // GyroTemp Calibration
gyroTempCalibration();
computeGyroRTBias();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data\n");
cliPrint("'b' Accel Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPrint("'d' Gyro Temp Calibration 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("\n");
cliPortPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPortPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPortPrintF("MXR9150 Accel in use: %s\n", eepromConfig.useMXR9150 ? "Yes" : "No");
cliPortPrintF("\n");
if (eepromConfig.useMXR9150 == true)
{
cliPortPrintF("MXR Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMXR[XAXIS],
eepromConfig.accelBiasMXR[YAXIS],
eepromConfig.accelBiasMXR[ZAXIS]);
cliPortPrintF("MXR Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMXR[XAXIS],
eepromConfig.accelScaleFactorMXR[YAXIS],
eepromConfig.accelScaleFactorMXR[ZAXIS]);
}
else
{
cliPortPrintF("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMPU[XAXIS],
eepromConfig.accelBiasMPU[YAXIS],
eepromConfig.accelBiasMPU[ZAXIS]);
cliPortPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMPU[XAXIS],
eepromConfig.accelScaleFactorMPU[YAXIS],
eepromConfig.accelScaleFactorMPU[ZAXIS]);
}
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Internal Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPortPrintF("External Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS + 3],
eepromConfig.magBias[YAXIS + 3],
eepromConfig.magBias[ZAXIS + 3]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(eepromConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n");
break;
default:
cliPortPrint("Normal\n");
}
if (eepromConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Bias and Scale Factor Calibration
if (eepromConfig.useMXR9150 == true)
accelCalibrationMXR();
else
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883 == 0)
eepromConfig.externalHMC5883 = 3;
else
eepromConfig.externalHMC5883 = 0;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle MXR9150 Use
if (eepromConfig.useMXR9150)
eepromConfig.useMXR9150 = false;
else
eepromConfig.useMXR9150 = true;
computeMPU6000RTData();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'h': // MXR Bias
eepromConfig.accelBiasMXR[XAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[YAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[ZAXIS] = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Sensor Orientation
eepromConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'N': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPortPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPortPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPortPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint("'f' Toggle External MS5611 State 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle MXR9150 Use\n");
cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
if (eepromConfig.useMpu6050 == true)
{
cliPortPrint("\nUsing MPU6050....\n\n");
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
}
else
{
cliPortPrint("\nUsing ADXL345/MPU3050....\n\n");
}
cliPortPrintF("Accel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS],
eepromConfig.accelScaleFactor[YAXIS],
eepromConfig.accelScaleFactor[ZAXIS]);
cliPortPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS],
eepromConfig.accelBias[YAXIS],
eepromConfig.accelBias[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
if (eepromConfig.useMpu6050 == true)
{
cliPortPrint("MPU6050 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
}
if (eepromConfig.useMs5611 == true)
cliPortPrint("\nUsing MS5611....\n\n");
else
cliPortPrint("\nUsing BMP085....\n\n");
if (eepromConfig.verticalVelocityHoldOnly == true)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU Calibration
if (eepromConfig.useMpu6050 == true)
mpu6050Calibration();
else
mpu3050Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Calibration
if (eepromConfig.useMpu6050 == false)
accelCalibrationADXL345();
else
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'm': // Toggle MPU3050/MPU6050
if (eepromConfig.useMpu6050)
{
eepromConfig.useMpu6050 = false;
initAdxl345();
initMpu3050();
}
else
{
eepromConfig.useMpu6050 = true;
initMpu6050();
}
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'p': // Toggle BMP085/MS5611
if (eepromConfig.useMs5611)
{
eepromConfig.useMs5611 = false;
initBmp085();
}
else
{
eepromConfig.useMs5611 = true;
initMs5611();
}
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6050 Digital Low Pass Filter
if (eepromConfig.useMpu6050 == true)
{
tempInt = (uint8_t)readFloatCLI();
switch (tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
i2cWrite(I2C2, MPU6050_ADDRESS, MPU6050_CONFIG, eepromConfig.dlpfSetting); // Accel and Gyro DLPF Setting
sensorQuery = 'a';
validQuery = true;
}
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc
eepromConfig.KpAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag
eepromConfig.KpMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'N': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
validQuery = false;
writeEEPROM();
break;
///////////////////////////
case '?':
cliPortPrint("\n");
if (eepromConfig.useMpu6050 == true)
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6050 DLPF A0 thru 3\n");
else
cliPortPrint("'a' Display Sensor Data\n");
cliPortPrint("'b' MPU Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc CKpAcc\n");
cliPortPrint("'d' Accel Calibration 'D' Set kpMag DKpMag\n");
cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint("'m' Toggle MPU3050/MPU6050\n");
cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n");
cliPortPrint("'p' Toggle BMP085/MS5611\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
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;
}
}
