cleaning up the instructions for use

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1519 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-21 06:10:26 +00:00
parent a6a4fffe46
commit 60f766f9e2
4 changed files with 67 additions and 117 deletions

View File

@ -5,10 +5,7 @@ void init_pids()
// this creates symmetry with the P gain value preventing oscillations // 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.kP() * 2500; // = 0.6 * 2500 = 1500 or 15°
//max_stabilize_dampener += pid_stabilize_roll.imax(); // = 0.1 * 300 = 1500 or 15°
max_yaw_dampener = pid_yaw.kP() * 6000; // = .5 * 6000 = 3000 max_yaw_dampener = pid_yaw.kP() * 6000; // = .5 * 6000 = 3000
//max_yaw_dampener += pid_yaw.imax(); // = 0.6 * 2500 = 1500 or 15°
} }

View File

@ -194,52 +194,29 @@ void set_servos_4(void)
motor_out[FRONT] = constrain(motor_out[FRONT], 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); 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++; num++;
if (num > 50){ if (num > 50){
num = 0; num = 0;
hold_yaw_dampener = (float)rc_6.control_in; //debugging with Channel 6
//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);
//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); // ROLL and PITCH
write_int(l_out); // make sure you init_pids() after changing the kP
write_int(f_out); pid_stabilize_roll.kP((float)rc_6.control_in / 1000);
write_int(b_out); init_pids();
write_int((int)(roll_sensor / 100)); //Serial.print("kP: ");
write_int((int)(pitch_sensor / 100)); //Serial.println(pid_stabilize_roll.kP(),3);
write_int((int)(yaw_sensor / 100)); */
write_int((int)(yaw_error / 100));
write_int((int)(current_loc.alt)); /*
write_int((int)(altitude_error)); // YAW
flush(10); // make sure you init_pids() after changing the kP
//*/ pid_yaw.kP((float)rc_6.control_in / 1000);
init_pids();
*/
}
// Send commands to motors // Send commands to motors
if(rc_3.servo_out > 0){ if(rc_3.servo_out > 0){

View File

@ -1,41 +1,67 @@
Make sure you update the libraries from the arducopter trunk and the latest code from the ardupilotmega branch. 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. I have a lot of setup commands now. I may reduce and consolidate these soon.
Radio: Radio setup:
ch1 = roll ch1 = roll
ch2 = pitch ch2 = pitch
ch3 = throttle ch3 = throttle
ch4 = yaw ch4 = yaw
ch5 = mode switch - use your 3 position switch ch5 = mode switch - use your 3 position switch
ch6 = used for tuning - not currently active, search for "rc_6" to enable 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 > 5 seconds on ground to reset the accelerometer offsets. 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 ch8 = not used - this is the hardware manual - it's dangerous to use for quads
setup: CLI interactive setup - You must go through each item and set the values to match your hardware
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
Flight modes to try: "setup" menu:
stabilize - yay erase - run this first! erases bad values from EEPROMS just in case
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. 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. 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. position_hold
RTL - not tested yet, will try and fly back to home at the current altitude. - 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.
Auto - not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon 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_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_ALTITUDE - will send the copter up from current position to desired altitude
- CMD_R_WAYPOINT - is just like a waypoint but relative to home - CMD_R_WAYPOINT - is just like a waypoint but relative to home

View File

@ -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_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(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_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_dcm(uint8_t argc, const Menu::arg *argv);
static int8_t test_omega(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_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_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(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_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_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(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); 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}, {"gps", test_gps},
{"adc", test_adc}, {"adc", test_adc},
{"imu", test_imu}, {"imu", test_imu},
{"gyro", test_gyro},
{"dcm", test_dcm}, {"dcm", test_dcm},
{"omega", test_omega}, {"omega", test_omega},
{"battery", test_battery}, {"battery", test_battery},
@ -55,7 +52,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"relay", test_relay}, {"relay", test_relay},
{"waypoints", test_wp}, {"waypoints", test_wp},
{"airpressure", test_pressure}, {"airpressure", test_pressure},
{"nav", test_nav_out},
{"compass", test_mag}, {"compass", test_mag},
{"xbee", test_xbee}, {"xbee", test_xbee},
{"eedump", test_eedump}, {"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 static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv) 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 static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {