diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 28061c4ee9..1bc096f9df 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -39,7 +39,7 @@ */ #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress -#define ACCEL_ALT_HOLD_GAIN 12.0 +#define ACCEL_ALT_HOLD_GAIN 20.0 // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode // See the config.h and defines.h files for how to set this up! diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fb017f079d..b51e0d59b3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.0.49 Beta" +#define THISFIRMWARE "ArduCopter V2.0.50 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -1022,8 +1022,10 @@ void update_throttle_mode(void) case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ - g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(); + g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(g.rc_3.control_in); }else{ + g.pi_stabilize_roll.reset_I(); + g.pi_stabilize_pitch.reset_I(); g.pi_rate_roll.reset_I(); g.pi_rate_pitch.reset_I(); g.rc_3.servo_out = 0; @@ -1050,8 +1052,7 @@ void update_throttle_mode(void) invalid_throttle = false; } - // apply throttle control at 200 hz - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost() + alt_hold_velocity(); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise); break; } } @@ -1171,14 +1172,6 @@ static void update_trig(void){ // 90° = cos_yaw: 1.00, sin_yaw: 0.00, // 180 = cos_yaw: 0.00, sin_yaw: -1.00, // 270 = cos_yaw: -1.00, sin_yaw: 0.00, - - - #if ACCEL_ALT_HOLD == 1 - Vector3f accel_filt = imu.get_accel_filtered(); - accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); - accels_rot_sum += accels_rot.z; - accels_rot_count++; - #endif } // updated at 10hz diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ccbd382dde..e7b232b359 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle) #define ALT_ERROR_MAX 400 static int -get_nav_throttle(long z_error) +get_nav_throttle(long z_error) { // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); @@ -98,28 +98,41 @@ get_nav_throttle(long z_error) return (int)g.pi_throttle.get_pi(rate_error, .1); } +#define ALT_ERROR_MAX2 300 +static int +get_nav_throttle2(long z_error) +{ + if (z_error > ALT_ERROR_MAX2){ + return g.pi_throttle.kP() * 80; + + }else if (z_error < -ALT_ERROR_MAX2){ + return g.pi_throttle.kP() * -60; + + } else{ + // limit error to prevent I term run up + z_error = constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2); + int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 + + rate_error = rate_error - altitude_rate; + + // limit the rate + rate_error = constrain(rate_error, -100, 120); + return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity(); + } +} + static int get_rate_roll(long target_rate) { - long error; - target_rate = constrain(target_rate, -2500, 2500); - error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); - target_rate = g.pi_rate_roll.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); + long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0); + return g.pi_acro_roll.get_pi(error, G_Dt); } static int get_rate_pitch(long target_rate) { - long error; - target_rate = constrain(target_rate, -2500, 2500); - error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); - target_rate = g.pi_rate_pitch.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); + long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0); + return g.pi_acro_pitch.get_pi(error, G_Dt); } static int @@ -139,7 +152,7 @@ get_rate_yaw(long target_rate) static void reset_hold_I(void) { g.pi_loiter_lat.reset_I(); - g.pi_loiter_lat.reset_I(); + g.pi_loiter_lon.reset_I(); g.pi_crosstrack.reset_I(); } @@ -189,19 +202,21 @@ get_nav_yaw_offset(int yaw_input, int reset) static int alt_hold_velocity() { #if ACCEL_ALT_HOLD == 1 + Vector3f accel_filt; + float error; + // subtract filtered Accel - float error = abs(next_WP.alt - current_loc.alt); + error = abs(next_WP.alt - current_loc.alt) - 25; + error = min(error, 50.0); + error = max(error, 0.0); + error = 1 - (error/ 50.0); - error -= 100; - error = min(error, 200.0); - error = max(error, 0.0); - error = 1 - (error/ 200.0); - float sum = accels_rot_sum / (float)accels_rot_count; + accel_filt = imu.get_accel_filtered(); + accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); + int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12 - accels_rot_sum = 0; - accels_rot_count = 0; - - int output = (sum + 9.81) * alt_hold_gain * error; + //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); + return constrain(output, -70, 70); // fast rise //s: -17.6241, g:0.0000, e:1.0000, o:0 @@ -209,17 +224,15 @@ static int alt_hold_velocity() //s: -19.3193, g:0.0000, e:1.0000, o:0 //s: -13.1310, g:47.8700, e:1.0000, o:-158 - //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); - return output; #else return 0; #endif } -static int get_angle_boost() +static int get_angle_boost(int value) { float temp = cos_pitch_x * cos_roll_x; temp = 1.0 - constrain(temp, .5, 1.0); - return (int)(temp * g.throttle_cruise); + return (int)(temp * value); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e24a43ed47..73c5f4cb81 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 111; + static const uint16_t k_format_version = 112; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -156,7 +156,7 @@ public: // // 240: PI/D Controllers // - k_param_pi_rate_roll = 240, + k_param_pi_rate_roll = 235, k_param_pi_rate_pitch, k_param_pi_rate_yaw, k_param_pi_stabilize_roll, @@ -169,6 +169,8 @@ public: k_param_pi_alt_hold, k_param_pi_throttle, k_param_pi_crosstrack, + k_param_pi_acro_roll, + k_param_pi_acro_pitch, // 254,255: reserved @@ -286,6 +288,9 @@ public: APM_PI pi_throttle; APM_PI pi_crosstrack; + APM_PI pi_acro_roll; + APM_PI pi_acro_pitch; + uint8_t junk; // Note: keep initializers here in the same order as they are declared above. @@ -396,6 +401,9 @@ public: pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX), + pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100), + pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100), + junk(0) // XXX just so that we can add things without worrying about the trailing comma { } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index dfd236e508..f0b8e5ee2d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -395,7 +395,30 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// Rate Control +// Acro Rate Control +// +#ifndef ACRO_ROLL_P +# define ACRO_ROLL_P 0.145 +#endif +#ifndef ACRO_ROLL_I +# define ACRO_ROLL_I 0.0 +#endif +#ifndef ACRO_ROLL_IMAX +# define ACRO_ROLL_IMAX 15 // degrees +#endif + +#ifndef ACRO_PITCH_P +# define ACRO_PITCH_P 0.145 +#endif +#ifndef ACRO_PITCH_I +# define ACRO_PITCH_I 0 //0.18 +#endif +#ifndef ACRO_PITCH_IMAX +# define ACRO_PITCH_IMAX 15 // degrees +#endif + +////////////////////////////////////////////////////////////////////////////// +// Stabilize Rate Control // #ifndef RATE_ROLL_P # define RATE_ROLL_P 0.145 diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 438bec2d6b..3d604f612d 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -139,8 +139,12 @@ static void auto_trim() led_mode = NORMAL_LEDS; clear_leds(); imu.save(); + //Serial.println("Done"); auto_level_counter = 0; + + // set TC + init_throttle_cruise(); } } } diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 3e1f5e2fea..36eff7d7af 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -2,8 +2,8 @@ #define ARM_DELAY 10 // one second #define DISARM_DELAY 10 // one second -#define LEVEL_DELAY 120 // twelve seconds -#define AUTO_LEVEL_DELAY 150 // twentyfive seconds +#define LEVEL_DELAY 70 // twelve seconds +#define AUTO_LEVEL_DELAY 90 // twentyfive seconds // called at 10hz diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 8c4f1ff6cb..1689d84d4c 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -92,19 +92,49 @@ static void calc_loiter(int x_error, int y_error) nav_lon = constrain(nav_lon, -3500, 3500); } +static void calc_loiter2(int x_error, int y_error) +{ + static int last_x_error = 0; + static int last_y_error = 0; + + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); + y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); + + int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); + int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); + + // find the rates: + x_actual_speed = (float)(last_x_error - x_error)/dTnav; + y_actual_speed = (float)(last_y_error - y_error)/dTnav; + + // save speeds + last_x_error = x_error; + last_y_error = y_error; + + y_rate_error = y_target_speed - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); + nav_lat = constrain(nav_lat, -3500, 3500); + + x_rate_error = x_target_speed - x_actual_speed; + x_rate_error = constrain(x_rate_error, -250, 250); + nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); + nav_lon = constrain(nav_lon, -3500, 3500); +} + // nav_roll, nav_pitch static void calc_loiter_pitch_roll() { - float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); - float _cos_yaw_x = cos(temp); - float _sin_yaw_y = sin(temp); + //float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); + //float _cos_yaw_x = cos(temp); + //float _sin_yaw_y = sin(temp); -// Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x); + //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y); // rotate the vector - nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; - nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y; + nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; + nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y; // flip pitch because forward is negative nav_pitch = -nav_pitch; diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index faca5889c5..649ef66774 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -4,6 +4,13 @@ // ---------------------------------------------------------------------------- static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static void default_dead_zones() +{ + g.rc_1.set_dead_zone(60); + g.rc_2.set_dead_zone(60); + g.rc_3.set_dead_zone(60); + g.rc_4.set_dead_zone(200); +} static void init_rc_in() { @@ -29,14 +36,9 @@ static void init_rc_in() g.rc_4.dead_zone = 300; */ - g.rc_1.set_dead_zone(60); - g.rc_2.set_dead_zone(60); - g.rc_3.set_dead_zone(60); - g.rc_4.set_dead_zone(200); //set auxiliary ranges g.rc_5.set_range(0,1000); - g.rc_5.set_filter(false); g.rc_6.set_range(0,1000); g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 2823ae6bd2..1979ee328a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -134,6 +134,9 @@ static void init_ardupilot() // save the new format version g.format_version.set_and_save(Parameters::k_format_version); + // save default radio values + default_dead_zones(); + Serial.printf_P(PSTR("Please Run Setup...\n")); while (true) { delay(1000); @@ -150,6 +153,8 @@ static void init_ardupilot() } }else{ + // save default radio values + //default_dead_zones(); // Load all auto-loaded EEPROM variables AP_Var::load_all(); diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index fbee3da2de..2ceb2ff77a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -266,19 +266,20 @@ static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of pla static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? static long hold_course = -1; // deg * 100 dir of plane -static byte command_must_index; // current command memory location -static byte command_may_index; // current command memory location -static byte command_must_ID; // current command ID -static byte command_may_ID; // current command ID +static byte command_index; // current command memory location +static byte nav_command_index; // active nav command memory location +static byte non_nav_command_index; // active non-nav command memory location +static byte nav_command_ID = NO_COMMAND; // active nav command ID +static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID // Airspeed // -------- static int airspeed; // m/s * 100 -static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -static float airspeed_error; // m/s * 100 -static float airspeed_fbwB; // m/s * 100 -static long energy_error; // energy state error (kinetic + potential) for altitude hold -static long airspeed_energy_error; // kinetic portion of energy error +static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +static float airspeed_error; // m/s * 100 +static float airspeed_fbwB; // m/s * 100 +static long energy_error; // energy state error (kinetic + potential) for altitude hold +static long airspeed_energy_error; // kinetic portion of energy error // Location Errors // --------------- @@ -360,8 +361,9 @@ static struct Location home; // home location static struct Location prev_WP; // last waypoint static struct Location current_loc; // current location static struct Location next_WP; // next waypoint -static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint +static struct Location guided_WP; // guided mode waypoint +static struct Location next_nav_command; // command preloaded +static struct Location next_nonnav_command; // command preloaded static long target_altitude; // used for altitude management between waypoints static long offset_altitude; // used for altitude management between waypoints static bool home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -739,7 +741,7 @@ static void update_current_flight_mode(void) if(control_mode == AUTO){ crash_checker(); - switch(command_must_ID){ + switch(nav_command_ID){ case MAV_CMD_NAV_TAKEOFF: if (hold_course > -1) { calc_nav_roll(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 23a4d4b298..22bbd0dfe3 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -532,7 +532,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_waypoint_current_send( chan, - g.waypoint_index); + g.command_index); } static void NOINLINE send_statustext(mavlink_channel_t chan) @@ -838,7 +838,7 @@ GCS_MAVLINK::update(void) } if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { send_message(MSG_NEXT_WAYPOINT); } @@ -1278,7 +1278,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_count_send( chan,msg->sysid, msg->compid, - g.waypoint_total + 1); // + home + g.command_total + 1); // + home waypoint_timelast_send = millis(); waypoint_sending = true; @@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; // send waypoint - tell_command = get_wp_with_index(packet.seq); + tell_command = get_cmd_with_index(packet.seq); // set frame of waypoint uint8_t frame; @@ -1320,7 +1320,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - if (packet.seq == (uint16_t)g.waypoint_index) + if (packet.seq == (uint16_t)g.command_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) @@ -1435,8 +1435,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; - // clear all waypoints - g.waypoint_total.set_and_save(0); + // clear all commands + g.command_total.set_and_save(0); // note that we don't send multiple acks, as otherwise a // GCS that is doing a clear followed by a set may see @@ -1455,7 +1455,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.waypoint_index); + mavlink_msg_waypoint_current_send(chan, g.command_index); break; } @@ -1470,7 +1470,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.count > MAX_WAYPOINTS) { packet.count = MAX_WAYPOINTS; } - g.waypoint_total.set_and_save(packet.count - 1); + g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); waypoint_receiving = true; @@ -1645,13 +1645,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) goto mission_failed; } - set_wp_with_index(tell_command, packet.seq); + set_cmd_with_index(tell_command, packet.seq); // update waypoint receiving state machine waypoint_timelast_receive = millis(); waypoint_request_i++; - if (waypoint_request_i > (uint16_t)g.waypoint_total){ + if (waypoint_request_i > (uint16_t)g.command_total){ mavlink_msg_waypoint_ack_send( chan, msg->sysid, @@ -1988,7 +1988,7 @@ void GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 09bfdd7cf7..310c837cef 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -380,15 +380,15 @@ static void Log_Write_Startup(byte type) DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_STARTUP_MSG); DataFlash.WriteByte(type); - DataFlash.WriteByte(g.waypoint_total); + DataFlash.WriteByte(g.command_total); DataFlash.WriteByte(END_BYTE); // create a location struct to hold the temp Waypoints for printing - struct Location cmd = get_wp_with_index(0); + struct Location cmd = get_cmd_with_index(0); Log_Write_Cmd(0, &cmd); - for (int i = 1; i <= g.waypoint_total; i++){ - cmd = get_wp_with_index(i); + for (int i = 1; i <= g.command_total; i++){ + cmd = get_cmd_with_index(i); Log_Write_Cmd(i, &cmd); } } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index e831686f48..330f19bd5b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -162,8 +162,8 @@ public: // 220: Waypoint data // k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, + k_param_command_total, + k_param_command_index, k_param_waypoint_radius, k_param_loiter_radius, @@ -254,8 +254,8 @@ public: // Waypoints // AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; + AP_Int8 command_total; + AP_Int8 command_index; AP_Int8 waypoint_radius; AP_Int8 loiter_radius; @@ -371,8 +371,8 @@ public: airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), /* XXX waypoint_mode missing here */ - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + command_total (0, k_param_command_total, PSTR("CMD_TOTAL")), + command_index (0, k_param_command_index, PSTR("CMD_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index e9d5ebd0aa..a9f7934e74 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -1,22 +1,45 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* Functions in this file: + void init_commands() + void update_auto() + void reload_commands_airstart() + struct Location get_cmd_with_index(int i) + void set_cmd_with_index(struct Location temp, int i) + void increment_cmd_index() + void decrement_cmd_index() + long read_alt_to_hold() + void set_next_WP(struct Location *wp) + void set_guided_WP(void) + void init_home() +************************************************************ +*/ + static void init_commands() { - //read_EEPROM_waypoint_info(); - g.waypoint_index.set_and_save(0); - command_must_index = 0; - command_may_index = 0; - next_command.id = CMD_BLANK; + g.command_index.set_and_save(0); + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; } static void update_auto() { - if (g.waypoint_index >= g.waypoint_total) { + if (g.command_index >= g.command_total) { handle_no_commands(); - if(g.waypoint_total == 0) { + if(g.command_total == 0) { next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs } + } else { + if(g.command_index != 0) { + g.command_index = nav_command_index; + nav_command_index--; + } + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; + process_next_command(); } } @@ -24,20 +47,20 @@ static void update_auto() static void reload_commands_airstart() { init_commands(); - g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? - decrement_WP_index(); + g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all? + decrement_cmd_index(); } // Getters // ------- -static struct Location get_wp_with_index(int i) +static struct Location get_cmd_with_index(int i) { struct Location temp; long mem; // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- - if (i > g.waypoint_total) { + if (i > g.command_total) { temp.id = CMD_BLANK; }else{ // read WP position @@ -70,9 +93,9 @@ static struct Location get_wp_with_index(int i) // Setters // ------- -static void set_wp_with_index(struct Location temp, int i) +static void set_cmd_with_index(struct Location temp, int i) { - i = constrain(i, 0, g.waypoint_total.get()); + i = constrain(i, 0, g.command_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); // Set altitude options bitmask @@ -101,17 +124,17 @@ static void set_wp_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); } -static void increment_WP_index() +static void increment_cmd_index() { - if (g.waypoint_index <= g.waypoint_total) { - g.waypoint_index.set_and_save(g.waypoint_index + 1); + if (g.command_index <= g.command_total) { + g.command_index.set_and_save(g.command_index + 1); } } -static void decrement_WP_index() +static void decrement_cmd_index() { - if (g.waypoint_index > 0) { - g.waypoint_index.set_and_save(g.waypoint_index - 1); + if (g.command_index > 0) { + g.command_index.set_and_save(g.command_index - 1); } } @@ -225,9 +248,9 @@ void init_home() gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); - // Save Home to EEPROM + // Save Home to EEPROM - Command 0 // ------------------- - set_wp_with_index(home, 0); + set_cmd_with_index(home, 0); // Save prev loc // ------------- diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 8f73520e31..11145912d8 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -4,13 +4,14 @@ // Command Event Handlers /********************************************************************************/ static void -handle_process_must() +handle_process_nav_cmd() { // reset navigation integrators // ------------------------- reset_I(); - switch(next_command.id){ + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); + switch(next_nav_command.id){ case MAV_CMD_NAV_TAKEOFF: do_takeoff(); @@ -46,9 +47,10 @@ handle_process_must() } static void -handle_process_may() +handle_process_condition_command() { - switch(next_command.id){ + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); + switch(next_nonnav_command.id){ case MAV_CMD_CONDITION_DELAY: do_wait_delay(); @@ -62,15 +64,14 @@ handle_process_may() do_change_alt(); break; - /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_may() also) + /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also) gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set")); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - landing_pitch = next_command.lng * 100; - g.airspeed_cruise = next_command.alt * 100; - g.throttle_cruise = next_command.lat; - landing_distance = next_command.p1; - //landing_roll = command.lng; + landing_pitch = next_nav_command.lng * 100; + g.airspeed_cruise = next_nav_command.alt * 100; + g.throttle_cruise = next_nav_command.lat; + landing_distance = next_nav_command.p1; SendDebug_P("MSG: throttle_cruise = "); SendDebugln(g.throttle_cruise,DEC); @@ -82,9 +83,10 @@ handle_process_may() } } -static void handle_process_now() +static void handle_process_do_command() { - switch(next_command.id){ + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); + switch(next_nonnav_command.id){ case MAV_CMD_DO_JUMP: do_jump(); @@ -118,18 +120,23 @@ static void handle_process_now() static void handle_no_commands() { - next_command = home; - next_command.alt = read_alt_to_hold(); - next_command.id = MAV_CMD_NAV_LOITER_UNLIM; + gcs_send_text_fmt(PSTR("Returning to Home")); + next_nav_command = home; + next_nav_command.alt = read_alt_to_hold(); + next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; + nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM; + non_nav_command_ID = WAIT_COMMAND; + handle_process_nav_cmd(); + } /********************************************************************************/ // Verify command Handlers /********************************************************************************/ -static bool verify_must() +static bool verify_nav_command() // Returns true if command complete { - switch(command_must_ID) { + switch(nav_command_ID) { case MAV_CMD_NAV_TAKEOFF: return verify_takeoff(); @@ -160,15 +167,15 @@ static bool verify_must() break; default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd")); return false; break; } } -static bool verify_may() +static bool verify_condition_command() // Returns true if command complete { - switch(command_may_ID) { + switch(non_nav_command_ID) { case NO_COMMAND: break; @@ -183,9 +190,14 @@ static bool verify_may() case MAV_CMD_CONDITION_CHANGE_ALT: return verify_change_alt(); break; + + case WAIT_COMMAND: + return 0; + break; + default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); break; } return false; @@ -212,12 +224,12 @@ static void do_RTL(void) static void do_takeoff() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - takeoff_pitch = (int)next_command.p1 * 100; + takeoff_pitch = (int)next_nav_command.p1 * 100; //Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch); //Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt); - takeoff_altitude = next_command.alt; + takeoff_altitude = next_nav_command.alt; //Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude); next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs @@ -227,30 +239,30 @@ static void do_takeoff() static void do_nav_wp() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_land() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_loiter_unlimited() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_loiter_turns() { - set_next_WP(&next_command); - loiter_total = next_command.p1 * 360; + set_next_WP(&next_nav_command); + loiter_total = next_nav_command.p1 * 360; } static void do_loiter_time() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); loiter_time = millis(); - loiter_time_max = next_command.p1; // units are (seconds * 10) + loiter_time_max = next_nav_command.p1; // units are (seconds * 10) } /********************************************************************************/ @@ -287,7 +299,7 @@ static bool verify_takeoff() static bool verify_land() { - // we don't verify landing - we never go to a new Must command after Land + // we don't verify landing - we never go to a new Nav command after Land if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300)){ @@ -317,7 +329,7 @@ static bool verify_nav_wp() hold_course = -1; update_crosstrack(); if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { - gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),command_must_index); + gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index); return true; } // add in a more complex case @@ -341,7 +353,7 @@ static bool verify_loiter_time() update_loiter(); calc_bearing_error(); if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); return true; } return false; @@ -353,7 +365,7 @@ static bool verify_loiter_turns() calc_bearing_error(); if(loiter_sum > loiter_total) { loiter_total = 0; - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); // clear the command queue; return true; } @@ -377,13 +389,13 @@ static bool verify_RTL() static void do_wait_delay() { condition_start = millis(); - condition_value = next_command.lat * 1000; // convert to milliseconds + condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds } static void do_change_alt() { - condition_rate = next_command.lat; - condition_value = next_command.alt; + condition_rate = next_nonnav_command.lat; + condition_value = next_nonnav_command.alt; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations @@ -391,7 +403,7 @@ static void do_change_alt() static void do_within_distance() { - condition_value = next_command.lat; + condition_value = next_nonnav_command.lat; } /********************************************************************************/ @@ -438,53 +450,55 @@ static void do_loiter_at_location() static void do_jump() { struct Location temp; - if(next_command.lat > 0) { + if(next_nonnav_command.lat > 0) { - command_must_index = 0; - command_may_index = 0; - temp = get_wp_with_index(g.waypoint_index); - temp.lat = next_command.lat - 1; // Decrement repeat counter + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + temp = get_cmd_with_index(g.command_index); + temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter - set_wp_with_index(temp, g.waypoint_index); - g.waypoint_index.set_and_save(next_command.p1 - 1); - } else if (next_command.lat == -1) { - g.waypoint_index.set_and_save(next_command.p1 - 1); + set_cmd_with_index(temp, g.command_index); + g.command_index.set_and_save(next_nonnav_command.p1 - 1); + } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + g.command_index.set_and_save(next_nonnav_command.p1 - 1); } } static void do_change_speed() { // Note: we have no implementation for commanded ground speed, only air speed and throttle - if(next_command.alt > 0) - g.airspeed_cruise.set_and_save(next_command.alt * 100); + if(next_nonnav_command.alt > 0) + g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100); - if(next_command.lat > 0) - g.throttle_cruise.set_and_save(next_command.lat); + if(next_nonnav_command.lat > 0) + g.throttle_cruise.set_and_save(next_nonnav_command.lat); } static void do_set_home() { - if(next_command.p1 == 1 && GPS_enabled) { + if(next_nonnav_command.p1 == 1 && GPS_enabled) { init_home(); } else { home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = next_command.lng; // Lon * 10**7 - home.lat = next_command.lat; // Lat * 10**7 - home.alt = max(next_command.alt, 0); + home.lng = next_nonnav_command.lng; // Lon * 10**7 + home.lat = next_nonnav_command.lat; // Lat * 10**7 + home.alt = max(next_nonnav_command.alt, 0); home_is_set = true; } } static void do_set_servo() { - APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); + APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); } static void do_set_relay() { - if (next_command.p1 == 1) { + if (next_nonnav_command.p1 == 1) { relay.on(); - } else if (next_command.p1 == 0) { + } else if (next_nonnav_command.p1 == 0) { relay.off(); }else{ relay.toggle(); @@ -493,16 +507,16 @@ static void do_set_relay() static void do_repeat_servo() { - event_id = next_command.p1 - 1; + event_id = next_nonnav_command.p1 - 1; - if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) { + if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { event_timer = 0; - event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.lat * 2; - event_value = next_command.alt; + event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.lat * 2; + event_value = next_nonnav_command.alt; - switch(next_command.p1) { + switch(next_nonnav_command.p1) { case CH_5: event_undo_value = g.rc_5.radio_trim; break; @@ -524,7 +538,7 @@ static void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; - event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.alt * 2; + event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.alt * 2; update_events(); } diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 35d3a02e64..5ef18b36b1 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -4,14 +4,16 @@ //---------------------------------------- static void change_command(uint8_t cmd_index) { - struct Location temp = get_wp_with_index(cmd_index); + struct Location temp = get_cmd_with_index(cmd_index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { - command_must_index = NO_COMMAND; - next_command.id = NO_COMMAND; - g.waypoint_index.set_and_save(cmd_index - 1); - load_next_command_from_EEPROM(); + gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); + nav_command_ID = NO_COMMAND; + next_nav_command.id = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + nav_command_index = cmd_index - 1; + g.command_index.set_and_save(cmd_index - 1); process_next_command(); } } @@ -21,136 +23,121 @@ static void change_command(uint8_t cmd_index) // -------------------- static void update_commands(void) { - // This function loads commands into three buffers - // when a new command is loaded, it is processed with process_XXX() - // ---------------------------------------------------------------- if(home_is_set == false){ return; // don't do commands } if(control_mode == AUTO){ - load_next_command_from_EEPROM(); process_next_command(); } // Other (eg GCS_Auto) modes may be implemented here } static void verify_commands(void) { - if(verify_must()){ - command_must_index = NO_COMMAND; + if(verify_nav_command()){ + nav_command_ID = NO_COMMAND; } - if(verify_may()){ - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + if(verify_condition_command()){ + non_nav_command_ID = NO_COMMAND; } } -static void load_next_command_from_EEPROM() -{ - // fetch next command if the next command queue is empty - // ----------------------------------------------------- - if(next_command.id == NO_COMMAND){ - next_command = get_wp_with_index(g.waypoint_index + 1); - } - - // If the preload failed, return or just Loiter - // generate a dynamic command for RTL - // -------------------------------------------- - if(next_command.id == NO_COMMAND){ - // we are out of commands! - gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - handle_no_commands(); - } -} static void process_next_command() { + // This function makes sure that we always have a current navigation command + // and loads conditional or immediate commands if applicable + + struct Location temp; + byte old_index; + // these are Navigation/Must commands // --------------------------------- - if (command_must_index == 0){ // no current command loaded - if (next_command.id < MAV_CMD_NAV_LAST ){ - increment_WP_index(); - //save_command_index(); // TO DO - fix - to Recover from in air Restart - command_must_index = g.waypoint_index; - - //SendDebug_P("MSG new command_must_id "); - //SendDebug(next_command.id,DEC); - //SendDebug_P(" index:"); - //SendDebugln(command_must_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_must(); + if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded + old_index = nav_command_index; + temp.id = MAV_CMD_NAV_LAST; + while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { + nav_command_index++; + temp = get_cmd_with_index(nav_command_index); + } + gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index); + if(nav_command_index > g.command_total){ + // we are out of commands! + gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); + handle_no_commands(); + } else { + next_nav_command = temp; + nav_command_ID = next_nav_command.id; + non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded + non_nav_command_ID = NO_COMMAND; + + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nav_command); + } + process_nav_cmd(); } } - // these are Condition/May commands - // ---------------------- - if (command_may_index == 0){ - if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ - increment_WP_index(); // this command is from the command list in EEPROM - command_may_index = g.waypoint_index; - //SendDebug_P("MSG new command_may_id "); - //SendDebug(next_command.id,DEC); - //Serial.printf_P(PSTR("new command_may_index ")); - //Serial.println(command_may_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_may(); + // these are Condition/May and Do/Now commands + // ------------------------------------------- + if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command + non_nav_command_index = old_index + 1; + //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); + } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command + non_nav_command_index++; + } + + //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index); + //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); + //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID); + if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) { + temp = get_cmd_with_index(non_nav_command_index); + if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do + g.command_index.set_and_save(nav_command_index); + non_nav_command_index = nav_command_index; + non_nav_command_ID = WAIT_COMMAND; + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); + } else { // The next command is a non-nav command. Prepare to execute it. + g.command_index.set_and_save(non_nav_command_index); + next_nonnav_command = temp; + non_nav_command_ID = next_nonnav_command.id; + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nonnav_command); + } + + process_non_nav_command(); } - // these are Do/Now commands - // --------------------------- - if (next_command.id > MAV_CMD_CONDITION_LAST){ - increment_WP_index(); // this command is from the command list in EEPROM - //SendDebug_P("MSG new command_now_id "); - //SendDebug(next_command.id,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_now(); - } } } /**************************************************/ // These functions implement the commands. /**************************************************/ -static void process_must() +static void process_nav_cmd() { - gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); + //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); - // clear May indexes - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + // clear non-nav command ID and index + non_nav_command_index = NO_COMMAND; // Redundant - remove? + non_nav_command_ID = NO_COMMAND; // Redundant - remove? - command_must_ID = next_command.id; - handle_process_must(); + handle_process_nav_cmd(); - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; } -static void process_may() +static void process_non_nav_command() { - gcs_send_text_P(SEVERITY_LOW,PSTR("")); + //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); - command_may_ID = next_command.id; - handle_process_may(); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; + if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { + handle_process_condition_command(); + } else { + handle_process_do_command(); + // flag command ID so a new one is loaded + // ----------------------------------------- + non_nav_command_ID = NO_COMMAND; + } } - -static void process_now() -{ - handle_process_now(); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; - - gcs_send_text(SEVERITY_LOW, ""); -} - diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b049d477bd..19a3dc5ba5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -77,6 +77,7 @@ // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library #define CMD_BLANK 0 // there is no command stored in the mem location requested #define NO_COMMAND 0 +#define WAIT_COMMAND 255 // Command/Waypoint/Location Options Bitmask //-------------------- @@ -169,7 +170,7 @@ enum gcs_severity { // Events // ------ #define EVENT_WILL_REACH_WAYPOINT 1 -#define EVENT_SET_NEW_WAYPOINT_INDEX 2 +#define EVENT_SET_NEW_COMMAND_INDEX 2 #define EVENT_LOADED_WAYPOINT 3 #define EVENT_LOOP 4 diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 3c67117767..2f1e803c7a 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -100,7 +100,7 @@ static void calc_altitude_error() }else{ target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); } - } else if (command_may_ID != MAV_CMD_CONDITION_CHANGE_ALT) { + } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { target_altitude = next_WP.alt; } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 9c0adbd26d..b60876b509 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -306,12 +306,12 @@ test_wp(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100); } - Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total); + Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - for(byte i = 0; i <= g.waypoint_total; i++){ - struct Location temp = get_wp_with_index(i); + for(byte i = 0; i <= g.command_total; i++){ + struct Location temp = get_cmd_with_index(i); test_wp_print(&temp, i); } diff --git a/libraries/APM_PI/APM_PI.cpp b/libraries/APM_PI/APM_PI.cpp index c07b55210f..5b18fc4ed7 100644 --- a/libraries/APM_PI/APM_PI.cpp +++ b/libraries/APM_PI/APM_PI.cpp @@ -12,7 +12,7 @@ APM_PI::get_pi(int32_t error, float dt) { _integrator += ((float)error * _ki) * dt; -if (_integrator < -_imax) { + if (_integrator < -_imax) { _integrator = -_imax; } else if (_integrator > _imax) { _integrator = _imax; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 11af18e10a..d290dd01cd 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -38,7 +38,7 @@ RC_Channel::set_angle(int angle) void RC_Channel::set_dead_zone(int dzone) { - _dead_zone = abs(dzone >>1); + _dead_zone.set_and_save(abs(dzone >>1)); } void diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 9ce43fd1c0..1bbe02fd58 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -26,9 +26,10 @@ class RC_Channel{ radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0), radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0), _high(1), - _filter(true), + _filter(false), _reverse (&_group, 3, 1, name ? PSTR("REV") : 0), - _dead_zone(0), + _dead_zone (&_group, 4, 0, name ? PSTR("DZ") : 0), + //_dead_zone(0), scale_output(1.0) {} @@ -97,7 +98,8 @@ class RC_Channel{ bool _filter; AP_Int8 _reverse; - int16_t _dead_zone; // used to keep noise down and create a dead zone. + AP_Int16 _dead_zone; + //int16_t _dead_zone; // used to keep noise down and create a dead zone. uint8_t _type; int16_t _high; int16_t _low;