void passive_position_correct() {
if (on_line(CENTER)) {
++cycles_on_line;
last_correct_distance = current_distance();
// activate line following if close enough to target and is on a line
}
else {
// false positive, not on line for enough cycles
if (cycles_on_line < CYCLES_CROSSING_LINE && cycles_on_line >= 0) { cycles_on_line = 0; return; }
else if (counted_lines >= LINES_PER_CORRECT && far_from_intersection(x, y)) {
counted_lines = 0;
// correct whichever one is closer to 0 or 200
// account for red line, can't detect along y so just correct to x
if (abs(y - RENDEZVOUS_Y) < 0.5*GRID_WIDTH) {
x = round(x / GRID_WIDTH) * GRID_WIDTH;
// leaving line forward
if (theta > -HALFPI && theta < HALFPI) x += HALF_LINE_WIDTH;
else x -= HALF_LINE_WIDTH;
}
else {
correct_to_grid();
}
SERIAL_PRINTLN('C');
}
else {
// false alarm, cool down
++counted_lines;
SERIAL_PRINTLN('L');
passive_status = PASSED_COOL_DOWN;
}
// either C or L will have last correct distance updated
// last_correct_distance = current_distance();
cycles_on_line = 0;
}
}
bool AppStorage::SaveAppToSlot(const App *app, size_t slot_index)
{
SERIAL_PRINTLN("Saving %04x '%s' to slot %u", app->id, app->name, slot_index);
auto &slot = slot_storage_[slot_index];
slot.Reset();
util::StreamBufferWriter stream_buffer{ slot.data, slot.data_size() };
size_t valid_length = app->Save(stream_buffer);
if (!valid_length || stream_buffer.overflow()) {
SERIAL_PRINTLN("Saving %04x '%s' to slot %u failed, valid_length=%u", app->id, app->name, slot_index, valid_length);
return false;
}
slot.header.id = app->id;
slot.header.version = app->storage_version;
slot.header.valid_length = valid_length;
slot.header.crc = slot.CalcCRC();
slot_storage_.Write(slot_index);
auto &slot_info = slots_[slot_index];
slot_info.id = app->id;
slot_info.state = SLOT_STATE::OK;
return true;
}
void correct_to_hopper() {
// extend theta backwards by the distance between the hopper and the center of the robot
float correct_x = boundaries[active_hopper].x - cos(theta)*BETWEEN_HOPPER_AND_CENTER;
float correct_y = boundaries[active_hopper].y - sin(theta)*BETWEEN_HOPPER_AND_CENTER;
if (abs(correct_x - x) > NEED_TO_HOPPER_CORRECT) {
SERIAL_PRINTLN("HX");
x = correct_x;
}
if (abs(correct_y - y) > NEED_TO_HOPPER_CORRECT) {
SERIAL_PRINTLN("HY");
y = correct_y;
}
}
void passive_red_line_correct() {
// only correct to red line if not backing up
if (abs(y - RENDEZVOUS_Y) < GRID_WIDTH*0.5 && layers[active_layer].speed > 0) {
digitalWrite(bottom_led, HIGH);
if (on_line(CENTER)) cycles_on_red_line = 0;
else if (on_line(RED) && !on_line(CENTER)) {
++cycles_on_red_line;
// navigate trying to get back to red line and x is far enough forward
if (seeking_red_line && RENDEZVOUS_X - x < 3*RENDEZVOUS_CLOSE) {
waypoint(LAYER_NAV);
}
}
else if (!on_line(RED)) {
// not false alarm
if (cycles_on_red_line >= CYCLES_CROSSING_LINE && current_distance() - last_correct_distance > DISTANCE_CENTER_TO_RED_ALLOWANCE) {
SERIAL_PRINT("RC");
SERIAL_PRINTLN(current_distance() - last_correct_distance);
// direction and signs are taken care of by sin and cos
float offset_y = DISTANCE_CENTER_TO_RED * sin(theta);
// avoid correcting when parallel to horizontal line
int offset_x = abs((int)x) % GRID_WIDTH;
if (abs(offset_y) > NEED_TO_HOPPER_CORRECT && (offset_x < INTERSECTION_TOO_CLOSE*0.5 || offset_x > GRID_WIDTH - INTERSECTION_TOO_CLOSE*0.5) && parallel_to_horizontal()) {
SERIAL_PRINTLN("-RC");
cycles_on_red_line = 0;
last_correct_distance = current_distance();
return;
}
y = RENDEZVOUS_Y;
// between [-180,0] left while going left
// x += DISTANCE_CENTER_TO_RED * cos(theta);
if (theta < 0) offset_y -= HALF_LINE_WIDTH;
else offset_y += HALF_LINE_WIDTH;
y += offset_y;
last_red_line_distance = current_distance();
last_correct_distance = last_red_line_distance;
if (side_of_board == SIDE_RIGHT) side_of_board = SIDE_LEFT;
else if (side_of_board == SIDE_LEFT) side_of_board = SIDE_RIGHT;
}
cycles_on_red_line = 0;
}
}
else {
digitalWrite(bottom_led, LOW);
cycles_on_red_line = 0;
}
}
bool AppStorage::LoadAppFromSlot(const App *app, size_t slot_index) const
{
SERIAL_PRINTLN("Loading %04x '%s' from slot %u", app->id, app->name, slot_index);
auto &slot = slot_storage_[slot_index];
if (app->id != slot.header.id || app->storage_version != slot.header.version)
return false;
app->Reset();
util::StreamBufferReader stream_buffer{ slot.data, slot.header.valid_length };
size_t len = app->Restore(stream_buffer);
if (len != slot.header.valid_length) {
SERIAL_PRINTLN("Load %04x from slot %u, restored %u but expected %u bytes", app->id, slot_index, len, slot.header.valid_length);
return false;
} else {
return true;
}
}
void AppStorage::Load()
{
slot_storage_.Load();
SERIAL_PRINTLN("AppStorage: LENGTH=%u, NUM_SLOTS=%u, SLOT_SIZE=%u",slot_storage_.LENGTH, slot_storage_.num_slots(), slot_storage_.SLOT_SIZE);
for (size_t s = 0; s < slot_storage_.num_slots(); ++s) {
CheckSlot(s);
}
}
void last_hopper_waypoint(int h) {
Hopper& hopper = hoppers[h];
Target& last_waypoint = hopper_waypoints[h][0];
int hx = boundaries[hopper.index].x;
int hy = boundaries[hopper.index].y;
last_waypoint.theta = atan2(hy - last_waypoint.y, hx - last_waypoint.x);
SERIAL_PRINT("last waypoint: ");
SERIAL_PRINT(hopper_waypoints[h][0].x);
SERIAL_PRINT(' ');
SERIAL_PRINT(hopper_waypoints[h][0].y);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(hopper_waypoints[h][0].theta * RADS);
}
void follow_hopper_waypoints(byte h) {
Hopper& hopper = hoppers[h];
if (hopper.waypoint == 0) {
SERIAL_PRINT("No waypoints:");
SERIAL_PRINTLN(h);
return;
}
add_target(hopper_waypoints[h][0].x, hopper_waypoints[h][0].y, hopper_waypoints[h][0].theta, TARGET_GET, true);
for (byte w = 1; w < hopper.waypoint; ++w) {
add_target(hopper_waypoints[h][w].x, hopper_waypoints[h][w].y, ANY_THETA);
}
}
// return from hopper to rendezvous in reverse direction
void return_from_hopper() {
byte h;
if (active_hopper == HOPPER1) h = 0;
else if (active_hopper == HOPPER2) h = 2;
else if (active_hopper == HOPPER3) h = 3;
else if (active_hopper == HOPPER4) h = 4;
Hopper& hopper = hoppers[h];
if (hopper.waypoint == 0) {
SERIAL_PRINT("No waypoints:");
SERIAL_PRINTLN(active_hopper);
return;
}
add_target(RENDEZVOUS_X, RENDEZVOUS_Y, 0, TARGET_PUT);
// skip the last target
for (byte w = hoppers[h].waypoint - 1; w > 0; --w) {
add_target(hopper_waypoints[h][w].x, hopper_waypoints[h][w].y);
}
}
void AppStorage::CheckSlot(size_t slot_index)
{
auto &slot = slot_storage_[slot_index];
auto &slot_info = slots_[slot_index];
slot_info.id = slot.header.id;
if (!slot.header.id || !slot.header.valid_length) {
SERIAL_PRINTLN("Slot %u: id=%04x, valid_length=%u -- Empty", slot_index, slot.header.id, slot.header.valid_length);
slot_info.state = SLOT_STATE::EMPTY;
return;
}
slot_info.state = SLOT_STATE::CORRUPT;
if (!slot.CheckCRC()) {
SERIAL_PRINTLN("Slot %u: id=%04x, valid_length=%u -- CRC check failed", slot_index, slot.header.id, slot.header.valid_length);
return;
}
SERIAL_PRINTLN("Slot %u: id=%04x, valid_length=%u", slot_index, slot.header.id, slot.header.valid_length);
auto app = app_switcher.find(slot_info.id);
if (!app) {
SERIAL_PRINTLN("Slot %u: id=%04x -- App not found!", slot_index, slot.header.id);
return;
}
SERIAL_PRINTLN("Slot %u: id=%04x found app '%s'", slot_index, slot.header.id, app->name);
if (slot.header.version != app->storage_version) {
SERIAL_PRINTLN("Slot %u: id=%04x -- version mismatch, expected %04x, got %04x", slot_index, slot.header.id, app->storage_version, slot.header.version);
}
// Some apps might have variable length settings, so there might not be a
// safe way to check this.
size_t expected_length = app->storageSize();
if (slot.header.valid_length > expected_length) {
SERIAL_PRINTLN("Slot %u: id=%04x -- storage length mismatch, expected %u, got %u", slot_index, slot.header.id, expected_length, slot.header.valid_length);
return;
}
slot_info.state = SLOT_STATE::OK;
}
void Ui::configure_encoders(EncoderConfig encoder_config) {
SERIAL_PRINTLN("Configuring encoders: %s (%x)", Strings::encoder_config_strings[encoder_config], encoder_config);
encoder_right_.reverse(encoder_config & ENCODER_CONFIG_R_REVERSED);
encoder_left_.reverse(encoder_config & ENCODER_CONFIG_L_REVERSED);
}
// correct passing a line not too close to an intersection
void passive_correct() {
// still on cool down
if (passive_status < PASSED_NONE) {++passive_status; return;}
if (!far_from_intersection(x, y)) {
if (passive_status > PASSED_NONE) passive_status = PASSED_NONE;
return;
}
// SERIAL_PRINTLN(passive_status, BIN);
// check if center one first activated; halfway there
if (on_line(CENTER) && !(passive_status & CENTER)) {
correct_half_distance = current_distance();
SERIAL_PRINTLN("PH");
}
// activate passive correct when either left or right sensor FIRST ACTIVATES
if ((on_line(LEFT) || on_line(RIGHT)) && passive_status == PASSED_NONE) {
correct_initial_distance = current_distance();
// only look at RIGHT LEFT CENTER
passive_status |= (on_lines & ENCOUNTERED_ALL);
SERIAL_PRINT("PS");
SERIAL_PRINTLN(passive_status, BIN);
// all hit at the same time, don't know heading
if (passive_status == ENCOUNTERED_ALL) hit_first = CENTER;
else if (passive_status & LEFT) hit_first = LEFT;
else hit_first = RIGHT;
// return;
}
if ((passive_status & LEFT) && !on_line(LEFT)) passive_status |= PASSED_LEFT;
if ((passive_status & RIGHT) && !on_line(RIGHT)) passive_status |= PASSED_RIGHT;
// check if encountering any additional lines
passive_status |= (on_lines & ENCOUNTERED_ALL);
// distance from first to center too far, probably too parallel to line
if (passive_status > PASSED_NONE && !(passive_status & CENTER) && (current_distance() - correct_initial_distance > CORRECT_TOO_FAR)) {
passive_status = PASSED_COOL_DOWN;
SERIAL_PRINT("PP");
SERIAL_PRINTLN(current_distance() - correct_initial_distance);
return;
}
// already hit center, see if second half distance is too far from first half distance
else if ((passive_status & CENTER) &&
((current_distance() - correct_half_distance) > // second half distance
(correct_half_distance - correct_initial_distance + CORRECT_CROSSING_TOLERANCE))) { // first half distance plus some room for error
SERIAL_PRINT("PD");
SERIAL_PRINT(correct_half_distance - correct_initial_distance);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(current_distance() - correct_half_distance);
passive_status = PASSED_COOL_DOWN;
}
// correct at the first encounter of line for each sensor
if ((passive_status & ENCOUNTERED_ALL) == ENCOUNTERED_ALL) {
// correct only if the 2 half distances are about the same
if (abs((current_distance() - correct_half_distance) - (correct_half_distance - correct_initial_distance)) < CORRECT_CROSSING_TOLERANCE) {
// distance since when passive correct was activated
float correct_elapsed_distance = current_distance() - correct_initial_distance;
// always positive
float theta_offset = atan2(correct_elapsed_distance, SIDE_SENSOR_DISTANCE);
// reverse theta correction if direction is backwards
if (layers[get_active_layer()].speed < 0) theta_offset = -theta_offset;
float theta_candidate;
// assume whichever one passed first was the first to hit
if (passive_status & PASSED_LEFT) theta_candidate = (square_heading()*DEGS) + theta_offset;
else if (passive_status & PASSED_RIGHT) theta_candidate = (square_heading()*DEGS) - theta_offset;
else if (hit_first == LEFT) theta_candidate = (square_heading()*DEGS) + theta_offset;
else if (hit_first == RIGHT) theta_candidate = (square_heading()*DEGS) - theta_offset;
// hit at the same time?
else theta_candidate = (square_heading()*DEGS);
// check how far away correction is from current theta
if (abs(theta - theta_candidate) > THETA_CORRECT_LIMIT) {
SERIAL_PRINT("PO");
SERIAL_PRINTLN(theta_candidate*RADS);
passive_status = PASSED_COOL_DOWN;
return;
}
// else correct to candidate value
theta = theta_candidate;
SERIAL_PRINT('P');
SERIAL_PRINTLN(theta_offset*RADS);
}
// suspicious of an intersection
else {
SERIAL_PRINT("PI");
SERIAL_PRINT(correct_half_distance - correct_initial_distance);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(current_distance() - correct_half_distance);
}
// reset even if not activated on this line (false alarm)
passive_status = PASSED_COOL_DOWN;
}
}
// get ball behaviour, should only occur at hopper approach
void get_ball() {
Layer& get = layers[LAYER_GET];
if (!get.active) return;
if (LAYER_GET == active_layer) {
SERIAL_PRINT(layers[LAYER_GET].speed);
SERIAL_PRINT('|');
SERIAL_PRINTLN(layers[LAYER_GET].angle);
}
// either going forward or backward, always no angle
// go forward until ball is detected or arbitrary additional distance travelled
if (ball_status < CAUGHT_BALL) {
if (caught_ball()) ++ball_status;
get.speed = GET_SPEED;
if (abs(boundaries[active_hopper].theta) < THETA_TOLERANCE) get.angle = 0;
// turn slightly to face hopper
else if (boundaries[active_hopper].theta < 0) get.angle = -GET_TURN;
else get.angle = GET_TURN;
if (ball_status == CAUGHT_BALL) {
// got to the ball, can also correct for position to be near hopper
correct_to_hopper();
// then close servo gate
close_gate();
hard_break(LAYER_GET);
get_initial_distance = tot_distance;
}
}
// after securing ball, drive backwards
else if (ball_status == SECURED_BALL) {
// initial kick to go straight
static int kick_cycle = 0;
if (get.speed > -GET_SPEED) {get.angle = 13; }
else if (get.angle > 0) {
if (kick_cycle <= 0) {
if (get.angle < 7) kick_cycle = 7 - get.angle;
--get.angle;
}
else --kick_cycle;
}
get.speed = -GET_SPEED;
// get.angle = 0;
if (paused) resume_drive(LAYER_GET);
// back up until you hit a line to correct position
if (on_line(CENTER)) {
corrected_while_backing_up = true;
correct_to_grid();
SERIAL_PRINTLN("CWB");
}
// backed up far enough
if (tot_distance - get_initial_distance > 5*GET_DISTANCE) {
// corrected_while_backing_up &&
// after getting ball, return to rendezvous point
corrected_while_backing_up = false;
layers[LAYER_GET].active = false;
close_hoppers();
return_from_hopper();
// hard_break(LAYER_GET, 3);
}
}
// in the middle of closing the gate, wait a couple cycles
else {
++ball_status;
}
}
void sendSerialTelemetry() {
switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor
break;
case 'a': // Send roll and pitch rate mode PID values
PrintPID(RATE_XAXIS_PID_IDX);
PrintPID(RATE_YAXIS_PID_IDX);
PrintValueComma(rotationSpeedFactor);
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'b': // Send roll and pitch attitude mode PID values
PrintPID(ATTITUDE_XAXIS_PID_IDX);
PrintPID(ATTITUDE_YAXIS_PID_IDX);
PrintPID(ATTITUDE_GYRO_XAXIS_PID_IDX);
PrintPID(ATTITUDE_GYRO_YAXIS_PID_IDX);
SERIAL_PRINTLN(windupGuard);
queryType = 'X';
break;
case 'c': // Send yaw PID values
PrintPID(ZAXIS_PID_IDX);
PrintPID(HEADING_HOLD_PID_IDX);
SERIAL_PRINTLN((int)headingHoldConfig);
queryType = 'X';
break;
case 'd': // Altitude Hold
#if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder
PrintPID(BARO_ALTITUDE_HOLD_PID_IDX);
PrintValueComma(PID[BARO_ALTITUDE_HOLD_PID_IDX].windupGuard);
PrintValueComma(altitudeHoldBump);
PrintValueComma(altitudeHoldPanicStickMovement);
PrintValueComma(minThrottleAdjust);
PrintValueComma(maxThrottleAdjust);
#if defined AltitudeHoldBaro
PrintValueComma(baroSmoothFactor);
#else
PrintValueComma(0);
#endif
PrintPID(ZDAMPENING_PID_IDX);
#else
PrintDummyValues(10);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'e': // miscellaneous config values
PrintValueComma(aref);
SERIAL_PRINTLN(minArmedThrottle);
queryType = 'X';
break;
case 'f': // Send transmitter smoothing values
PrintValueComma(receiverXmitFactor);
for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) {
PrintValueComma(receiverSmoothFactor[axis]);
}
PrintDummyValues(10 - LASTCHANNEL);
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'g': // Send transmitter calibration data
for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) {
Serial.print(receiverSlope[axis], 6);
Serial.print(',');
}
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'h': // Send transmitter calibration data
for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) {
Serial.print(receiverOffset[axis], 6);
Serial.print(',');
}
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'i': // Send sensor data
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
PrintValueComma(gyroRate[axis]);
}
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
PrintValueComma(filteredAccel[axis]);
}
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
#if defined(HeadingMagHold)
PrintValueComma(getMagnetometerData(axis));
#else
PrintValueComma(0);
#endif
}
SERIAL_PRINTLN();
break;
case 'j': // Send raw mag values
#ifdef HeadingMagHold
PrintValueComma(getMagnetometerRawData(XAXIS));
PrintValueComma(getMagnetometerRawData(YAXIS));
SERIAL_PRINTLN(getMagnetometerRawData(ZAXIS));
#endif
break;
case 'k': // Send accelerometer cal values
SERIAL_PRINT(accelScaleFactor[XAXIS], 6);
comma();
SERIAL_PRINT(runTimeAccelBias[XAXIS], 6);
comma();
SERIAL_PRINT(accelScaleFactor[YAXIS], 6);
comma();
SERIAL_PRINT(runTimeAccelBias[YAXIS], 6);
comma();
SERIAL_PRINT(accelScaleFactor[ZAXIS], 6);
comma();
SERIAL_PRINTLN(runTimeAccelBias[ZAXIS], 6);
queryType = 'X';
break;
case 'l': // Send raw accel values
measureAccelSum();
PrintValueComma((int)(accelSample[XAXIS]/accelSampleCount));
accelSample[XAXIS] = 0;
PrintValueComma((int)(accelSample[YAXIS]/accelSampleCount));
accelSample[YAXIS] = 0;
SERIAL_PRINTLN ((int)(accelSample[ZAXIS]/accelSampleCount));
accelSample[ZAXIS] = 0;
accelSampleCount = 0;
break;
case 'm': // Send magnetometer cal values
#ifdef HeadingMagHold
SERIAL_PRINT(magBias[XAXIS], 6);
comma();
SERIAL_PRINT(magBias[YAXIS], 6);
comma();
SERIAL_PRINTLN(magBias[ZAXIS], 6);
#endif
queryType = 'X';
break;
case 'n': // battery monitor
#ifdef BattMonitor
PrintValueComma(batteryMonitorAlarmVoltage);
PrintValueComma(batteryMonitorThrottleTarget);
PrintValueComma(batteryMonitorGoingDownTime);
#else
PrintDummyValues(3);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'o': // send waypoints
#ifdef UseGPSNavigator
for (byte index = 0; index < MAX_WAYPOINTS; index++) {
PrintValueComma(index);
PrintValueComma(waypoint[index].latitude);
PrintValueComma(waypoint[index].longitude);
PrintValueComma(waypoint[index].altitude);
}
#else
PrintDummyValues(4);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'p': // Send Camera values
#ifdef CameraControl
PrintValueComma(cameraMode);
PrintValueComma(servoCenterPitch);
PrintValueComma(servoCenterRoll);
PrintValueComma(servoCenterYaw);
PrintValueComma(mCameraPitch);
PrintValueComma(mCameraRoll);
PrintValueComma(mCameraYaw);
PrintValueComma(servoMinPitch);
PrintValueComma(servoMinRoll);
PrintValueComma(servoMinYaw);
PrintValueComma(servoMaxPitch);
PrintValueComma(servoMaxRoll);
PrintValueComma(servoMaxYaw);
#ifdef CameraTXControl
PrintValueComma(servoTXChannels);
#endif
#else
#ifdef CameraTXControl
PrintDummyValues(14);
#else
PrintDummyValues(13);
#endif
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'q': // Send Vehicle State Value
SERIAL_PRINTLN(vehicleState);
queryType = 'X';
break;
case 'r': // Vehicle attitude
PrintValueComma(kinematicsAngle[XAXIS]);
PrintValueComma(kinematicsAngle[YAXIS]);
SERIAL_PRINTLN(getHeading());
break;
case 's': // Send all flight data
PrintValueComma(motorArmed);
PrintValueComma(kinematicsAngle[XAXIS]);
PrintValueComma(kinematicsAngle[YAXIS]);
PrintValueComma(getHeading());
#if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder
#if defined AltitudeHoldBaro
PrintValueComma(getBaroAltitude());
#elif defined AltitudeHoldRangeFinder
PrintValueComma(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] != INVALID_RANGE ? rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] : 0.0);
#endif
PrintValueComma((int)altitudeHoldState);
#else
PrintValueComma(0);
PrintValueComma(0);
#endif
for (byte channel = 0; channel < 8; channel++) { // Configurator expects 8 values
PrintValueComma((channel < LASTCHANNEL) ? receiverCommand[channel] : 0);
}
for (byte motor = 0; motor < LASTMOTOR; motor++) {
PrintValueComma(motorCommand[motor]);
}
PrintDummyValues(8 - LASTMOTOR); // max of 8 motor outputs supported
#ifdef BattMonitor
PrintValueComma((float)batteryData[0].voltage/100.0); // voltage internally stored at 10mV:s
#else
PrintValueComma(0);
#endif
PrintValueComma(flightMode);
SERIAL_PRINTLN();
break;
case 't': // Send processed transmitter values
for (byte axis = 0; axis < LASTCHANNEL; axis++) {
PrintValueComma(receiverCommand[axis]);
}
SERIAL_PRINTLN();
break;
case 'u': // Send range finder values
#if defined (AltitudeHoldRangeFinder)
PrintValueComma(maxRangeFinderRange);
SERIAL_PRINTLN(minRangeFinderRange);
#else
PrintValueComma(0);
SERIAL_PRINTLN(0);
#endif
queryType = 'X';
break;
case 'v': // Send GPS PIDs
#if defined (UseGPSNavigator)
PrintPID(GPSROLL_PID_IDX);
PrintPID(GPSPITCH_PID_IDX);
PrintPID(GPSYAW_PID_IDX);
queryType = 'X';
#else
PrintDummyValues(9);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'y': // send GPS info
#if defined (UseGPS)
PrintValueComma(gpsData.state);
PrintValueComma(gpsData.lat);
PrintValueComma(gpsData.lon);
PrintValueComma(gpsData.height);
PrintValueComma(gpsData.course);
PrintValueComma(gpsData.speed);
PrintValueComma(gpsData.accuracy);
PrintValueComma(gpsData.sats);
PrintValueComma(gpsData.fixtime);
PrintValueComma(gpsData.sentences);
PrintValueComma(gpsData.idlecount);
#else
PrintDummyValues(11);
#endif
SERIAL_PRINTLN();
break;
case 'z': // Send all Altitude data
#if defined (AltitudeHoldBaro)
PrintValueComma(getBaroAltitude());
#else
PrintValueComma(0);
#endif
#if defined (AltitudeHoldRangeFinder)
SERIAL_PRINTLN(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX]);
#else
SERIAL_PRINTLN(0);
#endif
break;
case '$': // send BatteryMonitor voltage/current readings
#if defined (BattMonitor)
PrintValueComma((float)batteryData[0].voltage/100.0); // voltage internally stored at 10mV:s
#if defined (BM_EXTENDED)
PrintValueComma((float)batteryData[0].current/100.0);
PrintValueComma((float)batteryData[0].usedCapacity/1000.0);
#else
PrintDummyValues(2);
#endif
#else
PrintDummyValues(3);
#endif
SERIAL_PRINTLN();
break;
case '%': // send RSSI
#if defined (UseAnalogRSSIReader) || defined (UseEzUHFRSSIReader) || defined (UseSBUSRSSIReader)
SERIAL_PRINTLN(rssiRawValue);
#else
SERIAL_PRINTLN(0);
#endif
break;
case 'x': // Stop sending messages
break;
case '!': // Send flight software version
SERIAL_PRINTLN(SOFTWARE_VERSION, 1);
queryType = 'X';
break;
case '#': // Send configuration
reportVehicleState();
queryType = 'X';
break;
case '6': // Report remote commands
for (byte motor = 0; motor < LASTMOTOR; motor++) {
PrintValueComma(motorCommand[motor]);
}
SERIAL_PRINTLN();
queryType = 'X';
break;
#if defined(OSD) && defined(OSD_LOADFONT)
case '&': // fontload
if (OFF == motorArmed) {
max7456LoadFont();
}
queryType = 'X';
break;
#endif
}
}
