From 2bc568e3b6a6f1dbdb3c32a55ba645be23584043 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 27 Dec 2010 23:09:08 +0000 Subject: [PATCH] some bug fixes, added basic camera leveling example for pitch only. Added DCM test. Added better Yaw gain defaults. Split radio and RC inits into two calls. Removed inappropriate auto_trim function. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1306 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 2 +- ArduCopterMega/ArduCopterMega.pde | 26 +++--- ArduCopterMega/Attitude.pde | 134 --------------------------- ArduCopterMega/config.h | 2 +- ArduCopterMega/control_modes.pde | 8 +- ArduCopterMega/defines.h | 2 + ArduCopterMega/radio.pde | 149 +++++++++++++++++++++++++++++- ArduCopterMega/setup.pde | 9 ++ ArduCopterMega/system.pde | 7 +- ArduCopterMega/test.pde | 90 ++++++++++++++---- 10 files changed, 257 insertions(+), 172 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 3b8a354fdb..7c5525ffdf 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -15,7 +15,7 @@ // These are all experimental and underwork, jp 23-12-10 //#define ENABLE_EXTRAS ENABLED //#define ENABLE_EXTRAINIT ENABLED -//#define ENABLE_CAM ENABLED +#define ENABLE_CAM ENABLED //#define ENABLE_AM ENABLED //#define ENABLE_xx ENABLED diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 081aa88af8..01ce304d32 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -124,6 +124,10 @@ RC_Channel rc_5(EE_RADIO_5); RC_Channel rc_6(EE_RADIO_6); RC_Channel rc_7(EE_RADIO_7); RC_Channel rc_8(EE_RADIO_8); + +RC_Channel rc_camera_pitch(EE_RADIO_9); +RC_Channel rc_camera_yaw(EE_RADIO_10); + int motor_out[4]; byte flight_mode_channel; byte frame_type = PLUS_FRAME; @@ -587,17 +591,17 @@ void medium_loop() output_HIL(); #endif - #if ENABLE_CAM - camera_stabilization(); - #endif - - #if ENABLE_AM - flight_lights(); - #endif - - #if ENABLE_xx - do_something_usefull(); - #endif + #if ENABLE_CAM + camera_stabilization(); + #endif + + #if ENABLE_AM + flight_lights(); + #endif + + #if ENABLE_xx + do_something_usefull(); + #endif if (millis() - perf_mon_timer > 20000) { diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 61a4afdcc1..6e5890dae0 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -96,138 +96,4 @@ void reset_I(void) } -/***************************************** - * Set the flight control servos based on the current calculated values - *****************************************/ -void set_servos_4(void) -{ - static byte num; - // Quadcopter mix - if (motor_armed == true) { - int out_min = rc_3.radio_min; - - // Throttle is 0 to 1000 only - rc_3.servo_out = constrain(rc_3.servo_out, 0, 1000); - - if(rc_3.servo_out > 0) - out_min = rc_3.radio_min + 50; - - //Serial.printf("out: %d %d %d %d\t\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); - - // creates the radio_out and pwm_out values - rc_1.calc_pwm(); - rc_2.calc_pwm(); - rc_3.calc_pwm(); - rc_4.calc_pwm(); - - //Serial.printf("out: %d %d %d %d\n", rc_1.radio_out, rc_2.radio_out, rc_3.radio_out, rc_4.radio_out); - //Serial.printf("yaw: %d ", rc_4.radio_out); - - if(frame_type == PLUS_FRAME){ - motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out; - motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out; - motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out; - motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out; - }else{ - int roll_out = rc_1.pwm_out / 2; - int pitch_out = rc_2.pwm_out / 2; - motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; - motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out; - motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; - motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; - } - - //Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); - - motor_out[RIGHT] += rc_4.pwm_out; - motor_out[LEFT] += rc_4.pwm_out; - motor_out[FRONT] -= rc_4.pwm_out; - motor_out[BACK] -= rc_4.pwm_out; - - //Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); - - motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max); - motor_out[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max); - motor_out[FRONT] = constrain(motor_out[FRONT], out_min, rc_3.radio_max); - motor_out[BACK] = constrain(motor_out[BACK], out_min, rc_3.radio_max); - - ///* - int r_out = ((long)(motor_out[RIGHT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); - int l_out = ((long)(motor_out[LEFT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); - int f_out = ((long)(motor_out[FRONT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); - int b_out = ((long)(motor_out[BACK] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); - //*/ - - // - /* debugging and dynamic kP - num++; - if (num > 50){ - num = 0; - //Serial.printf("control_in: %d ", rc_3.control_in); - //Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); - //Serial.printf(" cwm: %d %d %d %d, %d\t", rc_1.pwm_out, rc_2.pwm_out, rc_3.pwm_out, rc_4.pwm_out, rc_3.radio_out); - //Serial.printf(" out: %d %d %d %d\n", r_out, l_out, f_out, b_out); - //Serial.printf(" pwm: %d, %d %d %d %d\n",rc_3.pwm_out, motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); - - pid_stabilize_roll.kP((float)rc_6.control_in / 1000); - stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25; - init_pids(); - - //Serial.print("kP: "); - //Serial.println(pid_stabilize_roll.kP(),3); - } - // out: 41 38 39 39 - // pwm: 358, 1412 1380 1395 1389 - //*/ - - //Serial.printf("set: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); - //Serial.printf("s: %d %d %d\t\t", (int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor); - ///Serial.printf("outmin: %d\n", out_min); - - /* - write_int(r_out); - write_int(l_out); - write_int(f_out); - write_int(b_out); - write_int((int)(roll_sensor / 100)); - write_int((int)(pitch_sensor / 100)); - write_int((int)(yaw_sensor / 100)); - write_int((int)(yaw_error / 100)); - write_int((int)(current_loc.alt)); - write_int((int)(altitude_error)); - flush(10); - //*/ - - // Send commands to motors - if(rc_3.servo_out > 0){ - APM_RC.OutputCh(CH_1, motor_out[RIGHT]); - APM_RC.OutputCh(CH_2, motor_out[LEFT]); - APM_RC.OutputCh(CH_3, motor_out[FRONT]); - APM_RC.OutputCh(CH_4, motor_out[BACK]); - }else{ - APM_RC.OutputCh(CH_1, rc_3.radio_min); - APM_RC.OutputCh(CH_2, rc_3.radio_min); - APM_RC.OutputCh(CH_3, rc_3.radio_min); - APM_RC.OutputCh(CH_4, rc_3.radio_min); - } - - // InstantPWM - APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out2_Out3(); - - }else{ - - // Send commands to motors - APM_RC.OutputCh(CH_1, rc_3.radio_min); - APM_RC.OutputCh(CH_2, rc_3.radio_min); - APM_RC.OutputCh(CH_3, rc_3.radio_min); - APM_RC.OutputCh(CH_4, rc_3.radio_min); - - // reset I terms of PID controls - reset_I(); - - // Initialize yaw command to actual yaw when throttle is down... - rc_4.control_in = ToDeg(yaw); - } - } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 42c1033617..2f7f75aa77 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -319,7 +319,7 @@ // YAW Control // #ifndef YAW_P -# define YAW_P 0.3 +# define YAW_P 0.8 #endif #ifndef YAW_I # define YAW_I 0.0 diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 9474f32a53..2b8f53c4cf 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -76,11 +76,12 @@ void read_trim_switch() // switch was just released if((millis() - trim_timer) > 2000){ - /* + ///* if(rc_3.control_in == 0){ imu.init_accel(); imu.print_accel_offsets(); - }*/ + } + /* if(rc_3.control_in == 0){ imu.zero_accel(); }else{ @@ -88,8 +89,9 @@ void read_trim_switch() // set new accel offset values imu.ax(((long)rc_1.control_in * -15) / 100); imu.ay(((long)rc_2.control_in * -15) / 100); + imu.print_accel_offsets(); - } + }*/ } else { // set the throttle nominal diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 9c2a922186..a6654e07cb 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -242,6 +242,8 @@ #define EE_RADIO_6 0x1E // all gains stored from here #define EE_RADIO_7 0x24 // all gains stored from here #define EE_RADIO_8 0x2A // all gains stored from here +#define EE_RADIO_9 0xD2 // camera pitch +#define EE_RADIO_10 0xD8 // camera roll // user gains #define EE_XTRACK_GAIN 0x30 diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 13a74f33b4..1fd8ff8de7 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -1,4 +1,4 @@ -void init_radio() +void init_rc_in() { rc_1.set_angle(4500); rc_1.dead_zone = 50; @@ -15,9 +15,13 @@ void init_radio() rc_6.set_range(200,800); rc_7.set_range(0,1000); rc_8.set_range(0,1000); - + +} + +void init_rc_out() +{ #if ARM_AT_STARTUP == 1 - motor_armed = 1; + motor_armed = 1; #endif APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs @@ -83,3 +87,142 @@ void arm_motors() } } } + + + + +/***************************************** + * Set the flight control servos based on the current calculated values + *****************************************/ +void set_servos_4(void) +{ + static byte num; + + // Quadcopter mix + if (motor_armed == true) { + int out_min = rc_3.radio_min; + + // Throttle is 0 to 1000 only + rc_3.servo_out = constrain(rc_3.servo_out, 0, 1000); + + if(rc_3.servo_out > 0) + out_min = rc_3.radio_min + 50; + + //Serial.printf("out: %d %d %d %d\t\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); + + // creates the radio_out and pwm_out values + rc_1.calc_pwm(); + rc_2.calc_pwm(); + rc_3.calc_pwm(); + rc_4.calc_pwm(); + + //Serial.printf("out: %d %d %d %d\n", rc_1.radio_out, rc_2.radio_out, rc_3.radio_out, rc_4.radio_out); + //Serial.printf("yaw: %d ", rc_4.radio_out); + + if(frame_type == PLUS_FRAME){ + motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out; + motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out; + motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out; + motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out; + }else{ + int roll_out = rc_1.pwm_out / 2; + int pitch_out = rc_2.pwm_out / 2; + motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; + motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out; + motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; + motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; + } + + //Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); + + motor_out[RIGHT] += rc_4.pwm_out; + motor_out[LEFT] += rc_4.pwm_out; + motor_out[FRONT] -= rc_4.pwm_out; + motor_out[BACK] -= rc_4.pwm_out; + + //Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); + + motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max); + motor_out[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max); + motor_out[FRONT] = constrain(motor_out[FRONT], out_min, rc_3.radio_max); + motor_out[BACK] = constrain(motor_out[BACK], out_min, rc_3.radio_max); + + /* + int r_out = ((long)(motor_out[RIGHT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); + int l_out = ((long)(motor_out[LEFT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); + int f_out = ((long)(motor_out[FRONT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); + int b_out = ((long)(motor_out[BACK] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); + //*/ + + // + /* debugging and dynamic kP + num++; + if (num > 50){ + num = 0; + //Serial.printf("control_in: %d ", rc_3.control_in); + //Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); + //Serial.printf(" cwm: %d %d %d %d, %d\t", rc_1.pwm_out, rc_2.pwm_out, rc_3.pwm_out, rc_4.pwm_out, rc_3.radio_out); + //Serial.printf(" out: %d %d %d %d\n", r_out, l_out, f_out, b_out); + //Serial.printf(" pwm: %d, %d %d %d %d\n",rc_3.pwm_out, motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); + + pid_stabilize_roll.kP((float)rc_6.control_in / 1000); + stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25; + init_pids(); + + //Serial.print("kP: "); + //Serial.println(pid_stabilize_roll.kP(),3); + } + // out: 41 38 39 39 + // pwm: 358, 1412 1380 1395 1389 + //*/ + + //Serial.printf("set: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); + //Serial.printf("s: %d %d %d\t\t", (int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor); + ///Serial.printf("outmin: %d\n", out_min); + + /* + write_int(r_out); + write_int(l_out); + write_int(f_out); + write_int(b_out); + write_int((int)(roll_sensor / 100)); + write_int((int)(pitch_sensor / 100)); + write_int((int)(yaw_sensor / 100)); + write_int((int)(yaw_error / 100)); + write_int((int)(current_loc.alt)); + write_int((int)(altitude_error)); + flush(10); + //*/ + + // Send commands to motors + if(rc_3.servo_out > 0){ + APM_RC.OutputCh(CH_1, motor_out[RIGHT]); + APM_RC.OutputCh(CH_2, motor_out[LEFT]); + APM_RC.OutputCh(CH_3, motor_out[FRONT]); + APM_RC.OutputCh(CH_4, motor_out[BACK]); + }else{ + APM_RC.OutputCh(CH_1, rc_3.radio_min); + APM_RC.OutputCh(CH_2, rc_3.radio_min); + APM_RC.OutputCh(CH_3, rc_3.radio_min); + APM_RC.OutputCh(CH_4, rc_3.radio_min); + } + + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + + }else{ + + // Send commands to motors + APM_RC.OutputCh(CH_1, rc_3.radio_min); + APM_RC.OutputCh(CH_2, rc_3.radio_min); + APM_RC.OutputCh(CH_3, rc_3.radio_min); + APM_RC.OutputCh(CH_4, rc_3.radio_min); + + // reset I terms of PID controls + reset_I(); + + // Initialize yaw command to actual yaw when throttle is down... + rc_4.control_in = ToDeg(yaw); + } + } \ No newline at end of file diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 3a22052aff..edf2c95a11 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -280,6 +280,15 @@ setup_radio(uint8_t argc, const Menu::arg *argv) rc_6.radio_max = rc_6.radio_in; rc_7.radio_max = rc_7.radio_in; rc_8.radio_max = rc_8.radio_in; + + rc_1.radio_trim = rc_1.radio_in; + rc_2.radio_trim = rc_2.radio_in; + rc_4.radio_trim = rc_4.radio_in; + // 3 is not trimed + rc_5.radio_trim = 1500; + rc_6.radio_trim = 1500; + rc_7.radio_trim = 1500; + rc_8.radio_trim = 1500; Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: ")); while(1){ diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 1ec2369783..19319217ee 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -96,7 +96,9 @@ void init_ardupilot() read_EEPROM_startup(); // Read critical config information to start init_pids(); - init_radio(); + init_rc_in(); // sets up rc channels from radio + init_rc_out(); // sets up the timer libs + init_camera(); adc.Init(); // APM ADC library initialization APM_BMP085.Init(); // APM Abs Pressure sensor initialization DataFlash.Init(); // DataFlash log initialization @@ -233,7 +235,8 @@ void startup_ground(void) // read the radio to set trims // --------------------------- - trim_radio(); + // I am disabling this. It's not appropriate for Copters, only planes + //trim_radio(); // Warm up and read Gyro offsets // ----------------------------- diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 34e21ea5b7..df7e338281 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -7,6 +7,7 @@ static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_imu(uint8_t argc, const Menu::arg *argv); static int8_t test_gyro(uint8_t argc, const Menu::arg *argv); +static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); static int8_t test_omega(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); @@ -44,6 +45,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"gps", test_gps}, {"imu", test_imu}, {"gyro", test_gyro}, + {"dcm", test_dcm}, {"omega", test_omega}, {"battery", test_battery}, {"relay", test_relay}, @@ -109,7 +111,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) // read the radio to set trims // --------------------------- - trim_radio(); + //trim_radio(); while(1){ delay(20); @@ -146,27 +148,16 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); delay(1000); - - //imu.init_gyro(); - // read the radio to set trims - // --------------------------- - trim_radio(); + // setup the radio + // --------------- + init_rc_in(); + control_mode = STABILIZE; Serial.printf_P(PSTR("pid_stabilize_roll.kP: ")); Serial.println(pid_stabilize_roll.kP(),3); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); - /* - Serial.printf_P(PSTR("pid_yaw.kP: ")); - Serial.println(pid_yaw.kP(),3); - Serial.printf_P(PSTR("max_yaw_dampener:%d\n\n "), max_yaw_dampener); - Serial.printf_P(PSTR("stabilize_rate_yaw ")); - Serial.print(stabilize_rate_yaw, 3); - Serial.printf_P(PSTR("stabilze_yaw_dampener ")); - Serial.print(stabilze_yaw_dampener, 3); - Serial.printf_P(PSTR("\n\n ")); - */ - + motor_armed = true; while(1){ @@ -319,6 +310,71 @@ test_gyro(uint8_t argc, const Menu::arg *argv) } } + +static int8_t +test_dcm(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + Serial.printf_P(PSTR("Gyro | Accel\n")); + Vector3f _cam_vector; + Vector3f _out_vector; + + G_Dt = .02; + + while(1){ + for(byte i = 0; i <= 50; i++){ + delay(20); + // IMU + // --- + read_AHRS(); + } + + Matrix3f temp = dcm.get_dcm_matrix(); + Matrix3f temp_t = dcm.get_dcm_transposed(); + + Serial.printf_P(PSTR("dcm\n" + "%d \t %d \t %d \n" + "%d \t %d \t %d \n" + "%d \t %d \t %d \n\n"), + (int)(temp.a.x * 100), (int)(temp.a.y * 100), (int)(temp.a.z * 100), + (int)(temp.b.x * 100), (int)(temp.b.y * 100), (int)(temp.b.z * 100), + (int)(temp.c.x * 100), (int)(temp.c.y * 100), (int)(temp.c.z * 100)); + + Serial.printf_P(PSTR("dcm T\n" + "%d \t %d \t %d \n" + "%d \t %d \t %d \n" + "%d \t %d \t %d \n\n"), + (int)(temp_t.a.x * 100), (int)(temp_t.a.y * 100), (int)(temp_t.a.z * 100), + (int)(temp_t.b.x * 100), (int)(temp_t.b.y * 100), (int)(temp_t.b.z * 100), + (int)(temp_t.c.x * 100), (int)(temp_t.c.y * 100), (int)(temp_t.c.z * 100)); + + int _pitch = degrees(-asin(temp.c.x)); + int _roll = degrees(atan2(temp.c.y, temp.c.z)); + int _yaw = degrees(atan2(temp.b.x, temp.a.x)); + Serial.printf_P(PSTR( "angles\n" + "%d \t %d \t %d\n\n"), + _pitch, _roll, _yaw); + + int _pitch_t = degrees(-asin(temp_t.c.x)); + int _roll_t = degrees(atan2(temp_t.c.y, temp_t.c.z)); + int _yaw_t = degrees(atan2(temp_t.b.x, temp_t.a.x)); + Serial.printf_P(PSTR( "angles_t\n" + "%d \t %d \t %d\n\n"), + _pitch_t, _roll_t, _yaw_t); + + //_out_vector = _cam_vector * temp; + //Serial.printf_P(PSTR( "cam\n" + // "%d \t %d \t %d\n\n"), + // (int)temp.a.x * 100, (int)temp.a.y * 100, (int)temp.a.x * 100); + + if(Serial.available() > 0){ + return (0); + } + } +} + + /* static int8_t test_dcm(uint8_t argc, const Menu::arg *argv)