Updates to guided mode from Michael O.

removed or commented unused static vars
This commit is contained in:
Jason Short 2011-10-15 13:09:04 -07:00
parent 65f26f7138
commit 142707711e
10 changed files with 72 additions and 171 deletions

View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.0.48 Beta"
#define THISFIRMWARE "ArduCopter V2.0.49 Beta"
/*
ArduCopter Version 2.0 Beta
Authors: Jason Short
@ -425,13 +425,13 @@ static int event_repeat; // how many times to fire : 0 = forever, 1 = do
static int event_value; // per command value, such as PWM for servos
static int event_undo_value; // the value used to undo commands
//static byte repeat_forever;
static byte undo_event; // counter for timing the undo
//static byte undo_event; // counter for timing the undo
// delay command
// --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start;
static int condition_rate;
//static int condition_rate;
// land command
// ------------
@ -445,12 +445,10 @@ static struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location
static struct Location next_WP; // next waypoint
static struct Location target_WP; // where do we want to you towards?
static struct Location simple_WP; //
static struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static long target_altitude; // used for
static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
static boolean new_location; // flag to tell us if location has been updated
// IMU variables
// -------------
@ -1047,9 +1045,14 @@ static void update_navigation()
case GUIDED:
wp_control = WP_MODE;
// check if we are close to point > loiter
verify_nav_wp();
wp_verify_byte = 0;
verify_nav_wp();
update_auto_yaw();
if (wp_control == WP_MODE) {
update_auto_yaw();
} else {
set_mode(LOITER);
}
update_nav_wp();
break;

View File

@ -42,7 +42,9 @@ static void reset_control_switch()
read_control_switch();
}
#if CH7_OPTION == CH7_SET_HOVER
static boolean trim_flag;
#endif
// read at 10 hz
// set this to your trainer switch

View File

@ -20,30 +20,30 @@ static void heli_init_swash()
int i;
int tilt_max[CH_3+1];
int total_tilt_max = 0;
// swash servo initialisation
g.heli_servo_1.set_range(0,1000);
g.heli_servo_2.set_range(0,1000);
g.heli_servo_3.set_range(0,1000);
g.heli_servo_4.set_angle(4500);
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos));
heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos));
heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos));
// roll factors
heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90));
heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90));
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90));
// collective min / max
total_tilt_max = 0;
for( i=CH_1; i<=CH_3; i++ ) {
tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100;
total_tilt_max = max(total_tilt_max,tilt_max[i]);
}
// servo min/max values - or should I use set_range?
g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1];
g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1];
@ -51,11 +51,11 @@ static void heli_init_swash()
g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2];
g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3];
g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3];
// reset the servo averaging
for( i=0; i<=3; i++ )
heli_servo_out[i] = 0;
// double check heli_servo_averaging is reasonable
if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) {
g.heli_servo_averaging = 0;
@ -87,36 +87,36 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500);
if( g.heli_servo_1.get_reverse() )
g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out;
g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500);
if( g.heli_servo_2.get_reverse() )
g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out;
g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500);
if( g.heli_servo_3.get_reverse() )
g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out;
if( g.heli_servo_4.get_reverse() )
g.heli_servo_4.servo_out = -yaw_out; // should probably just use rc_4 directly like we do for a tricopter
else
g.heli_servo_4.servo_out = yaw_out;
// use servo_out to calculate pwm_out and radio_out
g.heli_servo_1.calc_pwm();
g.heli_servo_2.calc_pwm();
g.heli_servo_3.calc_pwm();
g.heli_servo_4.calc_pwm();
g.heli_servo_4.calc_pwm();
// add the servo values to the averaging
heli_servo_out[0] += g.heli_servo_1.servo_out;
heli_servo_out[1] += g.heli_servo_2.servo_out;
heli_servo_out[2] += g.heli_servo_3.servo_out;
heli_servo_out[3] += g.heli_servo_4.radio_out;
heli_servo_out_count++;
// is it time to move the servos?
if( heli_servo_out_count >= g.heli_servo_averaging ) {
// average the values if necessary
if( g.heli_servo_averaging >= 2 ) {
heli_servo_out[0] /= g.heli_servo_averaging;
@ -124,22 +124,24 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
heli_servo_out[2] /= g.heli_servo_averaging;
heli_servo_out[3] /= g.heli_servo_averaging;
}
// actually move the servos
APM_RC.OutputCh(CH_1, heli_servo_out[0]);
APM_RC.OutputCh(CH_2, heli_servo_out[1]);
APM_RC.OutputCh(CH_3, heli_servo_out[2]);
APM_RC.OutputCh(CH_4, heli_servo_out[3]);
// output gyro value
if( g.heli_ext_gyro_enabled ) {
APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain);
}
// InstantPWM - force message to the servos
#if INSTANT_PWM == 1
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
#endif
// reset the averaging
heli_servo_out_count = 0;
heli_servo_out[0] = 0;
@ -149,6 +151,15 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
}
}
static void init_motors_out()
{
#if INSTANT_PWM == 0
ICR5 = 5000; // 400 hz output CH 1, 2, 9
ICR1 = 5000; // 400 hz output CH 3, 4, 10
ICR3 = 40000; // 50 hz output CH 7, 8, 11
#endif
}
// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better
static void output_motors_armed()
{

View File

@ -95,9 +95,16 @@ static void calc_loiter(int x_error, int y_error)
// 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);
Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x);
// 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;
@ -232,7 +239,7 @@ static void reset_crosstrack()
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
*/
static long get_altitude_above_home(void)
/*static long get_altitude_above_home(void)
{
// This is the altitude above the home location
// The GPS gives us altitude at Sea Level
@ -240,7 +247,7 @@ static long get_altitude_above_home(void)
// -------------------------------------------------------------
return current_loc.alt - home.alt;
}
*/
// distance is returned in meters
static long get_distance(struct Location *loc1, struct Location *loc2)
{
@ -252,12 +259,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2)
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
/*
static long get_alt_distance(struct Location *loc1, struct Location *loc2)
{
return abs(loc1->alt - loc2->alt);
}
*/
static long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;

View File

@ -184,11 +184,3 @@ static void trim_radio()
g.rc_4.save_eeprom();
}
static void trim_yaw()
{
for (byte i = 0; i < 30; i++){
read_radio();
}
g.rc_4.trim(); // yaw
}

View File

@ -1,112 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if 0
#define INPUT_BUF_LEN 40
char input_buffer[INPUT_BUF_LEN];
static void readCommands(void)
{
static byte header[2];
const byte read_GS_header[] = {0x21, 0x21}; //!! Used to verify the payload msg header
if(Serial.available()){
//Serial.println("Serial.available");
byte bufferPointer;
header[0] = Serial.read();
header[1] = Serial.read();
if((header[0] == read_GS_header[0]) && (header[1] == read_GS_header[1])){
// Block until we read full command
// --------------------------------
delay(20);
byte incoming_val = 0;
// Ground Station communication
// ----------------------------
while(Serial.available() > 0)
{
incoming_val = Serial.read();
if (incoming_val != 13 && incoming_val != 10 ) {
input_buffer[bufferPointer++] = incoming_val;
}
if(bufferPointer >= INPUT_BUF_LEN){
Serial.println("Big buffer overrun");
bufferPointer = 0;
input_buffer[0] = 1;
Serial.flush();
memset(input_buffer,0,sizeof(input_buffer));
return;
}
}
parseCommand(input_buffer);
// clear buffer of old data
// ------------------------
memset(input_buffer,0,sizeof(input_buffer));
}else{
Serial.flush();
}
}
}
// Commands can be sent as !!a:100|b:200|c:1
// -----------------------------------------
static void parseCommand(char *buffer)
{
Serial.println("got cmd ");
Serial.println(buffer);
char *token, *saveptr1, *saveptr2;
for (int j = 1;; j++, buffer = NULL) {
token = strtok_r(buffer, "|", &saveptr1);
if (token == NULL) break;
char * cmd = strtok_r(token, ":", &saveptr2);
long value = strtol(strtok_r (NULL,":", &saveptr2), NULL,0);
///*
Serial.print("cmd ");
Serial.print(cmd[0]);
Serial.print("\tval ");
Serial.println(value);
Serial.println("");
//*/
///*
switch(cmd[0]){
case 'P':
g.pi_stabilize_roll.kP((float)value / 1000);
g.pi_stabilize_pitch.kP((float)value / 1000);
g.pi_stabilize_pitch.save_gains();
break;
case 'I':
g.pi_stabilize_roll.kI((float)value / 1000);
g.pi_stabilize_pitch.kI((float)value / 1000);
g.pi_stabilize_pitch.save_gains();
break;
case 'D':
//g.pi_stabilize_roll.kD((float)value / 1000);
//g.pi_stabilize_pitch.kD((float)value / 1000);
break;
case 'X':
g.pi_stabilize_roll.imax(value * 100);
g.pi_stabilize_pitch.imax(value * 100);
g.pi_stabilize_pitch.save_gains();
break;
case 'R':
//g.stabilize_dampener.set_and_save((float)value / 1000);
break;
}
//*/
}
}
#endif

View File

