Ejemplo n.º 1
0
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;
	}
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
0
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;
	}
}
Ejemplo n.º 4
0
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;
	}
}
Ejemplo n.º 5
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;
  }
}
Ejemplo n.º 6
0
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);
  }
}
Ejemplo n.º 7
0
Archivo: get.cpp Proyecto: LemonPi/rbot
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);
}
Ejemplo n.º 8
0
Archivo: get.cpp Proyecto: LemonPi/rbot
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);
	}
}
Ejemplo n.º 9
0
Archivo: get.cpp Proyecto: LemonPi/rbot
// 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);
	}

}
Ejemplo n.º 10
0
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;
}
Ejemplo n.º 11
0
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);
}
Ejemplo n.º 12
0
// 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;
	}

}
Ejemplo n.º 13
0
Archivo: get.cpp Proyecto: LemonPi/rbot
// 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;
	}
}
Ejemplo n.º 14
0
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

  }
}