Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Ap state updates

Browse Source
This commit is contained in:
Jason Short 2012-11-09 21:55:51 -08:00
parent 4d7b9137fe
commit d2a5928c06
18 changed files with 314 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/GCS_Mavlink.pde
View File

@ -1732,7 +1732,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
current_loc.lng = g_gps->longitude; current_loc.lng = g_gps->longitude;
current_loc.lat = g_gps->latitude; current_loc.lat = g_gps->latitude;
current_loc.alt = g_gps->altitude - gps_base_alt; current_loc.alt = g_gps->altitude - gps_base_alt;
if (!home_is_set) { if (!ap.home_is_set) {
init_home(); init_home();
} }

13
ArduCopter/Parameters.h
View File

@ -69,6 +69,8 @@ public:
// Yaw Rate 1 = fast, // Yaw Rate 1 = fast,
// 2 = med, 3 = slow // 2 = med, 3 = slow
k_param_crosstrack_min_distance,
// 65: AP_Limits Library // 65: AP_Limits Library
k_param_limits = 65, k_param_limits = 65,
k_param_gpslock_limit, k_param_gpslock_limit,
@ -198,9 +200,9 @@ public:
// //
// 220: PI/D Controllers // 220: PI/D Controllers
// //
k_param_stabilize_d_schedule = 219, //k_param_stabilize_d_schedule = 219,
k_param_stabilize_d = 220, //k_param_stabilize_d = 220,
k_param_acro_p, k_param_acro_p = 221,
k_param_axis_lock_p, k_param_axis_lock_p,
k_param_pid_rate_roll, k_param_pid_rate_roll,
k_param_pid_rate_pitch, k_param_pid_rate_pitch,
@ -269,6 +271,7 @@ public:
AP_Int16 loiter_radius; AP_Int16 loiter_radius;
AP_Int16 waypoint_speed_max; AP_Int16 waypoint_speed_max;
AP_Float crosstrack_gain; AP_Float crosstrack_gain;
AP_Int16 crosstrack_min_distance;
AP_Int32 auto_land_timeout; AP_Int32 auto_land_timeout;
@ -342,8 +345,8 @@ public:
#endif #endif
AP_Int16 rc_speed; // speed of fast RC Channels in Hz AP_Int16 rc_speed; // speed of fast RC Channels in Hz
AP_Float stabilize_d; //AP_Float stabilize_d;
AP_Float stabilize_d_schedule; //AP_Float stabilize_d_schedule;
// Acro parameters // Acro parameters
AP_Float acro_p; AP_Float acro_p;

14
ArduCopter/Parameters.pde
View File

@ -166,6 +166,16 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Units: Dimensionless // @Units: Dimensionless
// @User: Standard // @User: Standard
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", CROSSTRACK_GAIN), GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", CROSSTRACK_GAIN),
// @Param: XTRK_MIN_DIST
// @DisplayName: Crosstrack mininum distance
// @Description: Minimum distance in meters between waypoints to do crosstrack correction.
// @Units: Meters
// @Range: 0 32767
// @Increment: 1
// @User: Standard
GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST", CROSSTRACK_MIN_DISTANCE),
GSCALAR(auto_land_timeout, "AUTO_LAND", AUTO_LAND_TIME*1000), GSCALAR(auto_land_timeout, "AUTO_LAND", AUTO_LAND_TIME*1000),
// @Param: THR_MIN // @Param: THR_MIN
@ -297,7 +307,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// variable // variable
//--------- //---------
GSCALAR(stabilize_d, "STAB_D", STABILIZE_D), //GSCALAR(stabilize_d, "STAB_D", STABILIZE_D),
// @Param: STAB_D_S // @Param: STAB_D_S
// @DisplayName: Stabilize D Schedule // @DisplayName: Stabilize D Schedule
@ -305,7 +315,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 1 // @Range: 0 1
// @Increment: .01 // @Increment: .01
// @User: Advanced // @User: Advanced
GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE), //GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE),
// Acro parameters // Acro parameters
GSCALAR(acro_p, "ACRO_P", ACRO_P), GSCALAR(acro_p, "ACRO_P", ACRO_P),

14
ArduCopter/commands.pde
View File