@ -83,7 +83,6 @@ static void init_ardupilot()
//
report_version();
// setup IO pins
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
@ -143,6 +142,7 @@ static void init_ardupilot()
}
}else{
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
}
@ -157,7 +157,6 @@ static void init_ardupilot()
//
Serial3.begin(map_baudrate(g.serial3_baud,SERIAL3_BAUD), 128, 128);
#ifdef RADIO_OVERRIDE_DEFAULTS
{
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
@ -216,11 +215,7 @@ static void init_ardupilot()
//
if (check_startup_for_CLI()) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\n"
"Entering interactive setup mode...\n"
"\n"
"Type 'help' to list commands, 'exit' to leave a submenu.\n"
"Visit the 'setup' menu for first-time configuration.\n\n"));
Serial.printf_P(PSTR("\nCLI:\n\n"));
for (;;) {
//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
main_menu.run();
@ -544,6 +539,6 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
case 111: return 111100;
case 115: return 115200;
}
Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
//Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
return default_baud;
}

View File

@ -29,7 +29,7 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#ifdef OPTFLOW_ENABLED
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
//static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
//static int8_t test_mission(uint8_t argc, const Menu::arg *argv);

View File

@ -1,7 +1,7 @@
#
# Copyright (c) 2010 Andrew Tridgell. All rights reserved.
# based on Arduino.mk, Copyright (c) 2010 Michael Smith
#
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
@ -10,7 +10,7 @@
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
#
# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@ -109,7 +109,7 @@ DEPFLAGS = -MD -MT $@
# XXX warning options TBD
CXXOPTS = -fno-exceptions -D__AVR_ATmega2560__ -I$(SKETCHBOOK)/libraries/Desktop/include -DDESKTOP_BUILD=1
COPTS = -I$(SKETCHBOOK)/libraries/Desktop/include -DDESKTOP_BUILD=1
ASOPTS = -assembler-with-cpp
ASOPTS = -assembler-with-cpp
CXXFLAGS = -g $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS)
CFLAGS = -g $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(COPTS)
@ -171,12 +171,12 @@ else
endif
# these are library objects we don't want in the desktop build (maybe we'll add them later)
NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp RC_Channel/RC_Channel_aux.cpp
#
# Find sketchbook libraries referenced by the sketch.
#
# Include paths for sketch libraries
# Include paths for sketch libraries
#
SKETCHLIBS := $(wildcard $(addprefix $(SKETCHBOOK)/libraries/,$(LIBTOKENS)))
SKETCHLIBNAMES := $(notdir $(SKETCHLIBS))
@ -196,10 +196,10 @@ ARDUINOLIBSRCDIRS := $(ARDUINOLIBS) $(addsuffix /utility,$(ARDUINOLIBS))
ARDUINOLIBSRCS := $(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(ARDUINOLIBSRCDIRS))))
ARDUINOLIBOBJS := $(addsuffix .o,$(basename $(subst $(ARDUINO),$(BUILDROOT),$(ARDUINOLIBSRCS))))
ARDUINOLIBINCLUDES := $(addprefix -I,$(ARDUINOLIBS))
ARDUINOLIBINCLUDES := $(ARDUINOLIBINCLUDES)
ARDUINOLIBINCLUDES := $(ARDUINOLIBINCLUDES)
# Library object files
LIBOBJS := $(SKETCHLIBOBJS)
LIBOBJS := $(SKETCHLIBOBJS)
################################################################################
# Built products
@ -212,7 +212,7 @@ SKETCHELF = $(BUILDROOT)/$(SKETCH).elf
SKETCHMAP = $(BUILDROOT)/$(SKETCH).map
# The core library
CORELIB =
CORELIB =
# All of the objects that may be built
ALLOBJS = $(SKETCHOBJS) $(LIBOBJS) $(CORELIBOBJS)
@ -338,7 +338,7 @@ $(CORELIB): $(CORELIBOBJS)
# This process strives to be as faithful to the Arduino implementation as
# possible. Conceptually, the process is as follows:
#
# * All of the .pde files are concatenated, starting with the file named
# * All of the .pde files are concatenated, starting with the file named
# for the sketch and followed by the others in alphabetical order.
# * An insertion point is created in the concatenated file at
# the first statement that isn't a preprocessor directive or comment.

View File

@ -3,7 +3,7 @@ DESKTOP=$(PWD)/../libraries/Desktop
include ../libraries/Desktop/Desktop.mk
default:
make -f $(DESKTOP)/Makefile.desktop
make -f $(DESKTOP)/Makefile.desktop
nologging:
make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DLOGGING_ENABLED=DISABLED"
@ -19,3 +19,6 @@ hilnocli:
heli:
make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
helihil:
make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"