mirror of https://github.com/ArduPilot/ardupilot
Big update - motors re-assigned to new positions.
New, fabulous Yaw control. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1398 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
cc86861b26
commit
8ce92b6711
|
|
@ -5,7 +5,9 @@
|
|||
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
|
||||
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
|
||||
#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
|
||||
//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
|
||||
#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
|
||||
#define ARM_AT_STARTUP 0
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -141,19 +141,23 @@ byte frame_type = PLUS_FRAME;
|
|||
PID pid_acro_rate_roll (EE_GAIN_1);
|
||||
PID pid_acro_rate_pitch (EE_GAIN_2);
|
||||
PID pid_acro_rate_yaw (EE_GAIN_3);
|
||||
float acro_rate_roll_pitch, acro_rate_yaw;
|
||||
|
||||
//Stabilize
|
||||
PID pid_stabilize_roll (EE_GAIN_4);
|
||||
PID pid_stabilize_pitch (EE_GAIN_5);
|
||||
PID pid_yaw (EE_GAIN_6);
|
||||
float stabilize_rate_roll_pitch;
|
||||
float stabilize_rate_yaw;
|
||||
float stabilze_dampener;
|
||||
|
||||
// roll pitch
|
||||
float stabilize_dampener;
|
||||
int max_stabilize_dampener;
|
||||
float stabilze_yaw_dampener;
|
||||
|
||||
// yaw
|
||||
float hold_yaw_dampener;
|
||||
int max_yaw_dampener;
|
||||
|
||||
// used to transition yaw control from Rate control to Yaw hold
|
||||
boolean rate_yaw_flag;
|
||||
|
||||
// Nav
|
||||
PID pid_nav (EE_GAIN_7);
|
||||
PID pid_throttle (EE_GAIN_8);
|
||||
|
|
@ -740,7 +744,9 @@ void update_current_flight_mode(void)
|
|||
calc_nav_throttle();
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
// ------------------------------------
|
||||
auto_yaw();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
|
||||
|
|
@ -760,12 +766,12 @@ void update_current_flight_mode(void)
|
|||
nav_pitch = 0;
|
||||
nav_roll = 0;
|
||||
|
||||
// get desired yaw control from radio
|
||||
input_yaw_hold();
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
|
||||
// Yaw control
|
||||
output_manual_yaw();
|
||||
|
||||
// apply throttle control
|
||||
output_manual_throttle();
|
||||
|
||||
|
|
@ -806,8 +812,9 @@ void update_current_flight_mode(void)
|
|||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
|
||||
// get desired yaw control from radio
|
||||
input_yaw_hold();
|
||||
// Yaw control
|
||||
// -----------
|
||||
output_manual_yaw();
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
|
|
@ -830,8 +837,9 @@ void update_current_flight_mode(void)
|
|||
// get desired height from the throttle
|
||||
next_WP.alt = home.alt + (rc_3.control_in * 4) -100; // 0 - 1000 (40 meters)
|
||||
|
||||
// get desired yaw control from radio
|
||||
input_yaw_hold();
|
||||
// Yaw control
|
||||
// -----------
|
||||
output_manual_yaw();
|
||||
|
||||
// based on altitude error
|
||||
// -----------------------
|
||||
|
|
@ -860,6 +868,8 @@ void update_current_flight_mode(void)
|
|||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
auto_yaw();
|
||||
|
||||
// apply throttle control
|
||||
output_auto_throttle();
|
||||
|
||||
|
|
@ -874,8 +884,9 @@ void update_current_flight_mode(void)
|
|||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
|
||||
// get desired yaw control from radio
|
||||
input_yaw_hold();
|
||||
// Yaw control
|
||||
// -----------
|
||||
output_manual_yaw();
|
||||
|
||||
// based on altitude error
|
||||
// -----------------------
|
||||
|
|
|
|||
|
|
@ -1,19 +1,20 @@
|
|||
|
||||
// desired angle in
|
||||
// motor commands out (in degrees)
|
||||
void init_pids()
|
||||
{
|
||||
max_stabilize_dampener = pid_stabilize_roll.kP() * 2500;
|
||||
stabilze_dampener = 5729.57795 * stabilize_rate_roll_pitch;
|
||||
|
||||
max_yaw_dampener = pid_yaw.kP() * 6000; // .3 * 6000 = 1800
|
||||
stabilze_yaw_dampener = 5729.57795 * stabilize_rate_yaw; // .3
|
||||
// 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_yaw_dampener = pid_yaw.kP() * 6000; // .5 * 6000 = 3000
|
||||
}
|
||||
|
||||
|
||||
void output_stabilize()
|
||||
{
|
||||
float roll_error, pitch_error;
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
long rate;
|
||||
int dampener;
|
||||
|
||||
//pitch_sensor = roll_sensor = 0; // testing only
|
||||
|
||||
|
|
@ -24,35 +25,31 @@ void output_stabilize()
|
|||
|
||||
roll_error = rc_1.servo_out - roll_sensor;
|
||||
pitch_error = rc_2.servo_out - pitch_sensor;
|
||||
yaw_error = nav_yaw - yaw_sensor;
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
roll_error = constrain(roll_error, -2500, 2500);
|
||||
pitch_error = constrain(pitch_error, -2500, 2500);
|
||||
yaw_error = constrain(yaw_error, -6000, 6000);
|
||||
|
||||
//Serial.printf("s: %d \t mix %d, err %d", (int)roll_sensor, (int)rc_1.servo_out, (int)roll_error);
|
||||
|
||||
// write out angles back to servo out - this will be converted to PWM by RC_Channel
|
||||
rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, deltaMiliSeconds, 1.0);
|
||||
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, deltaMiliSeconds, 1.0);
|
||||
rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .3 = 198pwm
|
||||
|
||||
//Serial.printf("\tpid: %d", (int)rc_1.servo_out);
|
||||
|
||||
// We adjust the output by the rate of rotation:
|
||||
// Rate control through bias corrected gyro rates
|
||||
// omega is the raw gyro reading
|
||||
int roll_dampener = (omega.x * stabilze_dampener);// Omega is in radians
|
||||
int pitch_dampener = (omega.y * stabilze_dampener);
|
||||
int yaw_dampener = (omega.z * stabilze_yaw_dampener);
|
||||
|
||||
|
||||
// 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);
|
||||
|
||||
rate = degrees(omega.x) * 100; // 6rad = 34377
|
||||
dampener = ((float)rate * stabilize_dampener); // 34377 * .175 = 6000
|
||||
rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500
|
||||
|
||||
rate = degrees(omega.y) * 100; // 6rad = 34377
|
||||
dampener = ((float)rate * stabilize_dampener); // 34377 * .175 = 6000
|
||||
rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500
|
||||
|
||||
//Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener);
|
||||
|
||||
|
|
@ -60,22 +57,90 @@ void output_stabilize()
|
|||
//Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm());
|
||||
}
|
||||
|
||||
// err -2500 pid: -1100 rd: 1117 limit: -1650, PWM: -152
|
||||
//s: -1247 mix 0, err 1247 pid: 548 rd: -153 limit: 395, PWM: 35
|
||||
void
|
||||
clear_yaw_control()
|
||||
{
|
||||
//Serial.print("Clear ");
|
||||
rate_yaw_flag = false; // exit rate_yaw_flag
|
||||
nav_yaw = yaw_sensor; // save our Yaw
|
||||
yaw_error = 0;
|
||||
}
|
||||
|
||||
void output_yaw_with_hold(boolean hold)
|
||||
{
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
|
||||
if(hold){
|
||||
// yaw hold
|
||||
if(rate_yaw_flag){
|
||||
// we are still in motion from rate control
|
||||
if(fabs(omega.y) < .15){
|
||||
//Serial.print("trans ");
|
||||
clear_yaw_control();
|
||||
hold = true; // just to be explicit
|
||||
}else{
|
||||
//Serial.print("coast ");
|
||||
// return to rate control until we slow down.
|
||||
hold = false;
|
||||
}
|
||||
}else{
|
||||
//Serial.print("hold ");
|
||||
}
|
||||
|
||||
} else {
|
||||
//Serial.print("rate ");
|
||||
// rate control
|
||||
rate_yaw_flag = true;
|
||||
yaw_error = 0;
|
||||
}
|
||||
|
||||
if(hold){
|
||||
yaw_error = nav_yaw - yaw_sensor; // +- 60°
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
|
||||
rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .5 * 6000 = 3000
|
||||
|
||||
// We adjust the output by the rate of rotation
|
||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
int dampener = ((float)rate * hold_yaw_dampener); // 18000 * .17 = 3000
|
||||
|
||||
// Limit dampening to be equal to propotional term for symmetry
|
||||
rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
|
||||
|
||||
}else{
|
||||
//yaw_error = 0;
|
||||
|
||||
// rate control
|
||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000
|
||||
// -error = CCW, +error = CW
|
||||
rc_4.servo_out = pid_acro_rate_yaw.get_pid(error, deltaMiliSeconds, 1.0); // .075 * 36000 = 2700
|
||||
rc_4.servo_out = constrain(rc_4.servo_out, -2400, 2400); // limit to 2400
|
||||
|
||||
// this indicates we are under rate control, when we enter Yaw Hold and
|
||||
// return to 0° per second, we exit rate control and hold the current Yaw
|
||||
//rate_yaw_flag = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void output_rate_control()
|
||||
{
|
||||
/*
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
|
||||
rc_4.servo_out = rc_4.control_in;
|
||||
rc_1.servo_out = rc_2.control_in;
|
||||
rc_2.servo_out = rc_2.control_in;
|
||||
|
||||
// Rate control through bias corrected gyro rates
|
||||
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
rc_1.servo_out -= (omega.x * 5729.57795 * acro_rate_roll_pitch);
|
||||
rc_2.servo_out -= (omega.y * 5729.57795 * acro_rate_roll_pitch);
|
||||
rc_4.servo_out -= (omega.z * 5729.57795 * acro_rate_yaw);
|
||||
rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener);
|
||||
rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener);
|
||||
|
||||
//Serial.printf("\trated out %d, omega ", rc_1.servo_out);
|
||||
//Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3);
|
||||
|
|
@ -83,7 +148,7 @@ void output_rate_control()
|
|||
// Limit 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);
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -133,10 +133,11 @@ void read_EEPROM_PID(void)
|
|||
pid_nav.load_gains();
|
||||
pid_throttle.load_gains();
|
||||
|
||||
stabilize_rate_roll_pitch = read_EE_compressed_float(EE_STAB_RATE_RP, 4);
|
||||
stabilize_rate_yaw = read_EE_compressed_float(EE_STAB_RATE_YAW, 4);
|
||||
acro_rate_roll_pitch = read_EE_compressed_float(EE_ACRO_RATE_RP, 4);
|
||||
acro_rate_yaw = read_EE_compressed_float(EE_ACRO_RATE_YAW, 4);
|
||||
// roll pitch
|
||||
stabilize_dampener = read_EE_compressed_float(EE_STAB_DAMPENER, 4);
|
||||
|
||||
// yaw
|
||||
hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
|
||||
}
|
||||
|
||||
void save_EEPROM_PID(void)
|
||||
|
|
@ -152,10 +153,11 @@ void save_EEPROM_PID(void)
|
|||
pid_nav.save_gains();
|
||||
pid_throttle.save_gains();
|
||||
|
||||
write_EE_compressed_float(stabilize_rate_roll_pitch, EE_STAB_RATE_RP, 4);
|
||||
write_EE_compressed_float(stabilize_rate_yaw, EE_STAB_RATE_YAW, 4);
|
||||
write_EE_compressed_float(acro_rate_roll_pitch, EE_ACRO_RATE_RP, 4);
|
||||
write_EE_compressed_float(acro_rate_yaw, EE_ACRO_RATE_YAW, 4);
|
||||
// roll pitch
|
||||
write_EE_compressed_float(stabilize_dampener, EE_STAB_DAMPENER, 4);
|
||||
|
||||
// yaw
|
||||
write_EE_compressed_float(hold_yaw_dampener, EE_HOLD_YAW_DAMPENER, 4);
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
|
|
|||
|
|
@ -134,10 +134,12 @@ void process_must()
|
|||
switch(command_must_ID){
|
||||
|
||||
case CMD_TAKEOFF: // TAKEOFF!
|
||||
takeoff_altitude = (int)next_command.alt;
|
||||
takeoff_altitude = (int)next_command.alt;
|
||||
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
||||
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
||||
next_WP.alt = current_loc.alt + takeoff_altitude;
|
||||
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
|
||||
//set_next_WP(&next_WP);
|
||||
break;
|
||||
|
||||
case CMD_WAYPOINT: // Navigate to Waypoint
|
||||
|
|
@ -395,7 +397,7 @@ void verify_must()
|
|||
break;
|
||||
|
||||
case CMD_TAKEOFF: // Takeoff!
|
||||
if (current_loc.alt > (home.alt + takeoff_altitude)){
|
||||
if (current_loc.alt > (next_WP.alt -100)){
|
||||
command_must_index = 0;
|
||||
takeoff_complete = true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -254,7 +254,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef ACRO_RATE_YAW_P
|
||||
# define ACRO_RATE_YAW_P .1
|
||||
# define ACRO_RATE_YAW_P .07
|
||||
#endif
|
||||
#ifndef ACRO_RATE_YAW_I
|
||||
# define ACRO_RATE_YAW_I 0.0
|
||||
|
|
@ -263,19 +263,7 @@
|
|||
# define ACRO_RATE_YAW_D 0.0
|
||||
#endif
|
||||
#ifndef ACRO_RATE_YAW_IMAX
|
||||
# define ACRO_RATE_YAW_IMAX 20
|
||||
#endif
|
||||
|
||||
// STABILZE RATE Control
|
||||
//
|
||||
#ifndef ACRO_RATE_RP
|
||||
# define ACRO_RATE_RP 0.1
|
||||
#endif
|
||||
|
||||
// STABILZE RATE Control
|
||||
//
|
||||
#ifndef ACRO_RATE_YAW
|
||||
# define ACRO_RATE_YAW 0.1
|
||||
# define ACRO_RATE_YAW_IMAX 0
|
||||
#endif
|
||||
|
||||
|
||||
|
|
@ -310,8 +298,8 @@
|
|||
|
||||
// STABILZE RATE Control
|
||||
//
|
||||
#ifndef STABILIZE_RATE_RP
|
||||
# define STABILIZE_RATE_RP 0.175
|
||||
#ifndef STABILIZE_DAMPENER
|
||||
# define STABILIZE_DAMPENER 0.175
|
||||
#endif
|
||||
|
||||
|
||||
|
|
@ -319,7 +307,7 @@
|
|||
// YAW Control
|
||||
//
|
||||
#ifndef YAW_P
|
||||
# define YAW_P 0.8
|
||||
# define YAW_P 0.5
|
||||
#endif
|
||||
#ifndef YAW_I
|
||||
# define YAW_I 0.0
|
||||
|
|
@ -333,8 +321,8 @@
|
|||
|
||||
// STABILZE YAW Control
|
||||
//
|
||||
#ifndef STABILIZE_RATE_YAW
|
||||
# define STABILIZE_RATE_YAW 0.000
|
||||
#ifndef HOLD_YAW_DAMPENER
|
||||
# define HOLD_YAW_DAMPENER 0.175
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@
|
|||
|
||||
#define PLUS_FRAME 0
|
||||
#define X_FRAME 1
|
||||
#define TRI_FRAME 2
|
||||
|
||||
// Auto Pilot modes
|
||||
// ----------------
|
||||
|
|
@ -263,10 +264,9 @@
|
|||
#define EE_GAIN_9 0x80 // all gains stored from here
|
||||
#define EE_GAIN_10 0x88 // all gains stored from here
|
||||
|
||||
#define EE_STAB_RATE_RP 0xA0
|
||||
#define EE_STAB_RATE_YAW 0xA2
|
||||
#define EE_ACRO_RATE_RP 0xA4
|
||||
#define EE_ACRO_RATE_YAW 0xA6
|
||||
#define EE_STAB_DAMPENER 0xA0
|
||||
#define EE_HOLD_YAW_DAMPENER 0xA2
|
||||
|
||||
#define EE_MAG_DECLINATION 0xA8
|
||||
#define EE_MAG_X 0xAA
|
||||
#define EE_MAG_Y 0xAC
|
||||
|
|
|
|||
|
|
@ -44,47 +44,26 @@ float angle_boost()
|
|||
yaw control
|
||||
****************************************************************/
|
||||
|
||||
void input_yaw_hold_2()
|
||||
void output_manual_yaw()
|
||||
{
|
||||
if(rc_3.control_in == 0){
|
||||
// Reset the yaw hold
|
||||
nav_yaw = yaw_sensor;
|
||||
|
||||
}else if(rc_4.control_in == 0){
|
||||
// do nothing
|
||||
|
||||
}else{
|
||||
// create up to 60° of yaw error
|
||||
nav_yaw = yaw_sensor + rc_4.control_in;
|
||||
nav_yaw = wrap_360(nav_yaw);
|
||||
clear_yaw_control();
|
||||
} else {
|
||||
// Yaw control
|
||||
if(rc_4.control_in == 0){
|
||||
//clear_yaw_control();
|
||||
output_yaw_with_hold(true); // hold yaw
|
||||
}else{
|
||||
output_yaw_with_hold(false); // rate control yaw
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void input_yaw_hold()
|
||||
void auto_yaw()
|
||||
{
|
||||
if(rc_3.control_in == 0){
|
||||
// Reset the yaw hold
|
||||
nav_yaw = yaw_sensor;
|
||||
|
||||
}else if(rc_4.control_in == 0){
|
||||
// do nothing
|
||||
|
||||
}else{
|
||||
// create up to 60° of yaw error
|
||||
nav_yaw += ((long)rc_4.control_in * (long)deltaMiliSeconds) / 500; // we'll get 60° * 2 or 120° per second
|
||||
nav_yaw = wrap_360(nav_yaw);
|
||||
}
|
||||
output_yaw_with_hold(true); // hold yaw
|
||||
}
|
||||
|
||||
/*void output_yaw_stabilize()
|
||||
{
|
||||
rc_4.servo_out = rc_4.control_in;
|
||||
rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
|
||||
}*/
|
||||
|
||||
|
||||
|
||||
|
||||
/*************************************************************
|
||||
picth and roll control
|
||||
****************************************************************/
|
||||
|
|
|
|||
|
|
@ -70,12 +70,6 @@ void calc_bearing_error()
|
|||
bearing_error = wrap_180(bearing_error);
|
||||
}
|
||||
|
||||
void calc_yaw_error()
|
||||
{
|
||||
yaw_error = nav_yaw - yaw_sensor;
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
}
|
||||
|
||||
void calc_distance_error()
|
||||
{
|
||||
wp_distance = GPS_wp_distance;
|
||||
|
|
|
|||
|
|
@ -7,14 +7,15 @@ void init_rc_in()
|
|||
rc_2.dead_zone = 60;
|
||||
rc_3.set_range(0,1000);
|
||||
rc_3.dead_zone = 20;
|
||||
rc_3.scale_output = .8;
|
||||
rc_3.scale_output = .9;
|
||||
rc_4.set_angle(6000);
|
||||
rc_4.dead_zone = 500;
|
||||
rc_5.set_range(0,1000);
|
||||
rc_5.set_filter(false);
|
||||
|
||||
// for kP values
|
||||
rc_6.set_range(200,800);
|
||||
//rc_6.set_range(200,800);
|
||||
rc_6.set_range(0,4000);
|
||||
|
||||
// for camera angles
|
||||
//rc_6.set_angle(4500);
|
||||
|
|
@ -133,26 +134,45 @@ void set_servos_4(void)
|
|||
//Serial.printf("yaw: %d ", rc_4.radio_out);
|
||||
|
||||
if(frame_type == PLUS_FRAME){
|
||||
//Serial.println("+");
|
||||
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{
|
||||
|
||||
}else if(frame_type == X_FRAME){
|
||||
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[LEFT] = rc_3.radio_out + roll_out - pitch_out;
|
||||
|
||||
motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out;
|
||||
motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out;
|
||||
}else{
|
||||
/*
|
||||
replace this with Tri-frame control law
|
||||
|
||||
int roll_out = rc_1.pwm_out / 2;
|
||||
int pitch_out = rc_2.pwm_out / 2;
|
||||
|
||||
motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out;
|
||||
motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out;
|
||||
|
||||
motor_out[RIGHT] = 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;
|
||||
|
||||
if((frame_type == PLUS_FRAME) || (frame_type == X_FRAME)){
|
||||
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);
|
||||
|
|
@ -174,9 +194,10 @@ void set_servos_4(void)
|
|||
if (num > 50){
|
||||
num = 0;
|
||||
|
||||
pid_stabilize_roll.kP((float)rc_6.control_in / 1000);
|
||||
stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25;
|
||||
init_pids();
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -101,7 +101,7 @@ void parseCommand(char *buffer)
|
|||
break;
|
||||
|
||||
case 'R':
|
||||
stabilize_rate_roll_pitch = (float)value / 1000;
|
||||
//stabilize_rate_roll_pitch = (float)value / 1000;
|
||||
//save_EEPROM_PID();
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
// Functions called from the setup menu
|
||||
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_esc (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_motors (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_accel (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_accel_flat (uint8_t argc, const Menu::arg *argv);
|
||||
|
|
@ -23,7 +23,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
|
|||
{"reset", setup_factory},
|
||||
{"erase", setup_erase},
|
||||
{"radio", setup_radio},
|
||||
{"esc", setup_esc},
|
||||
{"motors", setup_motors},
|
||||
{"level", setup_accel},
|
||||
{"flat", setup_accel_flat},
|
||||
{"modes", setup_flightmodes},
|
||||
|
|
@ -61,27 +61,33 @@ static int8_t
|
|||
setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
Serial.printf_P(PSTR("\nRadio:\n"));
|
||||
|
||||
print_blanks(10);
|
||||
Serial.printf_P(PSTR("Radio\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
read_EEPROM_radio();
|
||||
print_radio_values();
|
||||
|
||||
|
||||
// frame
|
||||
print_blanks(3);
|
||||
Serial.printf_P(PSTR("Frame\n"));
|
||||
print_divider();
|
||||
|
||||
read_EEPROM_frame();
|
||||
Serial.printf_P(PSTR("\nFrame type:"));
|
||||
Serial.printf_P(PSTR("%d \n"), (int)frame_type);
|
||||
|
||||
if(frame_type == X_FRAME)
|
||||
Serial.printf_P(PSTR(" X\n"));
|
||||
Serial.printf_P(PSTR("X "));
|
||||
else if(frame_type == PLUS_FRAME)
|
||||
Serial.printf_P(PSTR("Plus\n"));
|
||||
Serial.printf_P(PSTR("Plus "));
|
||||
Serial.printf_P(PSTR("(%d)\n"), (int)frame_type);
|
||||
|
||||
|
||||
print_blanks(3);
|
||||
Serial.printf_P(PSTR("Gains\n"));
|
||||
print_divider();
|
||||
|
||||
read_EEPROM_PID();
|
||||
Serial.printf_P(PSTR("\nGains:\n"));
|
||||
// Acro
|
||||
Serial.printf_P(PSTR("Acro:\nroll:\n"));
|
||||
print_PID(&pid_acro_rate_roll);
|
||||
|
|
@ -89,21 +95,18 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
|||
print_PID(&pid_acro_rate_pitch);
|
||||
Serial.printf_P(PSTR("yaw:\n"));
|
||||
print_PID(&pid_acro_rate_yaw);
|
||||
Serial.printf_P(PSTR("rates:\n"));
|
||||
Serial.println(acro_rate_roll_pitch,3);
|
||||
Serial.printf_P(PSTR("rates_yaw:\n"));
|
||||
Serial.println(acro_rate_yaw,3);
|
||||
Serial.println(" ");
|
||||
|
||||
// Stabilize
|
||||
Serial.printf_P(PSTR("Stabilize:\nroll:\n"));
|
||||
Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
|
||||
print_PID(&pid_stabilize_roll);
|
||||
Serial.printf_P(PSTR("pitch:\n"));
|
||||
print_PID(&pid_stabilize_pitch);
|
||||
Serial.printf_P(PSTR("yaw:\n"));
|
||||
print_PID(&pid_yaw);
|
||||
Serial.printf_P(PSTR("Rate stabilize:\n"));
|
||||
Serial.println(stabilize_rate_roll_pitch,3);
|
||||
Serial.println(" ");
|
||||
|
||||
Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener);
|
||||
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener);
|
||||
|
||||
// Nav
|
||||
Serial.printf_P(PSTR("Nav:\npitch:\n"));
|
||||
print_PID(&pid_nav);
|
||||
|
|
@ -111,29 +114,33 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
|||
print_PID(&pid_throttle);
|
||||
Serial.println(" ");
|
||||
|
||||
|
||||
print_blanks(3);
|
||||
Serial.printf_P(PSTR("User Configs\n"));
|
||||
print_divider();
|
||||
// Crosstrack
|
||||
read_EEPROM_nav();
|
||||
Serial.printf_P(PSTR("XTRACK:"));
|
||||
Serial.println(x_track_gain,DEC);
|
||||
Serial.printf_P(PSTR("XTRACK: %4.2f\n"), x_track_gain);
|
||||
Serial.printf_P(PSTR("XTRACK angle: %d\n"), x_track_angle);
|
||||
Serial.printf_P(PSTR("PITCH_MAX: %d\n\n"), pitch_max);
|
||||
|
||||
Serial.printf_P(PSTR("PITCH_MAX: %d\n"), pitch_max);
|
||||
// User Configs
|
||||
read_EEPROM_configs();
|
||||
Serial.printf_P(PSTR("\nUser config:\n"));
|
||||
Serial.printf_P(PSTR("throttle_min: %d\n"), throttle_min);
|
||||
Serial.printf_P(PSTR("throttle_max: %d\n"), throttle_max);
|
||||
Serial.printf_P(PSTR("throttle_cruise: %d\n"), throttle_cruise);
|
||||
Serial.printf_P(PSTR("throttle_failsafe_enabled: %d\n"), throttle_failsafe_enabled);
|
||||
Serial.printf_P(PSTR("throttle_failsafe_value: %d\n"), throttle_failsafe_value);
|
||||
Serial.printf_P(PSTR("log_bitmask: %d\n\n"), log_bitmask);
|
||||
Serial.printf_P(PSTR("log_bitmask: %d\n"), log_bitmask);
|
||||
|
||||
print_blanks(3);
|
||||
Serial.printf_P(PSTR("IMU\n"));
|
||||
print_divider();
|
||||
imu.print_gyro_offsets();
|
||||
imu.print_accel_offsets();
|
||||
|
||||
// mag declination
|
||||
Serial.printf_P(PSTR("\nCompass "));
|
||||
print_blanks(3);
|
||||
Serial.printf_P(PSTR("Compass\n"));
|
||||
print_divider();
|
||||
|
||||
if(compass_enabled)
|
||||
Serial.printf_P(PSTR("en"));
|
||||
else
|
||||
|
|
@ -324,41 +331,94 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
static int8_t
|
||||
setup_esc(uint8_t argc, const Menu::arg *argv)
|
||||
setup_motors(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
Serial.println("\n\nESC Setup - Motors armed");
|
||||
init_rc_in();
|
||||
// read the radio to set trims
|
||||
// ---------------------------
|
||||
trim_radio();
|
||||
|
||||
rc_3.load_eeprom();
|
||||
rc_1.set_filter(false);
|
||||
rc_2.set_filter(false);
|
||||
rc_3.set_filter(false);
|
||||
rc_4.set_filter(false);
|
||||
motor_armed = true;
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
|
||||
int out_min = rc_3.radio_min + 70;
|
||||
|
||||
if(frame_type == PLUS_FRAME){
|
||||
Serial.printf_P(PSTR("PLUS"));
|
||||
}else{
|
||||
Serial.printf_P(PSTR("X"));
|
||||
}
|
||||
Serial.printf_P(PSTR(" Frame\n"));
|
||||
|
||||
while(1){
|
||||
|
||||
delay(20);
|
||||
read_radio();
|
||||
|
||||
APM_RC.OutputCh(CH_1, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_4, rc_3.radio_in);
|
||||
motor_out[LEFT] = rc_3.radio_min;
|
||||
motor_out[BACK] = rc_3.radio_min;
|
||||
motor_out[FRONT] = rc_3.radio_min;
|
||||
motor_out[RIGHT] = rc_3.radio_min;
|
||||
|
||||
|
||||
|
||||
if(frame_type == PLUS_FRAME){
|
||||
if(rc_1.control_in > 0){
|
||||
motor_out[RIGHT] = out_min;
|
||||
Serial.println("0");
|
||||
|
||||
}else if(rc_1.control_in < 0){
|
||||
motor_out[LEFT] = out_min;
|
||||
Serial.println("1");
|
||||
}
|
||||
|
||||
if(rc_2.control_in > 0){
|
||||
motor_out[BACK] = out_min;
|
||||
Serial.println("3");
|
||||
|
||||
}else if(rc_2.control_in < 0){
|
||||
motor_out[FRONT] = out_min;
|
||||
Serial.println("2");
|
||||
}
|
||||
|
||||
}else{
|
||||
// lower right
|
||||
if((rc_1.control_in > 0) && (rc_2.control_in > 0)){
|
||||
motor_out[BACK] = out_min;
|
||||
Serial.println("3");
|
||||
// lower left
|
||||
}else if((rc_1.control_in < 0) && (rc_2.control_in > 0)){
|
||||
motor_out[LEFT] = out_min;
|
||||
Serial.println("1");
|
||||
|
||||
// upper left
|
||||
}else if((rc_1.control_in < 0) && (rc_2.control_in < 0)){
|
||||
motor_out[FRONT] = out_min;
|
||||
Serial.println("2");
|
||||
|
||||
// upper right
|
||||
}else if((rc_1.control_in > 0) && (rc_2.control_in < 0)){
|
||||
motor_out[RIGHT] = out_min;
|
||||
Serial.println("0");
|
||||
}
|
||||
}
|
||||
|
||||
if(rc_3.control_in > 0){
|
||||
APM_RC.OutputCh(CH_1, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_4, rc_3.radio_in);
|
||||
}else{
|
||||
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]);
|
||||
}
|
||||
|
||||
if(Serial.available() > 0){
|
||||
motor_armed = false;
|
||||
rc_1.set_filter(true);
|
||||
rc_2.set_filter(true);
|
||||
rc_3.set_filter(true);
|
||||
rc_4.set_filter(true);
|
||||
read_radio();
|
||||
Serial.println("\n\nMotors disarmed)");
|
||||
break;
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
return(0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
setup_accel(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
@ -408,8 +468,6 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
|
|||
pid_acro_rate_yaw.kD(ACRO_RATE_YAW_D);
|
||||
pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100);
|
||||
|
||||
acro_rate_roll_pitch = ACRO_RATE_RP;
|
||||
acro_rate_yaw = ACRO_RATE_YAW;
|
||||
|
||||
// stabilize, angle error
|
||||
pid_stabilize_roll.kP(STABILIZE_ROLL_P);
|
||||
|
|
@ -422,15 +480,21 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
|
|||
pid_stabilize_pitch.kD(STABILIZE_PITCH_D);
|
||||
pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100);
|
||||
|
||||
// rate control for angle error
|
||||
stabilize_rate_roll_pitch = STABILIZE_RATE_RP;
|
||||
stabilize_rate_yaw = STABILIZE_RATE_YAW;
|
||||
|
||||
// YAW hold
|
||||
pid_yaw.kP(YAW_P);
|
||||
pid_yaw.kI(YAW_I);
|
||||
pid_yaw.kD(YAW_D);
|
||||
pid_yaw.imax(YAW_IMAX * 100);
|
||||
|
||||
|
||||
// custom dampeners
|
||||
// roll pitch
|
||||
stabilize_dampener = STABILIZE_DAMPENER;
|
||||
|
||||
//yaw
|
||||
hold_yaw_dampener = HOLD_YAW_DAMPENER;
|
||||
|
||||
|
||||
// navigation
|
||||
pid_nav.kP(NAV_P);
|
||||
pid_nav.kI(NAV_I);
|
||||
|
|
@ -539,19 +603,26 @@ static int8_t
|
|||
setup_frame(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if(argv[1].i < 1){
|
||||
Serial.printf_P(PSTR("\nUsage:\nPlus frame =>frame 1\nX frame =>frame 2\n\n"));
|
||||
Serial.printf_P(PSTR("\nUsage:\nPlus frame =>frame 1\nX frame =>frame 2\nTRI frame =>frame 3\n\n"));
|
||||
return 0;
|
||||
}
|
||||
Serial.printf_P(PSTR("\nSaving "));
|
||||
|
||||
if(argv[1].i == 1){
|
||||
Serial.printf_P(PSTR("\nSaving Plus frame\n\n"));
|
||||
Serial.printf_P(PSTR("Plus "));
|
||||
frame_type = PLUS_FRAME;
|
||||
|
||||
}else if(argv[1].i == 2){
|
||||
|
||||
Serial.printf_P(PSTR("\nSaving X frame\n\n"));
|
||||
Serial.printf_P(PSTR("X "));
|
||||
frame_type = X_FRAME;
|
||||
|
||||
}else if(argv[1].i == 3){
|
||||
|
||||
Serial.printf_P(PSTR("Tri "));
|
||||
frame_type = X_FRAME;
|
||||
}
|
||||
Serial.printf_P(PSTR("frame\n\n"));
|
||||
save_EEPROM_frame();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -640,14 +711,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
void print_PID(PID * pid)
|
||||
{
|
||||
Serial.printf_P(PSTR("P: "));
|
||||
Serial.print((float)pid->kP(),3);
|
||||
Serial.printf_P(PSTR(",I: "));
|
||||
Serial.print((float)pid->kI(),3);
|
||||
Serial.printf_P(PSTR(",D: "));
|
||||
Serial.print((float)pid->kD(),3);
|
||||
Serial.printf_P(PSTR(",IMAX: "));
|
||||
Serial.println(pid->imax()/100,DEC); // imax is stored as *100 degrees internally
|
||||
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%d\n"), pid->kP(), pid->kI(), pid->kD(), (int)(pid->imax()/100));
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -685,6 +749,15 @@ print_blanks(int num)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
print_divider(void)
|
||||
{
|
||||
for (int i = 0; i < 40; i++) {
|
||||
Serial.print("-");
|
||||
}
|
||||
Serial.println("");
|
||||
}
|
||||
|
||||
|
||||
// for reading in vales for mode switch
|
||||
boolean
|
||||
|
|
|
|||
|
|
@ -2,9 +2,9 @@
|
|||
// are defined below. Order matters to the compiler.
|
||||
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_flaps(uint8_t argc, const Menu::arg *argv);
|
||||
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_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);
|
||||
|
|
@ -40,9 +40,9 @@ static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
|
|||
const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"pwm", test_radio_pwm},
|
||||
{"radio", test_radio},
|
||||
{"flaps", test_flaps},
|
||||
{"stabilize", test_stabilize},
|
||||
{"gps", test_gps},
|
||||
{"adc", test_adc},
|
||||
{"imu", test_imu},
|
||||
{"gyro", test_gyro},
|
||||
{"dcm", test_dcm},
|
||||
|
|
@ -111,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);
|
||||
|
|
@ -147,6 +147,8 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
test_stabilize(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
static byte ts_num;
|
||||
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
|
|
@ -155,15 +157,15 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
|||
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("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP());
|
||||
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
|
||||
|
||||
motor_armed = true;
|
||||
|
||||
trim_radio();
|
||||
|
||||
while(1){
|
||||
// 50 hz
|
||||
if (millis() - fast_loopTimer > 49) {
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
deltaMiliSeconds = millis() - fast_loopTimer;
|
||||
fast_loopTimer = millis();
|
||||
G_Dt = (float)deltaMiliSeconds / 1000.f;
|
||||
|
|
@ -186,16 +188,40 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
|||
// IMU
|
||||
// ---
|
||||
read_AHRS();
|
||||
|
||||
|
||||
// allow us to zero out sensors with control switches
|
||||
if(rc_5.control_in < 600){
|
||||
roll_sensor = pitch_sensor = 0;
|
||||
}
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
update_current_flight_mode();
|
||||
|
||||
//Serial.println(" ");
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
set_servos_4();
|
||||
|
||||
ts_num++;
|
||||
if (ts_num > 10){
|
||||
ts_num = 0;
|
||||
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld, R: %d, L: %d F: %d B: %d\n"),
|
||||
(int)(roll_sensor/100),
|
||||
(int)(pitch_sensor/100),
|
||||
rc_1.pwm_out,
|
||||
rc_2.pwm_out,
|
||||
rc_4.pwm_out,
|
||||
nav_yaw,
|
||||
dcm.yaw_sensor,
|
||||
yaw_error,
|
||||
motor_out[RIGHT],
|
||||
motor_out[LEFT],
|
||||
motor_out[FRONT],
|
||||
motor_out[BACK]);
|
||||
}
|
||||
|
||||
// R: 1417, L: 1453 F: 1453 B: 1417
|
||||
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
|
||||
|
||||
|
|
@ -207,6 +233,26 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_adc(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
adc.Init();
|
||||
delay(1000);
|
||||
Serial.printf_P(PSTR("ADC\n"));
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
for(int i = 0; i < 9; i++){
|
||||
Serial.printf_P(PSTR("i:%d\t"),adc.Ch(i));
|
||||
}
|
||||
Serial.println();
|
||||
delay(20);
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
}
|
||||
static int8_t
|
||||
test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
@ -402,20 +448,38 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
test_omega(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
static byte ts_num;
|
||||
float old_yaw;
|
||||
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
Serial.printf_P(PSTR("Omega"));
|
||||
delay(1000);
|
||||
|
||||
G_Dt = .02;
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
// IMU
|
||||
// ---
|
||||
read_AHRS();
|
||||
float my_oz = (dcm.yaw - old_yaw) * 50;
|
||||
|
||||
old_yaw = dcm.yaw;
|
||||
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
Serial.printf_P(PSTR("R: %d\tP: %d\tY: %d\n"), (int)(ToDeg(omega.x)), (int)(ToDeg(omega.y)), (int)(ToDeg(omega.z)));
|
||||
delay(100);
|
||||
ts_num++;
|
||||
if (ts_num > 2){
|
||||
ts_num = 0;
|
||||
//Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz);
|
||||
Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz);
|
||||
}
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
return (0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
|
|
@ -465,27 +529,6 @@ test_relay(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_flaps(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
delay(300);
|
||||
read_radio();
|
||||
float temp = (float)rc_6.control_in / 1000;
|
||||
|
||||
Serial.print("flaps: ");
|
||||
Serial.println(temp, 3);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue