aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1306 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-27 23:09:08 +00:00
parent 8a5d7c3fb5
commit 4bd32750a4
10 changed files with 257 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/APM_Config.h
View File

@ -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

26
ArduCopterMega/ArduCopterMega.pde
View File

@ -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) {

134
ArduCopterMega/Attitude.pde
View File

@ -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);
}
}

2
ArduCopterMega/config.h
View File

@ -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

8
ArduCopterMega/control_modes.pde
View File

@ -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

2
ArduCopterMega/defines.h
View File

@ -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

149
ArduCopterMega/radio.pde
View File

@ -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);
}
}

9
ArduCopterMega/setup.pde
View File

@ -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){

7
ArduCopterMega/system.pde
View File

@ -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
// -----------------------------

90
ArduCopterMega/test.pde
View File

@ -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)