This commit is contained in:
James Goppert 2011-10-27 16:21:44 -04:00
commit 3de8533c5e
23 changed files with 395 additions and 288 deletions

View File

@ -39,7 +39,7 @@
*/ */
#define ACCEL_ALT_HOLD 0 // disabled by default, work in progress #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 // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode
// See the config.h and defines.h files for how to set this up! // See the config.h and defines.h files for how to set this up!

View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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 ArduCopter Version 2.0 Beta
Authors: Jason Short Authors: Jason Short
@ -1022,8 +1022,10 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL: case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){ 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{ }else{
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
g.pi_rate_roll.reset_I(); g.pi_rate_roll.reset_I();
g.pi_rate_pitch.reset_I(); g.pi_rate_pitch.reset_I();
g.rc_3.servo_out = 0; g.rc_3.servo_out = 0;
@ -1050,8 +1052,7 @@ void update_throttle_mode(void)
invalid_throttle = false; invalid_throttle = false;
} }
// apply throttle control at 200 hz g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost() + alt_hold_velocity();
break; break;
} }
} }
@ -1171,14 +1172,6 @@ static void update_trig(void){
// 90° = cos_yaw: 1.00, sin_yaw: 0.00, // 90° = cos_yaw: 1.00, sin_yaw: 0.00,
// 180 = cos_yaw: 0.00, sin_yaw: -1.00, // 180 = cos_yaw: 0.00, sin_yaw: -1.00,
// 270 = cos_yaw: -1.00, sin_yaw: 0.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 // updated at 10hz

View File

@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle)
#define ALT_ERROR_MAX 400 #define ALT_ERROR_MAX 400
static int static int
get_nav_throttle(long z_error) get_nav_throttle(long z_error)
{ {
// limit error to prevent I term run up // limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); 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); 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 static int
get_rate_roll(long target_rate) get_rate_roll(long target_rate)
{ {
long error; long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
target_rate = constrain(target_rate, -2500, 2500); return g.pi_acro_roll.get_pi(error, G_Dt);
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);
} }
static int static int
get_rate_pitch(long target_rate) get_rate_pitch(long target_rate)
{ {
long error; long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
target_rate = constrain(target_rate, -2500, 2500); return g.pi_acro_pitch.get_pi(error, G_Dt);
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);
} }
static int static int
@ -139,7 +152,7 @@ get_rate_yaw(long target_rate)
static void reset_hold_I(void) static void reset_hold_I(void)
{ {
g.pi_loiter_lat.reset_I(); g.pi_loiter_lat.reset_I();
g.pi_loiter_lat.reset_I(); g.pi_loiter_lon.reset_I();
g.pi_crosstrack.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() static int alt_hold_velocity()
{ {
#if ACCEL_ALT_HOLD == 1 #if ACCEL_ALT_HOLD == 1
Vector3f accel_filt;
float error;
// subtract filtered Accel // 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; accel_filt = imu.get_accel_filtered();
error = min(error, 200.0); accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
error = max(error, 0.0); int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12
error = 1 - (error/ 200.0);
float sum = accels_rot_sum / (float)accels_rot_count;
accels_rot_sum = 0; //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
accels_rot_count = 0; return constrain(output, -70, 70);
int output = (sum + 9.81) * alt_hold_gain * error;
// fast rise // fast rise
//s: -17.6241, g:0.0000, e:1.0000, o:0 //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: -19.3193, g:0.0000, e:1.0000, o:0
//s: -13.1310, g:47.8700, e:1.0000, o:-158 //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 #else
return 0; return 0;
#endif #endif
} }
static int get_angle_boost() static int get_angle_boost(int value)
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0); temp = 1.0 - constrain(temp, .5, 1.0);
return (int)(temp * g.throttle_cruise); return (int)(temp * value);
} }

