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

Rover: fixed EKF usage for rover 

added barometer object, and use shared home object from AHRS. This
gives basic EKF operation.
This commit is contained in:
Andrew Tridgell 2014-02-24 09:25:50 +11:00
parent 203073e3ba
commit eac4b1ec96
10 changed files with 54 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
APMrover2/APMrover2.pde
View File

@ -406,7 +406,6 @@ static uint8_t ground_start_count = 5;
// on the ground or in the air. Used to decide if a ground start is appropriate if we
// booted with an air start.
static int16_t ground_start_avg;
static int32_t gps_base_alt;
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
@ -510,7 +509,7 @@ static int16_t condition_rate;
// Location structure defined in AP_Common
////////////////////////////////////////////////////////////////////////////////
// The home location used for RTL. The location is set when we first get stable GPS lock
static struct Location home;
static const struct Location &home = ahrs.get_home();
// Flag for if we have g_gps lock and have set the home location
static bool home_is_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
@ -572,6 +571,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ set_servos, 1, 1500 },
{ update_GPS_50Hz, 1, 2500 },
{ update_GPS_10Hz, 5, 2500 },
{ update_alt, 5, 3400 },
{ navigate, 5, 1600 },
{ update_compass, 5, 2000 },
{ update_commands, 5, 1000 },
@ -684,6 +684,14 @@ static void mount_update(void)
#endif
}
static void update_alt()
{
barometer.read();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
}
/*
check for GCS failsafe - 10Hz
*/

5
APMrover2/Log.pde
View File

@ -519,6 +519,11 @@ static void Log_Write_RC(void)
DataFlash.Log_Write_RCOUT();
}
static void Log_Write_Baro(void)
{
DataFlash.Log_Write_Baro(barometer);
}
static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES,
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),

1
APMrover2/Parameters.h
View File

@ -168,6 +168,7 @@ public:
k_param_rcmap,
k_param_L1_controller,
k_param_steerController,
k_param_barometer,
// 254,255: reserved
};

6
APMrover2/Parameters.pde
View File

@ -427,6 +427,12 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
// @Group: GND_
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
GOBJECT(barometer, "GND_", AP_Baro),
// @Group: RELAY_
// @Path: ../libraries/AP_Relay/AP_Relay.cpp
GOBJECT(relay, "RELAY_", AP_Relay),

7
APMrover2/commands.pde
View File

@ -151,12 +151,7 @@ void init_home()
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
gps_base_alt = max(g_gps->altitude_cm, 0);
home.alt = g_gps->altitude_cm;
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
home_is_set = true;
// Save Home to EEPROM - Command 0

5
APMrover2/commands_logic.pde
View File

@ -360,10 +360,7 @@ static void do_set_home()
if(next_nonnav_command.p1 == 1 && have_position) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
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);
ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt);
home_is_set = true;
}
}

11
APMrover2/config.h
View File

@ -60,31 +60,42 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_BARO AP_BARO_BMP085
# define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085
# else // APM2 Production Hardware (default)
# define CONFIG_BARO AP_BARO_MS5611
# define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
# endif
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
# define CONFIG_INS_TYPE CONFIG_INS_HIL
# define CONFIG_COMPASS AP_COMPASS_HIL
# define CONFIG_BARO AP_BARO_HIL
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define CONFIG_INS_TYPE CONFIG_INS_PX4
# define CONFIG_COMPASS AP_COMPASS_PX4
# define CONFIG_BARO AP_BARO_PX4
# define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
# define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_BARO AP_BARO_BMP085
# define BATTERY_PIN_1 20
# define CURRENT_PIN_1 19
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
# define CONFIG_INS_TYPE CONFIG_INS_L3G4200D
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_BARO AP_BARO_BMP085
# define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1
#endif

6
APMrover2/defines.h
View File

@ -173,6 +173,12 @@ enum mode {
#define CONFIG_INS_FLYMAPLE 5
#define CONFIG_INS_L3G4200D 6
// barometer driver types
#define AP_BARO_BMP085 1
#define AP_BARO_MS5611 2
#define AP_BARO_PX4 3
#define AP_BARO_HIL 4
// compass driver types
#define AP_COMPASS_HMC5843 1
#define AP_COMPASS_PX4 2

7
APMrover2/sensors.pde
View File

@ -1,5 +1,12 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_barometer(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
barometer.calibrate();
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
}
static void init_sonar(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1

6
APMrover2/system.pde
View File

@ -117,6 +117,9 @@ static void init_ardupilot()
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// init the GCS
gcs[0].init(hal.uartA);
@ -174,6 +177,9 @@ static void init_ardupilot()
// initialise sonar
init_sonar();
// and baro for EKF
init_barometer();
// Do GPS init
g_gps = &g_gps_driver;
// GPS initialisation