INT_PTR dialog_proc(int msg, int param1, void* param2) { if (msg == DN_CLOSE && param1 >= 0 && param1 != cancel_ctrl_id) { options = read_controls(); } else if (msg == DN_INITDIALOG) { set_control_state(); } else if (msg == DN_BTNCLICK) { set_control_state(); } else if (msg == DN_EDITCHANGE && param1 == module_ctrl_id) { set_control_state(); } else if (msg == DN_EDITCHANGE && param1 == profile_ctrl_id) { unsigned profile_idx = get_list_pos(profile_ctrl_id); if (profile_idx != -1 && profile_idx < profiles.size()) { write_controls(profiles[profile_idx].options); set_control_state(); } } if (msg == DN_EDITCHANGE || msg == DN_BTNCLICK) { unsigned profile_idx = static_cast<unsigned>(profiles.size()); SfxOptions options; bool valid_options = true; try { options = read_controls(); } catch (const Error&) { valid_options = false; } if (valid_options) { for (unsigned i = 0; i < profiles.size(); i++) { if (options == profiles[i].options) { profile_idx = i; break; } } } if (profile_idx != get_list_pos(profile_ctrl_id)) { DisableEvents de(*this); set_list_pos(profile_ctrl_id, profile_idx); set_control_state(); } } return default_dialog_proc(msg, param1, param2); }
ret_code_t ctrl_block(void) { SaHpiRdrT rdr_entry; SaHpiResourceIdT rptid; SaHpiInstrumentIdT rdrnum; SaHpiRdrTypeT type; SaErrorT rv; char buf[256]; ret_code_t ret; term_def_t *term; int res; ret = ask_rpt(&rptid); if (ret != HPI_SHELL_OK) return(ret); type = SAHPI_CTRL_RDR; ret = ask_rdr(rptid, type, &rdrnum); if (ret != HPI_SHELL_OK) return(ret); rv = saHpiRdrGetByInstrumentId(Domain->sessionId, rptid, type, rdrnum, &rdr_entry); if (rv != SA_OK) { printf("saHpiRdrGetByInstrumentId error %s\n", oh_lookup_error(rv)); printf("ERROR!!! Can not get rdr: ResourceId=%d RdrType=%d RdrNum=%d\n", rptid, type, rdrnum); return(HPI_SHELL_CMD_ERROR); }; show_control(Domain->sessionId, rptid, rdrnum, ui_print); for (;;) { block_type = CTRL_COM; res = get_new_command("control block ==> "); if (res == 2) { unget_term(); return HPI_SHELL_OK; }; term = get_next_term(); if (term == NULL) continue; snprintf(buf, 256, "%s", term->term); if ((strcmp(buf, "q") == 0) || (strcmp(buf, "quit") == 0)) break; if (strcmp(buf, "state") == 0) { show_control_state(Domain->sessionId, rptid, rdrnum, ui_print); continue; }; if (strcmp(buf, "setstate") == 0) { set_control_state(Domain->sessionId, rptid, rdrnum); continue; }; if (strcmp(buf, "show") == 0) { show_control(Domain->sessionId, rptid, rdrnum, ui_print); continue; } }; block_type = MAIN_COM; return SA_OK; }
/** * Navdata handling callback * This callback is registered to ARDroneTool in the BEGIN_NAVDATA_HANDLER_TABLE in navdata_client.c * @param pnd Pointer to the ready-to-use navdata structure * @return Always returns C_OK. */ C_RESULT navdata_ihm_process( const navdata_unpacked_t* const pnd ) { //char text_buffer[1024]; const navdata_vision_detect_t* pndvision = &pnd->navdata_vision_detect; gdk_threads_enter(); // States' display update for IHM vp_os_memset( label_mykonos_state_value, '\0', sizeof(label_mykonos_state_value) ); sprintf(label_mykonos_state_value, "%08x", pnd->ardrone_state); strcpy(label_ctrl_state_value, ctrl_state_str( pnd->navdata_demo.ctrl_state )); set_control_state(pnd->navdata_demo.ctrl_state); ihm_val_idx--; if (ihm_val_idx < 0) ihm_val_idx = KIHM_N_PT2PLOT-1; // Angular rates received and displayed in deg/s ihm_CA[KIHM_CURVE_GYRO].tval[0][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_X]); ihm_CA[KIHM_CURVE_GYRO].tval[1][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_Y]); ihm_CA[KIHM_CURVE_GYRO].tval[2][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_Z]); // Accelerations are received in mg // They are displayed in g: conversion gain is 1/1000 ihm_CA[KIHM_CURVE_ACC].tval[0][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_X]) / KIHM_MIL_F; ihm_CA[KIHM_CURVE_ACC].tval[1][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_Y]) / KIHM_MIL_F; ihm_CA[KIHM_CURVE_ACC].tval[2][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_Z]) / KIHM_MIL_F; // Filtered temperature is drawn in the Vbat window ihm_CA[KIHM_CURVE_VBAT].tval[0][ihm_val_idx] = ((double) (pnd->navdata_phys_measures.accs_temp)); // Filtered temperature ihm_CA[KIHM_CURVE_VBAT].tval[1][ihm_val_idx] = ((double) (pnd->navdata_demo.vbat_flying_percentage)) ; // Filtered vbat in % ihm_CA[KIHM_CURVE_THETA].tval[2][ihm_val_idx] = ((double) pnd->navdata_euler_angles.theta_a) / KIHM_MIL_F; // theta accelerometer value ihm_CA[KIHM_CURVE_PHI ].tval[2][ihm_val_idx] = ((double) pnd->navdata_euler_angles.phi_a) / KIHM_MIL_F; // phi accelerometer value ihm_CA[KIHM_CURVE_THETA].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.theta) / KIHM_MIL_F; // theta fused value ihm_CA[KIHM_CURVE_PHI ].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.phi) / KIHM_MIL_F; // phi fused value ihm_psi = ((double) pnd->navdata_demo.psi) / KIHM_MIL_F; earef.theta = pnd->navdata_references.ref_theta; ihm_CA[KIHM_CURVE_THETA].tval[1][ihm_val_idx] = ((double) earef.theta) / KIHM_MIL_F; // theta reference value earef.phi = pnd->navdata_references.ref_phi; ihm_CA[KIHM_CURVE_PHI ].tval[1][ihm_val_idx] = ((double) earef.phi) / KIHM_MIL_F; // phi reference value earef.psi = pnd->navdata_references.ref_psi; wref.p = pnd->navdata_references.ref_roll; wref.q = pnd->navdata_references.ref_pitch; wref.r = pnd->navdata_references.ref_yaw; rc_ref_gaz = pnd->navdata_rc_references.rc_ref_gaz; ihm_CA[KIHM_CURVE_ALTITUDE].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.altitude); ihm_CA[KIHM_CURVE_ALTITUDE].tval[1][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_vision); ihm_CA[KIHM_CURVE_ALTITUDE].tval[2][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_ref); ihm_CA[KIHM_CURVE_ALTITUDE].tval[3][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_raw); ihm_CA[KIHM_CURVE_VZ].tval[1][ihm_val_idx] = (double) (pnd->navdata_altitude.altitude_vz); // Fusion Vz tab_trims[0] = (double)pnd->navdata_trims.euler_angles_trim_theta; tab_trims[1] = (double)pnd->navdata_trims.euler_angles_trim_phi; tab_trims[2] = (double)pnd->navdata_trims.angular_rates_trim_r; ihm_CA[KIHM_CURVE_PWM].tval[0][ihm_val_idx] = (double) (pnd->navdata_pwm.motor1); // PWM motor 1 ihm_CA[KIHM_CURVE_PWM].tval[1][ihm_val_idx] = (double) (pnd->navdata_pwm.motor2); // PWM motor 2 ihm_CA[KIHM_CURVE_PWM].tval[2][ihm_val_idx] = (double) (pnd->navdata_pwm.motor3); // PWM motor 3 ihm_CA[KIHM_CURVE_PWM].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.motor4); // PWM motor 4 ihm_CA[KIHM_CURVE_CURRENT].tval[0][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor1); // current motor 1 ihm_CA[KIHM_CURVE_CURRENT].tval[1][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor2); // current motor 2 ihm_CA[KIHM_CURVE_CURRENT].tval[2][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor3); // current motor 3 ihm_CA[KIHM_CURVE_CURRENT].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor4); // current motor 4 ihm_CA[KIHM_CURVE_VZ].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.vz_ref); // VZ reference ihm_CA[KIHM_CURVE_VX].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_tx_raw); // VISION trans. en x ihm_CA[KIHM_CURVE_VY].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_ty_raw); // VISION trans. en y ihm_CA[KIHM_CURVE_VZ].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_tz_raw); // VISION trans. en z if( pnd->vision_defined ) { if( fabsf(pnd->navdata_vision_raw.vision_tx_raw) <= 10.0f && fabsf(pnd->navdata_vision_raw.vision_ty_raw) <= 10.0f ) { ihm_dir = HOVER_VAL; } else { ihm_dir = (double) atan2f(pnd->navdata_vision_raw.vision_ty_raw, pnd->navdata_vision_raw.vision_tx_raw); } } else { ihm_dir = NOT_DEF_VAL; } //Vision States' display update for IHM vp_os_memset( label_vision_state_value, '\0', sizeof(label_vision_state_value) ); sprintf(label_vision_state_value, "%08x", pnd->navdata_vision.vision_state); ihm_CA[KIHM_CURVE_VX].tval[1][ihm_val_idx] = (double) (pnd->navdata_demo.vx); // Fusion Vx ihm_CA[KIHM_CURVE_VY].tval[1][ihm_val_idx] = (double) (pnd->navdata_demo.vy); // Fusion Vy #ifdef PC_USE_VISION draw_trackers_cfg.num_points = NUM_MAX_SCREEN_POINTS; vp_os_memset(&draw_trackers_cfg.locked[0], 0, NUM_MAX_SCREEN_POINTS * sizeof(uint32_t)); vp_os_memcpy(&draw_trackers_cfg.locked[0], pnd->navdata_trackers_send.locked, NUM_MAX_SCREEN_POINTS * sizeof(uint32_t)); vp_os_memset(&draw_trackers_cfg.points[0], 0, NUM_MAX_SCREEN_POINTS * sizeof(screen_point_t)); vp_os_memcpy(&draw_trackers_cfg.points[0], pnd->navdata_trackers_send.point, NUM_MAX_SCREEN_POINTS * sizeof(screen_point_t)); int i; draw_trackers_cfg.detected = pnd->navdata_vision_detect.nb_detected; for(i = 0 ; i < pnd->navdata_vision_detect.nb_detected ; i++) { draw_trackers_cfg.type[i] = pnd->navdata_vision_detect.type[i]; draw_trackers_cfg.patch_center[i].x = pnd->navdata_vision_detect.xc[i]; draw_trackers_cfg.patch_center[i].y = pnd->navdata_vision_detect.yc[i]; draw_trackers_cfg.width[i] = pnd->navdata_vision_detect.width[i]; draw_trackers_cfg.height[i] = pnd->navdata_vision_detect.height[i]; } set_draw_trackers_config(&draw_trackers_cfg); #endif /*Stephane*/ extern char label_detection_state_value[32]; switch(pnd->navdata_demo.detection_camera_type) { case CAD_TYPE_NONE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Detection disabled"); break; case CAD_TYPE_VISION: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching 2D tags"); break; case CAD_TYPE_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching roundels"); break; case CAD_TYPE_ORIENTED_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching roundels"); break; case CAD_TYPE_H_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching H roundels"); break; case CAD_TYPE_H_ORIENTED_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching H roundels"); break; case CAD_TYPE_STRIPE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching ground stripe (15Hz)"); break; case CAD_TYPE_STRIPE_V: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching ground stripe (60Hz)"); break; default : snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Navdata error"); } char nbDetectedTags_label_buffer[1024]; if (pndvision->nb_detected==1 && ( pnd->navdata_demo.detection_camera_type==CAD_TYPE_ORIENTED_COCARDE || pnd->navdata_demo.detection_camera_type==CAD_TYPE_H_ORIENTED_COCARDE ) ) { snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i (%4.2f deg)",pndvision->nb_detected,pndvision->orientation_angle[0]); } else if (pndvision->nb_detected==1 && (pnd->navdata_demo.detection_camera_type==CAD_TYPE_STRIPE || pnd->navdata_demo.detection_camera_type==CAD_TYPE_STRIPE_V)) { snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i (%4.2f deg - w: %i)", pndvision->nb_detected, pndvision->orientation_angle[0], pndvision->width[0]); } else { snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i",pndvision->nb_detected); } if(nbDetectedTags_label!=NULL) gtk_label_set_text((GtkLabel*)nbDetectedTags_label,(const gchar*)nbDetectedTags_label_buffer); gdk_threads_leave(); return C_OK; }
ret_code_t ctrl_block_setst(void) { return(set_control_state(Domain->sessionId, ctrl_block_env.rptid, ctrl_block_env.rdrnum)); }