diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 156ceaa86d..2758b10f89 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -4,11 +4,8 @@ void init_pids() // create limits to how much dampening we'll allow // this creates symmetry with the P gain value preventing oscillations - max_stabilize_dampener = pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15° - //max_stabilize_dampener += pid_stabilize_roll.imax(); // = 0.1 * 300 = 1500 or 15° - + max_stabilize_dampener = pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15° max_yaw_dampener = pid_yaw.kP() * 6000; // = .5 * 6000 = 3000 - //max_yaw_dampener += pid_yaw.imax(); // = 0.6 * 2500 = 1500 or 15° } diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index ca209ee1a0..c65917432f 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -193,53 +193,30 @@ void set_servos_4(void) 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; - hold_yaw_dampener = (float)rc_6.control_in; - //pid_stabilize_roll.kP((float)rc_6.control_in / 1000); - //stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25; - //init_pids(); - - //Serial.print("nav_yaw: "); - //Serial.println(nav_yaw,DEC); + //debugging with Channel 6 + /* + // ROLL and PITCH + // make sure you init_pids() after changing the kP + pid_stabilize_roll.kP((float)rc_6.control_in / 1000); + init_pids(); //Serial.print("kP: "); //Serial.println(pid_stabilize_roll.kP(),3); + */ + + /* + // YAW + // make sure you init_pids() after changing the kP + pid_yaw.kP((float)rc_6.control_in / 1000); + init_pids(); + */ } - // 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){ diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text index 8f9308d730..8d7cca12bb 100644 --- a/ArduCopterMega/read_me.text +++ b/ArduCopterMega/read_me.text @@ -1,41 +1,67 @@ Make sure you update the libraries from the arducopter trunk and the latest code from the ardupilotmega branch. I have a lot of setup commands now. I may reduce and consolidate these soon. -Radio: +Radio setup: ch1 = roll ch2 = pitch ch3 = throttle ch4 = yaw ch5 = mode switch - use your 3 position switch -ch6 = used for tuning - not currently active, search for "rc_6" to enable -ch7 = use to set throttle hold value while hovering (quick toggle), hold > 5 seconds on ground to reset the accelerometer offsets. -ch8 = not used +ch6 = used for in-air tuning - not currently active, look for "debugging with Channel 6" in radio.pde to enable +ch7 = use to set throttle hold value while hovering (quick toggle), hold to trim in air values - don't use your radio's trims! +ch8 = not used - this is the hardware manual - it's dangerous to use for quads -setup: -erase - run this first, just in case -reset - run this second -radio - run this third -esc - just ignore this for now -level - optional - sets accelerometer offsets -flat - optional - sets accelerometer offsets to 0 -modes - interactive setup for flight modes -pid - optional - writes default PID values to eeprom -frame - optional - default is "+" -enable_mag - enables the compass -disable_mag - disables the compass -compass - interactive setup for compass offsets -declination - usage: "declination 14.25" -show - shows all values +CLI interactive setup - You must go through each item and set the values to match your hardware -Flight modes to try: -stabilize - yay -Alt_hold - You need to set your throttle_cruise value by toggling ch_7 for less than 1 second. (Mine is 330), altitude is controlled by the throttle lever. +"setup" menu: +erase - run this first! erases bad values from EEPROMS just in case +reset - Performs factory reset and initialization of EEPROM values +radio - records the limits of ALL radio channels - very important! +pid - restores default PID values +frame - sets your frame config: [x, +, tri] +motors - interactive setup of your ESC and motors, enter this mode, then plug-in battery, + point at motors to make them spin + throttle will output full range to each motor - this is good for esc calibration +level - sets initial value of accelerometers - hold copter level +modes - sets the flight modes assigned to each switch position (you have 5 available) +current - enables an Attopilot current sensor: [on, off, milliamp hours] +compass - enables the compass [on, off] +mag_offset - interactive mode to set the limits of your compass +declination - look up your declination online for accuracy [decimal degrees] +show - a formatted output of all the settings + +"test" menu: + +pwm - outputs the pwm values of all 8 radio channels +radio - outputs the control values of all 8 radio channels in degrees * 100 or other value (see radio.pde) +stabilize - lets you test the stabilization with debugging output to the terminal - may spin the props +fbw - developers only +gps - outputs GPS data +adc - outputs raw adc values +imu - outputs euler angles +dcm - outputs DCM matrix values +battery - outputs voltage readings to analog in 0-3 +current - outputs voltage and current from an AttoPilot current sensor +relay - toggles the relay +waypoints - dumps stored waypoint commands +airpressure - raw output of absolute presure sensor +compass - outputs compass angles in degrees * 100 +xbee - outputs an XBEE sequence used for range testing +eedump - raw output of bytes in eeprom + +Flight modes: +stabilize - copter will hold -45 to 45° angle, throttle is manual. +Alt_hold - You need to set your throttle_cruise value by toggling ch_7 for less than 1 second. (Mine is 330), + altitude is controlled by the throttle lever using absolute position - from 0 to 40 meters. + this control method will change after testing. FBW - Simulates GPS Hold with the sticks being the position offset. Manual Throttle. -position_hold - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden with the radio. -RTL - not tested yet, will try and fly back to home at the current altitude. -Auto - not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon +position_hold + - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden temporarily with the radio. +RTL - Will try and fly back to home at the current altitude. +Auto - Will fly the mission loaded by the Waypoint writer -- what's new + +what's new to commands for ACM: - CMD_ANGLE - will set the desired yaw with control of angle/second and direction. - CMD_ALTITUDE - will send the copter up from current position to desired altitude - CMD_R_WAYPOINT - is just like a waypoint but relative to home diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 8e15d54c12..940160e170 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -7,7 +7,6 @@ static int8_t test_fbw(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_adc(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); @@ -15,7 +14,6 @@ static int8_t test_current(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); static int8_t test_wp(uint8_t argc, const Menu::arg *argv); static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); -static int8_t test_nav_out(uint8_t argc, const Menu::arg *argv); static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); @@ -47,7 +45,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"gps", test_gps}, {"adc", test_adc}, {"imu", test_imu}, - {"gyro", test_gyro}, {"dcm", test_dcm}, {"omega", test_omega}, {"battery", test_battery}, @@ -55,7 +52,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"relay", test_relay}, {"waypoints", test_wp}, {"airpressure", test_pressure}, - {"nav", test_nav_out}, {"compass", test_mag}, {"xbee", test_xbee}, {"eedump", test_eedump}, @@ -456,27 +452,6 @@ test_gps(uint8_t argc, const Menu::arg *argv) } } -static int8_t -test_gyro(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - Serial.printf_P(PSTR("Gyro | Accel\n")); - delay(1000); - - while(1){ - Vector3f gyros = imu.get_gyro(); - Vector3f accels = imu.get_accel(); - Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"), (int)gyros.x, (int)gyros.y, (int)gyros.z, (int)accels.x, (int)accels.y, (int)accels.z); - delay(100); - - if(Serial.available() > 0){ - return (0); - } - } -} - - static int8_t test_dcm(uint8_t argc, const Menu::arg *argv) { @@ -774,31 +749,6 @@ test_pressure(uint8_t argc, const Menu::arg *argv) } } -static int8_t -test_nav_out(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Nav test\n")); - print_hit_enter(); - - wp_distance = 100; - dTnav = 50; - - while(1){ - delay(50); - //bearing_error += 100; - //bearing_error = wrap_360(bearing_error); - //calc_nav_pid(); - //calc_nav_pitch(); - //calc_nav_roll(); - - //Serial.printf("error %ld,\troll %ld,\tpitch %ld\n", bearing_error, nav_roll, nav_pitch); - - if(Serial.available() > 0){ - return (0); - } - } -} - static int8_t test_mag(uint8_t argc, const Menu::arg *argv) {