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

ACM : Arducopter.pde

Browse Source 
command_nav_index upped to in16t for negative number
renamed trim flag > CH7_flag
Added fast_corner var
removed unused alt vars
Alt hold I term update
This commit is contained in:
Jason Short 2012-08-09 16:44:21 -07:00
parent 4a56e8ea2c
commit 02649f3f11
1 changed files with 29 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
ArduCopter/ArduCopter.pde
View File

@ -544,7 +544,7 @@ static int32_t target_bearing;
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE // NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control; static byte wp_control;
// Register containing the index of the current navigation command in the mission script // Register containing the index of the current navigation command in the mission script
static uint8_t command_nav_index; static int16_t command_nav_index;
// Register containing the index of the previous navigation command in the mission script // Register containing the index of the previous navigation command in the mission script
// Used to manage the execution of conditional commands // Used to manage the execution of conditional commands
static uint8_t prev_nav_index; static uint8_t prev_nav_index;
@ -632,8 +632,8 @@ static struct Location circle_WP;
static bool do_flip = false; static bool do_flip = false;
// Used to track the CH7 toggle state. // Used to track the CH7 toggle state.
// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true // When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
// This allows advanced functionality to know when to execute // Allows advanced functionality to know when to execute
static boolean trim_flag; static boolean CH7_flag;
// This register tracks the current Mission Command index when writing // This register tracks the current Mission Command index when writing
// a mission using CH7 in flight // a mission using CH7 in flight
static int8_t CH7_wp_index; static int8_t CH7_wp_index;
@ -710,26 +710,6 @@ static int16_t ground_detector;
// have we reached our desired altitude brefore heading home? // have we reached our desired altitude brefore heading home?
static bool rtl_reached_alt; static bool rtl_reached_alt;
////////////////////////////////////////////////////////////////////////////////
// Toy Mode - THOR
////////////////////////////////////////////////////////////////////////////////
#define TOY_LOOKUP 0
#define TOY_DELAY 150 // Equal to 1.5 s at 100hz
static uint8_t toy_input_timer; // A delay timer to engage loiter or WP mode
static int16_t toy_speed; // TO remember how fast we are going when we enage WP mode
#if TOY_LOOKUP == 1
static const int16_t toy_lookup[] = {
186, 373, 558, 745,
372, 745, 1117, 1490,
558, 1118, 1675, 2235,
743, 1490, 2233, 2980,
929, 1863, 2792, 3725,
1115, 2235, 3350, 4470,
1301, 2608, 3908, 4500,
1487, 2980, 4467, 4500,
1673, 3353, 4500, 4500};
#endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Navigation general // Navigation general
@ -763,6 +743,9 @@ static struct Location command_cond_queue;
// Holds the current loaded command from the EEPROM for guided mode // Holds the current loaded command from the EEPROM for guided mode
static struct Location guided_WP; static struct Location guided_WP;
// should we take the waypoint quickly or slow down?
static bool fast_corner;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Crosstrack // Crosstrack
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -809,13 +792,8 @@ static uint32_t throttle_integrator;
// Climb rate control // Climb rate control
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Time when we intiated command in millis - used for controlling decent rate // Time when we intiated command in millis - used for controlling decent rate
// The orginal altitude used to base our new altitude during decent
static int32_t original_altitude;
// Used to track the altitude offset for climbrate control // Used to track the altitude offset for climbrate control
static int32_t target_altitude;
static uint32_t alt_change_timer;
static int8_t alt_change_flag; static int8_t alt_change_flag;
static uint32_t alt_change;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Navigation Yaw control // Navigation Yaw control
@ -1163,11 +1141,14 @@ static void medium_loop()
case 2: case 2:
medium_loopCounter++; medium_loopCounter++;
// Read altitude from sensors if(control_mode == TOY){
// -------------------------- update_toy_throttle();
//#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
//update_altitude(); if(throttle_mode == THROTTLE_AUTO){
//#endif update_toy_altitude();
}
}
alt_sensor_flag = true; alt_sensor_flag = true;
break; break;
@ -1257,6 +1238,10 @@ static void fifty_hz_loop()
} }
#endif #endif
#if TOY_EDF == ENABLED
edf_toy();
#endif
// syncronise optical flow reads with altitude reads // syncronise optical flow reads with altitude reads
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
if(g.optflow_enabled){ if(g.optflow_enabled){
@ -1575,6 +1560,11 @@ void update_yaw_mode(void)
return; return;
break; break;
// update to allow external roll/yaw mixing
#if TOY_LOOKUP == TOY_EXTERNAL_MIXER
case YAW_TOY:
#endif
case YAW_HOLD: case YAW_HOLD:
if(g.rc_4.control_in != 0){ if(g.rc_4.control_in != 0){
g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in);
@ -1602,23 +1592,16 @@ void update_yaw_mode(void)
case YAW_LOOK_AT_HOME: case YAW_LOOK_AT_HOME:
//nav_yaw updated in update_navigation() //nav_yaw updated in update_navigation()
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break; break;
case YAW_AUTO: case YAW_AUTO:
nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second
//Serial.printf("nav_yaw %d ", nav_yaw); //Serial.printf("nav_yaw %d ", nav_yaw);
nav_yaw = wrap_360(nav_yaw); nav_yaw = wrap_360(nav_yaw);
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break; break;
case YAW_TOY:
return;
} }
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
//Serial.printf("4: %d\n",g.rc_4.servo_out);
} }
void update_roll_pitch_mode(void) void update_roll_pitch_mode(void)
@ -1626,7 +1609,7 @@ void update_roll_pitch_mode(void)
int control_roll, control_pitch; int control_roll, control_pitch;
if (do_flip){ if (do_flip){
if(abs(g.rc_1.control_in) < 2000){ if(abs(g.rc_1.control_in) < 4000){
roll_flip(); roll_flip();
return; return;
}else{ }else{
@ -1845,7 +1828,6 @@ void update_throttle_mode(void)
// calculate angle boost // calculate angle boost
angle_boost = get_angle_boost(g.throttle_cruise); angle_boost = get_angle_boost(g.throttle_cruise);
// 10hz
if(motors.auto_armed() == true){ if(motors.auto_armed() == true){
// how far off are we // how far off are we
@ -1854,6 +1836,7 @@ void update_throttle_mode(void)
int16_t desired_speed; int16_t desired_speed;
if(alt_change_flag == REACHED_ALT){ // we are at or above the target alt if(alt_change_flag == REACHED_ALT){ // we are at or above the target alt
desired_speed = g.pi_alt_hold.get_p(altitude_error); // calculate desired speed from lon error desired_speed = g.pi_alt_hold.get_p(altitude_error); // calculate desired speed from lon error
update_throttle_cruise(g.pi_alt_hold.get_i(altitude_error, .02));
desired_speed = constrain(desired_speed, -250, 250); desired_speed = constrain(desired_speed, -250, 250);
nav_throttle = get_throttle_rate(desired_speed); nav_throttle = get_throttle_rate(desired_speed);
}else{ }else{
@ -2102,9 +2085,9 @@ static void update_trig(void){
static void update_altitude() static void update_altitude()
{ {
static int16_t old_sonar_alt = 0; static int16_t old_sonar_alt = 0;
static int32_t old_baro_alt = 0;
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
static int32_t old_baro_alt = 0;
// we are in the SIM, fake out the baro and Sonar // we are in the SIM, fake out the baro and Sonar
int fake_relative_alt = g_gps->altitude - gps_base_alt; int fake_relative_alt = g_gps->altitude - gps_base_alt;
baro_alt = fake_relative_alt; baro_alt = fake_relative_alt;
@ -2189,7 +2172,7 @@ static void update_altitude()
} }
// update the target altitude // update the target altitude
next_WP.alt = get_new_altitude(); verify_altitude();
// calc error // calc error
climb_rate_error = (climb_rate_actual - climb_rate) / 5; climb_rate_error = (climb_rate_actual - climb_rate) / 5;
@ -2453,9 +2436,6 @@ static void update_nav_wp()
// use error as the desired rate towards the target // use error as the desired rate towards the target
calc_nav_rate(speed); calc_nav_rate(speed);
}else if(wp_control == TOY_MODE){ // THOR added to navigate to Virtual WP
calc_nav_rate(toy_speed);
}else if(wp_control == NO_NAV_MODE){ }else if(wp_control == NO_NAV_MODE){
// clear out our nav so we can do things like land straight down // clear out our nav so we can do things like land straight down
// or change Loiter position // or change Loiter position