Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

latest updates 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1267 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-26 05:25:52 +00:00
parent 03ccf8aef8
commit 7329cf6100
5 changed files with 36 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopterMega/ArduCopterMega.pde
View File

@ -21,6 +21,7 @@ version 2.1 of the License, or (at your option) any later version.
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_EEPROMB.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // ArduPilot Mega RC Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
@ -886,4 +887,4 @@ void read_AHRS(void)
roll_sensor = dcm.roll_sensor;
pitch_sensor = dcm.pitch_sensor;
yaw_sensor = dcm.yaw_sensor;
}
}

42
ArduCopterMega/Attitude.pde
View File

@ -13,13 +13,9 @@ void init_pids()
void output_stabilize()
{
float roll_error, pitch_error;
int max_out;
Vector3f omega = dcm.get_gyro();
/*testing code:*/
//pitch_sensor = roll_sensor = 0; // testing only
//stabilize_rate_roll_pitch = (float)rc_6.control_in / 1000;
//init_pids();
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
@ -55,7 +51,8 @@ void output_stabilize()
// Limit dampening to be equal to propotional term for symmetry
rc_1.servo_out -= constrain(roll_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15°
rc_2.servo_out -= constrain(pitch_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15°
rc_4.servo_out -= constrain(yaw_dampener, -max_yaw_dampener, max_yaw_dampener); // +- 15°
rc_4.servo_out -= constrain(yaw_dampener, -max_yaw_dampener, max_yaw_dampener);
//Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener);
@ -84,9 +81,9 @@ void output_rate_control()
//Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3);
// Limit output
rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
}
@ -105,7 +102,7 @@ void reset_I(void)
void set_servos_4(void)
{
static byte num;
//motor_armed = false;
// Quadcopter mix
if (motor_armed == true) {
int out_min = rc_3.radio_min;
@ -118,7 +115,7 @@ void set_servos_4(void)
//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 anf pwm_out values
// creates the radio_out and pwm_out values
rc_1.calc_pwm();
rc_2.calc_pwm();
rc_3.calc_pwm();
@ -140,6 +137,7 @@ void set_servos_4(void)
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;
@ -160,15 +158,23 @@ void set_servos_4(void)
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);
//*/
//~#*set_servos_4: 398, -39 38 38 -36
/*
/* 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(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out);
//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(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out);
//Serial.printf(" pwm: %d %d %d %d\n", 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() *.24;
init_pids();
Serial.print("kP: ");
Serial.println(pid_stabilize_roll.kP(),3);
}
//*/
@ -222,9 +228,3 @@ void set_servos_4(void)
rc_4.control_in = ToDeg(yaw);
}
}
void demo_servos(byte i) {
// nothing to do
}

1
ArduCopterMega/flight_control.pde
View File

@ -18,7 +18,6 @@ void output_auto_throttle()
rc_3.servo_out = (float)nav_throttle * angle_boost();
rc_3.servo_out = max(rc_3.servo_out, 1);
}
// Jason
void calc_nav_throttle()
{

11
ArduCopterMega/radio.pde
View File

@ -6,12 +6,13 @@ void init_radio()
rc_2.dead_zone = 50;
rc_3.set_range(0,1000);
rc_3.dead_zone = 20;
rc_3.radio_max += 300; // hack for better throttle control
//rc_3.radio_max += 300; // hack for better throttle control
rc_3.scale_output = .8;
rc_4.set_angle(6000);
rc_4.dead_zone = 500;
rc_5.set_range(0,1000);
rc_5.set_filter(false);
rc_6.set_range(50,200);
rc_6.set_range(200,800);
rc_7.set_range(0,1000);
rc_8.set_range(0,1000);
@ -33,7 +34,7 @@ void init_radio()
}
void read_radio()
{
{
rc_1.set_pwm(APM_RC.InputCh(CH_1));
rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3));
@ -47,7 +48,9 @@ void read_radio()
void trim_radio()
{
read_radio();
for (byte i = 0; i < 50; i++){
read_radio();
}
rc_1.trim(); // roll
rc_2.trim(); // pitch
rc_4.trim(); // yaw

11
ArduCopterMega/read_commands.pde
View File

@ -79,32 +79,33 @@ void parseCommand(char *buffer)
case 'P':
pid_stabilize_roll.kP((float)value / 1000);
pid_stabilize_pitch.kP((float)value / 1000);
save_EEPROM_PID();
//save_EEPROM_PID();
break;
case 'I':
pid_stabilize_roll.kI((float)value / 1000);
pid_stabilize_pitch.kI((float)value / 1000);
save_EEPROM_PID();
//save_EEPROM_PID();
break;
case 'D':
pid_stabilize_roll.kD((float)value / 1000);
pid_stabilize_pitch.kD((float)value / 1000);
save_EEPROM_PID();
//save_EEPROM_PID();
break;
case 'X':
pid_stabilize_roll.imax((int)(value * 100));
pid_stabilize_pitch.imax((int)(value * 100));
save_EEPROM_PID();
//save_EEPROM_PID();
break;
case 'R':
stabilize_rate_roll_pitch = (float)value / 1000;
save_EEPROM_PID();
//save_EEPROM_PID();
break;
}
init_pids();
//*/
}
}