mirror of https://github.com/ArduPilot/ardupilot
Updates to guided mode from Michael O.
removed or commented unused static vars
This commit is contained in:
Jason Short
parent
65f26f7138
commit
142707711e
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -136,9 +136,11 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
|
|||
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;
|
||||
|
|
@ -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()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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(¤t_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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -171,7 +171,7 @@ 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.
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
Loading…
Reference in New Issue