mirror of https://github.com/ArduPilot/ardupilot
This commit is contained in:
commit
3de8533c5e
|
@ -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!
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 -= 100;
|
||||
error = min(error, 200.0);
|
||||
error = abs(next_WP.alt - current_loc.alt) - 25;
|
||||
error = min(error, 50.0);
|
||||
error = max(error, 0.0);
|
||||
error = 1 - (error/ 200.0);
|
||||
float sum = accels_rot_sum / (float)accels_rot_count;
|
||||
error = 1 - (error/ 50.0);
|
||||
|
||||
accels_rot_sum = 0;
|
||||
accels_rot_count = 0;
|
||||
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
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
{
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -266,10 +266,11 @@ 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
|
||||
// --------
|
||||
|
@ -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 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();
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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")),
|
||||
|
||||
|
|
|
@ -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
|
||||
// -------------
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
@ -184,8 +191,13 @@ static bool verify_may()
|
|||
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();
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
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;
|
||||
|
||||
//SendDebug_P("MSG <process_next_command> 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 (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 <process_next_command> 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++;
|
||||
}
|
||||
|
||||
// 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();
|
||||
//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 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
|
||||
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("<process_may>"));
|
||||
//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
|
||||
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
|
||||
// -----------------------------------------
|
||||
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>");
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue