Пример #1
0
ServoConverter::ServoConverter(BotParam *param) {

    rad_to_servo_.elevL_slope = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevL_slope");
    rad_to_servo_.elevL_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevL_y_intercept");

    rad_to_servo_.elevR_slope = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevR_slope");
    rad_to_servo_.elevR_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevR_y_intercept");

    rad_to_servo_.throttle_slope = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.throttle_slope");
    rad_to_servo_.throttle_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.throttle_y_intercept");

    servo_to_rad_.elevL_slope = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevL_slope");
    servo_to_rad_.elevL_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevL_y_intercept");

    servo_to_rad_.elevR_slope = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevR_slope");
    servo_to_rad_.elevR_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevR_y_intercept");

    servo_to_rad_.throttle_slope = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.throttle_slope");
    servo_to_rad_.throttle_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.throttle_y_intercept");


    elevL_min_ = bot_param_get_int_or_fail(param, "servo_commands.elevL_min");
    elevL_max_ = bot_param_get_int_or_fail(param, "servo_commands.elevL_max");

    elevR_min_ = bot_param_get_int_or_fail(param, "servo_commands.elevR_min");
    elevR_max_ = bot_param_get_int_or_fail(param, "servo_commands.elevR_max");

    throttle_min_ = bot_param_get_int_or_fail(param, "servo_commands.throttle_min");
    throttle_max_ = bot_param_get_int_or_fail(param, "servo_commands.throttle_max");

    elevL_trim_ = bot_param_get_int_or_fail(param, "servo_commands.elevL_flight_trim");
    elevR_trim_ = bot_param_get_int_or_fail(param, "servo_commands.elevR_flight_trim");
    throttle_trim_ = bot_param_get_int_or_fail(param, "servo_commands.throttle_flight_trim");

}
Пример #2
0
int main()
{
  lcm_t * lcm = lcm_create(NULL);

  BotParam * param = bot_param_new_from_server(lcm, 1);
  if (param == NULL) {
    fprintf(stderr, "could not get params!\n");
    exit(1);
  }

  bot_param_add_update_subscriber(param,param_update_handler,lcm);

  char * s;
  int ret = bot_param_write_to_string(param, &s);
  fprintf(stderr, "%s", s);
  free(s);

//  double foo = bot_param_get_double_or_fail(param, "foo");
//  double bar = bot_param_get_double_or_fail(param, "bar");
//  printf("foo=%f, bar = %f\n", foo, bar);

  bot_param_write(param, stderr);

  while (1) {
    lcm_handle(lcm);
    char * key = "coordinate_frames.body.history";
    fprintf(stderr, "%s = %d\n", key, bot_param_get_int_or_fail(param, key));
  }
  return 0;
}
Пример #3
0
MavStateEstimator::MavStateEstimator(RBISResetUpdate * init_state, BotParam * param) :
    history(init_state)
{
  utime_history_span = bot_param_get_int_or_fail(param, "state_estimator.utime_history_span");
  init_state->updateFilter(RBIS(), RBIM::Zero(), 0); //apply update from zero... should reset the state
  unprocessed_updates_start = history.updateMap.end();

  eigen_dump(init_state->posterior_state);
  eigen_dump(init_state->posterior_covariance);
  printf("\n");
}