From 88fe2d3052eccd542ffb5d3473d720b66b8679fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 19:04:26 +0100 Subject: mission feasibility checker: add check for waypoint altitude relative to home position altitude --- .../navigator/mission_feasibility_checker.cpp | 43 ++++++++++++++++++---- .../navigator/mission_feasibility_checker.h | 7 ++-- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 40 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217d..41670e247 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +75,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +118,33 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + if (throw_error) { + return false; + } else { + return true; + } + } + } +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a296..819dcf9c3 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -59,14 +59,15 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); public: MissionFeasibilityChecker(); @@ -75,7 +76,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c572972b5..11181ff64 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); -- cgit v1.2.3 From 053ad5b6388ef653d1dfe255ea4f3eb00aeccaba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 22:19:21 +0100 Subject: mission feasibility checker: remove audio warning for waypoint below home altitude --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 41670e247..b41e9d355 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,10 +135,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } if (home_alt > missionitem.altitude) { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } -- cgit v1.2.3 From c6e2ad918b50f01ec9e26ccd7fdb88c7c0d2a60c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Feb 2014 00:34:51 +0100 Subject: mission feasibility checker: add missing default return in checkHomePositionAltitude --- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b41e9d355..02e35f1a8 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -144,6 +144,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } } + + return true; } bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) -- cgit v1.2.3 From 7e7d78f50655f2f5f1291cf79a249e06f6a08017 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 21:53:21 +0200 Subject: ekf_att_pos_estimator: Default printing to off --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 23ecd89ac..5db1adbb3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -5,7 +5,7 @@ // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely // optimized out by the compiler. -#define EKF_DEBUG +//#define EKF_DEBUG #ifdef EKF_DEBUG #include -- cgit v1.2.3 From 00d40d095ddb4a5ad645044bed1ffdc1f977dba3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:12:59 +0200 Subject: mavlink mission item takeoff: read correct param for minimal pitch --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28dd97fca..bb1ad86ef 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -886,7 +886,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + mission_item->pitch_min = mavlink_mission_item->param1; break; default: -- cgit v1.2.3 From 366e9c633d7d37894ab71d46951605ebe951c062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:46:16 +0200 Subject: FMUv1 board prettifying --- src/drivers/boards/px4fmu-v1/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 58273f2d2..c944007a5 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,6 +86,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to @@ -98,7 +99,7 @@ __BEGIN_DECLS /* * Optional devices on IO's external port */ -#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_ACCEL_MAG 2 /* * I2C busses -- cgit v1.2.3 From 096a4673e90240ea31fc414dbc09d3a00f24b180 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:51:18 +0200 Subject: Add SPI4 init bits for FMUv2 --- src/drivers/boards/px4fmu-v2/board_config.h | 5 +++++ src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 12 ++++++++++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 35 +++++++++++++++++++++++++++++ 3 files changed, 52 insertions(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index c2de1bfba..3ed9888e4 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -106,14 +106,19 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_EXT0 5 +#define PX4_SPIDEV_EXT1 6 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 71414d62c..bf41bb1fe 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -192,6 +192,7 @@ stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi2; +static struct spi_dev_s *spi4; static struct sdio_dev_s *sdio; #include @@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); + spi4 = up_spiinitialize(4); + + /* Default SPI4 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi4, 10000000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); + SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); + + message("[boot] Initialized SPI port 4\n"); + #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index c66c490a7..903329052 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -94,6 +94,13 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_FRAM); stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI_CS_EXT0); + stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); +#endif } __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) @@ -157,3 +164,31 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi return SPI_STATUS_PRESENT; } #endif + +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_EXT0: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(PX4_SPIDEV_EXT0, !selected); + stm32_gpiowrite(PX4_SPIDEV_EXT1, 1); + break; + + case PX4_SPIDEV_EXT1: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(PX4_SPIDEV_EXT1, !selected); + stm32_gpiowrite(PX4_SPIDEV_EXT0, 1); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} -- cgit v1.2.3 From 2bfde311db7daa0771c7be7f165b7601bf2946ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 10:45:11 +0200 Subject: Fix SPI select on port 4 to use the right defines --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 903329052..01dbd6e77 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -172,14 +172,14 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_EXT0: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(PX4_SPIDEV_EXT0, !selected); - stm32_gpiowrite(PX4_SPIDEV_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(PX4_SPIDEV_EXT1, !selected); - stm32_gpiowrite(PX4_SPIDEV_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); break; default: -- cgit v1.2.3 From 65344133a4a6944b71f2ca134c6c4443ff7c96cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 10:45:38 +0200 Subject: Count devices on SPI4 / EXT from 1 as for the other buses --- src/drivers/boards/px4fmu-v2/board_config.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 3ed9888e4..36eb7bec4 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -117,8 +117,10 @@ __BEGIN_DECLS #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 -#define PX4_SPIDEV_EXT0 5 -#define PX4_SPIDEV_EXT1 6 + +/* External bus */ +#define PX4_SPIDEV_EXT0 1 +#define PX4_SPIDEV_EXT1 2 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 -- cgit v1.2.3