diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 49f4736fe7..6554751b71 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -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 */ diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index c73174d699..438696f6d6 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -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), diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 480373dd9a..ff2da2e84a 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -168,6 +168,7 @@ public: k_param_rcmap, k_param_L1_controller, k_param_steerController, + k_param_barometer, // 254,255: reserved }; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index b61172f440..70f1982982 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -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), diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index a1da28d295..e4952514ad 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -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 diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index d594249f40..605286a3d5 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -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; } } diff --git a/APMrover2/config.h b/APMrover2/config.h index 721ac63537..99a35ab8b1 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -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 diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 4f8e555d64..9bcd2bc4ec 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 3852b57e85..52c2d92f7f 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -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 diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 723f8d8b7d..5d4524a2bd 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -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