/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
while (IsOperatorControl())
{
if ( first_iteration ) {
bool is_calibrating = imu->IsCalibrating();
if ( !is_calibrating ) {
Wait( 0.3 );
imu->ZeroYaw();
first_iteration = false;
}
}
SmartDashboard::PutBoolean( "IMU_Connected", imu->IsConnected());
SmartDashboard::PutNumber("IMU_Yaw", imu->GetYaw());
SmartDashboard::PutNumber("IMU_Pitch", imu->GetPitch());
SmartDashboard::PutNumber("IMU_Roll", imu->GetRoll());
SmartDashboard::PutNumber("IMU_CompassHeading", imu->GetCompassHeading());
SmartDashboard::PutNumber("IMU_Update_Count", imu->GetUpdateCount());
SmartDashboard::PutNumber("IMU_Byte_Count", imu->GetByteCount());
//SmartDashboard::PutNumber("IMU_Accel_X", imu->GetWorldLinearAccelX());
//SmartDashboard::PutNumber("IMU_Accel_Y", imu->GetWorldLinearAccelY());
//SmartDashboard::PutBoolean("IMU_IsMoving", imu->IsMoving());
//SmartDashboard::PutNumber("IMU_Temp_C", imu->GetTempC());
SmartDashboard::PutBoolean("IMU_IsCalibrating", imu->IsCalibrating());
Wait(0.2); // wait for a while
}
}
void IMU::imuThread(void *obj) {
//cast the obj to a thread to get original instance
IMU *threadIMU = (IMU *) obj;
threadIMU->init();
while (threadIMU->getThreadRunning()) {
threadIMU->update();
usleep(MICRO_TO_WAIT);
}
return;
}
int main()
{
//
// IMU Test Driver
//
IMU myIMU;
myIMU.Init();
uint delay = DELTAT*1000000;
for (;;) {
myIMU.getNewValues();
cout << myIMU;
//cout << myIMU.getTheta();
this_thread::sleep_for(chrono::microseconds(delay));
}
}
void setup(void)
{
board.init();
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// initialize our RC, IMU, mixer, and PID controller
rc.init(&board);
imu.init(&board);
pid.init();
mixer.init(&board, &rc, &pid);
msp.init(&board, &imu, &mixer, &rc);
// set initial time
previousTime = board.getMicros();
// always do gyro calibration at startup
calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;
// trigger accelerometer calibration requirement
haveSmallAngle = true;
} // setup
// ----- TIM_IRQHandler() ----------------------------------------------------
extern "C" void TIM3_IRQHandler(void) {
if (__HAL_TIM_GET_ITSTATUS(&imu10DOF.TimHandle, TIM_IT_UPDATE ) != RESET) {
semaphore_timerInterrupt = 1;
imu10DOF.timerAction();
semaphore_timerInterrupt = 0;
__HAL_TIM_CLEAR_IT(&imu10DOF.TimHandle, TIM_IT_UPDATE);
}
}
IMUTracker::IMUTracker(const IMU& imu)
:gravity(9.81),
driftCorrectionWeight(0.0001),
magnetometer(imu.getCalibrationData().magnetometer),
useMagnetometer(magnetometer),
numInitialSamples(10),initialAccel(Vector::zero),initialMag(Vector::zero)
{
}
/*
* terminate
* If the program receives a SIGTERM or SIGINT (Control+C), stop the copter and exit
* gracefully.
*/
void terminate(int signum) {
cout << "\033[33m[THRIFT]\033[0m Signal " << signum << " received. Stopping copter. Exiting." << endl;
handlerInternal->allStop();
exitProgram = true;
cam.close();
fb.close();
gps.close();
imu.close();
thriftServer->stop();
}
int main (int argc, char* argcv[]) {
cout << "Starting program" << endl;
//Signal to exit program.
struct sigaction signalHandler;
signalHandler.sa_handler = terminate;
sigemptyset(&signalHandler.sa_mask);
signalHandler.sa_flags = 0;
sigaction(SIGTERM, &signalHandler, NULL);
sigaction(SIGINT, &signalHandler, NULL);
//Main program
IMU imu = IMU();
if(imu.setup() != IMU_OK) {
cout << "Error opening imu: check it's plugged in" << endl;
return -1;
}
imu.start();
IMU_Data positionData;
while(!exitProgram) {
imu.getIMU_Data(&positionData);
cout << "Pitch:\t" << setprecision(12) << positionData.pitch << endl;
cout << "Roll:\t" << setprecision(12) << positionData.roll << endl;
cout << "Yaw:\t" << setprecision(12) << positionData.yaw << endl;
cout << endl;
usleep(500*1000);
}
imu.stop();
imu.close();
return 0;
}
void setup(void)
{
uint32_t calibratingGyroMsec;
// Get particulars for board
board.init(imuLooptimeUsec, calibratingGyroMsec);
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// compute cycles for calibration based on board's time constant
calibratingGyroCycles = (uint16_t)(1000. * calibratingGyroMsec / imuLooptimeUsec);
calibratingAccCycles = (uint16_t)(1000. * CONFIG_CALIBRATING_ACC_MSEC / imuLooptimeUsec);
// initializing timing tasks
imuTask.init(imuLooptimeUsec);
rcTask.init(CONFIG_RC_LOOPTIME_MSEC * 1000);
accelCalibrationTask.init(CONFIG_CALIBRATE_ACCTIME_MSEC * 1000);
altitudeEstimationTask.init(CONFIG_ALTITUDE_UPDATE_MSEC * 1000);
// initialize our external objects with objects they need
rc.init(&board);
stab.init(&rc, &imu);
imu.init(&board, calibratingGyroCycles, calibratingAccCycles);
mixer.init(&board, &rc, &stab);
msp.init(&board, &imu, &nav, &mixer, &rc);
nav.init(&board, &imu, &baro, &rc);
// always do gyro calibration at startup
calibratingG = calibratingGyroCycles;
// assume shallow angle (no accelerometer calibration needed)
haveSmallAngle = true;
// ensure not armed
armed = false;
// attempt to initialize barometer
baro.init(&board);
} // setup
int main(int argc, char* argv[]) {
//Setup exit signal
struct sigaction signalHandler;
signalHandler.sa_handler = terminate;
sigemptyset(&signalHandler.sa_mask);
signalHandler.sa_flags = 0;
sigaction(SIGTERM, &signalHandler, NULL);
sigaction(SIGINT, &signalHandler, NULL);
//Setup hardware
FlightBoard fb;
GPS gps;
IMU imu;
CAMERA_STREAM cam;
hardware hardware_list = initialise(&fb, &gps, &imu, &cam);
//Turn off camera
cam.close();
hardware_list.CAM_Working = false;
//Start loging
Logger log = Logger("bax_waypoints3.csv");
//Get waypoints
deque<coord> waypoints_list = deque<coord>();
populate_waypoints_list(&waypoints_list);
//Start loop
waypoints_loop3(hardware_list, log, waypoints_list, CONFIG_FILE);
//Close hardware
fb.stop();
gps.close();
imu.close();
log.closeLog();
}
unsigned long AccelSampler::Run()
{
unsigned long before = millis();
faccXn__ = faccXn_;
faccYn__ = faccYn_;
faccZn__ = faccZn_;
faccXn_ = faccXn;
faccYn_ = faccYn;
faccZn_ = faccZn;
Imu.GetAccel(faccXn, faccYn, faccZn);
unsigned long now = millis();
return now - before;
}
void loop(void)
{
static bool accCalibrated;
static uint16_t calibratingA;
static uint32_t currentTime;
static uint32_t disarmTime;
if (rcTask.checkAndUpdate(currentTime)) {
// update RC channels
rc.update();
// when landed, reset integral component of PID
if (rc.throttleIsDown())
stab.resetIntegral();
if (rc.changed()) {
if (armed) { // actions during armed
// Disarm on throttle down + yaw
if (rc.sticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (armed) {
armed = false;
// Reset disarm time so that it works next time we arm the board.
if (disarmTime != 0)
disarmTime = 0;
}
}
} else { // actions during not armed
// gyro calibration
if (rc.sticks == THR_LO + YAW_LO + PIT_LO + ROL_CE)
calibratingG = calibratingGyroCycles;
// Arm via throttle-low / yaw-right
if (rc.sticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)
if (calibratingG == 0 && accCalibrated)
if (!rc.auxState()) // aux switch must be in zero position
if (!armed)
armed = true;
// accel calibration
if (rc.sticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = calibratingAccCycles;
} // not armed
} // rc.changed()
// Switch to alt-hold when switch moves to position 1 or 2
nav.checkSwitch();
} else { // not in rc loop
static int taskOrder; // never call all functions in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
if (baro.available())
baro.update();
taskOrder++;
break;
case 1:
if (baro.available() && altitudeEstimationTask.checkAndUpdate(currentTime)) {
nav.updateAltitudePid(armed);
}
taskOrder++;
break;
case 2:
taskOrder++;
break;
case 3:
taskOrder++;
break;
case 4:
taskOrder = 0;
break;
}
}
currentTime = board.getMicros();
if (imuTask.checkAndUpdate(currentTime)) {
imu.update(currentTime, armed, calibratingA, calibratingG);
haveSmallAngle = abs(imu.angle[0]) < CONFIG_SMALL_ANGLE && abs(imu.angle[1]) < CONFIG_SMALL_ANGLE;
// measure loop rate just afer reading the sensors
currentTime = board.getMicros();
// compute exponential RC commands
rc.computeExpo();
// use LEDs to indicate calibration status
if (calibratingA > 0 || calibratingG > 0)
board.ledGreenOn();
else {
if (accCalibrated)
board.ledGreenOff();
if (armed)
board.ledRedOn();
else
board.ledRedOff();
}
// periodically update accelerometer calibration status
if (accelCalibrationTask.check(currentTime)) {
if (!haveSmallAngle) {
accCalibrated = false;
board.ledGreenToggle();
accelCalibrationTask.update(currentTime);
} else {
accCalibrated = true;
}
}
// handle serial communications
msp.update(armed);
// perform navigation tasks (alt-hold etc.)
nav.perform();
// update stability PID controller
stab.update();
// update mixer
mixer.update(armed);
} // IMU update
} // loop()
bool compute()
{
bool waiting_for_image_set = false;
updated = false;
++frame_count;
if (image_current_frame.cols == 0)
{
console_log("bad frame");
return false;
}
Mat image_flipped;
flip(image_current_frame, image_flipped, 0);
Mat image0 = image_flipped(Rect(0, 0, 640, 480));
Mat image1 = image_flipped(Rect(640, 0, 640, 480));
if (frame_count == 1)
setup_on_first_frame();
if (!play || settings.touch_control != "1")
{
if (enable_imshow)
waitKey(1);
return false;
}
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
imu.compute(x_accel, y_accel, z_accel);
//----------------------------------------core algorithm----------------------------------------
Mat image_small0;
Mat image_small1;
resize(image0, image_small0, Size(160, 120), 0, 0, INTER_LINEAR);
resize(image1, image_small1, Size(160, 120), 0, 0, INTER_LINEAR);
Mat image_preprocessed0;
Mat image_preprocessed1;
static bool exposure_set = false;
bool normalized = compute_channel_diff_image(image_small0, image_preprocessed0, exposure_set, "image_preprocessed0", true, exposure_set);
compute_channel_diff_image(image_small1, image_preprocessed1, exposure_set, "image_preprocessed1");
GaussianBlur(image_preprocessed0, image_preprocessed0, Size(3, 9), 0, 0);
GaussianBlur(image_preprocessed1, image_preprocessed1, Size(3, 9), 0, 0);
if (!CameraInitializerNew::adjust_exposure(camera, image_preprocessed0))
{
static int step_count = 0;
if (step_count == 3)
surface_computer.init(image0);
++step_count;
return false;
}
exposure_set = true;
#if 0
{
Mat image_remapped0 = reprojector.remap(&image_small0, 0, true);
Mat image_remapped1 = reprojector.remap(&image_small1, 1, true);
reprojector.y_align(image_remapped0, image_remapped1, false);
GaussianBlur(image_remapped0, image_remapped0, Size(9, 9), 0, 0);
GaussianBlur(image_remapped1, image_remapped1, Size(9, 9), 0, 0);
Mat image_disparity;
static StereoSGBM stereo_sgbm(0, 32, 21, 0, 0, 0, 0, 20, 0, 0, false);
stereo_sgbm(image_remapped0, image_remapped1, image_disparity);
Mat image_disparity_8u;
double minVal, maxVal;
minMaxLoc(image_disparity, &minVal, &maxVal);
image_disparity.convertTo(image_disparity_8u, CV_8UC1, 255 / (maxVal - minVal));
imshow("image_remapped0", image_remapped0);
imshow("image_remapped1", image_remapped1);
imshow("image_disparity_8u", image_disparity_8u);
}
#endif
/*static bool show_wiggle_sent = false;
if (!show_wiggle_sent)
{
if (child_module_name != "")
ipc->open_udp_channel(child_module_name);
ipc->send_message("menu_plus", "show window", "");
ipc->send_message("menu_plus", "show wiggle", "");//todo
}
show_wiggle_sent = true;*/
if (enable_imshow)
{
imshow("image_small1", image_small1);
// imshow("image_preprocessed1", image_preprocessed1);
// setMouseCallback("image_preprocessed1", left_mouse_cb, NULL);
// waitKey(1);
// return false;
}
algo_name_vec_old = algo_name_vec;
algo_name_vec.clear();
static bool motion_processor_proceed = false;
static bool construct_background = false;
static bool first_pass = true;
bool proceed0;
bool proceed1;
if (normalized)
{
proceed0 = motion_processor0.compute(image_preprocessed0, image_small0, surface_computer.y_reflection, imu.pitch,
construct_background, "0", true);
proceed1 = motion_processor1.compute(image_preprocessed1, image_small1, surface_computer.y_reflection, imu.pitch,
construct_background, "1", false);
}
if (first_pass && motion_processor0.both_moving && motion_processor1.both_moving)
{
console_log("readjusting exposure");
first_pass = false;
exposure_set = false;
mat_functions_low_pass_filter.reset();
CameraInitializerNew::adjust_exposure(camera, image_preprocessed0, true);
}
else if (!first_pass)
construct_background = true;
if (!construct_background)
{
proceed0 = false;
proceed1 = false;
}
if (proceed0 && proceed1)
motion_processor_proceed = true;
bool proceed = motion_processor_proceed;
static bool menu_plus_signal0 = false;
if (!menu_plus_signal0)
{
menu_plus_signal0 = true;
// ipc->send_message("menu_plus", "hide window", "");
}
if (proceed)
{
proceed0 = foreground_extractor0.compute(image_preprocessed0, motion_processor0, "0", true);
proceed1 = foreground_extractor1.compute(image_preprocessed1, motion_processor1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
proceed0 = hand_splitter0.compute(foreground_extractor0, motion_processor0, "0", false);
proceed1 = hand_splitter1.compute(foreground_extractor1, motion_processor1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
waiting_for_image = true;
waiting_for_image_set = true;
proceed1 = scopa1.compute_mono0(hand_splitter1, pose_estimator, "1", false);
proceed0 = scopa0.compute_mono0(hand_splitter0, pose_estimator, "0", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
motion_processor0.target_frame = 10;
motion_processor1.target_frame = 10;
SCOPA::compute_stereo();
scopa1.compute_mono1("1");
scopa0.compute_mono1("0");
}
if (enable_imshow)
waitKey(1);
return waiting_for_image_set;
}
void setup_on_first_frame()
{
console_log("on first frame");
ipc->send_message("menu_plus", "set status", "verifying serial number");
serial_number = camera->getSerialNumber();
if (serial_number.size() == 10)
{
string char_string = "";
int char_count = 0;
for (char& c : serial_number)
{
char_string += c;
++char_count;
if (char_count == 4)
break;
}
if (char_string == "0101")
serial_verified = true;
}
if (!serial_verified)
wait_for_device();
ipc->send_message("menu_plus", "set loading progress", "10");
console_log("serial number: " + serial_number);
data_path_current_module = data_path + slash + serial_number;
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
Point3f heading = imu.compute_azimuth(x_accel, y_accel, z_accel);
ipc->send_message("menu_plus", "set status", "loading calibration data");
reprojector.load(*ipc, true);
ipc->send_message("menu_plus", "set loading progress", "40");
ipc->send_message("menu_plus", "set status", "initializing camera");
CameraInitializerNew::init(camera);
ipc->send_message("menu_plus", "set status", "loading pose data");
pose_estimator.init();
ipc->send_message("menu_plus", "set loading progress", "70");
ipc->send_message("menu_plus", "set status", "starting cursor subprocess");
#ifdef _WIN32
child_module_name = "win_cursor_plus";
if (IsWindows8OrGreater())
child_module_path = executable_path + slash + "win_cursor_plus" + slash + "win_cursor_plus" + extension0;
else
child_module_path = executable_path + slash + "win_cursor_plus_fallback" + slash + "win_cursor_plus" + extension0;
#elif __APPLE__
//todo: port to OSX
#endif
ipc->map_function("toggle imshow", [](const string message_body)
{
if (enable_imshow)
enable_imshow = false;
else
enable_imshow = true;
});
ipc->map_function("load settings", [](const string message_body)
{
load_settings();
});
ipc->send_message("menu_plus", "set loading progress", "100");
}
void compute()
{
updated = false;
if (image_current_frame.cols == 0)
{
COUT << "bad frame" << endl;
return;
}
Mat image_flipped;
flip(image_current_frame, image_flipped, 0);
Mat image0 = image_flipped(Rect(0, 0, 640, 480));
Mat image1 = image_flipped(Rect(640, 0, 640, 480));
if (first_frame)
{
on_first_frame();
first_frame = false;
}
if (!play || settings.touch_control != "1")
{
hide_cursors();
if (enable_imshow)
waitKey(1);
return;
}
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
imu.compute(x_accel, y_accel, z_accel);
//----------------------------------------core algorithm----------------------------------------
Mat image_small0;
Mat image_small1;
resize(image0, image_small0, Size(160, 120), 0, 0, INTER_LINEAR);
resize(image1, image_small1, Size(160, 120), 0, 0, INTER_LINEAR);
Mat image_preprocessed0;
Mat image_preprocessed1;
bool normalized = compute_channel_diff_image(image_small0, image_preprocessed0, exposure_adjusted, "image_preprocessed0", true);
compute_channel_diff_image(image_small1, image_preprocessed1, exposure_adjusted, "image_preprocessed1", false);
GaussianBlur(image_preprocessed0, image_preprocessed0, Size(3, 9), 0, 0);
GaussianBlur(image_preprocessed1, image_preprocessed1, Size(3, 9), 0, 0);
if (!CameraInitializerNew::adjust_exposure(camera, image_preprocessed0))
{
static int step_count = 0;
if (step_count == 3)
{
surface_computer.init(image0);
}
++step_count;
return;
}
exposure_adjusted = true;
#ifdef false
{
Mat image_remapped0 = reprojector.remap(&image_small0, 0, true);
Mat image_remapped1 = reprojector.remap(&image_small1, 1, true);
reprojector.y_align(image_remapped0, image_remapped1, false);
GaussianBlur(image_remapped0, image_remapped0, Size(9, 9), 0, 0);
GaussianBlur(image_remapped1, image_remapped1, Size(9, 9), 0, 0);
Mat image_disparity;
static StereoSGBM stereo_sgbm(0, 32, 21, 0, 0, 0, 0, 20, 0, 0, false);
stereo_sgbm(image_remapped0, image_remapped1, image_disparity);
Mat image_disparity_8u;
double minVal, maxVal;
minMaxLoc(image_disparity, &minVal, &maxVal);
image_disparity.convertTo(image_disparity_8u, CV_8UC1, 255 / (maxVal - minVal));
imshow("image_remapped0", image_remapped0);
imshow("image_remapped1", image_remapped1);
imshow("image_disparity_8u", image_disparity_8u);
}
#endif
/*static bool show_wiggle_sent = false;
if (!show_wiggle_sent)
{
if (child_module_name != "")
ipc->open_udp_channel(child_module_name);
ipc->send_message("menu_plus", "show window", "");
ipc->send_message("menu_plus", "show wiggle", "");//todo
}
show_wiggle_sent = true;*/
if (enable_imshow)
{
imshow("image_small1", image_small1);
imshow("image_preprocessed1", image_preprocessed1);
// setMouseCallback("image_preprocessed1", left_mouse_cb, NULL);
// waitKey(1);
// return;
}
algo_name_vec_old = algo_name_vec;
algo_name_vec.clear();
static bool motion_processor_proceed = false;
static bool construct_background = false;
static bool first_pass = true;
bool proceed0;
bool proceed1;
if (normalized)
{
proceed0 = motion_processor0.compute(image_preprocessed0, image_small0, surface_computer.y_reflection, imu.pitch,
construct_background, "0", true);
proceed1 = motion_processor1.compute(image_preprocessed1, image_small1, surface_computer.y_reflection, imu.pitch,
construct_background, "1", true);
}
if (first_pass && motion_processor0.both_moving)
{
COUT << "readjusting exposure" << endl;
first_pass = false;
exposure_adjusted = false;
CameraInitializerNew::adjust_exposure(camera, image_preprocessed0, true);
}
else if (!first_pass)
construct_background = true;
if (!construct_background)
{
proceed0 = false;
proceed1 = false;
}
if (proceed0 && proceed1)
motion_processor_proceed = true;
bool proceed = motion_processor_proceed;
if (proceed)
{
proceed0 = foreground_extractor0.compute(image_preprocessed0, motion_processor0, "0", false);
proceed1 = foreground_extractor1.compute(image_preprocessed1, motion_processor1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
proceed0 = hand_splitter0.compute(foreground_extractor0, motion_processor0, "0");
proceed1 = hand_splitter1.compute(foreground_extractor1, motion_processor1, "1");
proceed = proceed0 && proceed1;
}
proceed = false;
if (proceed)
{
proceed0 = mono_processor0.compute(hand_splitter0, "0", false);
proceed1 = mono_processor1.compute(hand_splitter1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
stereo_processor.compute(mono_processor0, mono_processor1, point_resolver, pointer_mapper, image0, image1);
}
++frame_num;
if (increment_keypress_count)
{
if (keypress_count == calibration_points_count)
{
COUT << "step " << calibration_step << " complete" << endl;
// ipc->send_message("menu_plus", "show calibration next", "");//todo
++calibration_step;
}
else if (keypress_count < calibration_points_count)
{
float percentage = (keypress_count * 100.0 / calibration_points_count);
COUT << percentage << endl;
pointer_mapper.add_calibration_point(calibration_step);
}
if (calibration_step == 4)
{
pointer_mapper.compute_calibration_points();
calibrating = false;
increment_keypress_count = false;
COUT << "calibration finished" << endl;
}
++keypress_count;
}
if (enable_imshow)
waitKey(1);
}
/*
* main()
*
* @function DronePi Application main
*
*/
int main(int argc, char* argv[])
{
//yaw pid
int count;
char ff;
int cnt=0;
int ycnt=0;
//network about
int tcp,udp;
int flag,str_len,t=0;
double x = 0,y=0,z=0;
double pp,pi,pd;
char pidbuf[100],tcp_flag,recv_flag;
json_t *data;
json_error_t error;
// Yaw PID 상수
double yp,yi,yd;
// Roll, Pitch PID 상수
double kp, ki, kd;
// 모터 출력량
int throttle = 0;
// 상대좌표 Yaw를 사용하기위한 보정 값
float adjustYaw=0.f;
// Quardcopter 로 모터 4개 선언
Motor motor[4];
// imu 장치 MPU6050 선언
IMU imu;
for(int i = 0 ; i < 4 ; i++)
{
motor[i].init();
}
// Setting Pin to Raspberry pi gpio
motor[0].setPin(22);
motor[1].setPin(23);
motor[2].setPin(24);
motor[3].setPin(25);
// ESC Calibration
motor[0].calibration();
motor[1].calibration();
motor[2].calibration();
motor[3].calibration();
if(imu.init())
{
printf("Plese check the MPU6050 device connection!!\n");
return 0; //센서 연결 실패
}
// PID 연산을 위한 각 축의 PID 연산자 객체 선언
PID rollPid, pitchPid, yawPid;
rollPid.init();
pitchPid.init();
yawPid.init();
kp = 2.0; // 1.2
ki = 0.1; // 0.00084
kd = 0.85; // 0.6134
rollPid.setTuning(2.0, 0.1, 0.78);
pitchPid.setTuning(2.0, 0.1, 0.78);
yawPid.setTuning(10.0, 0.f, 10.0);
float roll, pitch, yaw, preYaw, rYaw;
int pidRoll, pidPitch, pidYaw;
// 자이로센서 init 및 calibration
imu.calibration();
int Motor1_speed, Motor2_speed, Motor3_speed, Motor4_speed;
printf("Connect Application!!\n");
// Network init
if(network_init(&tcp,&udp)==-1)
{
printf("socket create error\n");
return 0;
}
printf("network init complete\n");
// 급격한 각도변화 발생시 비상제어. 착륙 실행 flag
int Emergency = 0;
// 상대 Yaw 좌표
preYaw = 0;
/*
* Drone Control routine
*
* 1. 조작 데이터 수신
* 2. 현재 각도 측정
* 3. PID 연산
* 4. 모터 속도 업데이트
*
*/
while(1)
{
// Get json data from android app
flag=json_read(udp,&x,&y,&z,&t);
if(flag==1)
{
throttle = t;
}
// 2. 현재 각도 측정
imu.getIMUData(&roll, &pitch, &yaw);
// 센서 오류 및 오동작으로 인한 동작 방지
// 특정 각도 이상으로 기울어지면 종료할 수 있도록 함
if( pitch < -50 || pitch > 50 || roll < -50 || roll > 50)
{
Emergency = 1;
}
// 상대 Yaw 연산
if(preYaw>100&&yaw<-100)
rYaw=360+yaw-preYaw;
else if(preYaw<-100&&yaw>100)
rYaw=360-yaw+preYaw;
else
rYaw= yaw - preYaw;
if(ycnt==33)
{
printf("rYaw: %.2lf preYaw: %.2lf yaw: %.2lf\n",rYaw,preYaw,yaw);
ycnt=0;
}
ycnt++;
preYaw = yaw;
// 각 축의 PID 연산
pidRoll = rollPid.calcPID(x, roll);
pidPitch = pitchPid.calcPID(-y, pitch);
pidYaw = yawPid.calcPID(0, rYaw-z);
// Quardcopter type X 의 모터 속도 연산
Motor1_speed = throttle + pidRoll + pidPitch + pidYaw;
Motor2_speed = throttle - pidRoll + pidPitch - pidYaw;
Motor3_speed = throttle - pidRoll - pidPitch + pidYaw;
Motor4_speed = throttle + pidRoll - pidPitch - pidYaw;
if(cnt==33)
{
printf("X: %.2lf Y: %.2lf Z: %.2lf T:%d \n",x,y,z,t);
printf("ROLL : %3.6f, PITCH %3.7f, YAW %3.7f\n", roll, pitch, yaw);
printf("pidRoll : %d, pidPitch : %d pidYaw: %d\n", pidRoll,pidPitch,pidYaw);
printf("motor speed [1]:%d [2]:%d [3]:%d [4]:%d\n\n\n", Motor1_speed, Motor2_speed, Motor3_speed, Motor4_speed);
cnt=0;
}
cnt++;
if(Emergency)
{
// 긴급상황. 모터속도 최소화
Motor1_speed = 700;
Motor2_speed = 700;
Motor3_speed = 700;
Motor4_speed = 700;
}
if(Motor1_speed < 700) Motor1_speed = 700;
if(Motor2_speed < 700) Motor2_speed = 700;
if(Motor3_speed < 700) Motor3_speed = 700;
if(Motor4_speed < 700) Motor4_speed = 700;
// 모터 속도 업데이트
motor[0].setSpeed(Motor1_speed);
motor[1].setSpeed(Motor2_speed);
motor[2].setSpeed(Motor3_speed);
motor[3].setSpeed(Motor4_speed);
tcp_flag=tcp_read(tcp);
if(tcp_flag==3)
{
motor[0].setSpeed(700);
motor[1].setSpeed(700);
motor[2].setSpeed(700);
motor[3].setSpeed(700);
memset(pidbuf,0,sizeof(pidbuf));
printf("pid reset flag\n");
str_len=recv(tcp,pidbuf,sizeof(pidbuf),MSG_DONTROUTE);
pidbuf[str_len]=0;
printf("pid reset data :%s\n",pidbuf);
data=json_loads(pidbuf,JSON_DECODE_ANY,&error);
if((json_unpack(data,"{s:f,s:f,s:f,s:f,s:f,s:f}","P_P",&pp,"P_I",&pi,"P_D",&pd,"Y_P",&yp,"Y_I",&yi,"Y_D",&yd))!=0)
{
printf("pid reset error\n");
recv_flag=0;
send(tcp,&recv_flag,1,MSG_DONTROUTE);
return 0;
}
recv_flag=1;
send(tcp,&recv_flag,1,MSG_DONTROUTE);
rollPid.initKpid(pp, pi, pd);
pitchPid.initKpid(pp, pi, pd);
yawPid.initKpid(yp, yi, yd);
Emergency=0;
printf("pid reset complete\n");
}
else if(tcp_flag==5)
{
motor[0].setSpeed(700);
motor[1].setSpeed(700);
motor[2].setSpeed(700);
motor[3].setSpeed(700);
printf("drone exit\n");
break;
}
else if(tcp_flag==0)
{
motor[0].setSpeed(700);
motor[1].setSpeed(700);
motor[2].setSpeed(700);
motor[3].setSpeed(700);
printf("Controller connection less\n");
break;
}
}//end while(1)
sleep(3);
}//end int main()
int main()
{
int ii;
//-----------------------------------------------------------------------------------------
//unsigned long baudrate = 9600, uint8_t databits = 8, uint8_t parity = 0, uint8_t stopbits = 1
serial1.init(115200,8,0,1);
TRACE("Balancing Robot V1.0\n");
TRACE((mpu->test()==0) ? "i2c has failed...\n\n" : "MPU Project V1.0\n\n");
TRACE("\t=>Restarting MPU6050...");
mpu->reset();
TRACE((mpu->test()==0) ? "i2c has failed...\n" : "OK\n");
//-----------------------------------------------------------------------------------------
mpu->init();
for(ii=0;ii<10;ii++)
{
io.toggle(LED3);
delay.ms(100);
}
//mpu->Get_Accel_Offset();
mpu->getGyroOffset();
mpu->getAccelVal();
imu.HADXL_P[0] = mpu->Accel_In_g[X]; //x;
imu.HADXL_P[1] = mpu->Accel_In_g[Y]; //y;
imu.HADXL_P[2] = mpu->Accel_In_g[Z]; //z;
for(ii=0;ii<10;ii++)
{
io.toggle(LED3);
delay.ms(50);
}
uint32_t routin_100HZ_start = timer.millis();
uint32_t routin_90ms_start = timer.millis();
while(1)
{
if(timer.deltaTmillis(routin_100HZ_start)>9) // --------> execute every 10ms -> 100Hz routin
{
routin_100HZ_start = timer.millis();
mpu->getAccelVal();
imu.interval = 10;//timer.deltaTmillis(gyro.getLastTime());
mpu->getGyroVal();
imu.calculate(
radians(mpu->GyroRate_Val[X]),
radians(mpu->GyroRate_Val[Y]),
radians(mpu->GyroRate_Val[Z]),
mpu->Accel_In_g[X],
mpu->Accel_In_g[Y],
mpu->Accel_In_g[Z]
);
imu.update();
} // End of 100Hz routin
/*******************************************************************/
if(timer.deltaTmillis(routin_90ms_start)>150) // --------> execute every 10ms -> 100Hz routin
{
TRACE("Interval: "); TRACEln(imu.interval);
TRACE("Roll: "); TRACEln(imu.roll);
TRACE("Pitch: "); TRACEln(imu.pitch);
TRACE("Yaw: "); TRACEln(imu.yaw);
TRACEln();
routin_90ms_start = millis();
}
}
}
int main()
{
//char* buffers for printing stuff on the LCD
char buffer[20];
char buffer2[20];
//create ESC object
Esc escFL(FL), escBL(BL),escBR(BR), escFR(FR);
//create objects for led strips
WS2812 LEDFRT(1, FRT); // 100 LED
cRGB valueFRT;
//create IMU object
IMU imu;
//Initialize modules; comment out to deactivate feature
initLCD();
//initRF();
//initializeESC();
//initWS2812();
initializeI2C();
imu.initialize();
initSerial(MYUBRR);
//After everything is initialized, start interrupts
startInterrupt();
while(1)
{
//LCD handler
if(flagLCD){
flagLCD = 0;
handleFSMLCD();
}
//
////receive
//sprintf(buffer, "%c", getSerialBuffer());
//sprintf(buffer2, "");
//changeLCDText(buffer,buffer2);
//send
//sprintf(buffer, "Simon Says Simon rules");
//serialTransmit(buffer);
//_delay_ms(1000);
//RF receiver handler
if(flagRF)
{
flagRF = 0;
handleFSMRF();
//sprintf(buffer, "1:%u 2:%u", ch_1_pw, ch_2_pw);
//sprintf(buffer2, "3:%u 4:%u", ch_3_pw, ch_4_pw);
//changeLCDText(buffer, buffer2);
}
//ESC handler
if(flagESC)
{
flagESC = 0;
escFL.set(ch_3_pw);
escBL.set(ch_3_pw);
escBR.set(ch_3_pw);
escFR.set(ch_3_pw);
}
////LED
//if(flagWS2812)
//{
//flagWS2812 = 0;
//
////example of LED gradually
//static int i = 0;
//static bool directionUp = true;
////Led strips shit
//for(int j = 0; j<1; j++)
//{
//valueFRT.b = i; valueFRT.g = i; valueFRT.r = i; // RGB Value -> red
//LEDFRT.set_crgb_at(j, valueFRT); // Set valueB at LED found at index j
//valueFRT.b = 255; valueFRT.g = 255; valueFRT.r = 255; // RGB Value -> Blue
//}
//LEDFRT.sync(); // Sends the value to the LED
//if(i>=255) directionUp = false;
//if(i<=0) directionUp = true;
//if (directionUp) i++;
//else i--;
//}
if(flagIMU)
{
flagIMU = 0;
imu.takeMeasures();
sprintf(buffer, "x:%i y:%i z:%i", imu.acc.X, imu.acc.Y, imu.acc.X);
//serialTransmit(buffer);
changeLCDText(buffer);
}
}
return 0;
}
void loop(void)
{
static uint32_t rcTime = 0;
static uint32_t loopTime;
static uint32_t calibratedAccTime;
static bool accCalibrated;
static bool armed;
static uint16_t calibratingA;
static uint32_t currentTime;
static uint32_t disarmTime = 0;
if (check_and_update_timed_task(&rcTime, CONFIG_RC_LOOPTIME_USEC, currentTime)) {
// update RC channels
rc.update();
// when landed, reset integral component of PID
if (rc.throttleIsDown())
pid.resetIntegral();
if (rc.changed()) {
if (armed) { // actions during armed
// Disarm on throttle down + yaw
if (rc.sticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (armed) {
armed = false;
// Reset disarm time so that it works next time we arm the board.
if (disarmTime != 0)
disarmTime = 0;
}
}
} else { // actions during not armed
// GYRO calibration
if (rc.sticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;
}
// Arm via YAW
if ((rc.sticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) {
if (calibratingG == 0 && accCalibrated) {
if (!armed) { // arm now!
armed = true;
}
} else if (!armed) {
blinkLED(2, 255, 1);
}
}
// Calibrating Acc
else if (rc.sticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
calibratingA = CONFIG_CALIBRATING_ACC_CYCLES;
} // not armed
} // rc.changed()
} else { // not in rc loop
static int taskOrder; // never call all functions in the same loop, to avoid high delay spikes
switch (taskOrder) {
case 0:
taskOrder++;
//sensorsGetBaro();
case 1:
taskOrder++;
//sensorsGetSonar();
case 2:
taskOrder++;
case 3:
taskOrder++;
case 4:
taskOrder = 0;
break;
}
}
currentTime = board.getMicros();
if (check_and_update_timed_task(&loopTime, CONFIG_IMU_LOOPTIME_USEC, currentTime)) {
imu.update(armed, calibratingA, calibratingG);
haveSmallAngle = abs(imu.angle[0]) < CONFIG_SMALL_ANGLE && abs(imu.angle[1]) < CONFIG_SMALL_ANGLE;
// measure loop rate just afer reading the sensors
currentTime = board.getMicros();
previousTime = currentTime;
// compute exponential RC commands
rc.computeExpo();
// use LEDs to indicate calibration status
if (calibratingA > 0 || calibratingG > 0) {
board.ledGreenToggle();
} else {
if (accCalibrated)
board.ledGreenOff();
if (armed)
board.ledRedOn();
else
board.ledRedOff();
}
if (check_timed_task(calibratedAccTime, currentTime)) {
if (!haveSmallAngle) {
accCalibrated = false; // accelerometer not calibrated or angle too steep
board.ledGreenToggle();
update_timed_task(&calibratedAccTime, CONFIG_CALIBRATE_ACCTIME_USEC, currentTime);
} else {
accCalibrated = true;
}
}
// handle serial communications
msp.update(armed);
// update PID controller
pid.update(&rc, &imu);
// update mixer
mixer.update(armed);
} // IMU update
} // loop()
int main (int argc, char** argv)
{
if (argc != 2) { //checking the validity of the input parameters
printf("usage: %s <port>\n", argv[0]);
exit(1);
}
int count_dir=0;
Timer T; //time
int i=0;
mypwm.init(1,0x40);
mypwm.StartMotors();
//mypwm.setPWM(0,2000);
MainTCPserver TCPserver(atoi(argv[1]));
TCPserver.start_listening();
printf("the main loop has started\n");
while(1)
{
T.StartCycle(); //start counting the time
MySensor.KalmanFiltering(); //reading IMU datas, and Kalman filtering
if(i==10)
{
TCPserver.report_state(); //send state report
i=0;
}
T.CountElapsedTime(); //stop counting time
//printf("Time: %f ms motor: %d ",T.elapsedTime,mypwm.MotorStateArray[0]);
T.WaitMs(dt*1000);//wait 10ms, dt is defined in the IMU library
if(mypwm.GetMainPower())
{
if(mypwm.MotorStateArray[0] == 2481)
{
count_dir=1;
}
if(mypwm.MotorStateArray[0] == 1300)
{
count_dir=0;
}
if(count_dir==0)
{
mypwm.setPWM(0,mypwm.MotorStateArray[0]+1);
}
if(count_dir==1)
{
mypwm.setPWM(0,mypwm.MotorStateArray[0]-1);
}
}
i++;
}
TCPserver.stop_listening();
}
void compute()
{
updated = false;
ipc->update();
if (wait_for_device_bool)
wait_for_device();
if (first_frame)
{
on_first_frame();
first_frame = false;
}
if (!play || settings.touch_control != "1")
{
hide_cursors();
if (enable_imshow)
waitKey(1);
return;
}
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
imu.compute(x_accel, y_accel, z_accel);
if ((mode == "surface" && imu.pitch > 60) || (mode == "tool" && imu.pitch < 60))
{
COUT << "exit 1" << endl;
if (child_module_name != "")
ipc->send_message(child_module_name, "exit", "");
exit(0);
}
//----------------------------------------core algorithm----------------------------------------
Mat image_flipped;
if (mode == "surface")
flip(image_current_frame, image_flipped, 0);
else
flip(image_current_frame, image_flipped, 0);
// image_flipped = image_current_frame.clone();
Mat image0 = image_flipped(Rect(0, 0, 640, 480));
Mat image1 = image_flipped(Rect(640, 0, 640, 480));
Mat image_small0;
Mat image_small1;
resize(image0, image_small0, Size(160, 120), 0, 0, INTER_LINEAR);
resize(image1, image_small1, Size(160, 120), 0, 0, INTER_LINEAR);
Mat image_preprocessed0;
Mat image_preprocessed1;
compute_channel_diff_image(image_small0, image_preprocessed0, exposure_adjusted, "image_preprocessed0");
compute_channel_diff_image(image_small1, image_preprocessed1, exposure_adjusted, "image_preprocessed1");
Mat image_preprocessed_smoothed0;
Mat image_preprocessed_smoothed1;
GaussianBlur(image_preprocessed0, image_preprocessed_smoothed0, Size(1, 19), 0, 0);
GaussianBlur(image_preprocessed1, image_preprocessed_smoothed1, Size(1, 19), 0, 0);
if (!CameraInitializerNew::adjust_exposure(camera, image_preprocessed0))
return;
exposure_adjusted = true;
// imshow("image_small0", image_small0);
// imshow("image_preprocessed0", image_preprocessed0);
bool proceed0 = motion_processor0.compute(image_preprocessed_smoothed0, "0", false);
bool proceed1 = motion_processor1.compute(image_preprocessed_smoothed1, "1", false);
bool proceed = proceed0 && proceed1;
if (proceed)
{
proceed0 = foreground_extractor0.compute(image_preprocessed0, image_preprocessed_smoothed0, motion_processor0, "0", true);
proceed1 = foreground_extractor1.compute(image_preprocessed1, image_preprocessed_smoothed1, motion_processor1, "1", true);
proceed = proceed0 && proceed1;
}
else
hide_cursors();
if (proceed)
{
proceed0 = hand_splitter0.compute(foreground_extractor0, motion_processor0, "0");
proceed1 = hand_splitter1.compute(foreground_extractor1, motion_processor1, "1");
proceed = proceed0 && proceed1;
}
else
hide_cursors();
if (mode == "surface" && proceed)
{
proceed0 = mono_processor0.compute(hand_splitter0, "0", false);
proceed1 = mono_processor1.compute(hand_splitter1, "1", false);
proceed = false;
proceed = proceed0 && proceed1;
if (proceed)
{
// stereo_processor.compute(mono_processor0, mono_processor1, motion_processor0, motion_processor1);
points_pool[points_pool_count] = mono_processor0.points_unwrapped_result;
points_ptr = &(points_pool[points_pool_count]);
++points_pool_count;
if (points_pool_count == points_pool_count_max)
points_pool_count = 0;
initialized = true;
if (!show_point_sent)
{
ipc->send_message("menu_plus", "show point", "");
show_point_sent = true;
}
if (pose_name == "point")
{
if (!show_calibration_sent)
{
ipc->send_message("menu_plus", "show calibration", "");
show_calibration_sent = true;
}
hand_resolver.compute(mono_processor0, mono_processor1, motion_processor0, motion_processor1, image0, image1, reprojector);
pointer_mapper.compute(hand_resolver, reprojector);
if (pointer_mapper.calibrated)
{
if (enable_imshow)
{
enable_imshow = false;
destroyAllWindows();
}
show_cursor_index = true;
}
if (show_cursor_index && pointer_mapper.thumb_down && pointer_mapper.index_down)
show_cursor_thumb = true;
else
show_cursor_thumb = false;
}
else
{
pointer_mapper.reset();
if (pose_name != "point")
{
show_cursor_index = false;
show_cursor_thumb = false;
}
}
}
else
hide_cursors();
if (!pinch_to_zoom)
{
if (show_cursor_index)
{
ipc->send_udp_message("win_cursor_plus", to_string(pointer_mapper.pt_cursor_index.x) + "!" +
to_string(pointer_mapper.pt_cursor_index.y) + "!" +
to_string(pointer_mapper.dist_cursor_index_plane) + "!" +
to_string(pointer_mapper.index_down) + "!index");
}
else
ipc->send_udp_message("win_cursor_plus", "hide_cursor_index");
ipc->send_udp_message("win_cursor_plus", "hide_cursor_thumb");
}
else
{
ipc->send_udp_message("win_cursor_plus", to_string(pointer_mapper.pt_pinch_to_zoom_index.x) + "!" +
to_string(pointer_mapper.pt_pinch_to_zoom_index.y) + "!0!1!index");
ipc->send_udp_message("win_cursor_plus", to_string(pointer_mapper.pt_pinch_to_zoom_thumb.x) + "!" +
to_string(pointer_mapper.pt_pinch_to_zoom_thumb.y) + "!0!1!thumb");
}
ipc->send_udp_message("win_cursor_plus", "update!" + to_string(frame_num));
}
else if (mode == "tool" && proceed)
{
enable_imshow = true;
Mat image_active_light0;
Mat image_active_light1;
compute_active_light_image(image_small0, image_preprocessed0, image_active_light0);
compute_active_light_image(image_small1, image_preprocessed1, image_active_light1);
proceed0 = tool_mono_processor0.compute(image_active_light0, image_preprocessed0, "0");
proceed1 = tool_mono_processor1.compute(image_active_light1, image_preprocessed1, "1");
proceed = proceed0 && proceed1;
if (proceed)
{
proceed0 = tool_stereo_processor.compute(tool_mono_processor0, tool_mono_processor1);
proceed1 = tool_stereo_processor.matches.size() >= 4;
proceed = proceed0 && proceed1;
if (proceed)
{
tool_pointer_mapper.compute(reprojector, tool_stereo_processor, image0, image1);
string data = "";
data += to_string(tool_pointer_mapper.pt0.x) + "!" +
to_string(tool_pointer_mapper.pt0.y) + "!" +
to_string(tool_pointer_mapper.pt0.z) + "!";
data += to_string(tool_pointer_mapper.pt1.x) + "!" +
to_string(tool_pointer_mapper.pt1.y) + "!" +
to_string(tool_pointer_mapper.pt1.z) + "!";
data += to_string(tool_pointer_mapper.pt2.x) + "!" +
to_string(tool_pointer_mapper.pt2.y) + "!" +
to_string(tool_pointer_mapper.pt2.z) + "!";
data += to_string(tool_pointer_mapper.pt3.x) + "!" +
to_string(tool_pointer_mapper.pt3.y) + "!" +
to_string(tool_pointer_mapper.pt3.z) + "!";
data += to_string(tool_pointer_mapper.pt_center.x) + "!" +
to_string(tool_pointer_mapper.pt_center.y) + "!" +
to_string(tool_pointer_mapper.pt_center.z);
ipc->send_udp_message("unity_demo", data);
}
}
}
++frame_num;
if (increment_keypress_count)
{
if (keypress_count == calibration_points_count)
{
ipc->send_message("menu_plus", "show calibration next", "");
COUT << "step " << calibration_step << " complete" << endl;
++calibration_step;
}
else if (keypress_count < calibration_points_count)
{
float percentage = (keypress_count * 100.0 / calibration_points_count);
COUT << percentage << endl;
pointer_mapper.add_calibration_point(calibration_step);
}
if (calibration_step == 4)
{
ipc->send_message("menu_plus", "show stage", "");
pointer_mapper.compute_calibration_points();
calibrating = false;
increment_keypress_count = false;
COUT << "calibration finished" << endl;
}
++keypress_count;
}
if (enable_imshow)
waitKey(1);
}
void on_first_frame()
{
bool serial_verfied = false;
serial_number = camera->getSerialNumber();
if (serial_number.size() == 10)
{
string char_string = "";
int char_count = 0;
for (char& c : serial_number)
{
char_string += c;
++char_count;
if (char_count == 4)
break;
}
if (char_string == "0101")
serial_verfied = true;
}
if (!serial_verfied)
{
hide_cursors();
COUT << "please reconnect your Touch+ sensor" << endl;
COUT << "restarting" << endl;
wait_for_device();
}
data_path_current_module = data_path + "\\" + serial_number;
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
Point3d heading = imu.compute_azimuth(x_accel, y_accel, z_accel);
// if (heading.y > 60)
// mode = "tool";
// else
mode = "surface";
ipc->send_message("menu_plus", "show notification", "Please wait:Initializing Touch+ Software");
reprojector.load(*ipc);
CameraInitializerNew::init(camera);
pose_estimator.init();
if (mode == "surface")
{
child_module_name = "win_cursor_plus";
if (IsWindows8OrGreater())
child_module_path = executable_path + "\\win_cursor_plus\\win_cursor_plus.exe";
else
child_module_path = executable_path + "\\win_cursor_plus_fallback\\win_cursor_plus.exe";
ipc->open_udp_channel("win_cursor_plus");
ipc->send_message("menu_plus", "show window", "");
ipc->send_message("menu_plus", "show wiggle", "");
}
else
{
ipc->open_udp_channel("unity_demo", 3333);
ipc->send_message("menu_plus", "hide window", "");
}
ipc->map_function("toggle imshow", [](const string message_body)
{
if (enable_imshow)
{
enable_imshow = false;
destroyAllWindows();
}
else
enable_imshow = true;
});
ipc->map_function("load settings", [](const string message_body)
{
load_settings();
});
increment_wait_count = true;
}
