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

Removed all calls to legacy trim_radio(). Handled by radio setup. 

Added AP_Var Fingerprint checker. Will not let users fly with OOD firmware.
Tweaked Yaw to give better response. Let me know how it goes. It looks fine, but I've not flown it.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@1878 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-14 05:56:39 +00:00
parent 2c3f6052db
commit 9b96a18ad4
7 changed files with 38 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopterMega/ArduCopterMega.pde
View File

@ -43,7 +43,7 @@ version 2.1 of the License, or (at your option) any later version.
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // MAVLink GCS definitions
//#include <GCS_SIMPLE.h>
// Configuration
@ -178,6 +178,7 @@ GPS *g_gps;
GCS_Class gcs;
#endif
//#include <GCS_SIMPLE.h>
//GCS_SIMPLE gcs_simple(&Serial);
AP_RangeFinder_MaxsonarXL sonar;

31
ArduCopterMega/Attitude.pde
View File

@ -139,13 +139,6 @@ output_yaw_with_hold(boolean hold)
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000
// We adjust the output by the rate of rotation
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000
// Limit dampening to be equal to propotional term for symmetry
g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
}else{
// rate control
@ -155,33 +148,29 @@ output_yaw_with_hold(boolean hold)
// -error = CCW, +error = CW
if(g.rc_4.control_in == 0){
// we are breaking;
g.rc_4.servo_out = (omega.z > 0) ? -1800 : 1800;
//g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
//switch comments to get old behavior.
//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 = (omega.z > 0) ? -600 : 600;
g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
}else{
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
}
}
// We adjust the output by the rate of rotation
//long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
//int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000
// Limit dampening to be equal to propotional term for symmetry
//g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
}
g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
}
// Limit Output
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
//Serial.printf("%d\n",g.rc_4.servo_out);
}
// slight left rudder gives right roll.
void
output_rate_roll()
{

8
ArduCopterMega/motors.pde
View File

@ -29,7 +29,6 @@ void arm_motors()
}else{
arming_counter = 0;
}
}else{
arming_counter = 0;
}
@ -55,7 +54,7 @@ set_servos_4()
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + 50;
out_min = g.rc_3.radio_min + 60;
//Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out);
@ -65,6 +64,10 @@ set_servos_4()
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
// limit Yaw control so we don't clip and loose altitude
// this is only a partial solution.
g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
//Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out);
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
@ -241,6 +244,7 @@ set_servos_4()
gcs_simple.write_int((int)nav_roll);
gcs_simple.write_int((int)nav_pitch);
gcs_simple.write_long(current_loc.lat); //28
gcs_simple.write_long(current_loc.lng); //32
gcs_simple.write_int((int)current_loc.alt); //34

6
ArduCopterMega/navigation.pde
View File

@ -54,6 +54,7 @@ void calc_loiter_nav()
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
1000 = 11m
1800 = 1980m = 60 feet
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
@ -89,26 +90,21 @@ void calc_waypoint_nav()
//nav_lat = max(wp_distance, -DIST_ERROR_MAX);
//nav_lat = min(wp_distance, DIST_ERROR_MAX);
//Serial.printf("nav_lat %ld, ", nav_lat);
// Scale response by kP
nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
//Serial.printf("%ld, ",nav_lat);
// get the sin and cos of the bearing error - rotated 90°
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
//Serial.printf("X%2.4f, Y%2.4f ", cos_nav_x, sin_nav_y);
// rotate the vector
nav_roll = (float)nav_lat * cos_nav_x;
nav_pitch = -(float)nav_lat * sin_nav_y;
//Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch);
long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
//Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch);
}
void calc_bearing_error()

5
ArduCopterMega/setup.pde
View File

@ -202,10 +202,6 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
init_rc_in();
// read the radio to set trims
// ---------------------------
trim_radio();
print_hit_enter();
delay(1000);
@ -382,7 +378,6 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter();
trim_radio();
while(1){
delay(20);

18
ArduCopterMega/system.pde
View File

@ -107,7 +107,21 @@ void init_ardupilot()
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
//Serial.println_P(PSTR("done."));
Serial.printf_P(PSTR("Please Run Setup...\n"));
while (true) {
delay(100);
if(motor_light){
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, HIGH);
}else{
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
}
motor_light = !motor_light;
}
}else{
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
@ -245,7 +259,7 @@ void startup_ground(void)
// read the radio to set trims
// ---------------------------
trim_radio();
//trim_radio();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);

10
ArduCopterMega/test.pde
View File

@ -129,10 +129,6 @@ test_radio(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
@ -183,10 +179,6 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
@ -243,8 +235,6 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
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);
trim_radio();
motor_auto_safe = false;
motor_armed = true;