Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

bug fixes and updates

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1750 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-07 00:57:06 +00:00
parent 756a5b0052
commit 97107d89d2
8 changed files with 62 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopterMega/ArduCopterMega.pde
View File

@ -206,6 +206,8 @@ const char* flight_mode_strings[] = {
8 TBD
*/
long error_a;
// Radio
// -----
int motor_out[4];
@ -678,7 +680,8 @@ void slow_loop()
// between 1 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
// XXX gcs.send_message(MSG_CPU_LOAD, load*100);
// XXX
// gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz

27
ArduCopterMega/Attitude.pde
View File

@ -88,16 +88,23 @@ clear_yaw_control()
void
output_yaw_with_hold(boolean hold)
{
//digitalWrite(B_LED_PIN, LOW);
if(hold){
// look to see if we have exited rate control properly - ie stopped turning
if(rate_yaw_flag){
// we are still in motion from rate control
if(fabs(omega.y) < .08){
if(fabs(omega.z) < .5){
clear_yaw_control();
hold = true; // just to be explicit
hold = true; // just to be explicit
//Serial.print("C");
}else{
//digitalWrite(B_LED_PIN, HIGH);
// return to rate control until we slow down.
hold = false;
//Serial.print("D");
}
}
@ -111,6 +118,7 @@ output_yaw_with_hold(boolean hold)
}
if(hold){
//Serial.println("H");
// try and hold the current nav_yaw setting
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
yaw_error = wrap_180(yaw_error);
@ -130,19 +138,26 @@ output_yaw_with_hold(boolean hold)
}else{
//Serial.println("R");
// rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
if(abs(rate) < 1000 ) //experiment to limit yaw reversing
rate = 0;
//if(abs(rate) < 1000 ) //experiment to limit yaw reversing
// rate = 0;
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
// -error = CCW, +error = CW
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
if(g.rc_4.control_in == 0)
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 3.0);// kP .07 * 36000 = 2520
else
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24°
}
}
// slight left rudder give right roll.
void

2
ArduCopterMega/commands_logic.pde
View File

@ -41,7 +41,7 @@ handle_process_may()
do_change_alt();
break;
case MAV_CMD_CONDITION_YAW:
case MAV_CMD_CONDITION_ANGLE:
do_yaw();
break;

2
ArduCopterMega/defines.h
View File

@ -117,7 +117,7 @@
#define RELAY_TOGGLE 5
#define STOP_REPEAT 10
#define MAV_CMD_CONDITION_YAW 23
//#define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's
#define MSG_ACKNOWLEDGE 0x00

5
ArduCopterMega/motors.pde
View File

@ -159,6 +159,7 @@ set_servos_4()
num++;
if (num > 50){
num = 0;
/*
Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ",
target_altitude,
current_loc.alt,
@ -169,6 +170,10 @@ set_servos_4()
}else{
Serial.println("Sonar");
}
*/
//Serial.print("!");
//debugging with Channel 6

14
ArduCopterMega/radio.pde
View File

@ -28,6 +28,8 @@ void init_rc_in()
g.rc_6.set_range(0,1000);
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
//catch bad RC_3 min values
}
void init_rc_out()
@ -46,15 +48,23 @@ void init_rc_out()
APM_RC.Init(); // APM Radio initialization
for(byte i = 0; i < 10; i++){
delay(20);
read_radio();
}
// sanity check on the EEPROM values for radio_min
if(abs(g.rc_3.radio_min - g.rc_3.radio_in) > 40){
g.rc_3.radio_min = g.rc_3.radio_in;
}
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
}
void read_radio()

3
ArduCopterMega/setup.pde
View File

@ -558,11 +558,12 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){
//mag_offset_x = offset[0];
//mag_offset_y = offset[1];
//mag_offset_z = offset[2];
//save_EEPROM_mag_offset();
//setup_mag_offset();
// set offsets to account for surrounding interference
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);

22
ArduCopterMega/test.pde
View File

@ -11,7 +11,7 @@ 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_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_current(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
@ -52,7 +52,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
#endif
{"imu", test_imu},
//{"dcm", test_dcm},
//{"omega", test_omega},
{"omega", test_omega},
{"battery", test_battery},
{"current", test_current},
{"relay", test_relay},
@ -238,6 +238,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
control_mode = STABILIZE;
Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP());
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
Serial.printf_P(PSTR("max_yaw_dampener:%d\n\n "), max_yaw_dampener);
trim_radio();
@ -285,14 +286,21 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
set_servos_4();
ts_num++;
if (ts_num > 10){
if (ts_num > 0){
ts_num = 0;
/*
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
(int)(dcm.roll_sensor/100),
(int)(dcm.pitch_sensor/100),
g.rc_1.pwm_out);
*/
print_motor_out();
Serial.printf_P(PSTR("e:%ld, 4out:%d, O%4.4f\n"),
error_a,
g.rc_4.servo_out,
omega.z);
//print_motor_out();
}
// R: 1417, L: 1453 F: 1453 B: 1417
@ -611,7 +619,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
}
*/
/*static int8_t
///*
static int8_t
test_omega(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
@ -629,6 +638,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
// IMU
// ---
read_AHRS();
float my_oz = (dcm.yaw - old_yaw) * 50;
old_yaw = dcm.yaw;
@ -646,7 +656,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
}
return (0);
}
*/
//*/
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)