mirror of https://github.com/ArduPilot/ardupilot
Cleaned up guided mode
Reduced ADC filter to 3 from 6 to increase speed of filter.
This commit is contained in:
Jason Short
parent
a6182965c1
commit
ecd14f4425
|
|
@ -1464,7 +1464,6 @@ static void tuning(){
|
|||
|
||||
static void update_nav_wp()
|
||||
{
|
||||
// XXX Guided mode!!!
|
||||
if(wp_control == LOITER_MODE){
|
||||
|
||||
// calc a pitch to the target
|
||||
|
|
|
|||
|
|
@ -1,5 +1,18 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
|
||||
/*static void init_auto()
|
||||
{
|
||||
//if (g.waypoint_index == g.waypoint_total) {
|
||||
// do_RTL();
|
||||
//}
|
||||
|
||||
// initialize commands
|
||||
// -------------------
|
||||
init_commands();
|
||||
}
|
||||
*/
|
||||
|
||||
static void init_commands()
|
||||
{
|
||||
// zero is home, but we always load the next command (1), in the code.
|
||||
|
|
@ -20,18 +33,6 @@ static void clear_command_queue(){
|
|||
next_command.id = NO_COMMAND;
|
||||
}
|
||||
|
||||
static void init_auto()
|
||||
{
|
||||
//if (g.waypoint_index == g.waypoint_total) {
|
||||
// Serial.println("ia_f");
|
||||
// do_RTL();
|
||||
//}
|
||||
|
||||
// initialize commands
|
||||
// -------------------
|
||||
init_commands();
|
||||
}
|
||||
|
||||
// Getters
|
||||
// -------
|
||||
static struct Location get_command_with_index(int i)
|
||||
|
|
@ -41,8 +42,6 @@ static struct Location get_command_with_index(int i)
|
|||
// Find out proper location in memory by using the start_byte position + the index
|
||||
// --------------------------------------------------------------------------------
|
||||
if (i > g.waypoint_total) {
|
||||
Serial.println("XCD");
|
||||
|
||||
// we do not have a valid command to load
|
||||
// return a WP with a "Blank" id
|
||||
temp.id = CMD_BLANK;
|
||||
|
|
@ -51,7 +50,6 @@ static struct Location get_command_with_index(int i)
|
|||
return temp;
|
||||
|
||||
}else{
|
||||
//Serial.println("LD");
|
||||
// we can load a command, we don't process it yet
|
||||
// read WP position
|
||||
long mem = (WP_START_BYTE) + (i * WP_SIZE);
|
||||
|
|
@ -75,10 +73,9 @@ static struct Location get_command_with_index(int i)
|
|||
}
|
||||
|
||||
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
|
||||
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
|
||||
//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
|
||||
//temp.alt += home.alt;
|
||||
}
|
||||
//Serial.println("ADD ALT");
|
||||
//}
|
||||
|
||||
if(temp.options & WP_OPTION_RELATIVE){
|
||||
// If were relative, just offset from home
|
||||
|
|
@ -217,14 +214,12 @@ static void init_home()
|
|||
home.lng = g_gps->longitude; // Lon * 10**7
|
||||
home.lat = g_gps->latitude; // Lat * 10**7
|
||||
//home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
|
||||
home.alt = 0; // this is a test
|
||||
home.alt = 0; // Home is always 0
|
||||
home_is_set = true;
|
||||
|
||||
// to point yaw towards home until we set it with Mavlink
|
||||
target_WP = home;
|
||||
|
||||
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
|
||||
|
||||
// Save Home to EEPROM
|
||||
// -------------------
|
||||
// no need to save this to EPROM
|
||||
|
|
@ -233,8 +228,14 @@ static void init_home()
|
|||
|
||||
// Save prev loc this makes the calcs look better before commands are loaded
|
||||
prev_WP = home;
|
||||
|
||||
// this is dangerous since we can get GPS lock at any time.
|
||||
//next_WP = home;
|
||||
|
||||
// Load home for a default guided_WP
|
||||
// -------------
|
||||
guided_WP = home;
|
||||
guided_WP.alt += g.RTL_altitude;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -105,17 +105,14 @@ static void handle_process_now()
|
|||
|
||||
static void handle_no_commands()
|
||||
{
|
||||
// we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous.
|
||||
// use landing commands
|
||||
/*
|
||||
switch (control_mode){
|
||||
default:
|
||||
//set_mode(RTL);
|
||||
set_mode(RTL);
|
||||
break;
|
||||
}
|
||||
return;
|
||||
*/
|
||||
Serial.println("Handle No CMDs");
|
||||
}*/
|
||||
//return;
|
||||
//Serial.println("Handle No CMDs");
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
|
|
|||
|
|
@ -407,7 +407,8 @@ static void set_mode(byte mode)
|
|||
init_throttle_cruise();
|
||||
|
||||
// loads the commands from where we left off
|
||||
init_auto();
|
||||
//init_auto();
|
||||
init_commands();
|
||||
|
||||
// do crosstrack correction
|
||||
// XXX move to flight commands
|
||||
|
|
@ -440,6 +441,7 @@ static void set_mode(byte mode)
|
|||
//xtrack_enabled = true;
|
||||
init_throttle_cruise();
|
||||
next_WP = current_loc;
|
||||
set_next_WP(&guided_WP);
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@
|
|||
#define ADC_DATAIN 50 // MISO
|
||||
#define ADC_SPICLOCK 52 // SCK
|
||||
#define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40
|
||||
#define ADC_FILTER_SIZE 6
|
||||
#define ADC_FILTER_SIZE 3
|
||||
|
||||
#include "AP_ADC.h"
|
||||
#include <inttypes.h>
|
||||
|
|
|
|||
Loading…
Reference in New Issue