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

Merge https://code.google.com/p/ardupilot-mega

This commit is contained in:
James Goppert 2011-10-27 16:21:44 -04:00
parent 5d9c37bbed c7d91a199c
commit 3de8533c5e
23 changed files with 395 additions and 288 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/APM_Config.h
View File

@ -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!

17
ArduCopter/ArduCopter.pde
View File

@ -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

73
ArduCopter/Attitude.pde
View File

@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle)
#define ALT_ERROR_MAX 400
static int
get_nav_throttle(long z_error)
get_nav_throttle(long z_error)
{
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
@ -98,28 +98,41 @@ get_nav_throttle(long z_error)
return (int)g.pi_throttle.get_pi(rate_error, .1);
}
#define ALT_ERROR_MAX2 300
static int
get_nav_throttle2(long z_error)
{
if (z_error > ALT_ERROR_MAX2){
return g.pi_throttle.kP() * 80;
}else if (z_error < -ALT_ERROR_MAX2){
return g.pi_throttle.kP() * -60;
} else{
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2);
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
rate_error = rate_error - altitude_rate;
// limit the rate
rate_error = constrain(rate_error, -100, 120);
return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity();
}
}
static int
get_rate_roll(long target_rate)
{
long error;
target_rate = constrain(target_rate, -2500, 2500);
error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
target_rate = g.pi_rate_roll.get_pi(error, G_Dt);
// output control:
return (int)constrain(target_rate, -2500, 2500);
long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
return g.pi_acro_roll.get_pi(error, G_Dt);
}
static int
get_rate_pitch(long target_rate)
{
long error;
target_rate = constrain(target_rate, -2500, 2500);
error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
target_rate = g.pi_rate_pitch.get_pi(error, G_Dt);
// output control:
return (int)constrain(target_rate, -2500, 2500);
long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
return g.pi_acro_pitch.get_pi(error, G_Dt);
}
static int
@ -139,7 +152,7 @@ get_rate_yaw(long target_rate)
static void reset_hold_I(void)
{
g.pi_loiter_lat.reset_I();
g.pi_loiter_lat.reset_I();
g.pi_loiter_lon.reset_I();
g.pi_crosstrack.reset_I();
}
@ -189,19 +202,21 @@ get_nav_yaw_offset(int yaw_input, int reset)
static int alt_hold_velocity()
{
#if ACCEL_ALT_HOLD == 1
Vector3f accel_filt;
float error;
// subtract filtered Accel
float error = abs(next_WP.alt - current_loc.alt);
error = abs(next_WP.alt - current_loc.alt) - 25;
error = min(error, 50.0);
error = max(error, 0.0);
error = 1 - (error/ 50.0);
error -= 100;
error = min(error, 200.0);
error = max(error, 0.0);
error = 1 - (error/ 200.0);
float sum = accels_rot_sum / (float)accels_rot_count;
accel_filt = imu.get_accel_filtered();
accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12
accels_rot_sum = 0;
accels_rot_count = 0;
int output = (sum + 9.81) * alt_hold_gain * error;
//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
return constrain(output, -70, 70);
// fast rise
//s: -17.6241, g:0.0000, e:1.0000, o:0
@ -209,17 +224,15 @@ static int alt_hold_velocity()
//s: -19.3193, g:0.0000, e:1.0000, o:0
//s: -13.1310, g:47.8700, e:1.0000, o:-158
//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
return output;
#else
return 0;
#endif
}
static int get_angle_boost()
static int get_angle_boost(int value)
{
float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0);
return (int)(temp * g.throttle_cruise);
return (int)(temp * value);
}

12
ArduCopter/Parameters.h
View File

@ -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
{
}

25
ArduCopter/config.h
View File

@ -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

4
ArduCopter/control_modes.pde
View File

@ -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();
}
}
}

4
ArduCopter/motors.pde
View File

@ -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

42
ArduCopter/navigation.pde
View File

@ -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;

12
ArduCopter/radio.pde
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 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);

5
ArduCopter/system.pde
View File

@ -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();

