void save_to_png(char* filename) {
int i;
// use malloc rather than the stack since an array of several million bytes will overflow the stack
uint8_t *pixels = malloc(width * height * 3 * sizeof(uint8_t));
// switch the framebuffer in order to force the full texture to be saved
glBindFramebuffer(GL_FRAMEBUFFER, framebuffers[fb]);
glViewport(0, 0, width, height);
// copy pixels from screen
glActiveTexture(0);
glBindTexture(GL_TEXTURE_2D, textures[fb]);
glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, width, height);
glPixelStorei(GL_PACK_ALIGNMENT, 1);
glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, (GLvoid *)pixels);
// use libpng to write the pixels to a png image
png_structp png = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
if (!png) goto png_fail;
png_infop info = png_create_info_struct(png);
if (!info) {
png_destroy_write_struct(&png, &info);
goto png_fail;
}
if(filename == NULL) {
// filename => current time in milliseconds
char tmp[256];
snprintf(tmp, 256, "%llu.png", get_time_us() * 1000);
filename = tmp;
}
FILE *file = fopen(filename, "wb");
if (!file) {
png_destroy_write_struct(&png, &info);
goto png_fail;
}
png_init_io(png, file);
png_set_IHDR(png, info, width, height, 8, PNG_COLOR_TYPE_RGB, PNG_INTERLACE_NONE,
PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
png_colorp palette = png_malloc(png, PNG_MAX_PALETTE_LENGTH * sizeof(png_color));
if (!palette) {
fclose(file);
png_destroy_write_struct(&png, &info);
goto png_fail;
}
png_set_PLTE(png, info, palette, PNG_MAX_PALETTE_LENGTH);
png_write_info(png, info);
png_set_packing(png);
png_bytepp rows = (png_bytepp)png_malloc(png, height * sizeof(png_bytep));
for (i = 0; i < height; ++i) {
rows[i] = (png_bytep)(pixels + (height - i - 1) * width * 3);
}
png_write_image(png, rows);
png_write_end(png, info);
png_free(png, palette);
png_destroy_write_struct(&png, &info);
fclose(file);
free(pixels);
printf("PNG: finished at %llu\n", get_time_us());
render_prepare_screen();
return;
png_fail:
fail("Failed to create PNG\n");
}
int run()
{
std::cout << std::endl << "Polynomials of degree " << degree << " with " << modulus << " bit coefficients and " << sizeof(T) * 8 << " bit limbs" << std::endl;
std::cout << "======================================================================" << std::endl;
using poly_t = nfl::poly_from_modulus<T, degree, modulus>;
using poly_proxy_t = nfl::tests::poly_tests_proxy<poly_t>;
auto start = std::chrono::steady_clock::now();
poly_t *resa = alloc_aligned<poly_t, 32>(REPETITIONS);
std::fill(resa, resa + REPETITIONS, 0);
std::fill(resa, resa + REPETITIONS, nfl::uniform());
start = std::chrono::steady_clock::now();
for (unsigned i = 0; i < REPETITIONS ; i++)
{
poly_t& p = resa[i];
ntt_new(&p(0, 0), poly_proxy_t::get_omegas(p), poly_proxy_t::get_shoupomegas(p), degree, p.get_modulus(0));
}
auto end = std::chrono::steady_clock::now();
std::cout << "Time per NTT (org): " << get_time_us(start, end, REPETITIONS) << " us" << std::endl;
std::fill(resa, resa + REPETITIONS, nfl::uniform());
start = std::chrono::steady_clock::now();
for (unsigned i = 0; i < REPETITIONS ; i++)
{
poly_t& p = resa[i];
poly_proxy_t::ntt(&p(0, 0), poly_proxy_t::get_omegas(p), poly_proxy_t::get_shoupomegas(p), p.get_modulus(0));
}
end = std::chrono::steady_clock::now();
std::cout << "Time per NTT (lib): " << get_time_us(start, end, REPETITIONS) << " us" << std::endl;
return 0;
}
// Exec_Timing::Exec_Timing(int order_, vector<double> &coeff) {
Exec_Timing::Exec_Timing(int rate) {
dt = 0;
wait_freq = rate ? 1000000 / rate : 0;
wait_time = wait_freq;
frequency = wait_time;
start = get_time_us(); // from utility
usec = 0;
gettimeofday(&tk, NULL);
gettimeofday(&tkm1, NULL);
}
static bool
match_perf_test (PERF_TEST_INDEX index)
{
bool ret = false;
char path[128];
char guid[128];
cb_info_t *cb;
uint64_t start;
int i;
cb_init ();
for (i = 0; i < TEST_CB_MAX_ENTRIES; i++)
{
sprintf (path, "/database/test%d/test%d", i, i);
sprintf (guid, "%zX", (size_t)g_str_hash (path));
cb = cb_create (&watch_list, guid, path, 1, 0);
cb_release (cb);
}
CU_ASSERT (g_list_length (watch_list) == TEST_CB_MAX_ENTRIES);
start = get_time_us ();
for (i = 0; i < TEST_CB_MAX_ITERATIONS; i++)
{
GList *matches;
int test = index == INDEX_FIRST ? 0 :
(index == INDEX_LAST ? (TEST_CB_MAX_ENTRIES - 1) :
random () % TEST_CB_MAX_ENTRIES);
sprintf (path, "/database/test%d/test%d", test, test);
matches = cb_match (&watch_list, path,
CB_MATCH_EXACT|CB_MATCH_WILD|CB_MATCH_CHILD);
if (g_list_length (matches) != 1)
goto exit;
g_list_free_full (matches, (GDestroyNotify) cb_release);
}
printf ("%"PRIu64"us ... ", (get_time_us () - start) / TEST_CB_MAX_ITERATIONS);
ret = true;
exit:
g_list_foreach (watch_list, cb_free, NULL);
CU_ASSERT (g_list_length (watch_list) == 0);
watch_list = NULL;
cb_shutdown ();
return ret;
}
void MAVLinkMKApp::handle_input(const mk_message_t& msg) {
uint64_t tmp64 = message_time;
message_time = get_time_us();
log("MAVLinkMKApp got mk_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);
//send heartbeat approx. after 1s
delta_time += message_time - tmp64;
if( delta_time > 1000000 ) {
send_heartbeat();
delta_time = 0;
}
//TODO: check coded
//TODO: check size of msg
switch(msg.type) {
case MK_MSG_TYPE_DEBUGOUT:
break;
case MK_MSG_TYPE_EXT_CTRL: {
mavlink_manual_control_t manual_control;
copy(&manual_control, (mk_extern_control_t*)msg.data);
Lock tx_mav_lock(tx_mav_mutex);
mavlink_msg_manual_control_encode(system_id(),
component_id,
&tx_mav_msg,
&manual_control);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MK_MSG_TYPE_COMPASS:
break;
case MK_MSG_TYPE_POLL_DEBUG:
break;
case MK_MSG_TYPE_ANGLE_OUT: {
mavlink_debug_t debug;
debug.ind = 0;
mk_angles_t *mk_angles = (mk_angles_t*)msg.data;
debug.value = ((float)mk_angles->pitch) / 10;
Lock tx_mav_lock(tx_mav_mutex);
mavlink_msg_debug_encode(system_id(),
component_id,
&tx_mav_msg,
&debug);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MK_MSG_TYPE_MOTORTEST:
break;
case MK_MSG_TYPE_SETTING:
break;
default:
break;
}
}
MAVLinkMKApp::MAVLinkMKApp(const Logger::log_level_t loglevel) :
AppInterface("mavlink_mk_app", loglevel),
AppLayer<mavlink_message_t>("mavlink_mk_app", loglevel),
AppLayer<mk_message_t>("mavlink_mk_app", loglevel),
message_time( get_time_us() ),
delta_time(0)
{
pthread_mutex_init(&tx_mav_mutex, NULL);
pthread_mutex_init(&tx_mk_mutex, NULL);
// mkhuchlink_msg_init(&tx_mk_msg);
}
MAVLinkMKHUCHApp::MAVLinkMKHUCHApp(const Logger::log_level_t loglevel) :
AppInterface("mavlink_mkhuch_app", loglevel),
AppLayer<mavlink_message_t>("mavlink_mkhuch_app", loglevel),
AppLayer<mkhuch_message_t>("mavlink_mkhuch_app", loglevel),
mkhuch_msg_time( get_time_us() ),
heartbeat_time(0)
// parameter_request(255),
// parameter_time(0) {
{
pthread_mutex_init(&tx_mav_mutex, NULL);
pthread_mutex_init(&tx_mkhuch_mutex, NULL);
mkhuchlink_msg_init(&tx_mkhuch_msg);
}
uint64_t Exec_Timing::updateExecStats() {
/* calculate frequency */
end = get_time_us();
frequency = (15 * frequency + end - start) / 16;
start = end;
// Logger::log("Ctrl_Hover slept for", wait_time, Logger::LOGLEVEL_INFO);
gettimeofday(&tk, NULL);
//timediff(tdiff, tkm1, tk);
dt = (tk.tv_sec - tkm1.tv_sec) * 1000000 + (tk.tv_usec - tkm1.tv_usec);
tkm1 = tk; // save current time
return dt;
}
static SERVER *
server_new(void)
{
SERVER* server = (SERVER*)malloc(sizeof(SERVER));
if (server)
{
memset(server, 0, sizeof(*server));
server->report = s_report;
server->initial_ts = get_time_us();
}
return server;
} /* server_new */
int Exec_Timing::calcSleeptime() {
usec = get_time_us();
end = usec;
wait_time = wait_freq - (end - start);
// Logger::log("Exec_Timing: wait_freq", wait_freq, Logger::LOGLEVEL_INFO);
// Logger::log("Exec_Timing: end, start", end, start, Logger::LOGLEVEL_INFO);
// Logger::log("Exec_Timing: usec, wait_time", usec, wait_time, Logger::LOGLEVEL_INFO);
// FIXME: adaptive timing on demand?
if(wait_time < 0) {
Logger::log("ALARM: time", Logger::LOGLEVEL_INFO);
wait_time = 0;
}
return wait_time;
}
void FC_Mpkg::handle_input(const mavlink_message_t &msg) {
// vector<int> v(16);
//mavlink_message_t msg_j;
//Logger::log("FC_Mpkg got mavlink_message [len, id]:", (int)msg.len, (int)msg.msgid, Logger::LOGLEVEL_DEBUG);
if(msg.msgid == MAVLINK_MSG_ID_MK_DEBUGOUT) {
//Logger::log("FC_Mpkg got MK_DEBUGOUT", Logger::LOGLEVEL_INFO);
mavlink_msg_mk_debugout_decode(&msg, (mavlink_mk_debugout_t *)&mk_debugout);
// MK FlightCtrl IMU data
debugout2attitude(&mk_debugout, &huch_attitude);
// MK FlightCtrl Barometric sensor data
debugout2altitude(&mk_debugout, &huch_altitude);
// MK huch-FlightCtrl I2C USS data
// XXX: this should be in kopter config, e.g. if settings == fc_has_uss
debugout2ranger(&mk_debugout, &huch_ranger);
// real debugout data
debugout2status(&mk_debugout, &mk_fc_status);
// put into standard pixhawk structs
// raw IMU
//set_pxh_raw_imu();
set_pxh_attitude();
set_pxh_manual_control();
publish_data(get_time_us());
// deadlock problem
// mavlink_msg_huch_attitude_encode(42, 23, &msg_j, &huch_attitude);
// send(msg_j);
}
// // compare with qk_datatypes
// mk_debugout_o = (DebugOut_t *)&mk_debugout;
// v[0] = (int16_t)mk_debugout_o->Analog[ADval_accnick];
// v[1] = (int16_t)mk_debugout_o->Analog[ADval_accroll];
// v[2] = (int16_t)mk_debugout_o->Analog[ADval_acctop];
// v[3] = (int16_t)mk_debugout_o->Analog[ADval_acctopraw];
// v[4] = (int16_t)mk_debugout_o->Analog[ATTmeanaccnick];
// v[5] = (int16_t)mk_debugout_o->Analog[ATTmeanaccroll];
// v[6] = (int16_t)mk_debugout_o->Analog[ATTmeanacctop];
// v[7] = (int16_t)mk_debugout_o->Analog[ADval_gyrnick];
// v[8] = (int16_t)mk_debugout_o->Analog[ADval_gyrroll];
// v[9] = (int16_t)mk_debugout_o->Analog[ADval_gyryaw];
// Logger::log("FC_Mpkg decoded: ", v, Logger::LOGLEVEL_INFO);
// if(msg.sysid == system_id && msg.msgid == 0) {//FIXME: set right msgid
// //TODO
// }
}
int MPID_DeviceCheck(MPID_BLOCKING_TYPE blocking)
{
static int last_polled = 0;
int ne = 0, progress = 0, count = 0;
int empty = 1, i, j;
struct ibv_wc wc;
static int recv = 0;
do {
/* Poll SMP */
#ifdef _SMP_
if (!disable_shared_mem && SMP_INIT &&
MPI_SUCCESS == MPID_SMP_Check_incoming()) {
progress = 1;
}
#endif
/* Poll RC FP channels */
for(i = last_polled, j = 0;
j < mvdev.polling_set_size;
i = (i + 1) % mvdev.polling_set_size, ++j) {
if(mvdev_check_rcfp(mvdev.polling_set[i])) {
progress = 1;
last_polled = i;
}
}
/* Poll the CQ */
wc.imm_data = 0;
for(i = 0; i < mvdev.num_cqs; i++) {
ne = ibv_poll_cq(mvdev.cq[i], 1, &wc);
if(MVDEV_UNLIKELY(ne < 0)) {
error_abort_all(IBV_RETURN_ERR, "Error polling CQ\n");
} else if (1 == ne) {
if (wc.status != IBV_WC_SUCCESS) {
MV_Poll_CQ_Error(&wc);
}
switch(wc.opcode) {
case IBV_WC_RDMA_WRITE:
case IBV_WC_RDMA_READ:
{
mv_sdescriptor *d = (mv_sdescriptor *) wc.wr_id;
if(MVDEV_LIKELY(MVDEV_RC_FP == ((mv_sbuf *)(d->parent))->flag)) {
mvdev_process_send(d, d->parent);
} else {
mv_qp *qp = (mv_qp *) d->qp;
mv_sbuf *v = (mv_sbuf *)(d->parent);
++(qp->send_wqes_avail);
if(NULL != qp->ext_sendq_head && qp->send_wqes_avail > 0) {
mvdev_ext_sendq_send(qp);
}
v->in_progress = 0;
if(0 == v->seqnum || v->rel_type == MV_RELIABLE_NONE) {
release_mv_sbuf((mv_sbuf *)(d->parent));
}
}
break;
}
case IBV_WC_SEND:
{
mv_sdescriptor *d = (mv_sdescriptor *) wc.wr_id;
mvdev_process_send(d, d->parent);
break;
}
case IBV_WC_RECV:
{
mv_rdescriptor *d = (mv_rdescriptor *) wc.wr_id;;
mv_rbuf *v = d->parent;
if(MVDEV_LIKELY(0 == wc.imm_data)) {
mvdev_packet_mheader *mh = (mvdev_packet_mheader *) v->mheader_ptr;
v->has_header = mh->has_header;
v->rank = mh->src_rank;
v->seqnum = mh->seqnum;
SET_BYTE_LEN_HEADER(v, wc);
SET_RECV_HEADER(v);
} else {
READ_THS_TUPLE(wc.imm_data, &(v->has_header),
&(v->rank), &(v->seqnum));
SET_BYTE_LEN_IMM(v, wc);
SET_RECV_IMM(v);
}
v->data_ptr = v->header_ptr;
mvdev_process_recv(v);
++recv;
break;
}
case IBV_WC_RECV_RDMA_WITH_IMM:
{
mv_rdescriptor *d = (mv_rdescriptor *) wc.wr_id;
mv_rbuf *v = d->parent;
DECR_RPOOL(((mv_rpool *) (v->rpool)));
CHECK_RPOOL(((mv_rpool *) (v->rpool)));
MV_Rndv_Put_Unreliable_Receive_Imm(v, wc.imm_data);
release_mv_rbuf(v);
break;
}
default:
error_abort_all(IBV_RETURN_ERR, "Invalid opcode\n");
}
progress = 1;
empty = 0;
wc.imm_data = 0;
} else {
empty = 1;
++count;
if(count % 1200 == 0) {
if(get_time_us() - mvdev.last_check > mvparams.progress_timeout) {
mvdev.last_check = get_time_us();
mvdev_unackq_traverse();
SEND_ACKS();
recv = 0;
} else if(recv > 20) {
SEND_ACKS();
recv = 0;
}
}
}
}
if(flowlist) {
process_flowlist();
}
} while ((blocking && !progress) || !empty);
if(recv > mvparams.send_ackafter_progress ||
(get_time_us() - mvdev.last_check > mvparams.progress_timeout)) {
mvdev_unackq_traverse();
SEND_ACKS();
mvdev.last_check = get_time_us();
recv = 0;
}
return progress ? MPI_SUCCESS : -1;
}
int main(int argc, char **argv) {
int i;
int filecount = 0;
char* filename = NULL;
{
int c, option_index = 0;
static struct option long_options[] = {
{"help", 0, 0, 0},
{"output", 2, 0, 0},
{"size", 1, 0, 0},
{NULL, 0, NULL, 0}
};
char* endptr = NULL;
bool opt_fail = false;
bool help = false;
while ((c = getopt_long(argc, argv, "ho::s:",
long_options, &option_index)) != -1) {
switch (c) {
case 0:
switch(option_index) {
case 0: // help
goto help;
case 1: // output
goto output;
case 2: // size
goto size;
default:
goto unknown;
}
break;
help:
case 'h': // help
help = true;
break;
output:
case 'o': // output
filecount = 1;
// avoid leakiness if the user provided multiple --output
// free(NULL) is a no-op, so this should be safe:
free(filename);
filename = NULL;
if(optarg != NULL) {
int tmp = strtol(optarg, &endptr, 10);
if (endptr == optarg || (endptr != NULL && *endptr != '\0')) {
int len = strlen(optarg);
filename = malloc(len + 1);
strcpy(filename, optarg);
filename[len] = '\0';
} else {
filecount = tmp;
}
}
break;
size:
case 's':
i = 0;
while(optarg[i] != '*' && optarg[i] != '\0') i++;
if(optarg[i] == '\0') {
goto size_fail;
}
optarg[i] = '\0';
width = strtol(optarg, &endptr, 10);
if (endptr == optarg || (endptr != NULL && *endptr != '\0')) {
goto size_fail;
}
height = strtol(optarg + i + 1, &endptr, 10);
if (endptr == optarg || (endptr != NULL && *endptr != '\0')) {
goto size_fail;
}
printf("width: %d, height: %d\n", width, height);
break;
size_fail:
fprintf(stderr, "Invalid size string '%s'\n", optarg);
print_help(1);
break;
unknown:
case '?':
opt_fail = true;
break;
default:
fprintf(stderr, "?? getopt returned character code 0%o ??\n", c);
}
}
if(opt_fail) {
print_help(1);
}
if(optind < argc) {
fprintf(stderr, "%s: unrecognized option '%s'\n", argv[0], argv[optind]);
print_help(1);
}
if(help) {
print_help(0);
}
}
scale = max(width, height);
srand(get_time_us());
GLFWwindow* window;
glfwSetErrorCallback(error_callback);
// Initialize the library
if (!glfwInit())
return -1;
glfwWindowHint(GLFW_SAMPLES, 4);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
// Create a windowed mode window and its OpenGL context
if(filecount) {
glfwWindowHint(GLFW_VISIBLE, false);
window = glfwCreateWindow(1, 1, "SpaceScape", NULL, NULL);
} else {
window = glfwCreateWindow(width, height, "SpaceScape", NULL, NULL);
}
if (!window) {
glfwTerminate();
return -1;
}
// Make the window's context current
glfwMakeContextCurrent(window);
glfwSetKeyCallback(window, key_callback);
// Init GLEW
glewExperimental = true;
GLenum err = glewInit();
if (GLEW_OK != err) {
fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
}
fprintf(stderr, "Using GLEW %s\n", glewGetString(GLEW_VERSION));
if (!GLEW_ARB_vertex_buffer_object) {
fputs("VBO not supported\n", stderr);
exit(1);
}
render_init();
if(filename) {
render_to_png(filename);
} else if(filecount) {
for(i = 0; i < filecount; i++) {
render_to_png(NULL);
}
} else {
// Render to our framebuffer
render_to_screen();
while (!glfwWindowShouldClose(window)) {
// Clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer);
glVertexAttribPointer(
0, // attribute 0. No particular reason for 0, but must match the layout in the shader.
3, // size
GL_FLOAT, // type
GL_FALSE, // normalized?
0, // stride
(void*) 0 // array buffer offset
);
// Draw the triangles !
glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles
glDisableVertexAttribArray(0);
// Swap buffers
glfwSwapBuffers(window);
glfwPollEvents();
}
}
render_cleanup();
// Close OpenGL window and terminate GLFW
glfwTerminate();
return 0;
}
void USB_host::poll()
{
usbh_poll(get_time_us());
delay_ms(1);
}
int run()
{
std::cout << std::endl << "Polynomials of degree " << degree << " with " << modulus << " bit coefficients and " << sizeof(T) * 8 << " bit limbs" << std::endl;
std::cout << "======================================================================" << std::endl;
using poly_t = nfl::poly_from_modulus<T, degree, modulus>;
static_assert(sizeof(poly_t) % 32 == 0, "sizeof(poly_t) must be 32-bytes aligned");
auto start = std::chrono::steady_clock::now();
auto end = std::chrono::steady_clock::now();
// Polynomial arrays to do the tests
start = std::chrono::steady_clock::now();
/* AG: on my system, this gives pointers non aligned on 32-bytes!
poly_t *resa = new poly_t[REPETITIONS],
*resb = new poly_t[REPETITIONS],
*resc = new poly_t[REPETITIONS],
*resd = new poly_t[REPETITIONS];
*/
poly_t *resa = alloc_aligned<poly_t, 32>(REPETITIONS),
*resb = alloc_aligned<poly_t, 32>(REPETITIONS),
*resc = alloc_aligned<poly_t, 32>(REPETITIONS),
*resd = alloc_aligned<poly_t, 32>(REPETITIONS);
if ((((uintptr_t)resa % 32) != 0) ||
(((uintptr_t)resb % 32) != 0) ||
(((uintptr_t)resc % 32) != 0) ||
(((uintptr_t)resd % 32) != 0)) {
printf("fatal error: pointer unaligned!\n");
exit(1);
}
std::fill(resa, resa + REPETITIONS, 0);
std::fill(resb, resb + REPETITIONS, 0);
std::fill(resc, resc + REPETITIONS, 0);
std::fill(resd, resd + REPETITIONS, 0);
#ifdef TEST_ADDITIONS_INPLACE
std::fill(resa, resa + REPETITIONS, nfl::uniform());
std::fill(resb, resb + REPETITIONS, nfl::uniform());
start = std::chrono::steady_clock::now();
for (unsigned i = 0; i < REPETITIONS ; i++)
{
nfl::add(resa[i], resa[i], resb[i]);
}
end = std::chrono::steady_clock::now();
std::cout << "Time per polynomial in-place addition a+=b: " << get_time_us(start, end, REPETITIONS) << " us" << std::endl;
#endif
#ifdef TEST_ADDITIONS
std::fill(resa, resa + REPETITIONS, nfl::uniform());
std::fill(resb, resb + REPETITIONS, nfl::uniform());
start = std::chrono::steady_clock::now();
for (unsigned i = 0; i < REPETITIONS ; i++)
{
nfl::add(resc[i], resa[i], resb[i]);
}
end = std::chrono::steady_clock::now();
std::cout << "Time per polynomial addition c=a+b: " << get_time_us(start, end, REPETITIONS) << " us" << std::endl;
#endif
#ifdef TEST_SUBTRACTIONS
std::fill(resa, resa + REPETITIONS, nfl::uniform());
std::fill(resb, resb + REPETITIONS, nfl::uniform());
start = std::chrono::steady_clock::now();
for (unsigned i = 0; i < REPETITIONS ; i++)
{
nfl::sub(resc[i], resa[i], resb[i]);
}
end = std::chrono::steady_clock::now();
std::cout << "Time per polynomial subtraction c=a-b: " << get_time_us(start, end, REPETITIONS) << " us" << std::endl;
#endif
#ifdef TEST_MULTIPLICATIONS
std::fill(resa, resa + REPETITIONS, nfl::uniform());
std::fill(resb, resb + REPETITIONS, nfl::uniform());
start = std::chrono::steady_clock::now();
for (unsigned i = 0; i < REPETITIONS ; i++)
{
nfl::mul(resc[i], resa[i], resb[i]);
}
end = std::chrono::steady_clock::now();
std::cout << "Time per polynomial multiplication (NTT form) c=a*b: " << get_time_us(start, end, REPETITIONS) << " us" << std::endl;
#endif
// Cleaning
free_aligned(REPETITIONS, resa);
free_aligned(REPETITIONS, resb);
free_aligned(REPETITIONS, resc);
free_aligned(REPETITIONS, resd);
return 0;
}
long CTimerCpu::get_time_ms(){
return get_time_us() / 1000;
}
static void render() {
GLuint program;
int i;
printf("\nRender: started at %llu\n", get_time_us());
if(gen_starfield) {
program = program_starfield;
glUseProgram(program);
render_set_framebuffer(program);
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer);
glVertexAttribPointer(
0, // attribute 0. No particular reason for 0, but must match the layout in the shader.
3, // size
GL_FLOAT, // type
GL_FALSE, // normalized?
0, // stride
(void*) 0 // array buffer offset
);
// Draw the triangles !
glUniform1f(glGetUniformLocation(program, "seed"), (float)(rand() & 0xAFF) / 10);
glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles
glDisableVertexAttribArray(0);
}
if(gen_stars) {
program = program_star;
glUseProgram(program);
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer);
glVertexAttribPointer(
0, // attribute 0. No particular reason for 0, but must match the layout in the shader.
3, // size
GL_FLOAT, // type
GL_FALSE, // normalized?
0, // stride
(void*) 0 // array buffer offset
);
int n = 1;
while (rand() < RAND_MAX / 2) n++;
// TODO: Could optimise this by only drawing regions which contain stars
for (i = 0; i < n; i++) {
render_set_framebuffer(program);
int x = rand() % width, y = rand() % height;
float radius = 1.f + scale * 0.005f + (rand() % scale) * 0.01f;
printf("Position: %d, %d; Radius: %f\n", x, y, radius);
glUniform2f(glGetUniformLocation(program, "coord"), x, y);
glUniform1f(glGetUniformLocation(program, "radius"), radius);
glDrawArrays(GL_TRIANGLES, 0, 6);
}
glDisableVertexAttribArray(0);
}
if(gen_nebulae) {
if(rand() < RAND_MAX / 2) {
program = program_nebula1;
} else {
program = program_nebula2;
}
glUseProgram(program);
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer);
glVertexAttribPointer(
0, // attribute 0. No particular reason for 0, but must match the layout in the shader.
3, // size
GL_FLOAT, // type
GL_FALSE, // normalized?
0, // stride
(void*) 0 // array buffer offset
);
int n = 1;
while (rand() < RAND_MAX / 2) n++;
for(i = 0; i < n; i++) {
render_set_framebuffer(program);
float r = (float)rand() / RAND_MAX;
float g = (float)rand() / RAND_MAX;
float b = (float)rand() / RAND_MAX;
float seed1 = (float)(rand() & 0xAFF) / 10;
float seed2 = 5.f + (float)(rand() % 5000) / 5000;
float seed3 = 5.f + (float)(rand() % 5000) / 5000;
float seed4 = 5.f + (float)(rand() % 5000) / 5000;
float intensity = 0.5 + (((float)rand() / RAND_MAX) * 0.5);
float falloff = 3 + ((float)rand() / RAND_MAX) * 3;
glUniform4f(glGetUniformLocation(program, "seed"), seed1, seed2, seed3, seed4);
glUniform3f(glGetUniformLocation(program, "colour"), r, g, b);
glUniform1f(glGetUniformLocation(program, "intensity"), intensity);
glUniform1f(glGetUniformLocation(program, "falloff"), falloff);
printf("Seed: %f; Intensity: %f; Falloff: %f; #%02x%02x%02x\n", seed1, intensity, falloff,
(int)(r * 255) & 0xFF, (int)(g * 255) & 0xFF, (int)(b * 255) & 0xFF);
glDrawArrays(GL_TRIANGLES, 0, 6);
}
glDisableVertexAttribArray(0);
}
if(gen_sun) {
program = program_sun;
glUseProgram(program);
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(0);
glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer);
glVertexAttribPointer(
0, // attribute 0. No particular reason for 0, but must match the layout in the shader.
3, // size
GL_FLOAT, // type
GL_FALSE, // normalized?
0, // stride
(void*) 0 // array buffer offset
);
int n = rand() > RAND_MAX / 2 ? 1 : 0;
while (rand() < RAND_MAX / 20) n++;
for(i = 0; i < n; i++) {
render_set_framebuffer(program);
int x = rand() % width, y = rand() % height;
float r,g,b;
if(rand() > RAND_MAX / 2) {
r = 1;
g = (float)rand() / RAND_MAX;
b = ((float)rand() / RAND_MAX) * 0.25f;
} else {
r = ((float)rand() / RAND_MAX) * 0.25f;
g = (float)rand() / RAND_MAX;
b = 1;
}
float radius = 1.f + scale * (rand() / (RAND_MAX * 20.f) + 0.02f);
printf("Position: %d, %d; Radius: %f; #%02x%02x%02x\n", x, y, radius,
(int)(r * 255) & 0xFF, (int)(g * 255) & 0xFF, (int)(b * 255) & 0xFF);
glUniform2f(glGetUniformLocation(program, "coord"), x, y);
glUniform1f(glGetUniformLocation(program, "radius"), radius);
glUniform1f(glGetUniformLocation(program, "radius_squared"), radius * radius);
glUniform3f(glGetUniformLocation(program, "colour"), r, g, b);
glDrawArrays(GL_TRIANGLES, 0, 6);
}
glDisableVertexAttribArray(0);
}
printf("Render: finished at %llu\n", get_time_us());
// swap framebuffer again so we get the final texture
if(++fb > 1) fb = 0;
}
// Tester
int def259(FILE *source) {
unsigned char in[CHUNK];
unsigned char in_res[CHUNK];
unsigned char out[CHUNK*2]; // In case we have an overflow.
unsigned char tree[512];
unsigned in_len, out_len, tree_len, tree_bytes, in_res_len, i;
int tmp;
int64_t start, end;
start = get_time_us();
tmp = fread(in, 1, CHUNK, source);
if (tmp < 0) {
fprintf(stderr, "Failed to read input file.\n");
return 1;
}
in_len = tmp;
FILE *fp = fopen("sam_tree.bin", "r");
if (fp == NULL) {
fprintf(stderr, "Failed to read tree file.\n");
return 1;
}
tree_bytes = fread(tree, 1, 512, fp);
fprintf(stderr, "Tree bytes: %d\n", tree_bytes);
// The last item of tree is how many bits are valid within the last non-empty byte.
// Value is from 0 to 8.
tree_len = 8*(tree_bytes-2)+tree[tree_bytes-1];
fprintf(stderr, "Huffman tree loaded, length = %d bits.\n", tree_len);
#ifdef SDACCEL_HOST
deflate259_opencl(in, in_len, tree, tree_len, out, &out_len);
#else
{
int j, k;
uint512 in_hw[CHUNK/64];
uint512 out_hw[CHUNK*2/64];
uint512 tree_hw[512/64];
uint512 tmp512;
for (i = 0; i < CHUNK/64; i++) {
for (j = 0; j < 64; j++)
tmp512 = apint_set_range(tmp512, (j+1)*8-1, j*8, in[i*64+j]);
in_hw[i] = tmp512;
}
for (i = 0; i < 512/64; i++) {
for (j = 0; j < 64; j++)
tmp512 = apint_set_range(tmp512, (j+1)*8-1, j*8, tree[i*64+j]);
tree_hw[i] = tmp512;
}
deflate259(in_hw, &in_len, tree_hw, &tree_len, out_hw, &out_len);
for (i = 0; i < CHUNK*2/64; i++) {
tmp512 = out_hw[i];
for (j = 0; j < 64; j++)
out[i*64+j] = apint_get_range(tmp512, (j+1)*8-1, j*8);
}
}
#endif
end = get_time_us();
fprintf(stderr, "Deflate complete, size after compression: %d. Verifying output...\n", out_len);
FILE *dest = fopen("ex1.sam.def0", "w");
if ( fwrite(out, 1, out_len, dest) == out_len) {
fprintf(stderr, "Dumped compressed output successfully.\n");
fclose(dest);
}
#if DO_VERIFY==1
unsigned char out_res[CHUNK*2]; // In case we have an overflow.
unsigned out_res_len;
fp = fopen("ex1.sam.gold.def0", "r");
if (fp == NULL) {
fprintf(stderr, "Failed to read golden file.\n");
return 1;
}
out_res_len = fread(out_res, 1, CHUNK*2, fp);
//out_res_len = 21461;
fprintf(stderr, "Output length: expected %d, got %d.\n", out_res_len, out_len);
for (i=0; i<((out_res_len>out_len)?out_res_len:out_len); i++) {
if (out[i] != out_res[i]) {
fprintf(stderr, "Byte mismatch at position %d: expected %02x, got %02x.", i, out_res[i], out[i]);
return 1;
}
}
/*
{
int st = system("diff --brief ex1.sam.gold.def0 ex1.sam.def0");
if (st != 0) return Z_DATA_ERROR;
}
*/
#endif
// Stop the timer and calculate throughput
#define PERFORMANCE_DUMP
#ifdef PERFORMANCE_DUMP
float elapsed = (float)(end-start)/1000000.0;
fprintf(stderr, "======================\n");
fprintf(stderr, "Input size: %d bytes (%f MB)\n", in_len, (float)1.0*in_len/1024/1024);
fprintf(stderr, "Output size: %d bytes (%f MB)\n", out_len, (float)1.0*out_len/1024/1024);
fprintf(stderr, "Compression ratio: %f\n", (float)1.0*in_len/out_len);
fprintf(stderr, "Elapsed time: %f seconds\n", elapsed);
fprintf(stderr, "Throughput: %f MB/s\n", (float)1.0*in_len/1024/1024/elapsed);
#endif
return 0;
}
static DBusHandlerResult
ping_object_message_handler_cb(DBusConnection* a_conn, DBusMessage *a_message, void* a_user_data)
{
/* Make compiler happy */
a_conn = a_conn;
a_user_data = a_user_data;
/* Is it our call?*/
if (dbus_message_is_method_call(a_message, INTERFACE, METHOD))
{
unsigned counter;
unsigned orig_timestamp;
if ( dbus_message_get_args(a_message, &s_error,
DBUS_TYPE_UINT32, &counter,
DBUS_TYPE_UINT32, &orig_timestamp,
DBUS_TYPE_INVALID) )
{
const unsigned now = get_time_us();
/* We shall be careful: message can be lost */
const unsigned diff = (now > orig_timestamp ? now - orig_timestamp : 0);
SERVER* server = s_server; /* Probably will be identified for each client */
/* If we're measuring the roundtrip performance, then just reply
and be done with it */
if (!dbus_message_get_no_reply(a_message))
{
DBusMessage *reply;
unsigned timestamp = get_time_us();
reply = dbus_message_new_method_return(a_message);
if (NULL == reply)
{
fatal("DPONG: ERROR: dbus_message_new_method_return() failure.");
}
dbus_message_append_args(reply,
DBUS_TYPE_UINT32, &counter,
DBUS_TYPE_UINT32, &orig_timestamp,
DBUS_TYPE_UINT32, ×tamp,
DBUS_TYPE_INVALID);
if (!dbus_connection_send(a_conn, reply, NULL))
{
fprintf(stderr, "DPONG: WARNING: dbus_connection_send() failure.\n");
}
dbus_message_unref(reply);
dbus_connection_flush(a_conn);
}
else
{
/* Check message losing */
if (counter == server->counter)
{
server->recv++;
if (diff > 0)
{
if (0 == server->min_time || server->min_time > diff)
server->min_time = diff;
if (0 == server->max_time || server->max_time < diff)
server->max_time = diff;
server->tot_time += diff;
}
}
else
{
/* Number of messages lost */
server->lost += (server->counter < counter ? counter - server->counter : server->report - server->counter);
}
/* Increase the index of message */
server->counter = counter + 1;
/* Reporting if it necessary */
if (0 == (server->counter % server->report))
{
/* fprintf (stdout, "dpong timestamp: %u microseconds\n", get_time_us()); */
fprintf (stdout, "MESSAGES recv %u lost %u LATENCY min %5u avg %5u max %5u THROUGHPUT %.1f m/s\n",
server->recv, server->lost,
server->min_time, (server->tot_time / server->recv), server->max_time,
server->recv/((double)(get_time_us()-server->initial_ts)/1000000)
);
/* Setup values for new test cycle */
if (server->counter < server->report)
{
/* We lose some messages and receive the first one from
the new cycle */
server->recv = 1;
server->lost = counter - 1;
server->min_time = diff;
server->tot_time = diff;
server->max_time = diff;
}
else
{
/* Normal flow, all messages received */
server->recv = 0;
server->lost = 0;
server->min_time = 0;
server->tot_time = 0;
server->max_time = 0;
server->initial_ts = get_time_us();
}
}
}
return 0;
}
}
return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
} /* ping_object_message_handler_cb */
void MAVLinkMKHUCHApp::handle_input(const mkhuch_message_t& msg) {
uint64_t last_mkhuch_msg_time = mkhuch_msg_time;
mkhuch_msg_time = get_time_us();
log("MAVLinkMKHUCHApp got mkhuch_message", static_cast<int>(msg.type), Logger::LOGLEVEL_DEBUG);
//send heartbeat approx. after 1s
heartbeat_time += mkhuch_msg_time - last_mkhuch_msg_time;
if( heartbeat_time > 1000000 ) {
send_heartbeat();
heartbeat_time = 0;
}
switch(msg.type) {
case MKHUCH_MSG_TYPE_MK_IMU: {
const mkhuch_mk_imu_t *mk_imu = reinterpret_cast<const mkhuch_mk_imu_t*>(msg.data);
mavlink_huch_imu_raw_adc_t imu_raw_adc;
imu_raw_adc.xacc = mk_imu->x_adc_acc;
imu_raw_adc.yacc = mk_imu->y_adc_acc;
imu_raw_adc.zacc = mk_imu->z_adc_acc;
imu_raw_adc.xgyro = mk_imu->x_adc_gyro;
imu_raw_adc.ygyro = mk_imu->y_adc_gyro;
imu_raw_adc.zgyro = mk_imu->z_adc_gyro;
DataCenter::set_huch_imu_raw_adc(imu_raw_adc);
Lock tx_lock(tx_mav_mutex);
//forward raw ADC
mavlink_msg_huch_imu_raw_adc_encode(system_id(),
component_id,
&tx_mav_msg,
&imu_raw_adc
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
//forward MK IMU
//TODO: add compass value and baro
mavlink_huch_mk_imu_t huch_mk_imu;
huch_mk_imu.usec = mkhuch_msg_time;
huch_mk_imu.xacc = (2500*mk_imu->x_acc)/1024; //convert normalized analog to mg
huch_mk_imu.yacc = (2500*mk_imu->y_acc)/1024;
huch_mk_imu.zacc = (2500*mk_imu->z_acc)/1024;
huch_mk_imu.xgyro = (6700*mk_imu->x_adc_gyro)/(3*1024); //convert analog to 0.1 deg./sec.
huch_mk_imu.ygyro = (6700*mk_imu->y_adc_gyro)/(3*1024);
huch_mk_imu.zgyro = (6700*mk_imu->z_adc_gyro)/(3*1024);
DataCenter::set_huch_mk_imu(huch_mk_imu);
mavlink_msg_huch_mk_imu_encode(system_id(),
component_id,
&tx_mav_msg,
&huch_mk_imu
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
//forward pressure
mavlink_raw_pressure_t raw_pressure;
raw_pressure.time_usec = mkhuch_msg_time;
raw_pressure.press_abs = mk_imu->press_abs;
raw_pressure.press_diff1 = 0; //press_diff1
raw_pressure.press_diff2 = 0; //press_diff2
raw_pressure.temperature = 0; //temperature
DataCenter::set_raw_pressure(raw_pressure);
mavlink_msg_raw_pressure_encode(system_id(),
component_id,
&tx_mav_msg,
&raw_pressure
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
//TODO: forward magneto
break;
}
/* case MKHUCH_MSG_TYPE_PARAM_VALUE: {
const mkhuch_param_t *param = reinterpret_cast<const mkhuch_param_t*>(msg.data);
//set parameter
uint8_t index;
if(param->index >= parameter_count)
index = parameter_count-1;
else
index = param->index;
parameters[index] = param->value;
//ask for next parameter
if(index < parameter_count - 1) {
parameter_request = index + 1;
mk_param_type_t param_type= static_cast<mk_param_type_t>(parameter_request);
send(MKHUCH_MSG_TYPE_PARAM_REQUEST, ¶m_type, sizeof(mk_param_type_t));
parameter_time = message_time;
} else { //got all parameters
parameter_request = 255;
}
//inform others
send_mavlink_param_value( static_cast<mk_param_type_t>(index) );
break;
}*/
/* case MKHUCH_MSG_TYPE_ACTION_ACK: {
Lock tx_lock(tx_mav_mutex);
mavlink_msg_action_ack_pack(owner()->system_id(), component_id, &tx_mav_msg, msg.data[0], msg.data[1]);
send(tx_mav_msg);
break;
}*/
case MKHUCH_MSG_TYPE_STICKS: {
const mkhuch_sticks_t *sticks = reinterpret_cast<const mkhuch_sticks_t*>(msg.data);
mavlink_huch_attitude_control_t attitude_control;
attitude_control.roll = (float)sticks->roll;
attitude_control.pitch = (float)sticks->pitch;
attitude_control.yaw = (float)sticks->yaw;
attitude_control.thrust = (float)sticks->thrust;
attitude_control.target = system_id();
attitude_control.mask = 0;
// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->roll), static_cast<int16_t>(sticks->pitch), Logger::LOGLEVEL_DEBUG);
// log("MAVLinkMKHUCHApp got mkhuch_sticks msg", static_cast<int16_t>(sticks->yaw), static_cast<int16_t>(sticks->thrust), Logger::LOGLEVEL_DEBUG);
// ALERT uint64_t -> uint32_t cast
uint32_t time_ms = get_time_ms();
Lock tx_lock(tx_mav_mutex);
mavlink_msg_huch_attitude_control_encode(
system_id(),
component_id,
&tx_mav_msg,
&attitude_control
);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_roll",
// sticks->roll);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_pitch",
// sticks->pitch);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_yaw",
// sticks->yaw);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
// mavlink_msg_named_value_int_pack(system_id(),
// component_id,
// &tx_mav_msg,
// time_ms,
// "stk_thrust",
// sticks->thrust);
// AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MKHUCH_MSG_TYPE_RC_CHANNELS_RAW: {
const mkhuch_rc_channels_raw_t *rc_channels_raw = reinterpret_cast<const mkhuch_rc_channels_raw_t*>(msg.data);
//log("MAVLinkMKHUCHApp got mkhuch_rc_channels_raw msg", static_cast<int16_t>(rc_channels_raw->channel_2), Logger::LOGLEVEL_DEBUG);
Lock tx_lock(tx_mav_mutex);
mavlink_msg_rc_channels_raw_pack(system_id(),
component_id,
&tx_mav_msg,
get_time_ms(),
0, //FIXME
rc_channels_raw->channel_1,
rc_channels_raw->channel_2,
rc_channels_raw->channel_3,
rc_channels_raw->channel_4,
rc_channels_raw->channel_5,
rc_channels_raw->channel_6,
rc_channels_raw->channel_7,
rc_channels_raw->channel_8,
rc_channels_raw->rssi);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
case MKHUCH_MSG_TYPE_SYSTEM_STATUS: {
/* FIXME
const mkhuch_system_status_t *sys_status = reinterpret_cast<const mkhuch_system_status_t*>(msg.data);
Lock tx_lock(tx_mav_mutex);
mavlink_msg_sys_status_pack(system_id(),
component_id,
&tx_mav_msg,
sys_status->mode,
sys_status->nav_mode,
sys_status->state,
1000, //FIXME: use glibtop to get load of linux system
sys_status->vbat*100, //convert dV to mV
0,//motor block (unsupported)
sys_status->packet_drop);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
*/
break;
}
/* case MKHUCH_MSG_TYPE_BOOT:
//TODO
break;*/
case MKHUCH_MSG_TYPE_ATTITUDE: {
const mkhuch_attitude_t *mkhuch_attitude = reinterpret_cast<const mkhuch_attitude_t*>(msg.data);
mavlink_attitude_t mavlink_attitude;
mavlink_attitude.time_boot_ms = mkhuch_msg_time / 1000;
mavlink_attitude.roll = 0.001745329251994329577*(mkhuch_attitude->roll_angle); //convert cdeg to rad with (pi/180)/10
mavlink_attitude.pitch = 0.001745329251994329577*(mkhuch_attitude->pitch_angle); //convert cdeg to rad with (pi/180)/10
mavlink_attitude.yaw = 0.001745329251994329577*(mkhuch_attitude->yaw_angle); //convert cdeg to rad with (pi/180)/10
//FIXME
mavlink_attitude.rollspeed = 0;
mavlink_attitude.pitchspeed = 0;
mavlink_attitude.yawspeed = 0;
Lock tx_lock(tx_mav_mutex);
mavlink_msg_attitude_encode(system_id(),
component_id,
&tx_mav_msg,
&mavlink_attitude);
AppLayer<mavlink_message_t>::send(tx_mav_msg);
break;
}
default:
break;
}
}
int main() {
// Timer
auto start = std::chrono::steady_clock::now();
auto end = std::chrono::steady_clock::now();
std::cout << "Timings:" << std::endl;
// Precomputation in the clear
ECPrecomputation<FV::mess_t> precomputation;
// Point G and variable for G+G
ECPoint<FV::mess_t> G(G_x, G_y, G_z, G_t), GpG;
// Compute 4G = (G+G)+(G+G) with 2 EC additions
start = std::chrono::steady_clock::now();
ec_addition(GpG, G, G, precomputation);
ec_addition(GpG, GpG, GpG, precomputation);
end = std::chrono::steady_clock::now();
std::cout << "\tEC Addition in clear: \t\t"
<< get_time_us(start, end, 2) << " us" << std::endl;
FV::mess_t result_x = GpG.X + FV::params::plaintextModulus<mpz_class>::value();
FV::mess_t result_y = GpG.Y + FV::params::plaintextModulus<mpz_class>::value();
// Multiply by the inverse of Z
FV::mess_t iZ = GpG.Z.invert();
GpG.X *= iZ;
GpG.Y *= iZ;
/**
* Let's do it homomorphically
*/
// Keygen
start = std::chrono::steady_clock::now();
FV::sk_t secret_key;
end = std::chrono::steady_clock::now();
std::cout << "\tSecret key generation: \t\t"
<< get_time_us(start, end, 1) << " us" << std::endl;
start = std::chrono::steady_clock::now();
FV::evk_t evaluation_key(secret_key, 32);
end = std::chrono::steady_clock::now();
std::cout << "\tEvaluation key generation: \t"
<< get_time_us(start, end, 1) << " us" << std::endl;
start = std::chrono::steady_clock::now();
FV::pk_t public_key(secret_key, evaluation_key);
end = std::chrono::steady_clock::now();
std::cout << "\tPublic key generation: \t\t"
<< get_time_us(start, end, 1) << " us" << std::endl;
// Precomputation
ECPrecomputation<FV::ciphertext_t> precomputation_encrypted;
// Point G and variable for G+G
FV::ciphertext_t eG_x, eG_y, eG_z, eG_t;
FV::mess_t mG_x(G_x), mG_y(G_y), mG_z(G_z), mG_t(G_t);
start = std::chrono::steady_clock::now();
FV::encrypt(eG_x, public_key, mG_x);
FV::encrypt(eG_y, public_key, mG_y);
FV::encrypt(eG_z, public_key, mG_z);
FV::encrypt(eG_t, public_key, mG_t);
end = std::chrono::steady_clock::now();
std::cout << "\tPoint Encryption: \t\t"
<< get_time_us(start, end, 1) << " us" << std::endl;
ECPoint<FV::ciphertext_t> eG(eG_x, eG_y, eG_z, eG_t), eGpG;
// Compute 4G = (G+G)+(G+G) with 2 EC additions
start = std::chrono::steady_clock::now();
ec_addition<FV::ciphertext_t>(eGpG, eG, eG, precomputation_encrypted);
ec_addition<FV::ciphertext_t>(eGpG, eGpG, eGpG, precomputation_encrypted);
end = std::chrono::steady_clock::now();
std::cout << "\tHomom. EC Addition: \t\t"
<< get_time_us(start, end, 2) / 1000 << " ms"
<< std::endl;
start = std::chrono::steady_clock::now();
FV::decrypt(mG_x, secret_key, public_key, eGpG.X);
FV::decrypt(mG_y, secret_key, public_key, eGpG.Y);
FV::decrypt(mG_z, secret_key, public_key, eGpG.Z);
FV::decrypt(mG_t, secret_key, public_key, eGpG.T);
end = std::chrono::steady_clock::now();
std::cout << "\tEC Point Decryption: \t\t"
<< get_time_us(start, end, 1) << " us" << std::endl;
// Noise
unsigned noise_x = noise(mG_x, secret_key, public_key, eGpG.X);
std::cout << "noise in ciphertext: \t" << noise_x << "/"
<< public_key.noise_max << std::endl;
// Multiply by the inverse of Z
FV::mess_t invZ = mG_z.invert();
mG_x *= invZ;
mG_y *= invZ;
// Results
std::cout << "4*G (clear): \t\t(" << GpG.X << "," << GpG.Y << ")"
<< std::endl;
std::cout << "4*G (enc.): \t\t(" << mG_x << "," << mG_y << ")" << std::endl;
return 0;
}
void SenCmp02::run() {
char buffer[256];
int wait_freq = update_rate? 1000000 / update_rate: 0;
int wait_time = wait_freq;
uint64_t frequency = wait_time;
uint64_t start = get_time_us();
uint64_t time_output = start + 1000000;
uint64_t usec;
vector<uint16_t> cmp_value(2);
//mavlink_message_t msg;
Logger::log("Cmp02: running (Hz)", update_rate, Logger::LOGLEVEL_INFO);
// uint64_t end = getTimeUs() + 1000000;
status = RUNNING;
while((status == RUNNING) && update_rate) {
try {
/* wait time */
usec = get_time_us();
uint64_t end = usec;
wait_time = wait_freq - (end - start);
wait_time = (wait_time < 0)? 0: wait_time;
/* wait */
usleep(wait_time);
//usleep(10);
/* calculate frequency */
end = get_time_us();
frequency = (15 * frequency + end - start) / 16;
start = end;
// Logger::log("sencmp2: pre i2c_start_conversion", Logger::LOGLEVEL_DEBUG);
i2c_start_conversion(fd, CMP02_ADR);
// set read register
buffer[0] = 0x00;
// Logger::log("sencmp2: pre i2c_write_bytes", Logger::LOGLEVEL_DEBUG);
i2c_write_bytes(fd, (uint8_t*)buffer, 1);
// read data
// Logger::log("sencmp2: pre i2c_read_bytes", Logger::LOGLEVEL_DEBUG);
i2c_read_bytes(fd, (uint8_t*)buffer, 1);
// get version
version = *buffer;
// Logger::log("Cmp02: version", (int)version, Logger::LOGLEVEL_INFO);
buffer[0] = 0x01;
// Logger::log("sencmp2: pre i2c_write_bytes 2", Logger::LOGLEVEL_DEBUG);
i2c_write_bytes(fd, (uint8_t*)buffer, 1);
// Logger::log("sencmp2: pre i2c_read_bytes 2", Logger::LOGLEVEL_DEBUG);
i2c_read_bytes(fd, (uint8_t*)buffer, 1);
// Logger::log("sencmp2: pre i2c_end_conversion", Logger::LOGLEVEL_DEBUG);
i2c_end_conversion(fd);
// get values
memcpy(&cmp_value[0], buffer, 1);
// Logger::log("Cmp02: value", (int)cmp_value[0], Logger::LOGLEVEL_INFO);
// FIXME: smart++
// exp_ctrl_rx_data.value0 = cmp_value[0];
// exp_ctrl_rx_data.value1 = cmp_value[1];
// exp_ctrl_rx_data.value2 = cmp_value[2];
// exp_ctrl_rx_data.value3 = cmp_value[3];
// FIXME: kopter specific mapping
// FIXME: 0 is USS
// huch_ranger.ranger2 = cmp_value[2];
// huch_ranger.ranger3 = cmp_value[0];
// home / qk01
// huch_ranger.ranger2 = cmp_value[0];
// huch_ranger.ranger3 = cmp_value[1];
/* assign buffer to data */
#ifdef MAVLINK_ENABLED_HUCH
{ // begin of data mutex scope
int i;
cpp_pthread::Lock ri_lock(data_mutex);
for(i=0; i < CMP02_NUMCHAN; i++) {
sensor_data[i].analog = cmp_value[i];
sensor_data[i].usec = start;
// Logger::log("Cmp02 sensor:", i, sensor_data[i].analog, Logger::LOGLEVEL_INFO);
}
} // end of data mutex scope
#endif // MAVLINK_ENABLED_HUCH
// FIXME: if(publish) else poll or whatever
publish_data(start);
//Logger::log("Cmp02:", (int)exp_ctrl_rx_data.version, cmp_value, Logger::LOGLEVEL_INFO);
//Logger::log("Cmp02:", DataCenter::get_sensor(chanmap[0]), Logger::LOGLEVEL_INFO);
// pass more data
// FIXME: system_id
// mavlink_msg_huch_exp_ctrl_rx_encode(39, static_cast<uint8_t>(component_id), &msg, &exp_ctrl_rx_data);
/* debug data */
if (debug) print_debug();
/* timings/benchmark output */
if (timings) {
if (time_output <= end) {
Logger::log("Cmp02 frequency: ", (float)1000000/frequency, Logger::LOGLEVEL_DEBUG);
time_output += 1000000;
}
}
} // end try
catch(const char *message) {
i2c_end_conversion(fd);
status = STRANGE;
string s(message);
Logger::log("sencmp2: would-be exception:", s, Logger::LOGLEVEL_DEBUG);
// fallback termination and free bus
i2c_end_conversion(fd);
Logger::log("sencmp2: would-be exception:", s, Logger::LOGLEVEL_DEBUG);
// FIXME: throw and exit on "write_bytes 2"
// throw ("Cmp02::run(): " + s).c_str();
status = RUNNING;
}
} // end while
Logger::debug("sencmp02: stopped, exiting run()");
return;
}
int main ()
{
// Prepare our context and socket
zmq::context_t context (1);
zmq::socket_t socket (context, ZMQ_REQ);
int rc = 0;
std::cout << "Connecting to hello world server..." << std::endl;
socket.connect ("tcp://localhost:5555");
//socket.connect ("epgm://eth0;127.0.0.1:5555");
//socket.connect("ipc:///tmp/feeds_mq");
//socket.connect ("tcp://176.9.101.85:5555");
assert(rc == 0);
// Initialize random number generator
//srandom ((unsigned) time (NULL));
// Start our clock now
unsigned long total_msec = get_time_us();
unsigned long resp_time_med = 0;
unsigned long resp_time_min = 10000000;
unsigned long resp_time_max = 0;
int start = within(1000);
int count = 10;
// Do 10 requests, waiting each time for a response
for (int request_nbr = start; request_nbr != start+count; request_nbr++) {
unsigned long resp_time = get_time_us();
//FIX: 100 is max length
zmq::message_t request (2000);
//std::istringstream iss(static_cast<char*>(request.data()));
//iss << "SET key" << request_nbr << " val" << request_nbr;
snprintf ((char *) request.data(), 2000 ,
"put('db/testx','aaa','dane'); get('db/testx','aaa'); ");
//"for(i=%d; i<%d+10; i++){ put('db/testb','aaa'+i,'dane'+i) }", request_nbr, request_nbr);
//std::cout << "Sending: " << static_cast<char*>(request.data()) << "..." << std::endl;
socket.send (request);
// Get the reply
zmq::message_t reply;
socket.recv (&reply);
std::cout << "Received: " << static_cast<char*>(reply.data()) << std::endl;
resp_time = get_time_us() - resp_time;
resp_time_med += resp_time;
resp_time_min = resp_time_min > resp_time ? resp_time : resp_time_min;
resp_time_max = resp_time_max < resp_time ? resp_time : resp_time_max;
}
resp_time_med = resp_time_med/count;
#if 0
for (int request_nbr = start; request_nbr != start+1; request_nbr++) {
//FIX: 100 is max length
zmq::message_t request (2000);
//memcpy ((void *) request.data (), "Hello", 5);
//std::istringstream iss(static_cast<char*>(request.data()));
//iss << "SET key" << request_nbr << " val" << request_nbr;
snprintf ((char *) request.data(), 2000,
"var ret = []; var i = 0; var it = it_new('db/testb'); for(it_seek(it, 'aaa2'); it_valid(it) && it_key(it)<'aaa4'; it_next(it)){ ret[i++] = {'key': it_key(it), 'val': it_val(it)}; }; JSON.stringify(ret)"
//"var ret = []; var i = 0; var it = it_new('db/testb'); for(it_first(it); it_valid(it); it_next(it)){ ret[i++] = {'key': it_key(it), 'val': it_val(it)}; };"
//"for(it_last(it); it_valid(it); it_prev(it)){ ret[i++] = {'key': it_key(it), 'val': it_val(it)}; del('db/testb', it_key(it)) }; JSON.stringify(ret)"
);
// var off = %d; for(i=0; i<10; i++){ var key = 'aaa'+(i+off); ret[i] = {'key': key, 'get': get('db/test', key)}; del('db/test', key) }; JSON.stringify(ret)", request_nbr);
//"var ret = []; var off = %d; for(i=0; i<10; i++){ var key = 'aaa'+(i+off); ret[i] = {'key': key, 'get': get('db/test', key)}; del('db/test', key) }; JSON.stringify(ret)", request_nbr);
//std::cout << "Sending: " << static_cast<char*>(request.data()) << "..." << std::endl;
socket.send (request);
// Get the reply
zmq::message_t reply;
socket.recv (&reply);
std::cout << "Received: " << static_cast<char*>(reply.data()) << std::endl;
}
#endif
total_msec = (get_time_us() - total_msec)/1000;
std::cout << "\nTotal elapsed time: " << total_msec << " msec\n" << " resp med: " << resp_time_med << " min:" << resp_time_min << " max:" << resp_time_max << std::endl;
return 0;
}