View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly // The increment will prevent old parameters from being used incorrectly
// by newer code. // 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 // The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega) // and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -156,7 +156,7 @@ public:
// //
// 240: PI/D Controllers // 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_pitch,
k_param_pi_rate_yaw, k_param_pi_rate_yaw,
k_param_pi_stabilize_roll, k_param_pi_stabilize_roll,
@ -169,6 +169,8 @@ public:
k_param_pi_alt_hold, k_param_pi_alt_hold,
k_param_pi_throttle, k_param_pi_throttle,
k_param_pi_crosstrack, k_param_pi_crosstrack,
k_param_pi_acro_roll,
k_param_pi_acro_pitch,
// 254,255: reserved // 254,255: reserved
@ -286,6 +288,9 @@ public:
APM_PI pi_throttle; APM_PI pi_throttle;
APM_PI pi_crosstrack; APM_PI pi_crosstrack;
APM_PI pi_acro_roll;
APM_PI pi_acro_pitch;
uint8_t junk; uint8_t junk;
// Note: keep initializers here in the same order as they are declared above. // 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_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_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 junk(0) // XXX just so that we can add things without worrying about the trailing comma
{ {
} }

View File

@ -395,7 +395,30 @@
#endif #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 #ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.145 # define RATE_ROLL_P 0.145

View File

@ -139,8 +139,12 @@ static void auto_trim()
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
clear_leds(); clear_leds();
imu.save(); imu.save();
//Serial.println("Done"); //Serial.println("Done");
auto_level_counter = 0; auto_level_counter = 0;
// set TC
init_throttle_cruise();
} }
} }
} }

View File

@ -2,8 +2,8 @@
#define ARM_DELAY 10 // one second #define ARM_DELAY 10 // one second
#define DISARM_DELAY 10 // one second #define DISARM_DELAY 10 // one second
#define LEVEL_DELAY 120 // twelve seconds #define LEVEL_DELAY 70 // twelve seconds
#define AUTO_LEVEL_DELAY 150 // twentyfive seconds #define AUTO_LEVEL_DELAY 90 // twentyfive seconds
// called at 10hz // called at 10hz

View File