@ -2,14 +2,14 @@
static void init_commands() static void init_commands()
{ {
g.command_index = NO_COMMAND; g.command_index = NO_COMMAND;
command_nav_index = NO_COMMAND; command_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND; command_cond_index = NO_COMMAND;
prev_nav_index = NO_COMMAND; prev_nav_index = NO_COMMAND;
command_cond_queue.id = NO_COMMAND; command_cond_queue.id = NO_COMMAND;
command_nav_queue.id = NO_COMMAND; command_nav_queue.id = NO_COMMAND;
fast_corner = false; ap.fast_corner = false;
reset_desired_speed(); reset_desired_speed();
// default Yaw tracking // default Yaw tracking
@ -162,7 +162,7 @@ static void set_next_WP(struct Location *wp)
set_new_altitude(next_WP.alt); set_new_altitude(next_WP.alt);
// this is used to offset the shrinking longitude as we go towards the poles // this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads); scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads); scaleLongUp = 1.0f/cos(rads);
@ -184,7 +184,7 @@ static void set_next_WP(struct Location *wp)
// ------------------------------- // -------------------------------
static void init_home() static void init_home()
{ {
home_is_set = true; set_home_is_set(true);
home.id = MAV_CMD_NAV_WAYPOINT; home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7 home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7 home.lat = g_gps->latitude; // Lat * 10**7

9
ArduCopter/commands_logic.pde
View File

@ -278,7 +278,7 @@ static void do_land()
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
// just to make sure // just to make sure
land_complete = false; set_land_complete(false);
// landing boost lowers the main throttle to mimmick // landing boost lowers the main throttle to mimmick
// the effect of a user's hand // the effect of a user's hand
@ -385,7 +385,7 @@ static bool verify_land_sonar()
if(ground_detector < 30) { if(ground_detector < 30) {
ground_detector++; ground_detector++;
}else if (ground_detector == 30) { }else if (ground_detector == 30) {
land_complete = true; set_land_complete(true);
if(g.rc_3.control_in == 0) { if(g.rc_3.control_in == 0) {
ground_detector++; ground_detector++;
init_disarm_motors(); init_disarm_motors();
@ -419,7 +419,7 @@ static bool verify_land_baro()
if(ground_detector < 30) { if(ground_detector < 30) {
ground_detector++; ground_detector++;
}else if (ground_detector == 30) { }else if (ground_detector == 30) {
land_complete = true; set_land_complete(true);
if(g.rc_3.control_in == 0) { if(g.rc_3.control_in == 0) {
ground_detector++; ground_detector++;
init_disarm_motors(); init_disarm_motors();
@ -792,7 +792,8 @@ static void do_set_home()
home.lng = command_cond_queue.lng; // Lon * 10**7 home.lng = command_cond_queue.lng; // Lon * 10**7
home.lat = command_cond_queue.lat; // Lat * 10**7 home.lat = command_cond_queue.lat; // Lat * 10**7
home.alt = 0; home.alt = 0;
home_is_set = true; //home_is_set = true;
set_home_is_set(true);
} }
} }

8
ArduCopter/commands_process.pde
View File

@ -77,16 +77,16 @@ static void update_commands()
Location tmp_loc = get_cmd_with_index(tmp_index); Location tmp_loc = get_cmd_with_index(tmp_index);
if(tmp_loc.lat == 0) { if(tmp_loc.lat == 0) {
fast_corner = false; ap.fast_corner = false;
}else{ }else{
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing; int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
temp = wrap_180(temp); temp = wrap_180(temp);
fast_corner = labs(temp) < 6000; ap.fast_corner = labs(temp) < 6000;
} }
// If we try and stop at a corner, lets reset our desired speed to prevent // If we try and stop at a corner, lets reset our desired speed to prevent
// too much jerkyness. // too much jerkyness.
if(false == fast_corner){ if(false == ap.fast_corner){
reset_desired_speed(); reset_desired_speed();
} }
} }
@ -213,7 +213,7 @@ static void exit_mission()
g.command_index = 255; g.command_index = 255;
// if we are on the ground, enter stabilize, else Land // if we are on the ground, enter stabilize, else Land
if(land_complete == true) { if(ap.land_complete) {
// we will disarm the motors after landing. // we will disarm the motors after landing.
}else{ }else{
// If the approach altitude is valid (above 1m), do approach, else land // If the approach altitude is valid (above 1m), do approach, else land

30
ArduCopter/control_modes.pde
View File

@ -16,7 +16,8 @@ static void read_control_switch()
if(g.ch7_option != CH7_SIMPLE_MODE) { if(g.ch7_option != CH7_SIMPLE_MODE) {
// set Simple mode using stored paramters from Mission planner // set Simple mode using stored paramters from Mission planner
// rather than by the control switch // rather than by the control switch
do_simple = (g.simple_modes & (1 << switchPosition)); //ap.simple_mode = (g.simple_modes & (1 << switchPosition));
set_simple_mode(g.simple_modes & (1 << switchPosition));
} }
}else{ }else{
switch_debouncer = true; switch_debouncer = true;
@ -60,29 +61,30 @@ static void read_trim_switch()
} }
if(option == CH7_SIMPLE_MODE) { if(option == CH7_SIMPLE_MODE) {
do_simple = (g.rc_7.radio_in > CH_7_PWM_TRIGGER); //ap.simple_mode = (g.rc_7.radio_in > CH_7_PWM_TRIGGER);
set_simple_mode(g.rc_7.radio_in > CH_7_PWM_TRIGGER);
}else if (option == CH7_FLIP) { }else if (option == CH7_FLIP) {
if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) { if (system.CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
CH7_flag = true; system.CH7_flag = true;
// don't flip if we accidentally engaged flip, but didn't notice and tried to takeoff // don't flip if we accidentally engaged flip, but didn't notice and tried to takeoff
if(g.rc_3.control_in != 0 && takeoff_complete) { if(g.rc_3.control_in != 0 && ap.takeoff_complete) {
init_flip(); init_flip();
} }
} }
if (CH7_flag == true && g.rc_7.control_in < 800) { if (system.CH7_flag == true && g.rc_7.control_in < 800) {
CH7_flag = false; system.CH7_flag = false;
} }
}else if (option == CH7_RTL) { }else if (option == CH7_RTL) {
if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) { if (system.CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
CH7_flag = true; system.CH7_flag = true;
set_mode(RTL); set_mode(RTL);
} }
if (CH7_flag == true && g.rc_7.control_in < 800) { if (system.CH7_flag == true && g.rc_7.control_in < 800) {
CH7_flag = false; system.CH7_flag = false;
if (control_mode == RTL || control_mode == LOITER) { if (control_mode == RTL || control_mode == LOITER) {
reset_control_switch(); reset_control_switch();
} }
@ -90,11 +92,11 @@ static void read_trim_switch()
}else if (option == CH7_SAVE_WP) { }else if (option == CH7_SAVE_WP) {
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) { // switch is engaged if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) { // switch is engaged
CH7_flag = true; system.CH7_flag = true;
}else{ // switch is disengaged }else{ // switch is disengaged
if(CH7_flag) { if(system.CH7_flag) {
CH7_flag = false; system.CH7_flag = false;
if(control_mode == AUTO) { if(control_mode == AUTO) {
// reset the mission // reset the mission

74
ArduCopter/defines.h
View File

@ -111,13 +111,13 @@
#define RC_CHANNEL_ANGLE_RAW 2 #define RC_CHANNEL_ANGLE_RAW 2
// HIL enumerations // HIL enumerations
#define HIL_MODE_DISABLED 0 #define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1 #define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2 #define HIL_MODE_SENSORS 2
#define ASCENDING 1
#define DESCENDING -1
#define REACHED_ALT 0 #define REACHED_ALT 0
#define DESCENDING 1
#define ASCENDING 2
// Auto Pilot modes // Auto Pilot modes
// ---------------- // ----------------
@ -131,10 +131,10 @@
#define CIRCLE 7 // AUTO control #define CIRCLE 7 // AUTO control
#define POSITION 8 // AUTO control #define POSITION 8 // AUTO control
#define LAND 9 // AUTO control #define LAND 9 // AUTO control
#define OF_LOITER 10 // Hold a single location using optical flow #define OF_LOITER 10 // Hold a single location using optical flow
// sensor // sensor
#define TOY_A 11 // THOR Enum for Toy mode #define TOY_A 11 // THOR Enum for Toy mode
#define TOY_M 12 // THOR Enum for Toy mode #define TOY_M 12 // THOR Enum for Toy mode
#define NUM_MODES 13 #define NUM_MODES 13
#define SIMPLE_1 1 #define SIMPLE_1 1
@ -225,12 +225,12 @@
// Waypoint options // Waypoint options
#define MASK_OPTIONS_RELATIVE_ALT 1 #define MASK_OPTIONS_RELATIVE_ALT 1
#define WP_OPTION_ALT_CHANGE 2 #define WP_OPTION_ALT_CHANGE 2
#define WP_OPTION_YAW 4 #define WP_OPTION_YAW 4
#define WP_OPTION_ALT_REQUIRED 8 #define WP_OPTION_ALT_REQUIRED 8
#define WP_OPTION_RELATIVE 16 #define WP_OPTION_RELATIVE 16
//#define WP_OPTION_ 32 //#define WP_OPTION_ 32
//#define WP_OPTION_ 64 //#define WP_OPTION_ 64
#define WP_OPTION_NEXT_CMD 128 #define WP_OPTION_NEXT_CMD 128
//repeating events //repeating events
#define NO_REPEAT 0 #define NO_REPEAT 0
@ -303,8 +303,8 @@ enum gcs_severity {
#define LOG_INDEX_MSG 0xF0 #define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50 #define MAX_NUM_LOGS 50
#define MASK_LOG_ATTITUDE_FAST (1<<0) #define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1) #define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2) #define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3) #define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4) #define MASK_LOG_CTUN (1<<4)
@ -320,6 +320,52 @@ enum gcs_severity {
#define MASK_LOG_INAV (1<<14) #define MASK_LOG_INAV (1<<14)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_MODE (1<<6)
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
#define MASK_LOG_MOTORS (1<<10)
#define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_PID (1<<12)
#define MASK_LOG_ITERM (1<<13)
// DATA - event logging
#define DATA_MAVLINK_FLOAT 1
#define DATA_MAVLINK_INT32 2
#define DATA_MAVLINK_INT16 3
#define DATA_MAVLINK_INT8 4
#define DATA_FAST_LOOP 5
#define DATA_MED_LOOP 6
#define DATA_AP_STATE 7
#define DATA_SIMPLE_BEARING 8
#define DATA_INIT_SIMPLE_BEARING 9
#define DATA_ARMED 10
#define DATA_DISARMED 11
#define DATA_FAILSAFE_ON 12
#define DATA_FAILSAFE_OFF 13
#define DATA_LOW_BATTERY 14
#define DATA_AUTO_ARMED 15
#define DATA_TAKEOFF 16
#define DATA_DID_REACH_ALT 17
#define DATA_LAND_COMPLETE 18
#define DATA_LOST_GPS 19
#define DATA_LOST_COMPASS 20
#define DATA_BEGIN_FLIP 21
#define DATA_END_FLIP 22
#define DATA_EXIT_FLIP 23
#define DATA_FLIP_ABORTED 24
#define DATA_SET_HOME 25
#define DATA_SET_SIMPLE_ON 26
#define DATA_SET_SIMPLE_OFF 27
#define DATA_REACHED_ALT 28
#define DATA_ASCENDING 29
#define DATA_DESCENDING 30
#define DATA_RTL_REACHED_ALT 31
// Waypoint Modes // Waypoint Modes
// ---------------- // ----------------
#define ABS_WP 0 #define ABS_WP 0

5
ArduCopter/events.pde
View File

@ -22,7 +22,7 @@ static void failsafe_on_event()
break; break;
default: default:
if(home_is_set == true) { if(ap.home_is_set) {
// same as above ^ // same as above ^
// do_rtl sets the altitude to the current altitude by default // do_rtl sets the altitude to the current altitude by default
set_mode(RTL); set_mode(RTL);
@ -73,8 +73,7 @@ static void low_battery_event(void)
last_low_battery_message = tnow; last_low_battery_message = tnow;
} }
low_batt = true; set_low_battery(true);
// if we are in Auto mode, come home // if we are in Auto mode, come home
if(control_mode >= AUTO) if(control_mode >= AUTO)
set_mode(RTL); set_mode(RTL);

7
ArduCopter/failsafe.pde
View File

@ -27,7 +27,7 @@ void failsafe_disable()
{ {
failsafe_enabled = false; failsafe_enabled = false;
} }
// //
// failsafe_check - this function is called from the core timer interrupt at 1kHz. // failsafe_check - this function is called from the core timer interrupt at 1kHz.
// //
@ -42,7 +42,7 @@ void failsafe_check(uint32_t tnow)
} }
if (failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) { if (failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
// motors are running but we have gone 2 second since the // motors are running but we have gone 2 second since the
// main loop ran. That means we're in trouble and should // main loop ran. That means we're in trouble and should
// disarm the motors. // disarm the motors.
in_failsafe = true; in_failsafe = true;
@ -51,8 +51,9 @@ void failsafe_check(uint32_t tnow)
if (in_failsafe && tnow - failsafe_last_timestamp > 1000000) { if (in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
// disarm motors every second // disarm motors every second
failsafe_last_timestamp = tnow; failsafe_last_timestamp = tnow;
if( motors.armed() ) { if(motors.armed()) {
motors.armed(false); motors.armed(false);
set_armed(true);
motors.output(); motors.output();
} }
} }

8
ArduCopter/flip.pde
View File

@ -17,10 +17,11 @@ static int8_t flip_dir;
void init_flip() void init_flip()
{ {
if(do_flip == false) { if(false == ap.do_flip) {
do_flip = true; ap.do_flip = true;
flip_state = 0; flip_state = 0;
flip_dir = (ahrs.roll_sensor >= 0) ? 1 : -1; flip_dir = (ahrs.roll_sensor >= 0) ? 1 : -1;
Log_Write_Event(DATA_BEGIN_FLIP);
} }
} }
@ -61,7 +62,8 @@ void roll_flip()
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
flip_timer++; flip_timer++;
}else{ }else{
do_flip = false; Log_Write_Event(DATA_END_FLIP);
ap.do_flip = false;
flip_state = 0; flip_state = 0;
} }
break; break;

64
ArduCopter/leds.pde
View File

@ -19,7 +19,7 @@ static void update_GPS_light(void)
switch (g_gps->status()) { switch (g_gps->status()) {
case (2): case (2):
if(home_is_set) { // JLN update if(ap.home_is_set) { // JLN update
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set. digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
} else { } else {
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF);
@ -28,8 +28,8 @@ static void update_GPS_light(void)
case (1): case (1):
if (g_gps->valid_read == true) { if (g_gps->valid_read == true) {
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock system.GPS_light = !system.GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light) { if (system.GPS_light) {
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF);
}else{ }else{
digitalWrite(C_LED_PIN, LED_ON); digitalWrite(C_LED_PIN, LED_ON);
@ -47,17 +47,17 @@ static void update_GPS_light(void)
static void update_motor_light(void) static void update_motor_light(void)
{ {
if(motors.armed() == false) { if(motors.armed() == false) {
motor_light = !motor_light; system.motor_light = !system.motor_light;
// blink // blink
if(motor_light) { if(system.motor_light) {
digitalWrite(A_LED_PIN, LED_ON); digitalWrite(A_LED_PIN, LED_ON);
}else{ }else{
digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(A_LED_PIN, LED_OFF);
} }
}else{ }else{
if(!motor_light) { if(!system.motor_light) {
motor_light = true; system.motor_light = true;
digitalWrite(A_LED_PIN, LED_ON); digitalWrite(A_LED_PIN, LED_ON);
} }
} }
@ -93,7 +93,7 @@ static void clear_leds()
digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_OFF); digitalWrite(B_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF);
motor_light = false; system.motor_light = false;
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
} }
@ -121,28 +121,28 @@ static void clear_leds()
static void update_copter_leds(void) static void update_copter_leds(void)
{ {
if (g.copter_leds_mode == 0) { if (g.copter_leds_mode == 0) {
copter_leds_reset(); //method of reintializing LED state copter_leds_reset(); //method of reintializing LED state
} }
if ( bitRead(g.copter_leds_mode, 0) ) { if ( bitRead(g.copter_leds_mode, 0) ) {
if (motors.armed() == true) { if (motors.armed() == true) {
if (low_batt == true) { if (ap.low_battery == true) {
if ( bitRead(g.copter_leds_mode, 4 )) { if ( bitRead(g.copter_leds_mode, 4 )) {
copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink
} else { } else {
copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate
} }
} else if ( !bitRead(g.copter_leds_mode, 5 ) ) { } else if ( !bitRead(g.copter_leds_mode, 5 ) ) {
copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
} else if ( bitRead(g.copter_leds_mode, 5 ) ) { } else if ( bitRead(g.copter_leds_mode, 5 ) ) {
if ( copter_leds_nav_blink >0 ) { if ( copter_leds_nav_blink >0 ) {
copter_leds_slow_blink(); //if nav command was seen, blink LEDs. copter_leds_slow_blink(); //if nav command was seen, blink LEDs.
} else { } else {
copter_leds_on(); copter_leds_on();
} }
} }
} else { } else {
copter_leds_slow_blink(); //if motors are not armed, blink motor leds copter_leds_slow_blink(); //if motors are not armed, blink motor leds
} }
} }
@ -153,38 +153,38 @@ static void update_copter_leds(void)
switch (g_gps->status()) { switch (g_gps->status()) {
case (2): case (2):
if(home_is_set) { if(ap.home_is_set) {
if ( !bitRead(g.copter_leds_mode, 6 ) ) { if ( !bitRead(g.copter_leds_mode, 6 ) ) {
copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
} else if (bitRead(g.copter_leds_mode, 6 ) ) { } else if (bitRead(g.copter_leds_mode, 6 ) ) {
if ( copter_leds_nav_blink >0 ) { if ( copter_leds_nav_blink >0 ) {
copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs. copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs.
} else { } else {
copter_leds_GPS_on(); copter_leds_GPS_on();
} }
} }
} else { } else {
copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast
} }
break; break;
case (1): case (1):
copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow
break; break;
default: default:
copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off
break; break;
} }
} }
if ( bitRead(g.copter_leds_mode, 2) ) { if ( bitRead(g.copter_leds_mode, 2) ) {
if (200 <= g.rc_7.control_in && g.rc_7.control_in < 400) { if (200 <= g.rc_7.control_in && g.rc_7.control_in < 400) {
copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on
} else if (g.rc_7.control_in < 200) { } else if (g.rc_7.control_in < 200) {
copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off
} }
} }
@ -308,7 +308,7 @@ static void copter_leds_oscillate(void) {
digitalWrite(COPTER_LED_7, COPTER_LED_ON); digitalWrite(COPTER_LED_7, COPTER_LED_ON);
digitalWrite(COPTER_LED_8, COPTER_LED_ON); digitalWrite(COPTER_LED_8, COPTER_LED_ON);
} }
else copter_leds_motor_blink = 0; // start blink cycle again else copter_leds_motor_blink = 0; // start blink cycle again
} }
@ -322,26 +322,26 @@ static void copter_leds_GPS_off(void) {
} }
static void copter_leds_GPS_slow_blink(void) { static void copter_leds_GPS_slow_blink(void) {
copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop
if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds
copter_leds_GPS_off(); copter_leds_GPS_off();
if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker... if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker...
copter_leds_nav_blink--; // decrement the Nav Blink counter copter_leds_nav_blink--; // decrement the Nav Blink counter
} }
}else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) { }else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) {
copter_leds_GPS_on(); copter_leds_GPS_on();
} }
else copter_leds_GPS_blink = 0; // start blink cycle again else copter_leds_GPS_blink = 0; // start blink cycle again
} }
static void copter_leds_GPS_fast_blink(void) { static void copter_leds_GPS_fast_blink(void) {
copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop
if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
copter_leds_GPS_off(); copter_leds_GPS_off();
}else if (2 < copter_leds_GPS_blink && copter_leds_GPS_blink < 5) { }else if (2 < copter_leds_GPS_blink && copter_leds_GPS_blink < 5) {
copter_leds_GPS_on(); copter_leds_GPS_on();
} }
else copter_leds_GPS_blink = 0; // start blink cycle again else copter_leds_GPS_blink = 0; // start blink cycle again
} }
static void copter_leds_aux_off(void){ static void copter_leds_aux_off(void){
@ -360,7 +360,7 @@ void piezo_off(){
digitalWrite(PIEZO_PIN,LOW); digitalWrite(PIEZO_PIN,LOW);
} }
void piezo_beep(){ // Note! This command should not be used in time sensitive loops void piezo_beep(){ // Note! This command should not be used in time sensitive loops
piezo_on(); piezo_on();
delay(100); delay(100);
piezo_off(); piezo_off();

15
ArduCopter/motors.pde
View File

@ -87,6 +87,7 @@ static void arm_motors()
static void init_arm_motors() static void init_arm_motors()
{ {
// arming marker
// Flag used to track if we have armed the motors the first time. // Flag used to track if we have armed the motors the first time.
// This is used to decide if we should run the ground_start routine // This is used to decide if we should run the ground_start routine
// which calibrates the IMU // which calibrates the IMU
@ -122,7 +123,7 @@ static void init_arm_motors()
// Reset home position // Reset home position
// ------------------- // -------------------
if(home_is_set) if(ap.home_is_set)
init_home(); init_home();
// all I terms are invalid // all I terms are invalid
@ -142,7 +143,7 @@ static void init_arm_motors()
#endif #endif
// temp hack // temp hack
motor_light = true; system.motor_light = true;
digitalWrite(A_LED_PIN, LED_ON); digitalWrite(A_LED_PIN, LED_ON);
// go back to normal AHRS gains // go back to normal AHRS gains
@ -153,6 +154,7 @@ static void init_arm_motors()
// finally actually arm the motors // finally actually arm the motors
motors.armed(true); motors.armed(true);
set_armed(true);
// reenable failsafe // reenable failsafe
failsafe_enable(); failsafe_enable();
@ -161,12 +163,19 @@ static void init_arm_motors()
static void init_disarm_motors() static void init_disarm_motors()
{ {
// disarming marker
//Serial.printf("\nDISARM\n"); //Serial.printf("\nDISARM\n");
#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD) #if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif #endif
motors.armed(false); motors.armed(false);
set_armed(false);
motors.auto_armed(false);
set_auto_armed(false);
compass.save_offsets(); compass.save_offsets();
g.throttle_cruise.save(); g.throttle_cruise.save();
@ -176,7 +185,7 @@ static void init_disarm_motors()
#endif #endif
// we are not in the air // we are not in the air
takeoff_complete = false; set_takeoff_complete(false);
#if COPTER_LEDS == ENABLED #if COPTER_LEDS == ENABLED
if ( bitRead(g.copter_leds_mode, 3) ) { if ( bitRead(g.copter_leds_mode, 3) ) {

99
ArduCopter/navigation.pde
View File

@ -194,9 +194,10 @@ static void run_navigation_contollers()
case RTL: case RTL:
// have we reached the desired Altitude? // have we reached the desired Altitude?
if(alt_change_flag <= REACHED_ALT) { // we are at or above the target alt if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) {
if(rtl_reached_alt == false) { // we are at or above the target alt
rtl_reached_alt = true; if(false == ap.rtl_reached_alt) {
set_rtl_reached_alt(true);
do_RTL(); do_RTL();
} }
wp_control = WP_MODE; wp_control = WP_MODE;
@ -223,12 +224,13 @@ static void run_navigation_contollers()
// go of the sticks // go of the sticks
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) { if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) {
if(wp_distance > 500) if(wp_distance > 500){
loiter_override = true; ap.loiter_override = true;
}
} }
// Allow the user to take control temporarily, // Allow the user to take control temporarily,
if(loiter_override) { if(ap.loiter_override) {
// this sets the copter to not try and nav while we control it // this sets the copter to not try and nav while we control it
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
@ -237,8 +239,8 @@ static void run_navigation_contollers()
next_WP.lng = current_loc.lng; next_WP.lng = current_loc.lng;
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) { if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) {
loiter_override = false; wp_control = LOITER_MODE;
wp_control = LOITER_MODE; ap.loiter_override = false;
} }
}else{ }else{
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
@ -294,7 +296,7 @@ static void run_navigation_contollers()
} }
// are we in SIMPLE mode? // are we in SIMPLE mode?
if(do_simple && g.super_simple) { if(ap.simple_mode && g.super_simple) {
// get distance to home // get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad // we reset the angular offset to be a vector from home to the quad
@ -304,7 +306,7 @@ static void run_navigation_contollers()
} }
if(yaw_mode == YAW_LOOK_AT_HOME) { if(yaw_mode == YAW_LOOK_AT_HOME) {
if(home_is_set) { if(ap.home_is_set) {
nav_yaw = get_bearing_cd(&current_loc, &home); nav_yaw = get_bearing_cd(&current_loc, &home);
} else { } else {
nav_yaw = 0; nav_yaw = 0;
@ -329,7 +331,7 @@ static void update_nav_RTL()
// If failsafe OR auto approach altitude is set // If failsafe OR auto approach altitude is set
// we will go into automatic land, (g.rtl_approach_alt) is the lowest point // we will go into automatic land, (g.rtl_approach_alt) is the lowest point
// -1 means disable feature // -1 means disable feature
if(failsafe || g.rtl_approach_alt >= 0) if(ap.failsafe || g.rtl_approach_alt >= 0)
loiter_timer = millis(); loiter_timer = millis();
else else
loiter_timer = 0; loiter_timer = 0;
@ -493,10 +495,10 @@ static int16_t get_desired_speed(int16_t max_speed)
V1 = sqrt(sq(V2) - 2*A*(X2-X1)) V1 = sqrt(sq(V2) - 2*A*(X2-X1))
*/ */
if(fast_corner) { if(ap.fast_corner) {
// don't slow down // don't slow down
}else{ }else{
if(wp_distance < 20000){ // limit the size of numbers we're dealing with to avoide overflow if(wp_distance < 20000){ // limit the size of numbers we're dealing with to avoid overflow
// go slower // go slower
int32_t temp = 2 * 100 * (int32_t)(wp_distance - g.waypoint_radius * 100); int32_t temp = 2 * 100 * (int32_t)(wp_distance - g.waypoint_radius * 100);
int32_t s_min = WAYPOINT_SPEED_MIN; int32_t s_min = WAYPOINT_SPEED_MIN;
@ -517,7 +519,43 @@ static void reset_desired_speed()
max_speed_old = 0; max_speed_old = 0;
} }
#define MAX_CLIMB_RATE 200
#define MIN_CLIMB_RATE 50
#define DECEL_CLIMB_RATE 30
static int16_t get_desired_climb_rate() static int16_t get_desired_climb_rate()
{
static int16_t climb_old = 0;
if(alt_change_flag == REACHED_ALT) {
climb_old = 0;
return 0;
}
int16_t climb = 0;
int32_t dist = labs(altitude_error);
if(dist < 20000){ // limit the size of numbers we're dealing with to avoid overflow
dist -= 300; // give ourselves 3 meter buffer to the desired alt
float temp = 2 * DECEL_CLIMB_RATE * dist + (MIN_CLIMB_RATE * MIN_CLIMB_RATE); // 50cm minium climb_rate;
climb = sqrt(temp);
climb = min(climb, MAX_CLIMB_RATE); // don't go to fast
}else{
climb = MAX_CLIMB_RATE; // no need to calc speed, just go the max
}
//climb = min(climb, climb_old + (100 * .02));// limit going faster
climb = max(climb, MIN_CLIMB_RATE); // don't go too slow
climb_old = climb;
if(alt_change_flag == DESCENDING){
climb = -climb;
}
return climb;
}
static int16_t get_desired_climb_rate_old()
{ {
if(alt_change_flag == ASCENDING) { if(alt_change_flag == ASCENDING) {
return constrain(altitude_error / 4, 100, 180); // 180cm /s up, minimum is 100cm/s return constrain(altitude_error / 4, 100, 180); // 180cm /s up, minimum is 100cm/s
@ -534,9 +572,15 @@ static void update_crosstrack(void)
{ {
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
// If we are too far off or too close we don't do track following if (wp_distance >= (g.crosstrack_min_distance * 100) &&
float temp = (target_bearing - original_target_bearing) * RADX100; abs(wrap_180(target_bearing - original_target_bearing)) < 4500) {
crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line
}else{
// fade out crosstrack
crosstrack_error >>= 1;
}
} }
static int32_t get_altitude_error() static int32_t get_altitude_error()
@ -555,30 +599,30 @@ static int32_t get_altitude_error()
static void clear_new_altitude() static void clear_new_altitude()
{ {
alt_change_flag = REACHED_ALT; set_alt_change(REACHED_ALT);
} }
static void force_new_altitude(int32_t new_alt) static void force_new_altitude(int32_t new_alt)
{ {
next_WP.alt = new_alt; next_WP.alt = new_alt;
alt_change_flag = REACHED_ALT; set_alt_change(REACHED_ALT);
} }
static void set_new_altitude(int32_t new_alt) static void set_new_altitude(int32_t new_alt)
{ {
next_WP.alt = new_alt; next_WP.alt = new_alt;
if(next_WP.alt > current_loc.alt + 20) { if(next_WP.alt > (current_loc.alt + 80)) {
// we are below, going up // we are below, going up
alt_change_flag = ASCENDING; set_alt_change(ASCENDING);
}else if(next_WP.alt < current_loc.alt - 20) { }else if(next_WP.alt < (current_loc.alt - 80)) {
// we are above, going down // we are above, going down
alt_change_flag = DESCENDING; set_alt_change(DESCENDING);
}else{ }else{
// No Change // No Change
alt_change_flag = REACHED_ALT; set_alt_change(REACHED_ALT);
} }
} }
@ -587,12 +631,13 @@ static void verify_altitude()
if(alt_change_flag == ASCENDING) { if(alt_change_flag == ASCENDING) {
// we are below, going up // we are below, going up
if(current_loc.alt > next_WP.alt - 50) { if(current_loc.alt > next_WP.alt - 50) {
alt_change_flag = REACHED_ALT; set_alt_change(REACHED_ALT);
} }
}else if (alt_change_flag == DESCENDING) { }else if (alt_change_flag == DESCENDING) {
// we are above, going down // we are above, going down
if(current_loc.alt <= next_WP.alt + 50) if(current_loc.alt <= next_WP.alt + 50){
alt_change_flag = REACHED_ALT; set_alt_change(REACHED_ALT);
}
} }
} }
@ -609,4 +654,4 @@ static int32_t wrap_180(int32_t error)
if (error > 18000) error -= 36000; if (error > 18000) error -= 36000;
if (error < -18000) error += 36000; if (error < -18000) error += 36000;
return error; return error;
} }

4
ArduCopter/radio.pde
View File

@ -129,7 +129,7 @@ void output_min()
static void read_radio() static void read_radio()
{ {
if (APM_RC.GetState() == 1) { if (APM_RC.GetState() == 1) {
new_radio_frame = true; system.new_radio_frame = true;
g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
@ -167,7 +167,7 @@ static void throttle_failsafe(uint16_t pwm)
// Don't enter Failsafe if we are not armed // Don't enter Failsafe if we are not armed
// home distance is in meters // home distance is in meters
// This is to prevent accidental RTL // This is to prevent accidental RTL
if(motors.armed() && takeoff_complete) { if(motors.armed() && ap.takeoff_complete) {
Serial.print_P(PSTR("MSG FS ON ")); Serial.print_P(PSTR("MSG FS ON "));
Serial.println(pwm, DEC); Serial.println(pwm, DEC);
set_failsafe(true); set_failsafe(true);

126
ArduCopter/system.pde
View File

@ -62,8 +62,8 @@ static void init_ardupilot()
// USB_MUX_PIN // USB_MUX_PIN
pinMode(USB_MUX_PIN, INPUT); pinMode(USB_MUX_PIN, INPUT);
usb_connected = !digitalRead(USB_MUX_PIN); system.usb_connected = !digitalRead(USB_MUX_PIN);
if (!usb_connected) { if (!system.usb_connected) {
// USB is not connected, this means UART0 may be a Xbee, with // USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at // its darned bricking problem. We can't write to it for at
// least one second after powering up. Simplest solution for // least one second after powering up. Simplest solution for
@ -163,7 +163,7 @@ static void init_ardupilot()
gcs0.init(&Serial); gcs0.init(&Serial);
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
if (!usb_connected) { if (!system.usb_connected) {
// we are not connected via USB, re-init UART0 with right // we are not connected via USB, re-init UART0 with right
// baud rate // baud rate
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
@ -192,12 +192,14 @@ static void init_ardupilot()
} }
#endif #endif
/*
#ifdef RADIO_OVERRIDE_DEFAULTS #ifdef RADIO_OVERRIDE_DEFAULTS
{ {
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
APM_RC.setHIL(rc_override); APM_RC.setHIL(rc_override);
} }
#endif #endif
*/
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
motors.servo_manual = false; motors.servo_manual = false;
@ -291,26 +293,6 @@ static void init_ardupilot()
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
Log_Write_Startup(); Log_Write_Startup();
Log_Write_Data(10, (float)g.pi_stabilize_roll.kP());
Log_Write_Data(11, (float)g.pi_stabilize_roll.kI());
Log_Write_Data(12, (float)g.pid_rate_roll.kP());
Log_Write_Data(13, (float)g.pid_rate_roll.kI());
Log_Write_Data(14, (float)g.pid_rate_roll.kD());
Log_Write_Data(15, (float)g.stabilize_d.get());
Log_Write_Data(16, (float)g.pi_loiter_lon.kP());
Log_Write_Data(17, (float)g.pi_loiter_lon.kI());
Log_Write_Data(18, (float)g.pid_nav_lon.kP());
Log_Write_Data(19, (float)g.pid_nav_lon.kI());
Log_Write_Data(20, (float)g.pid_nav_lon.kD());
Log_Write_Data(21, (int32_t)g.auto_slew_rate.get());
Log_Write_Data(22, (float)g.pid_loiter_rate_lon.kP());
Log_Write_Data(23, (float)g.pid_loiter_rate_lon.kI());
Log_Write_Data(24, (float)g.pid_loiter_rate_lon.kD());
#endif #endif
@ -389,10 +371,8 @@ static void startup_ground(void)
static void set_mode(byte mode) static void set_mode(byte mode)
{ {
Log_Write_Data(31, initial_simple_bearing);
// if we don't have GPS lock // if we don't have GPS lock
if(home_is_set == false) { if(false == ap.home_is_set) {
// THOR // THOR
// We don't care about Home if we don't have lock yet in Toy mode // We don't care about Home if we don't have lock yet in Toy mode
if(mode == TOY_A || mode == TOY_M || mode == OF_LOITER) { if(mode == TOY_A || mode == TOY_M || mode == OF_LOITER) {
@ -414,6 +394,7 @@ static void set_mode(byte mode)
// used to stop fly_aways // used to stop fly_aways
// set to false if we have low throttle // set to false if we have low throttle
motors.auto_armed(g.rc_3.control_in > 0); motors.auto_armed(g.rc_3.control_in > 0);
set_auto_armed(g.rc_3.control_in > 0);
// clearing value used in interactive alt hold // clearing value used in interactive alt hold
reset_throttle_counter = 0; reset_throttle_counter = 0;
@ -425,24 +406,28 @@ static void set_mode(byte mode)
loiter_timer = 0; loiter_timer = 0;
// if we change modes, we must clear landed flag // if we change modes, we must clear landed flag
land_complete = false; set_land_complete(false);
// have we acheived the proper altitude before RTL is enabled // have we achieved the proper altitude before RTL is enabled
rtl_reached_alt = false; set_rtl_reached_alt(false);
// debug to Serial terminal // debug to Serial terminal
//Serial.println(flight_mode_strings[control_mode]); //Serial.println(flight_mode_strings[control_mode]);
ap.loiter_override = false;
// report the GPS and Motor arming status // report the GPS and Motor arming status
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
switch(control_mode) switch(control_mode)
{ {
case ACRO: case ACRO:
ap.manual_throttle = true;
ap.manual_attitude = true;
yaw_mode = YAW_ACRO; yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO; roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
// reset acro axis targets to current attitude // reset acro axis targets to current attitude
if( g.axis_enabled ) { if(g.axis_enabled){
roll_axis = ahrs.roll_sensor; roll_axis = ahrs.roll_sensor;
pitch_axis = ahrs.pitch_sensor; pitch_axis = ahrs.pitch_sensor;
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
@ -450,20 +435,25 @@ static void set_mode(byte mode)
break; break;
case STABILIZE: case STABILIZE:
ap.manual_throttle = true;
ap.manual_attitude = true;
yaw_mode = YAW_HOLD; yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE; roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
break; break;
case ALT_HOLD: case ALT_HOLD:
ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = ALT_HOLD_YAW; yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP; roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR; throttle_mode = ALT_HOLD_THR;
force_new_altitude(max(current_loc.alt, 100)); force_new_altitude(max(current_loc.alt, 100));
break; break;
case AUTO: case AUTO:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = AUTO_YAW; yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP; roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR; throttle_mode = AUTO_THR;
@ -473,6 +463,8 @@ static void set_mode(byte mode)
break; break;
case CIRCLE: case CIRCLE:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = CIRCLE_YAW; yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP; roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR; throttle_mode = CIRCLE_THR;
@ -482,6 +474,8 @@ static void set_mode(byte mode)
break; break;
case LOITER: case LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = LOITER_YAW; yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP; roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR; throttle_mode = LOITER_THR;
@ -489,6 +483,8 @@ static void set_mode(byte mode)
break; break;
case POSITION: case POSITION:
ap.manual_throttle = true;
ap.manual_attitude = false;
yaw_mode = YAW_HOLD; yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
@ -496,6 +492,8 @@ static void set_mode(byte mode)
break; break;
case GUIDED: case GUIDED:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = YAW_AUTO; yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO; throttle_mode = THROTTLE_AUTO;
@ -504,6 +502,8 @@ static void set_mode(byte mode)
break; break;
case LAND: case LAND:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = LOITER_YAW; yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP; roll_pitch_mode = LOITER_RP;
throttle_mode = THROTTLE_AUTO; throttle_mode = THROTTLE_AUTO;
@ -511,15 +511,19 @@ static void set_mode(byte mode)
break; break;
case RTL: case RTL:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = RTL_YAW; yaw_mode = RTL_YAW;
roll_pitch_mode = RTL_RP; roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR; throttle_mode = RTL_THR;
rtl_reached_alt = false; set_rtl_reached_alt(false);
set_next_WP(&current_loc); set_next_WP(&current_loc);
set_new_altitude(get_RTL_alt()); set_new_altitude(get_RTL_alt());
break; break;
case OF_LOITER: case OF_LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = OF_LOITER_YAW; yaw_mode = OF_LOITER_YAW;
roll_pitch_mode = OF_LOITER_RP; roll_pitch_mode = OF_LOITER_RP;
throttle_mode = OF_LOITER_THR; throttle_mode = OF_LOITER_THR;
@ -530,10 +534,12 @@ static void set_mode(byte mode)
// These are the flight modes for Toy mode // These are the flight modes for Toy mode
// See the defines for the enumerated values // See the defines for the enumerated values
case TOY_A: case TOY_A:
yaw_mode = YAW_TOY; ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY; roll_pitch_mode = ROLL_PITCH_TOY;
throttle_mode = THROTTLE_AUTO; throttle_mode = THROTTLE_AUTO;
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
// save throttle for fast exit of Alt hold // save throttle for fast exit of Alt hold
saved_toy_throttle = g.rc_3.control_in; saved_toy_throttle = g.rc_3.control_in;
@ -543,25 +549,34 @@ static void set_mode(byte mode)
break; break;
case TOY_M: case TOY_M:
yaw_mode = YAW_TOY; ap.manual_throttle = false;
roll_pitch_mode = ROLL_PITCH_TOY; ap.manual_attitude = true;
wp_control = NO_NAV_MODE; yaw_mode = YAW_TOY;
throttle_mode = THROTTLE_HOLD; roll_pitch_mode = ROLL_PITCH_TOY;
wp_control = NO_NAV_MODE;
throttle_mode = THROTTLE_HOLD;
break; break;
default: default:
break; break;
} }
if(failsafe) { if(ap.failsafe) {
// this is to allow us to fly home without interactive throttle control // this is to allow us to fly home without interactive throttle control
throttle_mode = THROTTLE_AUTO; throttle_mode = THROTTLE_AUTO;
ap.manual_throttle = false;
// does not wait for us to be in high throttle, since the // does not wait for us to be in high throttle, since the
// Receiver will be outputting low throttle // Receiver will be outputting low throttle
motors.auto_armed(true); motors.auto_armed(true);
set_auto_armed(true);
} }
if(roll_pitch_mode <= ROLL_PITCH_ACRO) { if(ap.manual_throttle) {
desired_climb_rate = 0;
}
if(ap.manual_attitude) {
// We are under manual attitude control // We are under manual attitude control
// remove the navigation from roll and pitch command // remove the navigation from roll and pitch command
reset_nav_params(); reset_nav_params();
@ -572,36 +587,13 @@ static void set_mode(byte mode)
} }
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
Log_Write_Data(32, initial_simple_bearing);
}
static void set_failsafe(boolean mode)
{
// only act on changes
// -------------------
if(failsafe != mode) {
// store the value so we don't trip the gate twice
// -----------------------------------------------
failsafe = mode;
if (failsafe == false) {
// We've regained radio contact
// ----------------------------
failsafe_off_event();
}else{
// We've lost radio contact
// ------------------------
failsafe_on_event();
}
}
} }
static void static void
init_simple_bearing() init_simple_bearing()
{ {
initial_simple_bearing = ahrs.yaw_sensor; initial_simple_bearing = ahrs.yaw_sensor;
Log_Write_Data(DATA_INIT_SIMPLE_BEARING, initial_simple_bearing);
} }
static void update_throttle_cruise(int16_t tmp) static void update_throttle_cruise(int16_t tmp)
@ -648,13 +640,13 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
static void check_usb_mux(void) static void check_usb_mux(void)
{ {
bool usb_check = !digitalRead(USB_MUX_PIN); bool usb_check = !digitalRead(USB_MUX_PIN);
if (usb_check == usb_connected) { if (usb_check == system.usb_connected) {
return; return;
} }
// the user has switched to/from the telemetry port // the user has switched to/from the telemetry port
usb_connected = usb_check; system.usb_connected = usb_check;
if (usb_connected) { if (system.usb_connected) {
Serial.begin(SERIAL0_BAUD); Serial.begin(SERIAL0_BAUD);
} else { } else {
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));

48
ArduCopter/test.pde
View File

@ -187,7 +187,15 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
//static int8_t //static int8_t
//test_toy(uint8_t argc, const Menu::arg *argv) //test_toy(uint8_t argc, const Menu::arg *argv)
{ {
wp_distance = 0; set_alt_change(ASCENDING)
for(altitude_error = 2000; altitude_error > -100; altitude_error--){
int16_t temp = get_desired_climb_rate();
Serial.printf("%ld, %d\n", altitude_error, temp);
}
return 0;
}
{ wp_distance = 0;
int16_t max_speed = 0; int16_t max_speed = 0;
for(int16_t i = 0; i < 200; i++){ for(int16_t i = 0; i < 200; i++){
@ -198,7 +206,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
wp_distance += 100; wp_distance += 100;
} }
return 0; return 0;
} }
//*/ //*/
/*static int8_t /*static int8_t
@ -517,42 +525,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
#endif #endif
} }
// used to test the gain scheduler for Stab_D
/*
* static int8_t
* test_stab_d(uint8_t argc, const Menu::arg *argv)
* {
* int16_t i = 0;
* g.stabilize_d = 1;
*
* g.stabilize_d_schedule = 1
* for (i = -4600; i < 4600; i+=10) {
* new_radio_frame = true;
* g.rc_1.control_in = i;
* g.rc_2.control_in = i;
* update_roll_pitch_mode();
* Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d);
* }
* g.stabilize_d_schedule = .5
* for (i = -4600; i < 4600; i+=10) {
* new_radio_frame = true;
* g.rc_1.control_in = i;
* g.rc_2.control_in = i;
* update_roll_pitch_mode();
* Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d);
* }
*
* g.stabilize_d_schedule = 0
* for (i = -4600; i < 4600; i+=10) {
* new_radio_frame = true;
* g.rc_1.control_in = i;
* g.rc_2.control_in = i;
* update_roll_pitch_mode();
* Serial.printf("rin:%d, d:%1.6f \tpin:%d, d:%1.6f\n",g.rc_1.control_in, roll_scale_d, g.rc_2.control_in, pitch_scale_d);
* }
*
* }*/
/* /*
* //static int8_t * //static int8_t
* //test_dcm(uint8_t argc, const Menu::arg *argv) * //test_dcm(uint8_t argc, const Menu::arg *argv)

6
ArduCopter/toy.pde
View File

@ -110,15 +110,15 @@ void roll_pitch_toy()
if(g.rc_1.control_in != 0) { // roll if(g.rc_1.control_in != 0) { // roll
get_acro_yaw(yaw_rate/2); get_acro_yaw(yaw_rate/2);
yaw_stopped = false; system.yaw_stopped = false;
yaw_timer = 150; yaw_timer = 150;
}else if (!yaw_stopped) { }else if (!system.yaw_stopped) {
get_acro_yaw(0); get_acro_yaw(0);
yaw_timer--; yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) { if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
yaw_stopped = true; system.yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
} }
}else{ }else{