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"); }
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; }
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"); }