26
ArduPlane/ArduPlane.pde
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 long hold_course = -1; // deg * 100 dir of plane
static byte command_must_index; // current command memory location
static byte command_may_index; // current command memory location
static byte command_must_ID; // current command ID
static byte command_may_ID; // current command ID
static byte command_index; // current command memory location
static byte nav_command_index; // active nav command memory location
static byte non_nav_command_index; // active non-nav command memory location
static byte nav_command_ID = NO_COMMAND; // active nav command ID
static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID
// Airspeed
// --------
static int airspeed; // m/s * 100
static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
static float airspeed_error; // m/s * 100
static float airspeed_fbwB; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error
static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
static float airspeed_error; // m/s * 100
static float airspeed_fbwB; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error
// Location Errors
// ---------------
@ -360,8 +361,9 @@ static struct Location home; // home location
static struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location
static struct Location next_WP; // next waypoint
static struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static struct Location guided_WP; // guided mode waypoint
static struct Location next_nav_command; // command preloaded
static struct Location next_nonnav_command; // command preloaded
static long target_altitude; // used for altitude management between waypoints
static long offset_altitude; // used for altitude management between waypoints
static bool home_is_set; // Flag for if we have g_gps lock and have set the home location
@ -739,7 +741,7 @@ static void update_current_flight_mode(void)
if(control_mode == AUTO){
crash_checker();
switch(command_must_ID){
switch(nav_command_ID){
case MAV_CMD_NAV_TAKEOFF:
if (hold_course > -1) {
calc_nav_roll();

24
ArduPlane/GCS_Mavlink.pde
View File

@ -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,

8
ArduPlane/Log.pde
View File

@ -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);
}
}

12
ArduPlane/Parameters.h
View File

@ -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")),

65
ArduPlane/commands.pde
View File

@ -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
// -------------

148
ArduPlane/commands_logic.pde
View File

@ -4,13 +4,14 @@
// Command Event Handlers
/********************************************************************************/
static void
handle_process_must()
handle_process_nav_cmd()
{
// reset navigation integrators
// -------------------------
reset_I();
switch(next_command.id){
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
switch(next_nav_command.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
@ -46,9 +47,10 @@ handle_process_must()
}
static void
handle_process_may()
handle_process_condition_command()
{
switch(next_command.id){
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
@ -62,15 +64,14 @@ handle_process_may()
do_change_alt();
break;
/* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_may() also)
/* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also)
gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
landing_pitch = next_command.lng * 100;
g.airspeed_cruise = next_command.alt * 100;
g.throttle_cruise = next_command.lat;
landing_distance = next_command.p1;
//landing_roll = command.lng;
landing_pitch = next_nav_command.lng * 100;
g.airspeed_cruise = next_nav_command.alt * 100;
g.throttle_cruise = next_nav_command.lat;
landing_distance = next_nav_command.p1;
SendDebug_P("MSG: throttle_cruise = ");
SendDebugln(g.throttle_cruise,DEC);
@ -82,9 +83,10 @@ handle_process_may()
}
}
static void handle_process_now()
static void handle_process_do_command()
{
switch(next_command.id){
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){
case MAV_CMD_DO_JUMP:
do_jump();
@ -118,18 +120,23 @@ static void handle_process_now()
static void handle_no_commands()
{
next_command = home;
next_command.alt = read_alt_to_hold();
next_command.id = MAV_CMD_NAV_LOITER_UNLIM;
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home;
next_nav_command.alt = read_alt_to_hold();
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd();
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
static bool verify_must()
static bool verify_nav_command() // Returns true if command complete
{
switch(command_must_ID) {
switch(nav_command_ID) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
@ -160,15 +167,15 @@ static bool verify_must()
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd"));
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
return false;
break;
}
}
static bool verify_may()
static bool verify_condition_command() // Returns true if command complete
{
switch(command_may_ID) {
switch(non_nav_command_ID) {
case NO_COMMAND:
break;
@ -183,9 +190,14 @@ static bool verify_may()
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
case WAIT_COMMAND:
return 0;
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd"));
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
break;
}
return false;
@ -212,12 +224,12 @@ static void do_RTL(void)
static void do_takeoff()
{
set_next_WP(&next_command);
set_next_WP(&next_nav_command);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch = (int)next_command.p1 * 100;
takeoff_pitch = (int)next_nav_command.p1 * 100;
//Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch);
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt);
takeoff_altitude = next_command.alt;
takeoff_altitude = next_nav_command.alt;
//Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude);
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
@ -227,30 +239,30 @@ static void do_takeoff()
static void do_nav_wp()
{
set_next_WP(&next_command);
set_next_WP(&next_nav_command);
}
static void do_land()
{
set_next_WP(&next_command);
set_next_WP(&next_nav_command);
}
static void do_loiter_unlimited()
{
set_next_WP(&next_command);
set_next_WP(&next_nav_command);
}
static void do_loiter_turns()
{
set_next_WP(&next_command);
loiter_total = next_command.p1 * 360;
set_next_WP(&next_nav_command);
loiter_total = next_nav_command.p1 * 360;
}
static void do_loiter_time()
{
set_next_WP(&next_command);
set_next_WP(&next_nav_command);
loiter_time = millis();
loiter_time_max = next_command.p1; // units are (seconds * 10)
loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
}
/********************************************************************************/
@ -287,7 +299,7 @@ static bool verify_takeoff()
static bool verify_land()
{
// we don't verify landing - we never go to a new Must command after Land
// we don't verify landing - we never go to a new Nav command after Land
if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|| (current_loc.alt <= next_WP.alt + 300)){
@ -317,7 +329,7 @@ static bool verify_nav_wp()
hold_course = -1;
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),command_must_index);
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
return true;
}
// add in a more complex case
@ -341,7 +353,7 @@ static bool verify_loiter_time()
update_loiter();
calc_bearing_error();
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true;
}
return false;
@ -353,7 +365,7 @@ static bool verify_loiter_turns()
calc_bearing_error();
if(loiter_sum > loiter_total) {
loiter_total = 0;
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete"));
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
// clear the command queue;
return true;
}
@ -377,13 +389,13 @@ static bool verify_RTL()
static void do_wait_delay()
{
condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
{
condition_rate = next_command.lat;
condition_value = next_command.alt;
condition_rate = next_nonnav_command.lat;
condition_value = next_nonnav_command.alt;
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations
offset_altitude = 0; // For future nav calculations
@ -391,7 +403,7 @@ static void do_change_alt()
static void do_within_distance()
{
condition_value = next_command.lat;
condition_value = next_nonnav_command.lat;
}
/********************************************************************************/
@ -438,53 +450,55 @@ static void do_loiter_at_location()
static void do_jump()
{
struct Location temp;
if(next_command.lat > 0) {
if(next_nonnav_command.lat > 0) {
command_must_index = 0;
command_may_index = 0;
temp = get_wp_with_index(g.waypoint_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
temp = get_cmd_with_index(g.command_index);
temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter
set_wp_with_index(temp, g.waypoint_index);
g.waypoint_index.set_and_save(next_command.p1 - 1);
} else if (next_command.lat == -1) {
g.waypoint_index.set_and_save(next_command.p1 - 1);
set_cmd_with_index(temp, g.command_index);
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
} else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
g.command_index.set_and_save(next_nonnav_command.p1 - 1);
}
}
static void do_change_speed()
{
// Note: we have no implementation for commanded ground speed, only air speed and throttle
if(next_command.alt > 0)
g.airspeed_cruise.set_and_save(next_command.alt * 100);
if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
if(next_command.lat > 0)
g.throttle_cruise.set_and_save(next_command.lat);
if(next_nonnav_command.lat > 0)
g.throttle_cruise.set_and_save(next_nonnav_command.lat);
}
static void do_set_home()
{
if(next_command.p1 == 1 && GPS_enabled) {
if(next_nonnav_command.p1 == 1 && GPS_enabled) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_command.lng; // Lon * 10**7
home.lat = next_command.lat; // Lat * 10**7
home.alt = max(next_command.alt, 0);
home.lng = next_nonnav_command.lng; // Lon * 10**7
home.lat = next_nonnav_command.lat; // Lat * 10**7
home.alt = max(next_nonnav_command.alt, 0);
home_is_set = true;
}
}
static void do_set_servo()
{
APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()
{
if (next_command.p1 == 1) {
if (next_nonnav_command.p1 == 1) {
relay.on();
} else if (next_command.p1 == 0) {
} else if (next_nonnav_command.p1 == 0) {
relay.off();
}else{
relay.toggle();
@ -493,16 +507,16 @@ static void do_set_relay()
static void do_repeat_servo()
{
event_id = next_command.p1 - 1;
event_id = next_nonnav_command.p1 - 1;
if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) {
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.lat * 2;
event_value = next_command.alt;
event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.lat * 2;
event_value = next_nonnav_command.alt;
switch(next_command.p1) {
switch(next_nonnav_command.p1) {
case CH_5:
event_undo_value = g.rc_5.radio_trim;
break;
@ -524,7 +538,7 @@ static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.alt * 2;
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.alt * 2;
update_events();
}

181
ArduPlane/commands_process.pde
View File

@ -4,14 +4,16 @@
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
struct Location temp = get_wp_with_index(cmd_index);
struct Location temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else {
command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND;
g.waypoint_index.set_and_save(cmd_index - 1);
load_next_command_from_EEPROM();
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index - 1);
process_next_command();
}
}
@ -21,136 +23,121 @@ static void change_command(uint8_t cmd_index)
// --------------------
static void update_commands(void)
{
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
// ----------------------------------------------------------------
if(home_is_set == false){
return; // don't do commands
}
if(control_mode == AUTO){
load_next_command_from_EEPROM();
process_next_command();
} // Other (eg GCS_Auto) modes may be implemented here
}
static void verify_commands(void)
{
if(verify_must()){
command_must_index = NO_COMMAND;
if(verify_nav_command()){
nav_command_ID = NO_COMMAND;
}
if(verify_may()){
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
if(verify_condition_command()){
non_nav_command_ID = NO_COMMAND;
}
}
static void load_next_command_from_EEPROM()
{
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if(next_command.id == NO_COMMAND){
next_command = get_wp_with_index(g.waypoint_index + 1);
}
// If the preload failed, return or just Loiter
// generate a dynamic command for RTL
// --------------------------------------------
if(next_command.id == NO_COMMAND){
// we are out of commands!
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
}
}
static void process_next_command()
{
// This function makes sure that we always have a current navigation command
// and loads conditional or immediate commands if applicable
struct Location temp;
byte old_index;
// these are Navigation/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){
increment_WP_index();
//save_command_index(); // TO DO - fix - to Recover from in air Restart
command_must_index = g.waypoint_index;
//SendDebug_P("MSG <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 (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
old_index = nav_command_index;
temp.id = MAV_CMD_NAV_LAST;
while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
nav_command_index++;
temp = get_cmd_with_index(nav_command_index);
}
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total){
// we are out of commands!
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
} else {
next_nav_command = temp;
nav_command_ID = next_nav_command.id;
non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
non_nav_command_ID = NO_COMMAND;
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nav_command);
}
process_nav_cmd();
}
}
// these are Condition/May commands
// ----------------------
if (command_may_index == 0){
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
increment_WP_index(); // this command is from the command list in EEPROM
command_may_index = g.waypoint_index;
//SendDebug_P("MSG <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++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
temp = get_cmd_with_index(non_nav_command_index);
if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do
g.command_index.set_and_save(nav_command_index);
non_nav_command_index = nav_command_index;
non_nav_command_ID = WAIT_COMMAND;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
} else { // The next command is a non-nav command. Prepare to execute it.
g.command_index.set_and_save(non_nav_command_index);
next_nonnav_command = temp;
non_nav_command_ID = next_nonnav_command.id;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nonnav_command);
}
process_non_nav_command();
}
// these are Do/Now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index(); // this command is from the command list in EEPROM
//SendDebug_P("MSG <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.
/**************************************************/
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
// -----------------------------------------
next_command.id = NO_COMMAND;
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command();
} else {
handle_process_do_command();
// flag command ID so a new one is loaded
// -----------------------------------------
non_nav_command_ID = NO_COMMAND;
}
}
static void process_now()
{
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
gcs_send_text(SEVERITY_LOW, "<process_now>");
}

3
ArduPlane/defines.h
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
#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

2
ArduPlane/navigation.pde
View File

@ -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;
}

6
ArduPlane/test.pde
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("%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);
}

2
libraries/APM_PI/APM_PI.cpp
View File

@ -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;

2
libraries/RC_Channel/RC_Channel.cpp
View File

@ -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

8
libraries/RC_Channel/RC_Channel.h
View File

@ -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;