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

Ap state updates

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.lat = g_gps->latitude;
current_loc.alt = g_gps->altitude - gps_base_alt;
if (!home_is_set) {
if (!ap.home_is_set) {
init_home();
}

13
ArduCopter/Parameters.h
View File

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

14
ArduCopter/Parameters.pde
View File

@ -166,6 +166,16 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Units: Dimensionless
// @User: Standard
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),
// @Param: THR_MIN
@ -297,7 +307,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// variable
//---------
GSCALAR(stabilize_d, "STAB_D", STABILIZE_D),
//GSCALAR(stabilize_d, "STAB_D", STABILIZE_D),
// @Param: STAB_D_S
// @DisplayName: Stabilize D Schedule
@ -305,7 +315,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 1
// @Increment: .01
// @User: Advanced
GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE),
//GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE),
// Acro parameters
GSCALAR(acro_p, "ACRO_P", ACRO_P),

14
ArduCopter/commands.pde
View File

@ -2,14 +2,14 @@
static void init_commands()
{
g.command_index = NO_COMMAND;
command_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND;
prev_nav_index = NO_COMMAND;
g.command_index = NO_COMMAND;
command_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND;
prev_nav_index = NO_COMMAND;
command_cond_queue.id = NO_COMMAND;
command_nav_queue.id = NO_COMMAND;
fast_corner = false;
ap.fast_corner = false;
reset_desired_speed();
// default Yaw tracking
@ -162,7 +162,7 @@ static void set_next_WP(struct Location *wp)
set_new_altitude(next_WP.alt);
// 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);
scaleLongUp = 1.0f/cos(rads);
@ -184,7 +184,7 @@ static void set_next_WP(struct Location *wp)
// -------------------------------
static void init_home()
{
home_is_set = true;
set_home_is_set(true);
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 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;
// just to make sure
land_complete = false;
set_land_complete(false);
// landing boost lowers the main throttle to mimmick
// the effect of a user's hand
@ -385,7 +385,7 @@ static bool verify_land_sonar()
if(ground_detector < 30) {
ground_detector++;
}else if (ground_detector == 30) {
land_complete = true;
set_land_complete(true);
if(g.rc_3.control_in == 0) {
ground_detector++;
init_disarm_motors();
@ -419,7 +419,7 @@ static bool verify_land_baro()
if(ground_detector < 30) {
ground_detector++;
}else if (ground_detector == 30) {
land_complete = true;
set_land_complete(true);
if(g.rc_3.control_in == 0) {
ground_detector++;
init_disarm_motors();
@ -792,7 +792,8 @@ static void do_set_home()
home.lng = command_cond_queue.lng; // Lon * 10**7
home.lat = command_cond_queue.lat; // Lat * 10**7
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);
if(tmp_loc.lat == 0) {
fast_corner = false;
ap.fast_corner = false;
}else{
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
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
// too much jerkyness.
if(false == fast_corner){
if(false == ap.fast_corner){
reset_desired_speed();
}
}
@ -213,7 +213,7 @@ static void exit_mission()
g.command_index = 255;
// 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.
}else{
// 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) {
// set Simple mode using stored paramters from Mission planner
// 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{
switch_debouncer = true;
@ -60,29 +61,30 @@ static void read_trim_switch()
}
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) {
if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
CH7_flag = true;
if (system.CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
system.CH7_flag = true;
// 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();
}
}
if (CH7_flag == true && g.rc_7.control_in < 800) {
CH7_flag = false;
if (system.CH7_flag == true && g.rc_7.control_in < 800) {
system.CH7_flag = false;
}
}else if (option == CH7_RTL) {
if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
CH7_flag = true;
if (system.CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
system.CH7_flag = true;
set_mode(RTL);
}
if (CH7_flag == true && g.rc_7.control_in < 800) {
CH7_flag = false;
if (system.CH7_flag == true && g.rc_7.control_in < 800) {
system.CH7_flag = false;
if (control_mode == RTL || control_mode == LOITER) {
reset_control_switch();
}
@ -90,11 +92,11 @@ static void read_trim_switch()
}else if (option == CH7_SAVE_WP) {
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) { // switch is engaged
CH7_flag = true;
system.CH7_flag = true;
}else{ // switch is disengaged
if(CH7_flag) {
CH7_flag = false;
if(system.CH7_flag) {
system.CH7_flag = false;
if(control_mode == AUTO) {
// reset the mission

74
ArduCopter/defines.h
View File

@ -111,13 +111,13 @@
#define RC_CHANNEL_ANGLE_RAW 2
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
#define ASCENDING 1
#define DESCENDING -1
#define REACHED_ALT 0
#define DESCENDING 1
#define ASCENDING 2
// Auto Pilot modes
// ----------------
@ -131,10 +131,10 @@
#define CIRCLE 7 // AUTO control
#define POSITION 8 // AUTO control
#define LAND 9 // AUTO control
#define OF_LOITER 10 // Hold a single location using optical flow
// sensor
#define TOY_A 11 // THOR Enum for Toy mode
#define TOY_M 12 // THOR Enum for Toy mode
#define OF_LOITER 10 // Hold a single location using optical flow
// sensor
#define TOY_A 11 // THOR Enum for Toy mode
#define TOY_M 12 // THOR Enum for Toy mode
#define NUM_MODES 13
#define SIMPLE_1 1
@ -225,12 +225,12 @@
// Waypoint options
#define MASK_OPTIONS_RELATIVE_ALT 1
#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_RELATIVE 16
#define WP_OPTION_RELATIVE 16
//#define WP_OPTION_ 32
//#define WP_OPTION_ 64
#define WP_OPTION_NEXT_CMD 128
#define WP_OPTION_NEXT_CMD 128
//repeating events
#define NO_REPEAT 0
@ -303,8 +303,8 @@ enum gcs_severity {
#define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4)
@ -320,6 +320,52 @@ enum gcs_severity {
#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
// ----------------
#define ABS_WP 0

5
ArduCopter/events.pde
View File

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

7
ArduCopter/failsafe.pde
View File

@ -27,7 +27,7 @@ void failsafe_disable()
{
failsafe_enabled = false;
}
//
// 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) {
// 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
// disarm the motors.
in_failsafe = true;
@ -51,8 +51,9 @@ void failsafe_check(uint32_t tnow)
if (in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
// disarm motors every second
failsafe_last_timestamp = tnow;
if( motors.armed() ) {
if(motors.armed()) {
motors.armed(false);
set_armed(true);
motors.output();
}
}

8
ArduCopter/flip.pde
View File

@ -17,10 +17,11 @@ static int8_t flip_dir;
void init_flip()
{
if(do_flip == false) {
do_flip = true;
if(false == ap.do_flip) {
ap.do_flip = true;
flip_state = 0;
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;
flip_timer++;
}else{
do_flip = false;
Log_Write_Event(DATA_END_FLIP);
ap.do_flip = false;
flip_state = 0;
}
break;

64
ArduCopter/leds.pde
View File

@ -19,7 +19,7 @@ static void update_GPS_light(void)
switch (g_gps->status()) {
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.
} else {
digitalWrite(C_LED_PIN, LED_OFF);
@ -28,8 +28,8 @@ static void update_GPS_light(void)
case (1):
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
if (GPS_light) {
system.GPS_light = !system.GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (system.GPS_light) {
digitalWrite(C_LED_PIN, LED_OFF);
}else{
digitalWrite(C_LED_PIN, LED_ON);
@ -47,17 +47,17 @@ static void update_GPS_light(void)
static void update_motor_light(void)
{
if(motors.armed() == false) {
motor_light = !motor_light;
system.motor_light = !system.motor_light;
// blink
if(motor_light) {
if(system.motor_light) {
digitalWrite(A_LED_PIN, LED_ON);
}else{
digitalWrite(A_LED_PIN, LED_OFF);
}
}else{
if(!motor_light) {
motor_light = true;
if(!system.motor_light) {
system.motor_light = true;
digitalWrite(A_LED_PIN, LED_ON);
}
}
@ -93,7 +93,7 @@ static void clear_leds()
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF);
motor_light = false;
system.motor_light = false;
led_mode = NORMAL_LEDS;
}
@ -121,28 +121,28 @@ static void clear_leds()
static void update_copter_leds(void)
{
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 (motors.armed() == true) {
if (low_batt == true) {
if (ap.low_battery == true) {
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 {
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 ) ) {
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 ) ) {
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 {
copter_leds_on();
}
}
} 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()) {
case (2):
if(home_is_set) {
if(ap.home_is_set) {
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 ) ) {
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 {
copter_leds_GPS_on();
}
}
} 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;
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;
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;
}
}
if ( bitRead(g.copter_leds_mode, 2) ) {
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) {
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_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) {
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
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
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
}
}else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) {
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) {
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
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
copter_leds_GPS_off();
}else if (2 < copter_leds_GPS_blink && copter_leds_GPS_blink < 5) {
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){
@ -360,7 +360,7 @@ void piezo_off(){
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();
delay(100);
piezo_off();

15
ArduCopter/motors.pde
View File

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

99
ArduCopter/navigation.pde
View File

@ -194,9 +194,10 @@ static void run_navigation_contollers()
case RTL:
// have we reached the desired Altitude?
if(alt_change_flag <= REACHED_ALT) { // we are at or above the target alt
if(rtl_reached_alt == false) {
rtl_reached_alt = true;
if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) {
// we are at or above the target alt
if(false == ap.rtl_reached_alt) {
set_rtl_reached_alt(true);
do_RTL();
}
wp_control = WP_MODE;
@ -223,12 +224,13 @@ static void run_navigation_contollers()
// go of the sticks
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) {
if(wp_distance > 500)
loiter_override = true;
if(wp_distance > 500){
ap.loiter_override = true;
}
}
// 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
wp_control = NO_NAV_MODE;
@ -237,8 +239,8 @@ static void run_navigation_contollers()
next_WP.lng = current_loc.lng;
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{
wp_control = LOITER_MODE;
@ -294,7 +296,7 @@ static void run_navigation_contollers()
}
// are we in SIMPLE mode?
if(do_simple && g.super_simple) {
if(ap.simple_mode && g.super_simple) {
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// 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(home_is_set) {
if(ap.home_is_set) {
nav_yaw = get_bearing_cd(&current_loc, &home);
} else {
nav_yaw = 0;
@ -329,7 +331,7 @@ static void update_nav_RTL()
// If failsafe OR auto approach altitude is set
// we will go into automatic land, (g.rtl_approach_alt) is the lowest point
// -1 means disable feature
if(failsafe || g.rtl_approach_alt >= 0)
if(ap.failsafe || g.rtl_approach_alt >= 0)
loiter_timer = millis();
else
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))
*/
if(fast_corner) {
if(ap.fast_corner) {
// don't slow down
}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
int32_t temp = 2 * 100 * (int32_t)(wp_distance - g.waypoint_radius * 100);
int32_t s_min = WAYPOINT_SPEED_MIN;
@ -517,7 +519,43 @@ static void reset_desired_speed()
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 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) {
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
// ----------------
// If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line
if (wp_distance >= (g.crosstrack_min_distance * 100) &&
abs(wrap_180(target_bearing - original_target_bearing)) < 4500) {
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()
@ -555,30 +599,30 @@ static int32_t get_altitude_error()
static void clear_new_altitude()
{
alt_change_flag = REACHED_ALT;
set_alt_change(REACHED_ALT);
}
static void force_new_altitude(int32_t 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)
{
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
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
alt_change_flag = DESCENDING;
set_alt_change(DESCENDING);
}else{
// 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) {
// we are below, going up
if(current_loc.alt > next_WP.alt - 50) {
alt_change_flag = REACHED_ALT;
set_alt_change(REACHED_ALT);
}
}else if (alt_change_flag == DESCENDING) {
// we are above, going down
if(current_loc.alt <= next_WP.alt + 50)
alt_change_flag = REACHED_ALT;
if(current_loc.alt <= next_WP.alt + 50){
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;
return error;
}
}

4
ArduCopter/radio.pde
View File

@ -129,7 +129,7 @@ void output_min()
static void read_radio()
{
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_2.set_pwm(APM_RC.InputCh(CH_2));
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
// home distance is in meters
// 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.println(pwm, DEC);
set_failsafe(true);

126
ArduCopter/system.pde
View File

@ -62,8 +62,8 @@ static void init_ardupilot()
// USB_MUX_PIN
pinMode(USB_MUX_PIN, INPUT);
usb_connected = !digitalRead(USB_MUX_PIN);
if (!usb_connected) {
system.usb_connected = !digitalRead(USB_MUX_PIN);
if (!system.usb_connected) {
// USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at
// least one second after powering up. Simplest solution for
@ -163,7 +163,7 @@ static void init_ardupilot()
gcs0.init(&Serial);
#if USB_MUX_PIN > 0
if (!usb_connected) {
if (!system.usb_connected) {
// we are not connected via USB, re-init UART0 with right
// baud rate
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
@ -192,12 +192,14 @@ static void init_ardupilot()
}
#endif
/*
#ifdef RADIO_OVERRIDE_DEFAULTS
{
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
APM_RC.setHIL(rc_override);
}
#endif
*/
#if FRAME_CONFIG == HELI_FRAME
motors.servo_manual = false;
@ -291,26 +293,6 @@ static void init_ardupilot()
#if LOGGING_ENABLED == ENABLED
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
@ -389,10 +371,8 @@ static void startup_ground(void)
static void set_mode(byte mode)
{
Log_Write_Data(31, initial_simple_bearing);
// if we don't have GPS lock
if(home_is_set == false) {
if(false == ap.home_is_set) {
// THOR
// 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) {
@ -414,6 +394,7 @@ static void set_mode(byte mode)
// used to stop fly_aways
// set to false if we have low throttle
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
reset_throttle_counter = 0;
@ -425,24 +406,28 @@ static void set_mode(byte mode)
loiter_timer = 0;
// 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
rtl_reached_alt = false;
// have we achieved the proper altitude before RTL is enabled
set_rtl_reached_alt(false);
// debug to Serial terminal
//Serial.println(flight_mode_strings[control_mode]);
ap.loiter_override = false;
// report the GPS and Motor arming status
led_mode = NORMAL_LEDS;
switch(control_mode)
{
case ACRO:
ap.manual_throttle = true;
ap.manual_attitude = true;
yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL;
// reset acro axis targets to current attitude
if( g.axis_enabled ) {
if(g.axis_enabled){
roll_axis = ahrs.roll_sensor;
pitch_axis = ahrs.pitch_sensor;
nav_yaw = ahrs.yaw_sensor;
@ -450,20 +435,25 @@ static void set_mode(byte mode)
break;
case STABILIZE:
ap.manual_throttle = true;
ap.manual_attitude = true;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL;
break;
case ALT_HOLD:
ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR;
force_new_altitude(max(current_loc.alt, 100));
break;
case AUTO:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR;
@ -473,6 +463,8 @@ static void set_mode(byte mode)
break;
case CIRCLE:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR;
@ -482,6 +474,8 @@ static void set_mode(byte mode)
break;
case LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR;
@ -489,6 +483,8 @@ static void set_mode(byte mode)
break;
case POSITION:
ap.manual_throttle = true;
ap.manual_attitude = false;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL;
@ -496,6 +492,8 @@ static void set_mode(byte mode)
break;
case GUIDED:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
@ -504,6 +502,8 @@ static void set_mode(byte mode)
break;
case LAND:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
throttle_mode = THROTTLE_AUTO;
@ -511,15 +511,19 @@ static void set_mode(byte mode)
break;
case RTL:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = RTL_YAW;
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
rtl_reached_alt = false;
set_rtl_reached_alt(false);
set_next_WP(&current_loc);
set_new_altitude(get_RTL_alt());
break;
case OF_LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = OF_LOITER_YAW;
roll_pitch_mode = OF_LOITER_RP;
throttle_mode = OF_LOITER_THR;
@ -530,10 +534,12 @@ static void set_mode(byte mode)
// These are the flight modes for Toy mode
// See the defines for the enumerated values
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;
throttle_mode = THROTTLE_AUTO;
wp_control = NO_NAV_MODE;
wp_control = NO_NAV_MODE;
// save throttle for fast exit of Alt hold
saved_toy_throttle = g.rc_3.control_in;
@ -543,25 +549,34 @@ static void set_mode(byte mode)
break;
case TOY_M:
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY;
wp_control = NO_NAV_MODE;
throttle_mode = THROTTLE_HOLD;
ap.manual_throttle = false;
ap.manual_attitude = true;
yaw_mode = YAW_TOY;
roll_pitch_mode = ROLL_PITCH_TOY;
wp_control = NO_NAV_MODE;
throttle_mode = THROTTLE_HOLD;
break;
default:
break;
}
if(failsafe) {
if(ap.failsafe) {
// this is to allow us to fly home without interactive throttle control
throttle_mode = THROTTLE_AUTO;
ap.manual_throttle = false;
// does not wait for us to be in high throttle, since the
// Receiver will be outputting low throttle
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
// remove the navigation from roll and pitch command
reset_nav_params();
@ -572,36 +587,13 @@ static void set_mode(byte 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
init_simple_bearing()
{
initial_simple_bearing = ahrs.yaw_sensor;
Log_Write_Data(DATA_INIT_SIMPLE_BEARING, initial_simple_bearing);
}
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)
{
bool usb_check = !digitalRead(USB_MUX_PIN);
if (usb_check == usb_connected) {
if (usb_check == system.usb_connected) {
return;
}
// the user has switched to/from the telemetry port
usb_connected = usb_check;
if (usb_connected) {
system.usb_connected = usb_check;
if (system.usb_connected) {
Serial.begin(SERIAL0_BAUD);
} else {
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
//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;
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;
}
return 0;
}
}
//*/
/*static int8_t
@ -517,42 +525,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
#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
* //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
get_acro_yaw(yaw_rate/2);
yaw_stopped = false;
system.yaw_stopped = false;
yaw_timer = 150;
}else if (!yaw_stopped) {
}else if (!system.yaw_stopped) {
get_acro_yaw(0);
yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
yaw_stopped = true;
system.yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;
}
}else{