@ -92,19 +92,49 @@ static void calc_loiter(int x_error, int y_error)
nav_lon = constrain(nav_lon, -3500, 3500); 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 // nav_roll, nav_pitch
static void calc_loiter_pitch_roll() static void calc_loiter_pitch_roll()
{ {
float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); //float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
float _cos_yaw_x = cos(temp); //float _cos_yaw_x = cos(temp);
float _sin_yaw_y = sin(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 // rotate the vector
nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; 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_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative // flip pitch because forward is negative
nav_pitch = -nav_pitch; nav_pitch = -nav_pitch;

View File

@ -4,6 +4,13 @@
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling 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() static void init_rc_in()
{ {
@ -29,14 +36,9 @@ static void init_rc_in()
g.rc_4.dead_zone = 300; 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 //set auxiliary ranges
g.rc_5.set_range(0,1000); g.rc_5.set_range(0,1000);
g.rc_5.set_filter(false);
g.rc_6.set_range(0,1000); g.rc_6.set_range(0,1000);
g.rc_7.set_range(0,1000); g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000); g.rc_8.set_range(0,1000);

View File

@ -134,6 +134,9 @@ static void init_ardupilot()
// save the new format version // save the new format version
g.format_version.set_and_save(Parameters::k_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")); Serial.printf_P(PSTR("Please Run Setup...\n"));
while (true) { while (true) {
delay(1000); delay(1000);
@ -150,6 +153,8 @@ static void init_ardupilot()
} }
}else{ }else{
// save default radio values
//default_dead_zones();
// Load all auto-loaded EEPROM variables // Load all auto-loaded EEPROM variables
AP_Var::load_all(); AP_Var::load_all();

View File

@ -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 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 long hold_course = -1; // deg * 100 dir of plane
static byte command_must_index; // current command memory location static byte command_index; // current command memory location
static byte command_may_index; // current command memory location static byte nav_command_index; // active nav command memory location
static byte command_must_ID; // current command ID static byte non_nav_command_index; // active non-nav command memory location
static byte command_may_ID; // current command ID 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 // Airspeed
// -------- // --------
static int airspeed; // m/s * 100 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 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_error; // m/s * 100
static float airspeed_fbwB; // m/s * 100 static float airspeed_fbwB; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error static long airspeed_energy_error; // kinetic portion of energy error
// Location Errors // Location Errors
// --------------- // ---------------
@ -360,8 +361,9 @@ static struct Location home; // home location
static struct Location prev_WP; // last waypoint static struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location static struct Location current_loc; // current location
static struct Location next_WP; // next waypoint 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 target_altitude; // used for altitude management between waypoints
static long offset_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 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){ if(control_mode == AUTO){
crash_checker(); crash_checker();
switch(command_must_ID){ switch(nav_command_ID){
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
if (hold_course > -1) { if (hold_course > -1) {
calc_nav_roll(); calc_nav_roll();

View File

@ -532,7 +532,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
{ {
mavlink_msg_waypoint_current_send( mavlink_msg_waypoint_current_send(
chan, chan,
g.waypoint_index); g.command_index);
} }
static void NOINLINE send_statustext(mavlink_channel_t chan) static void NOINLINE send_statustext(mavlink_channel_t chan)
@ -838,7 +838,7 @@ GCS_MAVLINK::update(void)
} }
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) { waypoint_request_i <= (unsigned)g.command_total) {
send_message(MSG_NEXT_WAYPOINT); send_message(MSG_NEXT_WAYPOINT);
} }
@ -1278,7 +1278,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_count_send( mavlink_msg_waypoint_count_send(
chan,msg->sysid, chan,msg->sysid,
msg->compid, msg->compid,
g.waypoint_total + 1); // + home g.command_total + 1); // + home
waypoint_timelast_send = millis(); waypoint_timelast_send = millis();
waypoint_sending = true; waypoint_sending = true;
@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
// send waypoint // send waypoint
tell_command = get_wp_with_index(packet.seq); tell_command = get_cmd_with_index(packet.seq);
// set frame of waypoint // set frame of waypoint
uint8_t frame; uint8_t frame;
@ -1320,7 +1320,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds // time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false) 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; current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false) 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); mavlink_msg_waypoint_clear_all_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) break; if (mavlink_check_target(packet.target_system, packet.target_component)) break;
// clear all waypoints // clear all commands
g.waypoint_total.set_and_save(0); g.command_total.set_and_save(0);
// note that we don't send multiple acks, as otherwise a // note that we don't send multiple acks, as otherwise a
// GCS that is doing a clear followed by a set may see // 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 // set current command
change_command(packet.seq); change_command(packet.seq);
mavlink_msg_waypoint_current_send(chan, g.waypoint_index); mavlink_msg_waypoint_current_send(chan, g.command_index);
break; break;
} }
@ -1470,7 +1470,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.count > MAX_WAYPOINTS) { if (packet.count > MAX_WAYPOINTS) {
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_timelast_receive = millis();
waypoint_receiving = true; waypoint_receiving = true;
@ -1645,13 +1645,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
goto mission_failed; goto mission_failed;
} }
set_wp_with_index(tell_command, packet.seq); set_cmd_with_index(tell_command, packet.seq);
// update waypoint receiving state machine // update waypoint receiving state machine
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_request_i++; 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( mavlink_msg_waypoint_ack_send(
chan, chan,
msg->sysid, msg->sysid,
@ -1988,7 +1988,7 @@ void
GCS_MAVLINK::queued_waypoint_send() GCS_MAVLINK::queued_waypoint_send()
{ {
if (waypoint_receiving && if (waypoint_receiving &&
waypoint_request_i <= (unsigned)g.waypoint_total) { waypoint_request_i <= (unsigned)g.command_total) {
mavlink_msg_waypoint_request_send( mavlink_msg_waypoint_request_send(
chan, chan,
waypoint_dest_sysid, waypoint_dest_sysid,

View File

@ -380,15 +380,15 @@ static void Log_Write_Startup(byte type)
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG); DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type); DataFlash.WriteByte(type);
DataFlash.WriteByte(g.waypoint_total); DataFlash.WriteByte(g.command_total);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing // 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); Log_Write_Cmd(0, &cmd);
for (int i = 1; i <= g.waypoint_total; i++){ for (int i = 1; i <= g.command_total; i++){
cmd = get_wp_with_index(i); cmd = get_cmd_with_index(i);
Log_Write_Cmd(i, &cmd); Log_Write_Cmd(i, &cmd);
} }
} }

View File

@ -162,8 +162,8 @@ public:
// 220: Waypoint data // 220: Waypoint data
// //
k_param_waypoint_mode = 220, k_param_waypoint_mode = 220,
k_param_waypoint_total, k_param_command_total,
k_param_waypoint_index, k_param_command_index,
k_param_waypoint_radius, k_param_waypoint_radius,
k_param_loiter_radius, k_param_loiter_radius,
@ -254,8 +254,8 @@ public:
// Waypoints // Waypoints
// //
AP_Int8 waypoint_mode; AP_Int8 waypoint_mode;
AP_Int8 waypoint_total; AP_Int8 command_total;
AP_Int8 waypoint_index; AP_Int8 command_index;
AP_Int8 waypoint_radius; AP_Int8 waypoint_radius;
AP_Int8 loiter_radius; AP_Int8 loiter_radius;
@ -371,8 +371,8 @@ public:
airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")),
/* XXX waypoint_mode missing here */ /* XXX waypoint_mode missing here */
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), command_total (0, k_param_command_total, PSTR("CMD_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), command_index (0, k_param_command_index, PSTR("CMD_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), 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")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),

View File

@ -1,22 +1,45 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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() static void init_commands()
{ {
//read_EEPROM_waypoint_info(); g.command_index.set_and_save(0);
g.waypoint_index.set_and_save(0); nav_command_ID = NO_COMMAND;
command_must_index = 0; non_nav_command_ID = NO_COMMAND;
command_may_index = 0; next_nav_command.id = CMD_BLANK;
next_command.id = CMD_BLANK;
} }
static void update_auto() static void update_auto()
{ {
if (g.waypoint_index >= g.waypoint_total) { if (g.command_index >= g.command_total) {
handle_no_commands(); 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.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 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() static void reload_commands_airstart()
{ {
init_commands(); init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_WP_index(); decrement_cmd_index();
} }
// Getters // Getters
// ------- // -------
static struct Location get_wp_with_index(int i) static struct Location get_cmd_with_index(int i)
{ {
struct Location temp; struct Location temp;
long mem; long mem;
// Find out proper location in memory by using the start_byte position + the index // 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; temp.id = CMD_BLANK;
}else{ }else{
// read WP position // read WP position
@ -70,9 +93,9 @@ static struct Location get_wp_with_index(int i)
// Setters // 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); uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask // 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); 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) { if (g.command_index <= g.command_total) {
g.waypoint_index.set_and_save(g.waypoint_index + 1); 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) { if (g.command_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1); 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); 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 // Save prev loc
// ------------- // -------------

View File

@ -4,13 +4,14 @@
// Command Event Handlers // Command Event Handlers
/********************************************************************************/ /********************************************************************************/
static void static void
handle_process_must() handle_process_nav_cmd()
{ {
// reset navigation integrators // reset navigation integrators
// ------------------------- // -------------------------
reset_I(); 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: case MAV_CMD_NAV_TAKEOFF:
do_takeoff(); do_takeoff();
@ -46,9 +47,10 @@ handle_process_must()
} }
static void 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: case MAV_CMD_CONDITION_DELAY:
do_wait_delay(); do_wait_delay();
@ -62,15 +64,14 @@ handle_process_may()
do_change_alt(); do_change_alt();
break; 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")); gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
landing_pitch = next_command.lng * 100; landing_pitch = next_nav_command.lng * 100;
g.airspeed_cruise = next_command.alt * 100; g.airspeed_cruise = next_nav_command.alt * 100;
g.throttle_cruise = next_command.lat; g.throttle_cruise = next_nav_command.lat;
landing_distance = next_command.p1; landing_distance = next_nav_command.p1;
//landing_roll = command.lng;
SendDebug_P("MSG: throttle_cruise = "); SendDebug_P("MSG: throttle_cruise = ");
SendDebugln(g.throttle_cruise,DEC); 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: case MAV_CMD_DO_JUMP:
do_jump(); do_jump();
@ -118,18 +120,23 @@ static void handle_process_now()
static void handle_no_commands() static void handle_no_commands()
{ {
next_command = home; gcs_send_text_fmt(PSTR("Returning to Home"));
next_command.alt = read_alt_to_hold(); next_nav_command = home;
next_command.id = MAV_CMD_NAV_LOITER_UNLIM; 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 // 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: case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff(); return verify_takeoff();
@ -160,15 +167,15 @@ static bool verify_must()
break; break;
default: 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; return false;
break; 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: case NO_COMMAND:
break; break;
@ -183,9 +190,14 @@ static bool verify_may()
case MAV_CMD_CONDITION_CHANGE_ALT: case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt(); return verify_change_alt();
break; break;
case WAIT_COMMAND:
return 0;
break;
default: 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; break;
} }
return false; return false;
@ -212,12 +224,12 @@ static void do_RTL(void)
static void do_takeoff() 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 // 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("TO pitch:")); Serial.println(takeoff_pitch);
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt); //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); //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.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 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() static void do_nav_wp()
{ {
set_next_WP(&next_command); set_next_WP(&next_nav_command);
} }
static void do_land() static void do_land()
{ {
set_next_WP(&next_command); set_next_WP(&next_nav_command);
} }
static void do_loiter_unlimited() static void do_loiter_unlimited()
{ {
set_next_WP(&next_command); set_next_WP(&next_nav_command);
} }
static void do_loiter_turns() static void do_loiter_turns()
{ {
set_next_WP(&next_command); set_next_WP(&next_nav_command);
loiter_total = next_command.p1 * 360; loiter_total = next_nav_command.p1 * 360;
} }
static void do_loiter_time() static void do_loiter_time()
{ {
set_next_WP(&next_command); set_next_WP(&next_nav_command);
loiter_time = millis(); 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() 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))) if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|| (current_loc.alt <= next_WP.alt + 300)){ || (current_loc.alt <= next_WP.alt + 300)){
@ -317,7 +329,7 @@ static bool verify_nav_wp()
hold_course = -1; hold_course = -1;
update_crosstrack(); update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { 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; return true;
} }
// add in a more complex case // add in a more complex case
@ -341,7 +353,7 @@ static bool verify_loiter_time()
update_loiter(); update_loiter();
calc_bearing_error(); calc_bearing_error();
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds 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 true;
} }
return false; return false;
@ -353,7 +365,7 @@ static bool verify_loiter_turns()
calc_bearing_error(); calc_bearing_error();
if(loiter_sum > loiter_total) { if(loiter_sum > loiter_total) {
loiter_total = 0; 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; // clear the command queue;
return true; return true;
} }
@ -377,13 +389,13 @@ static bool verify_RTL()
static void do_wait_delay() static void do_wait_delay()
{ {
condition_start = millis(); 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() static void do_change_alt()
{ {
condition_rate = next_command.lat; condition_rate = next_nonnav_command.lat;
condition_value = next_command.alt; condition_value = next_nonnav_command.alt;
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations next_WP.alt = condition_value; // For future nav calculations
offset_altitude = 0; // 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() 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() static void do_jump()
{ {
struct Location temp; struct Location temp;
if(next_command.lat > 0) { if(next_nonnav_command.lat > 0) {
command_must_index = 0; nav_command_ID = NO_COMMAND;
command_may_index = 0; non_nav_command_ID = NO_COMMAND;
temp = get_wp_with_index(g.waypoint_index); temp = get_cmd_with_index(g.command_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
set_wp_with_index(temp, g.waypoint_index); set_cmd_with_index(temp, g.command_index);
g.waypoint_index.set_and_save(next_command.p1 - 1); g.command_index.set_and_save(next_nonnav_command.p1 - 1);
} else if (next_command.lat == -1) { } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
g.waypoint_index.set_and_save(next_command.p1 - 1); 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() static void do_change_speed()
{ {
// Note: we have no implementation for commanded ground speed, only air speed and throttle // Note: we have no implementation for commanded ground speed, only air speed and throttle
if(next_command.alt > 0) if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set_and_save(next_command.alt * 100); g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
if(next_command.lat > 0) if(next_nonnav_command.lat > 0)
g.throttle_cruise.set_and_save(next_command.lat); g.throttle_cruise.set_and_save(next_nonnav_command.lat);
} }
static void do_set_home() static void do_set_home()
{ {
if(next_command.p1 == 1 && GPS_enabled) { if(next_nonnav_command.p1 == 1 && GPS_enabled) {
init_home(); init_home();
} else { } else {
home.id = MAV_CMD_NAV_WAYPOINT; home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_command.lng; // Lon * 10**7 home.lng = next_nonnav_command.lng; // Lon * 10**7
home.lat = next_command.lat; // Lat * 10**7 home.lat = next_nonnav_command.lat; // Lat * 10**7
home.alt = max(next_command.alt, 0); home.alt = max(next_nonnav_command.alt, 0);
home_is_set = true; home_is_set = true;
} }
} }
static void do_set_servo() 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() static void do_set_relay()
{ {
if (next_command.p1 == 1) { if (next_nonnav_command.p1 == 1) {
relay.on(); relay.on();
} else if (next_command.p1 == 0) { } else if (next_nonnav_command.p1 == 0) {
relay.off(); relay.off();
}else{ }else{
relay.toggle(); relay.toggle();
@ -493,16 +507,16 @@ static void do_set_relay()
static void do_repeat_servo() 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_timer = 0;
event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.lat * 2; event_repeat = next_nonnav_command.lat * 2;
event_value = next_command.alt; event_value = next_nonnav_command.alt;
switch(next_command.p1) { switch(next_nonnav_command.p1) {
case CH_5: case CH_5:
event_undo_value = g.rc_5.radio_trim; event_undo_value = g.rc_5.radio_trim;
break; break;
@ -524,7 +538,7 @@ static void do_repeat_relay()
{ {
event_id = RELAY_TOGGLE; event_id = RELAY_TOGGLE;
event_timer = 0; event_timer = 0;
event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.alt * 2; event_repeat = next_nonnav_command.alt * 2;
update_events(); update_events();
} }

View File

@ -4,14 +4,16 @@
//---------------------------------------- //----------------------------------------
static void change_command(uint8_t cmd_index) 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 ){ if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else { } else {
command_must_index = NO_COMMAND; gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
next_command.id = NO_COMMAND; nav_command_ID = NO_COMMAND;
g.waypoint_index.set_and_save(cmd_index - 1); next_nav_command.id = NO_COMMAND;
load_next_command_from_EEPROM(); non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index - 1);
process_next_command(); process_next_command();
} }
} }
@ -21,136 +23,121 @@ static void change_command(uint8_t cmd_index)
// -------------------- // --------------------
static void update_commands(void) 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){ if(home_is_set == false){
return; // don't do commands return; // don't do commands
} }
if(control_mode == AUTO){ if(control_mode == AUTO){
load_next_command_from_EEPROM();
process_next_command(); process_next_command();
} // Other (eg GCS_Auto) modes may be implemented here } // Other (eg GCS_Auto) modes may be implemented here
} }
static void verify_commands(void) static void verify_commands(void)
{ {
if(verify_must()){ if(verify_nav_command()){
command_must_index = NO_COMMAND; nav_command_ID = NO_COMMAND;
} }
if(verify_may()){ if(verify_condition_command()){
command_may_index = NO_COMMAND; non_nav_command_ID = NO_COMMAND;
command_may_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() 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 // these are Navigation/Must commands
// --------------------------------- // ---------------------------------
if (command_must_index == 0){ // no current command loaded if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){ old_index = nav_command_index;
increment_WP_index(); temp.id = MAV_CMD_NAV_LAST;
//save_command_index(); // TO DO - fix - to Recover from in air Restart while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
command_must_index = g.waypoint_index; nav_command_index++;
temp = get_cmd_with_index(nav_command_index);
//SendDebug_P("MSG <process_next_command> new command_must_id "); }
//SendDebug(next_command.id,DEC); gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
//SendDebug_P(" index:"); if(nav_command_index > g.command_total){
//SendDebugln(command_must_index,DEC); // we are out of commands!
if (g.log_bitmask & MASK_LOG_CMD) gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
Log_Write_Cmd(g.waypoint_index, &next_command); handle_no_commands();
process_must(); } 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 // these are Condition/May and Do/Now commands
// ---------------------- // -------------------------------------------
if (command_may_index == 0){ if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ non_nav_command_index = old_index + 1;
increment_WP_index(); // this command is from the command list in EEPROM //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
command_may_index = g.waypoint_index; } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
//SendDebug_P("MSG <process_next_command> new command_may_id "); non_nav_command_index++;
//SendDebug(next_command.id,DEC); }
//Serial.printf_P(PSTR("new command_may_index "));
//Serial.println(command_may_index,DEC); //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
if (g.log_bitmask & MASK_LOG_CMD) //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
Log_Write_Cmd(g.waypoint_index, &next_command); //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
process_may(); 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 <process_next_command> 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. // These functions implement the commands.
/**************************************************/ /**************************************************/
static void process_must() static void process_nav_cmd()
{ {
gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>")); //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
// clear May indexes // clear non-nav command ID and index
command_may_index = NO_COMMAND; non_nav_command_index = NO_COMMAND; // Redundant - remove?
command_may_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND; // Redundant - remove?
command_must_ID = next_command.id; handle_process_nav_cmd();
handle_process_must();
// 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("<process_may>")); //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
command_may_ID = next_command.id; if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_may(); handle_process_condition_command();
} else {
// invalidate command so a new one is loaded handle_process_do_command();
// ----------------------------------------- // flag command ID so a new one is loaded
next_command.id = NO_COMMAND; // -----------------------------------------
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, "<process_now>");
}

View File

@ -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 // 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 CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0 #define NO_COMMAND 0
#define WAIT_COMMAND 255
// Command/Waypoint/Location Options Bitmask // Command/Waypoint/Location Options Bitmask
//-------------------- //--------------------
@ -169,7 +170,7 @@ enum gcs_severity {
// Events // Events
// ------ // ------
#define EVENT_WILL_REACH_WAYPOINT 1 #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_LOADED_WAYPOINT 3
#define EVENT_LOOP 4 #define EVENT_LOOP 4

View File

@ -100,7 +100,7 @@ static void calc_altitude_error()
}else{ }else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); 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; target_altitude = next_WP.alt;
} }

View File

@ -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("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("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.waypoint_total; i++){ for(byte i = 0; i <= g.command_total; i++){
struct Location temp = get_wp_with_index(i); struct Location temp = get_cmd_with_index(i);
test_wp_print(&temp, i); test_wp_print(&temp, i);
} }

View File

@ -12,7 +12,7 @@ APM_PI::get_pi(int32_t error, float dt)
{ {
_integrator += ((float)error * _ki) * dt; _integrator += ((float)error * _ki) * dt;
if (_integrator < -_imax) { if (_integrator < -_imax) {
_integrator = -_imax; _integrator = -_imax;
} else if (_integrator > _imax) { } else if (_integrator > _imax) {
_integrator = _imax; _integrator = _imax;

View File

@ -38,7 +38,7 @@ RC_Channel::set_angle(int angle)
void void
RC_Channel::set_dead_zone(int dzone) RC_Channel::set_dead_zone(int dzone)
{ {
_dead_zone = abs(dzone >>1); _dead_zone.set_and_save(abs(dzone >>1));
} }
void void

View File

@ -26,9 +26,10 @@ class RC_Channel{
radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0), radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0),
radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0), radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0),
_high(1), _high(1),
_filter(true), _filter(false),
_reverse (&_group, 3, 1, name ? PSTR("REV") : 0), _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) scale_output(1.0)
{} {}
@ -97,7 +98,8 @@ class RC_Channel{
bool _filter; bool _filter;
AP_Int8 _reverse; 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; uint8_t _type;
int16_t _high; int16_t _high;
int16_t _low; int16_t _low;