From b852d0b1bdc9e5734e9ae13645ad2e94c9e10d83 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 17 May 2023 19:28:27 +0200 Subject: [PATCH 01/77] rft: changed Readme for space systems setup --- README.md | 152 +++++------------------------------------------------- 1 file changed, 14 insertions(+), 138 deletions(-) diff --git a/README.md b/README.md index 0665aef3fab2..0d9a936d2484 100644 --- a/README.md +++ b/README.md @@ -1,138 +1,14 @@ -# PX4 Drone Autopilot - -[![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot) - -[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22) - -[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) - -This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones. - -PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box. - -* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/main/LICENSE)) -* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](https://px4.io/ecosystem/commercial-systems/)): - * [Multicopters](https://docs.px4.io/main/en/frames_multicopter/) - * [Fixed wing](https://docs.px4.io/main/en/frames_plane/) - * [VTOL](https://docs.px4.io/main/en/frames_vtol/) - * [Autogyro](https://docs.px4.io/main/en/frames_autogyro/) - * [Rover](https://docs.px4.io/main/en/frames_rover/) - * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc) -* Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases) - - -## Building a PX4 based drone, rover, boat or robot - -The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. -See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! - - -## Changing code and contributing - -This [Developer Guide](https://docs.px4.io/main/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle. - -Developers should read the [Guide for Contributions](https://docs.px4.io/main/en/contribute/). -See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! - - -### Weekly Dev Call - -The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/contribute/). - -> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform. It includes a QA session for newcomers. All regular calls are listed in the [Dronecode calendar](https://www.dronecode.org/calendar/). - - -## Maintenance Team - -Note: This is the source of truth for the active maintainers of PX4 ecosystem. - -| Sector | Maintainer | -|---|---| -| Founder | [Lorenz Meier](https://github.com/LorenzMeier) | -| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)| -| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) | -| OS/NuttX | [David Sidrane](https://github.com/davids5) | -| Drivers | [Daniel Agar](https://github.com/dagar) | -| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) | -| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) | -| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) | -| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) | - -| Vehicle Type | Maintainer | -|---|---| -| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) | -| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) | -| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) | -| Boat | x | -| Rover | x | - -See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date. - -## Supported Hardware - -Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed). - -For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/). - -### Pixhawk Standard Boards - -These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team - -* FMUv6X and FMUv6C - * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html) - * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html) - * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html) - * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html) -* FMUv5 and FMUv5X (STM32F7, 2019/20) - * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html) - * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html) - * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html) - * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html) - * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode) -* FMUv4 (STM32F4, 2015) - * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html) - * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html) -* FMUv3 (STM32F4, 2014) - * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html) - * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html) - * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html) -* FMUv2 (STM32F4, 2013) - * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html) - -### Manufacturer supported - -These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers. - -* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html) -* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html) -* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html) -* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html) -* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html) -* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf) -* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf) -* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html) - -### Community supported - -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members. - -### Experimental - -These boards are nor maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases. - -* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html) -* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html) - -## Project Roadmap - -**Note: Outdated** - -A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25). - -## Project Governance - -The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation. - -Dronecode Logo -Linux Foundation Logo -
 
+# PX4 Space Systems +This repository is a fork of [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot) intended to add modules and functionality for space systems. Currently, this repository is a work in progress at KTH Space Robotics Lab. For more information, reach out the DISCOWER organization members. + +## Setup for Air Carriages: +1. Make sure this repository is in the external/PX4 folder of your Astrobee source code +2. Run the following commands: +``` +git submodule update --init --recursive +bash ./Tools/setup/ubuntu.sh +make px4_sitl gazebo-classic +``` +At this point, your simulation should run with the the default Iris quadcopter. + +Then you can proceed with the Astrobee setup. \ No newline at end of file From 46e6d7f979c2799a5307ebf7771976dbbec2141d Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Mon, 19 Jun 2023 19:45:53 +0200 Subject: [PATCH 02/77] Readme for space systems setup --- README.md | 151 +++++------------------------------------------------- 1 file changed, 14 insertions(+), 137 deletions(-) diff --git a/README.md b/README.md index 2191afdbdd2c..0d9a936d2484 100644 --- a/README.md +++ b/README.md @@ -1,137 +1,14 @@ -# PX4 Drone Autopilot - -[![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot) - -[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22) - -[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) - -This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones. - -PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box. - -* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/main/LICENSE)) -* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](https://px4.io/ecosystem/commercial-systems/)): - * [Multicopters](https://docs.px4.io/main/en/frames_multicopter/) - * [Fixed wing](https://docs.px4.io/main/en/frames_plane/) - * [VTOL](https://docs.px4.io/main/en/frames_vtol/) - * [Autogyro](https://docs.px4.io/main/en/frames_autogyro/) - * [Rover](https://docs.px4.io/main/en/frames_rover/) - * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc) -* Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases) - - -## Building a PX4 based drone, rover, boat or robot - -The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. -See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! - - -## Changing code and contributing - -This [Developer Guide](https://docs.px4.io/main/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle. - -Developers should read the [Guide for Contributions](https://docs.px4.io/main/en/contribute/). -See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! - - -### Weekly Dev Call - -The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/contribute/). - -> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform. It includes a QA session for newcomers. All regular calls are listed in the [Dronecode calendar](https://www.dronecode.org/calendar/). - - -## Maintenance Team - -Note: This is the source of truth for the active maintainers of PX4 ecosystem. - -| Sector | Maintainer | -|---|---| -| Founder | [Lorenz Meier](https://github.com/LorenzMeier) | -| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)| -| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) | -| OS/NuttX | [David Sidrane](https://github.com/davids5) | -| Drivers | [Daniel Agar](https://github.com/dagar) | -| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) | -| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) | -| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) | -| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) | - -| Vehicle Type | Maintainer | -|---|---| -| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) | -| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) | -| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) | -| Boat | x | -| Rover | x | - -See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date. - -## Supported Hardware - -Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed). - -For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/). - -### Pixhawk Standard Boards - -These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team - -* FMUv6X and FMUv6C - * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html) - * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html) - * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html) - * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html) -* FMUv5 and FMUv5X (STM32F7, 2019/20) - * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html) - * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html) - * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html) - * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html) - * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode) -* FMUv4 (STM32F4, 2015) - * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html) - * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html) -* FMUv3 (STM32F4, 2014) - * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html) - * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html) - * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html) -* FMUv2 (STM32F4, 2013) - * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html) - -### Manufacturer supported - -These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers. - -* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html) -* [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html) -* [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html) -* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html) -* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf) -* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf) -* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html) - -### Community supported - -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members. - -### Experimental - -These boards are nor maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases. - -* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html) -* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html) - -## Project Roadmap - -**Note: Outdated** - -A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25). - -## Project Governance - -The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation. - -Dronecode Logo -Linux Foundation Logo -
 
+# PX4 Space Systems +This repository is a fork of [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot) intended to add modules and functionality for space systems. Currently, this repository is a work in progress at KTH Space Robotics Lab. For more information, reach out the DISCOWER organization members. + +## Setup for Air Carriages: +1. Make sure this repository is in the external/PX4 folder of your Astrobee source code +2. Run the following commands: +``` +git submodule update --init --recursive +bash ./Tools/setup/ubuntu.sh +make px4_sitl gazebo-classic +``` +At this point, your simulation should run with the the default Iris quadcopter. + +Then you can proceed with the Astrobee setup. \ No newline at end of file From f7cf8c8814a9ed0d2e206756d29df712b066ca60 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Wed, 21 Jun 2023 21:04:11 +0200 Subject: [PATCH 03/77] Added Space Robot airframe and thruster controller (work in progress) --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 2 + .../init.d/airframes/70000_kth_space_robot | 79 +++++++++ .../init.d/airframes/CMakeLists.txt | 3 + ROMFS/px4fmu_common/init.d/rc.sr_apps | 36 ++++ ROMFS/px4fmu_common/init.d/rc.sr_defaults | 16 ++ ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 9 +- Tools/px4airframes/srcparser.py | 2 + msg/CMakeLists.txt | 1 + msg/ThrusterCommand.msg | 6 + .../sr_thruster_controller/CMakeLists.txt | 39 +++++ src/examples/sr_thruster_controller/Kconfig | 5 + .../sr_thruster_controller.c | 162 ++++++++++++++++++ src/modules/microdds_client/dds_topics.yaml | 6 + src/modules/microdds_client/module.yaml | 2 +- 14 files changed, 366 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot create mode 100644 ROMFS/px4fmu_common/init.d/rc.sr_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.sr_defaults create mode 100644 msg/ThrusterCommand.msg create mode 100644 src/examples/sr_thruster_controller/CMakeLists.txt create mode 100644 src/examples/sr_thruster_controller/Kconfig create mode 100644 src/examples/sr_thruster_controller/sr_thruster_controller.c diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 3f19c22f01d6..d59159f1c717 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -48,6 +48,8 @@ px4_add_romfs_files( rc.mc_defaults rcS rc.sensors + rc.sr_apps + rc.sr_defaults rc.thermal_cal rc.rover_apps rc.rover_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot new file mode 100644 index 000000000000..dc15bd017b8a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -0,0 +1,79 @@ +#!/bin/sh +# +# @name KTH Space Robot +# +# @type Space Robot +# @class 2D Space Robot +# +# @maintainer DISCOWER +# + +. ${R}etc/init.d/rc.sr_defaults + +param set-default CA_AIRFRAME 7 +param set-default CA_ROTOR_COUNT 8 + +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 + +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 + +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 + +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 + +param set-default CA_ROTOR4_PX -0.12 +param set-default CA_ROTOR4_PY 0.12 + +param set-default CA_ROTOR5_PX -0.12 +param set-default CA_ROTOR5_PY -0.12 + +param set-default CA_ROTOR6_PX 0.12 +param set-default CA_ROTOR6_PY 0.12 + +param set-default CA_ROTOR7_PX 0.12 +param set-default CA_ROTOR7_PY -0.12 + +param set-default PWM_MAIN_TIM0 400 +param set-default PWM_MAIN_TIM1 400 +param set-default PWM_MAIN_TIM2 400 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 105 +param set-default PWM_MAIN_FUNC6 106 +param set-default PWM_MAIN_FUNC7 107 +param set-default PWM_MAIN_FUNC8 108 + +param set-default PWM_MAIN_DIS1 0 +param set-default PWM_MAIN_DIS2 0 +param set-default PWM_MAIN_DIS3 0 +param set-default PWM_MAIN_DIS4 0 +param set-default PWM_MAIN_DIS5 0 +param set-default PWM_MAIN_DIS6 0 +param set-default PWM_MAIN_DIS7 0 +param set-default PWM_MAIN_DIS8 0 + +param set-default PWM_MAIN_MIN1 0 +param set-default PWM_MAIN_MIN2 0 +param set-default PWM_MAIN_MIN3 0 +param set-default PWM_MAIN_MIN4 0 +param set-default PWM_MAIN_MIN5 0 +param set-default PWM_MAIN_MIN6 0 +param set-default PWM_MAIN_MIN7 0 +param set-default PWM_MAIN_MIN8 0 + +param set-default PWM_MAIN_MAX1 2500 +param set-default PWM_MAIN_MAX2 2500 +param set-default PWM_MAIN_MAX3 2500 +param set-default PWM_MAIN_MAX4 2500 +param set-default PWM_MAIN_MAX5 2500 +param set-default PWM_MAIN_MAX6 2500 +param set-default PWM_MAIN_MAX7 2500 +param set-default PWM_MAIN_MAX8 2500 + diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 8e9931aa89f8..4e408483a3a0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -124,4 +124,7 @@ px4_add_romfs_files( 60000_uuv_generic 60001_uuv_hippocampus 60002_uuv_bluerov2_heavy + + # [70000, 71000] (Unmanned) Space Robots + 70000_kth_space_robot ) diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps new file mode 100644 index 000000000000..1c34f49aad7e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -0,0 +1,36 @@ +#!/bin/sh +# +# Standard apps for sr. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +############################################################################### +# Begin Estimator Group Selection # +############################################################################### + +ekf2 start & + +############################################################################### +# End Estimator Group Selection # +############################################################################### +##Copied from uuv +# +# Start Control Allocator +# +#control_allocator start + +# +# Start UUV Attitude Controller. +# +#uuv_att_control start + +# +# Start UUV Land Detector. +# +#land_detector start rover + + +#commander arm -f + +#control_allocator stop diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_defaults b/ROMFS/px4fmu_common/init.d/rc.sr_defaults new file mode 100644 index 000000000000..4a80fca79d4a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sr_defaults @@ -0,0 +1,16 @@ +#!/bin/sh +# +# UUV default parameters. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE sr + +# MAV_TYPE_QUADROTOR 2 +param set-default MAV_TYPE 12 + +# Disable preflight disarm to not interfere with external launching +# param set-default COM_DISARM_PRFLT -1 + +# param set-default CP_DIST -1 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 4f8497bc19b2..a43b00d5f4a9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -59,7 +59,14 @@ then . ${R}etc/init.d/rc.uuv_apps fi - +# +# SR setup +# +if [ $VEHICLE_TYPE = sr ] +then + # Start standard sr apps. + . ${R}etc/init.d/rc.sr_apps +fi # # Generic setup (autostart ID not found). diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index 56c3ce52be16..a07e695c1c82 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -107,6 +107,8 @@ def GetImageName(self): return "Balloon" elif (self.type == "Vectored 6 DOF UUV"): return "Vectored6DofUUV" + elif (self.type == "Space Robot"): + return "SpaceRobot" return "AirframeUnknown" def GetAirframes(self): diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 611fea9e85e3..281572b00f12 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -179,6 +179,7 @@ set(msg_files TaskStackInfo.msg TecsStatus.msg TelemetryStatus.msg + ThrusterCommand.msg TiltrotorExtraControls.msg TimesyncStatus.msg TrajectoryBezier.msg diff --git a/msg/ThrusterCommand.msg b/msg/ThrusterCommand.msg new file mode 100644 index 000000000000..74a25f0f769c --- /dev/null +++ b/msg/ThrusterCommand.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 x1 # duty cycle of thruster-pair x1 [-1;1] +float32 x2 # duty cycle of thruster-pair x2 [-1;1] +float32 y1 # duty cycle of thruster-pair y1 [-1;1] +float32 y2 # duty cycle of thruster-pair y2 [-1;1] diff --git a/src/examples/sr_thruster_controller/CMakeLists.txt b/src/examples/sr_thruster_controller/CMakeLists.txt new file mode 100644 index 000000000000..ba6306be8f58 --- /dev/null +++ b/src/examples/sr_thruster_controller/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE examples__sr_thruster_controller + MAIN sr_thruster_controller + SRCS + sr_thruster_controller.c + DEPENDS + ) diff --git a/src/examples/sr_thruster_controller/Kconfig b/src/examples/sr_thruster_controller/Kconfig new file mode 100644 index 000000000000..e31e32af9dad --- /dev/null +++ b/src/examples/sr_thruster_controller/Kconfig @@ -0,0 +1,5 @@ +menuconfig EXAMPLES_SR_THRUSTER_CONTROLLER + bool "sr_thruster_controller" + default n + ---help--- + Enable support for SR Thruster Controller diff --git a/src/examples/sr_thruster_controller/sr_thruster_controller.c b/src/examples/sr_thruster_controller/sr_thruster_controller.c new file mode 100644 index 000000000000..0877949d0902 --- /dev/null +++ b/src/examples/sr_thruster_controller/sr_thruster_controller.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sr_thruster_controller.c + * For controlling solenoid cold gas thrusters in space robotics + * + * @author Elias Krantz + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + + +__EXPORT int sr_thruster_controller_main(int argc, char *argv[]); + +void delay(unsigned int milliseconds) { + struct timespec ts; + ts.tv_sec = milliseconds / 1000; + ts.tv_nsec = (milliseconds % 1000) * 1000000; + nanosleep(&ts, NULL); +} + +int sr_thruster_controller_main(int argc, char *argv[]) +{ + PX4_INFO("Thruster controller started!"); + + // Create publisher for pwm values + struct actuator_motors_s actuator_motors_msg; + memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); + orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); + + float values[] = {1, 0, 0, 0, 0, 0, 0, 0, NAN, NAN, NAN, NAN}; + memcpy(actuator_motors_msg.control, values, sizeof(values)); + + PX4_INFO("Sending control signal to PWM!"); + for (int i = 0; i < 5000/2.5; i++) + { + PX4_INFO("Message #%d\n", i); + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + delay(2.5); + } + + // PX4_INFO("Testing GPIO"); + // PX4_INFO("Setting GPIO 0 to high"); + // px4_arch_configgpio(GPIO_GPIO0_OUTPUT); + // px4_arch_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + // PX4_INFO("Done"); + + // int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); + // /* limit the update rate to 1 Hz */ + // orb_set_interval(sensor_sub_fd, 1000); + + // PX4_INFO("orb_sensor_sub created"); + + // int thrustercmd_sub_fd = orb_subscribe(ORB_ID(thruster_command)); + // /* limit the update rate to 1 Hz */ + // orb_set_interval(thrustercmd_sub_fd, 1000); + + // PX4_INFO("Subscriber to thruster command created"); + + + // /* Log message pub topic */ + // struct log_message_s log_msg; + // memset(&log_msg, 0, sizeof(log_msg)); + // // orb_advert_t log_msg_pub_fd = orb_advertise(ORB_ID(log_message), &log_msg); + // PX4_INFO("Publisher of log messages created"); + + // /* one could wait for multiple topics with this technique, just using one here */ + // px4_pollfd_struct_t fds[] = { + // { .fd = thrustercmd_sub_fd, .events = POLLIN }, + // }; + + // int error_counter = 0; + + // // for (int i = 0; i < 100; i++) { + // while (false) { + // /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ + // int poll_ret = px4_poll(fds, 1, 5000); + + // /* handle the poll result */ + // if (poll_ret == 0) { + // /* this means none of our providers is giving us data */ + // PX4_INFO("Got no data within a second"); + + // } else if (poll_ret < 0) { + // /* this is seriously bad - should be an emergency */ + // if (error_counter < 10 || error_counter % 50 == 0) { + // /* use a counter to prevent flooding (and slowing us down) */ + // PX4_ERR("ERROR return value from poll(): %d", poll_ret); + // } + + // error_counter++; + + // } else { + // if (fds[0].revents & POLLIN) { + // /* obtained data for the first file descriptor */ + // struct thruster_command_s raw; + // /* copy sensors raw data into local buffer */ + // orb_copy(ORB_ID(thruster_command), thrustercmd_sub_fd, &raw); + // PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", + // (double)raw.x1, + // (double)raw.x2, + // (double)raw.y1, + // (double)raw.y2); + + + // /* set att and publish this information for other apps + // the following does not have any meaning, it's just an example + // */ + // // strcpy(log_msg.text, "New thruster command received!"); + + // // orb_publish(ORB_ID(log_message), log_msg_pub_fd, &log_msg); + // } + // } + // } + + return OK; +} diff --git a/src/modules/microdds_client/dds_topics.yaml b/src/modules/microdds_client/dds_topics.yaml index 3a871848bb23..8c65d4ffc574 100644 --- a/src/modules/microdds_client/dds_topics.yaml +++ b/src/modules/microdds_client/dds_topics.yaml @@ -44,6 +44,9 @@ publications: - topic: /fmu/out/vehicle_trajectory_waypoint_desired type: px4_msgs::msg::VehicleTrajectoryWaypoint + - topic: /fmu/out/actuator_motors + type: px4_msgs::msg::ActuatorMotors + subscriptions: - topic: /fmu/in/offboard_control_mode @@ -84,3 +87,6 @@ subscriptions: - topic: /fmu/in/vehicle_trajectory_waypoint type: px4_msgs::msg::VehicleTrajectoryWaypoint + + - topic: /fmu/in/actuator_motors + type: px4_msgs::msg::ActuatorMotors diff --git a/src/modules/microdds_client/module.yaml b/src/modules/microdds_client/module.yaml index 0b30eb2319e9..0cffc8a9ee30 100644 --- a/src/modules/microdds_client/module.yaml +++ b/src/modules/microdds_client/module.yaml @@ -17,7 +17,7 @@ serial_config: if [ $SERIAL_DEV != ethernet ]; then set XRCE_DDS_ARGS "-t serial -d ${SERIAL_DEV} -b p:${BAUD_PARAM}" else - set XRCE_DDS_ARGS "-t udp" + set XRCE_DDS_ARGS "-t udp -h 192.168.0.1" fi microdds_client start ${XRCE_DDS_ARGS} From ddf2a5b1fbcf31c6b65d0751d2bea64a77e913b2 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Thu, 22 Jun 2023 15:26:13 +0200 Subject: [PATCH 04/77] Added support for thruster control messages. Fixed initialization params. --- ROMFS/px4fmu_common/init.d/rc.sr_apps | 8 + ROMFS/px4fmu_common/init.d/rc.sr_defaults | 16 +- .../sr_thruster_controller.c | 148 ++++++++++-------- src/modules/microdds_client/dds_topics.yaml | 6 + 4 files changed, 114 insertions(+), 64 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index 1c34f49aad7e..5e6b9d85e633 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -34,3 +34,11 @@ ekf2 start & #commander arm -f #control_allocator stop + +microdds_client start -h 192.168.0.1 + +############################################################################### +# Thruster Controller # +############################################################################### + +sr_thruster_controller diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_defaults b/ROMFS/px4fmu_common/init.d/rc.sr_defaults index 4a80fca79d4a..5e39530c65cc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sr_defaults @@ -8,9 +8,19 @@ set VEHICLE_TYPE sr # MAV_TYPE_QUADROTOR 2 -param set-default MAV_TYPE 12 +#param set-default MAV_TYPE 12 # Disable preflight disarm to not interfere with external launching -# param set-default COM_DISARM_PRFLT -1 +param set-default COM_DISARM_PRFLT -1 -# param set-default CP_DIST -1 +param set-default XRCE_DDS_0_CFG 1000 +param set-default CP_DIST -1 + +param set-default CBRK_AIRSPD_CHK 162128 +param set-default CBRK_SUPPLY_CHK 894281 +param set-default COM_ARM_ARSP_EN 0 +param set-default COM_ARM_HFLT_CHK 0 +param set-default COM_FLTMODE 0 +param set-default COM_FLTMODE 7 +param set-default COM_FLTMODE 2 +param set-default MAN_ARM_GESTURE 0 diff --git a/src/examples/sr_thruster_controller/sr_thruster_controller.c b/src/examples/sr_thruster_controller/sr_thruster_controller.c index 0877949d0902..23f3b73d7bd4 100644 --- a/src/examples/sr_thruster_controller/sr_thruster_controller.c +++ b/src/examples/sr_thruster_controller/sr_thruster_controller.c @@ -54,7 +54,11 @@ #include #include - +/** + * SR thruster controller app start / stop handling function + * + * @ingroup apps + */ __EXPORT int sr_thruster_controller_main(int argc, char *argv[]); void delay(unsigned int milliseconds) { @@ -73,16 +77,28 @@ int sr_thruster_controller_main(int argc, char *argv[]) memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); - float values[] = {1, 0, 0, 0, 0, 0, 0, 0, NAN, NAN, NAN, NAN}; - memcpy(actuator_motors_msg.control, values, sizeof(values)); - + /* Testing */ + for (int i = 0; i < 8; i++) + { + actuator_motors_msg.control[i] = 1; + } + for (int i = 8; i < 12; i++) + { + actuator_motors_msg.control[i] = NAN; + } PX4_INFO("Sending control signal to PWM!"); for (int i = 0; i < 5000/2.5; i++) { - PX4_INFO("Message #%d\n", i); + PX4_INFO("Message #%d", i); orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); delay(2.5); } + for (int i = 0; i < 8; i++) + { + actuator_motors_msg.control[i] = 0; + } + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + PX4_INFO("Turning off PWM"); // PX4_INFO("Testing GPIO"); // PX4_INFO("Setting GPIO 0 to high"); @@ -90,73 +106,83 @@ int sr_thruster_controller_main(int argc, char *argv[]) // px4_arch_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // PX4_INFO("Done"); - // int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); - // /* limit the update rate to 1 Hz */ - // orb_set_interval(sensor_sub_fd, 1000); + //int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); + /* limit the update rate to 1 Hz */ + //orb_set_interval(sensor_sub_fd, 1000); // PX4_INFO("orb_sensor_sub created"); - // int thrustercmd_sub_fd = orb_subscribe(ORB_ID(thruster_command)); - // /* limit the update rate to 1 Hz */ - // orb_set_interval(thrustercmd_sub_fd, 1000); + int thrustercmd_sub_fd = orb_subscribe(ORB_ID(thruster_command)); + /* limit the update rate to 10 Hz */ + orb_set_interval(thrustercmd_sub_fd, 100); - // PX4_INFO("Subscriber to thruster command created"); + PX4_INFO("Subscriber to thruster command created"); - // /* Log message pub topic */ + /* Log message pub topic */ // struct log_message_s log_msg; // memset(&log_msg, 0, sizeof(log_msg)); // // orb_advert_t log_msg_pub_fd = orb_advertise(ORB_ID(log_message), &log_msg); // PX4_INFO("Publisher of log messages created"); - // /* one could wait for multiple topics with this technique, just using one here */ - // px4_pollfd_struct_t fds[] = { - // { .fd = thrustercmd_sub_fd, .events = POLLIN }, - // }; - - // int error_counter = 0; - - // // for (int i = 0; i < 100; i++) { - // while (false) { - // /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ - // int poll_ret = px4_poll(fds, 1, 5000); - - // /* handle the poll result */ - // if (poll_ret == 0) { - // /* this means none of our providers is giving us data */ - // PX4_INFO("Got no data within a second"); - - // } else if (poll_ret < 0) { - // /* this is seriously bad - should be an emergency */ - // if (error_counter < 10 || error_counter % 50 == 0) { - // /* use a counter to prevent flooding (and slowing us down) */ - // PX4_ERR("ERROR return value from poll(): %d", poll_ret); - // } - - // error_counter++; - - // } else { - // if (fds[0].revents & POLLIN) { - // /* obtained data for the first file descriptor */ - // struct thruster_command_s raw; - // /* copy sensors raw data into local buffer */ - // orb_copy(ORB_ID(thruster_command), thrustercmd_sub_fd, &raw); - // PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", - // (double)raw.x1, - // (double)raw.x2, - // (double)raw.y1, - // (double)raw.y2); - - - // /* set att and publish this information for other apps - // the following does not have any meaning, it's just an example - // */ - // // strcpy(log_msg.text, "New thruster command received!"); - - // // orb_publish(ORB_ID(log_message), log_msg_pub_fd, &log_msg); - // } - // } - // } + /* one could wait for multiple topics with this technique, just using one here */ + px4_pollfd_struct_t fds[] = { + { .fd = thrustercmd_sub_fd, .events = POLLIN }, + }; + + int error_counter = 0; + + // for (int i = 0; i < 100; i++) { + while (true) { + /* wait for sensor update of 1 file descriptor for 10 s (10 second) */ + int poll_ret = px4_poll(fds, 1, 10000); + + /* handle the poll result */ + if (poll_ret == 0) { + /* this means none of our providers is giving us data */ + PX4_INFO("Got no data within a second"); + + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (error_counter < 10 || error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("ERROR return value from poll(): %d", poll_ret); + } + + error_counter++; + + } else { + if (fds[0].revents & POLLIN) { + /* obtained data for the first file descriptor */ + struct thruster_command_s thruster_cmd; + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(thruster_command), thrustercmd_sub_fd, &thruster_cmd); + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", + (double)thruster_cmd.x1, + (double)thruster_cmd.x2, + (double)thruster_cmd.y1, + (double)thruster_cmd.y2); + + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; + + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + + /* set att and publish this information for other apps + the following does not have any meaning, it's just an example + */ + // strcpy(log_msg.text, "New thruster command received!"); + + // orb_publish(ORB_ID(log_message), log_msg_pub_fd, &log_msg); + } + } + } return OK; } diff --git a/src/modules/microdds_client/dds_topics.yaml b/src/modules/microdds_client/dds_topics.yaml index 8c65d4ffc574..4dd7fbffb9b3 100644 --- a/src/modules/microdds_client/dds_topics.yaml +++ b/src/modules/microdds_client/dds_topics.yaml @@ -47,6 +47,9 @@ publications: - topic: /fmu/out/actuator_motors type: px4_msgs::msg::ActuatorMotors + - topic: /fmu/out/log_message + type: px4_msgs::msg::LogMessage + subscriptions: - topic: /fmu/in/offboard_control_mode @@ -90,3 +93,6 @@ subscriptions: - topic: /fmu/in/actuator_motors type: px4_msgs::msg::ActuatorMotors + + - topic: /fmu/in/thruster_command + type: px4_msgs::msg::ThrusterCommand From 892e35b2f009de4d34fd6bedba55137a5a68476a Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Tue, 27 Jun 2023 18:14:16 +0200 Subject: [PATCH 05/77] Fixed PWM to be controllable at 10 Hz by sr_thruster_controller.c --- .../init.d/airframes/70000_kth_space_robot | 133 +++++++++--------- ROMFS/px4fmu_common/init.d/rc.sr_apps | 35 +---- ROMFS/px4fmu_common/init.d/rc.sr_defaults | 3 +- msg/CMakeLists.txt | 1 + msg/MyMessage.msg | 3 + .../sr_thruster_controller.c | 73 +++------- src/modules/microdds_client/dds_topics.yaml | 3 + 7 files changed, 95 insertions(+), 156 deletions(-) create mode 100644 msg/MyMessage.msg diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index dc15bd017b8a..7b352f532eae 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -10,70 +10,71 @@ . ${R}etc/init.d/rc.sr_defaults -param set-default CA_AIRFRAME 7 -param set-default CA_ROTOR_COUNT 8 - -param set-default CA_ROTOR0_PX -0.12 -param set-default CA_ROTOR0_PY -0.12 - -param set-default CA_ROTOR1_PX 0.12 -param set-default CA_ROTOR1_PY -0.12 - -param set-default CA_ROTOR2_PX -0.12 -param set-default CA_ROTOR2_PY 0.12 - -param set-default CA_ROTOR3_PX 0.12 -param set-default CA_ROTOR3_PY 0.12 - -param set-default CA_ROTOR4_PX -0.12 -param set-default CA_ROTOR4_PY 0.12 - -param set-default CA_ROTOR5_PX -0.12 -param set-default CA_ROTOR5_PY -0.12 - -param set-default CA_ROTOR6_PX 0.12 -param set-default CA_ROTOR6_PY 0.12 - -param set-default CA_ROTOR7_PX 0.12 -param set-default CA_ROTOR7_PY -0.12 - -param set-default PWM_MAIN_TIM0 400 -param set-default PWM_MAIN_TIM1 400 -param set-default PWM_MAIN_TIM2 400 - -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 -param set-default PWM_MAIN_FUNC5 105 -param set-default PWM_MAIN_FUNC6 106 -param set-default PWM_MAIN_FUNC7 107 -param set-default PWM_MAIN_FUNC8 108 - -param set-default PWM_MAIN_DIS1 0 -param set-default PWM_MAIN_DIS2 0 -param set-default PWM_MAIN_DIS3 0 -param set-default PWM_MAIN_DIS4 0 -param set-default PWM_MAIN_DIS5 0 -param set-default PWM_MAIN_DIS6 0 -param set-default PWM_MAIN_DIS7 0 -param set-default PWM_MAIN_DIS8 0 - -param set-default PWM_MAIN_MIN1 0 -param set-default PWM_MAIN_MIN2 0 -param set-default PWM_MAIN_MIN3 0 -param set-default PWM_MAIN_MIN4 0 -param set-default PWM_MAIN_MIN5 0 -param set-default PWM_MAIN_MIN6 0 -param set-default PWM_MAIN_MIN7 0 -param set-default PWM_MAIN_MIN8 0 - -param set-default PWM_MAIN_MAX1 2500 -param set-default PWM_MAIN_MAX2 2500 -param set-default PWM_MAIN_MAX3 2500 -param set-default PWM_MAIN_MAX4 2500 -param set-default PWM_MAIN_MAX5 2500 -param set-default PWM_MAIN_MAX6 2500 -param set-default PWM_MAIN_MAX7 2500 -param set-default PWM_MAIN_MAX8 2500 +# param set-default CA_AIRFRAME 7 +# param set-default CA_ROTOR_COUNT 8 + +# param set-default CA_ROTOR0_PX -0.12 +# param set-default CA_ROTOR0_PY -0.12 + +# param set-default CA_ROTOR1_PX 0.12 +# param set-default CA_ROTOR1_PY -0.12 + +# param set-default CA_ROTOR2_PX -0.12 +# param set-default CA_ROTOR2_PY 0.12 + +# param set-default CA_ROTOR3_PX 0.12 +# param set-default CA_ROTOR3_PY 0.12 + +# param set-default CA_ROTOR4_PX -0.12 +# param set-default CA_ROTOR4_PY 0.12 + +# param set-default CA_ROTOR5_PX -0.12 +# param set-default CA_ROTOR5_PY -0.12 + +# param set-default CA_ROTOR6_PX 0.12 +# param set-default CA_ROTOR6_PY 0.12 + +# param set-default CA_ROTOR7_PX 0.12 +# param set-default CA_ROTOR7_PY -0.12 + +param set-default PWM_AUX_TIM0 10 +param set-default PWM_AUX_TIM1 10 +param set-default PWM_AUX_TIM2 10 + +param set-default PWM_AUX_FUNC1 101 +param set-default PWM_AUX_FUNC2 102 +param set-default PWM_AUX_FUNC3 103 +param set-default PWM_AUX_FUNC4 104 +param set-default PWM_AUX_FUNC5 105 +param set-default PWM_AUX_FUNC6 106 +param set-default PWM_AUX_FUNC7 107 +param set-default PWM_AUX_FUNC8 108 + +param set-default PWM_AUX_DIS1 0 +param set-default PWM_AUX_DIS2 0 +param set-default PWM_AUX_DIS3 0 +param set-default PWM_AUX_DIS4 0 +param set-default PWM_AUX_DIS5 0 +param set-default PWM_AUX_DIS6 0 +param set-default PWM_AUX_DIS7 0 +param set-default PWM_AUX_DIS8 0 + +param set-default PWM_AUX_MIN1 0 +param set-default PWM_AUX_MIN2 0 +param set-default PWM_AUX_MIN3 0 +param set-default PWM_AUX_MIN4 0 +param set-default PWM_AUX_MIN5 0 +param set-default PWM_AUX_MIN6 0 +param set-default PWM_AUX_MIN7 0 +param set-default PWM_AUX_MIN8 0 + +# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec +param set-default PWM_AUX_MAX1 10000 +param set-default PWM_AUX_MAX2 10000 +param set-default PWM_AUX_MAX3 10000 +param set-default PWM_AUX_MAX4 10000 +param set-default PWM_AUX_MAX5 10000 +param set-default PWM_AUX_MAX6 10000 +param set-default PWM_AUX_MAX7 10000 +param set-default PWM_AUX_MAX8 10000 diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index 5e6b9d85e633..7f0b4905bc72 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -5,40 +5,9 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator Group Selection # -############################################################################### - +# Estimator Group Selection ekf2 start & -############################################################################### -# End Estimator Group Selection # -############################################################################### -##Copied from uuv -# -# Start Control Allocator -# -#control_allocator start - -# -# Start UUV Attitude Controller. -# -#uuv_att_control start - -# -# Start UUV Land Detector. -# -#land_detector start rover - - -#commander arm -f - -#control_allocator stop - -microdds_client start -h 192.168.0.1 - -############################################################################### -# Thruster Controller # -############################################################################### +# microdds_client start -h 192.168.0.1 sr_thruster_controller diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_defaults b/ROMFS/px4fmu_common/init.d/rc.sr_defaults index 5e39530c65cc..951c8a961b9c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sr_defaults @@ -14,7 +14,8 @@ set VEHICLE_TYPE sr param set-default COM_DISARM_PRFLT -1 param set-default XRCE_DDS_0_CFG 1000 -param set-default CP_DIST -1 + +# param set-default CP_DIST -1 param set-default CBRK_AIRSPD_CHK 162128 param set-default CBRK_SUPPLY_CHK 894281 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 281572b00f12..c6128e5beb65 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -127,6 +127,7 @@ set(msg_files MissionResult.msg MountOrientation.msg ModeCompleted.msg + MyMessage.msg NavigatorMissionItem.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg diff --git a/msg/MyMessage.msg b/msg/MyMessage.msg new file mode 100644 index 000000000000..67e087290622 --- /dev/null +++ b/msg/MyMessage.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) + +char[127] text diff --git a/src/examples/sr_thruster_controller/sr_thruster_controller.c b/src/examples/sr_thruster_controller/sr_thruster_controller.c index 23f3b73d7bd4..9ac07dbf34fd 100644 --- a/src/examples/sr_thruster_controller/sr_thruster_controller.c +++ b/src/examples/sr_thruster_controller/sr_thruster_controller.c @@ -53,6 +53,8 @@ #include #include #include +#include +#include /** * SR thruster controller app start / stop handling function @@ -77,40 +79,10 @@ int sr_thruster_controller_main(int argc, char *argv[]) memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); - /* Testing */ - for (int i = 0; i < 8; i++) - { - actuator_motors_msg.control[i] = 1; - } - for (int i = 8; i < 12; i++) - { - actuator_motors_msg.control[i] = NAN; - } - PX4_INFO("Sending control signal to PWM!"); - for (int i = 0; i < 5000/2.5; i++) - { - PX4_INFO("Message #%d", i); - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - delay(2.5); - } - for (int i = 0; i < 8; i++) - { - actuator_motors_msg.control[i] = 0; - } - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - PX4_INFO("Turning off PWM"); - - // PX4_INFO("Testing GPIO"); - // PX4_INFO("Setting GPIO 0 to high"); - // px4_arch_configgpio(GPIO_GPIO0_OUTPUT); - // px4_arch_gpiowrite(GPIO_GPIO0_OUTPUT, 1); - // PX4_INFO("Done"); - - //int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); - /* limit the update rate to 1 Hz */ - //orb_set_interval(sensor_sub_fd, 1000); - - // PX4_INFO("orb_sensor_sub created"); + /* For publishing debug messages */ + struct my_message_s my_msg; + memset(&my_msg, 0, sizeof(my_msg)); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); int thrustercmd_sub_fd = orb_subscribe(ORB_ID(thruster_command)); /* limit the update rate to 10 Hz */ @@ -118,13 +90,6 @@ int sr_thruster_controller_main(int argc, char *argv[]) PX4_INFO("Subscriber to thruster command created"); - - /* Log message pub topic */ - // struct log_message_s log_msg; - // memset(&log_msg, 0, sizeof(log_msg)); - // // orb_advert_t log_msg_pub_fd = orb_advertise(ORB_ID(log_message), &log_msg); - // PX4_INFO("Publisher of log messages created"); - /* one could wait for multiple topics with this technique, just using one here */ px4_pollfd_struct_t fds[] = { { .fd = thrustercmd_sub_fd, .events = POLLIN }, @@ -163,23 +128,19 @@ int sr_thruster_controller_main(int argc, char *argv[]) (double)thruster_cmd.y1, (double)thruster_cmd.y2); - actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; - actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; - actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; - actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; - actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; - actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; - actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; - actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; - - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; - /* set att and publish this information for other apps - the following does not have any meaning, it's just an example - */ - // strcpy(log_msg.text, "New thruster command received!"); + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - // orb_publish(ORB_ID(log_message), log_msg_pub_fd, &log_msg); + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); } } } diff --git a/src/modules/microdds_client/dds_topics.yaml b/src/modules/microdds_client/dds_topics.yaml index 4dd7fbffb9b3..0bd9d1c2cd2c 100644 --- a/src/modules/microdds_client/dds_topics.yaml +++ b/src/modules/microdds_client/dds_topics.yaml @@ -50,6 +50,9 @@ publications: - topic: /fmu/out/log_message type: px4_msgs::msg::LogMessage + - topic: /fmu/out/my_message + type: px4_msgs::msg::MyMessage + subscriptions: - topic: /fmu/in/offboard_control_mode From 75b25a8e1ecc4518c4cd7c339380148f96e4f602 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Tue, 27 Jun 2023 19:42:08 +0200 Subject: [PATCH 06/77] Patch for previous commit --- ROMFS/px4fmu_common/init.d/rc.sr_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.sr_defaults | 3 +++ .../nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 7 +++++-- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index 7f0b4905bc72..78a6bea229b7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -8,6 +8,6 @@ # Estimator Group Selection ekf2 start & -# microdds_client start -h 192.168.0.1 +microdds_client start -h 192.168.0.1 sr_thruster_controller diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_defaults b/ROMFS/px4fmu_common/init.d/rc.sr_defaults index 951c8a961b9c..ac0d64afc519 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sr_defaults @@ -25,3 +25,6 @@ param set-default COM_FLTMODE 0 param set-default COM_FLTMODE 7 param set-default COM_FLTMODE 2 param set-default MAN_ARM_GESTURE 0 +param set-default RTL_LAND_DELAY -1 +param set-default RTL_DESCEND_ALT -1 +param set-default RTL_RETURN_ALT 60 diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5a72cc9f5e30..89e97f01d956 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -87,7 +87,9 @@ static int io_timer_handler7(int irq, void *context, void *arg); * taken into account */ #if !defined(BOARD_PWM_FREQ) -#define BOARD_PWM_FREQ 1000000 +// #define BOARD_PWM_FREQ 1000000 +/* Downscaling by 10 */ +#define BOARD_PWM_FREQ 100000 #endif #if !defined(BOARD_ONESHOT_FREQ) @@ -663,7 +665,8 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) * default to updating at 50Hz */ - timer_set_rate(timer, 50); + timer_set_rate(timer, 10); + // timer_set_rate(timer, 50); /* * Note that the timer is left disabled with IRQ subs installed From b54f398a1a128ef565d02162f00741cfd8e5fb90 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Tue, 27 Jun 2023 21:03:00 +0200 Subject: [PATCH 07/77] Converted sr_thruster_controller to a PX4-module --- ROMFS/px4fmu_common/init.d/rc.sr_apps | 4 +- src/examples/sr_thruster_controller/Kconfig | 5 - .../sr_thruster_controller.c | 149 ---------- .../sr_thruster_controller/CMakeLists.txt | 9 +- src/modules/sr_thruster_controller/Kconfig | 5 + .../sr_thruster_controller.cpp | 256 ++++++++++++++++++ .../sr_thruster_controller.hpp | 108 ++++++++ 7 files changed, 377 insertions(+), 159 deletions(-) delete mode 100644 src/examples/sr_thruster_controller/Kconfig delete mode 100644 src/examples/sr_thruster_controller/sr_thruster_controller.c rename src/{examples => modules}/sr_thruster_controller/CMakeLists.txt (92%) create mode 100644 src/modules/sr_thruster_controller/Kconfig create mode 100644 src/modules/sr_thruster_controller/sr_thruster_controller.cpp create mode 100644 src/modules/sr_thruster_controller/sr_thruster_controller.hpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index 78a6bea229b7..8ef5de97a8a9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -8,6 +8,8 @@ # Estimator Group Selection ekf2 start & +# Start MicroDDS Client microdds_client start -h 192.168.0.1 -sr_thruster_controller +# Start Space Robot Thruster Controller +sr_thruster_controller start diff --git a/src/examples/sr_thruster_controller/Kconfig b/src/examples/sr_thruster_controller/Kconfig deleted file mode 100644 index e31e32af9dad..000000000000 --- a/src/examples/sr_thruster_controller/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig EXAMPLES_SR_THRUSTER_CONTROLLER - bool "sr_thruster_controller" - default n - ---help--- - Enable support for SR Thruster Controller diff --git a/src/examples/sr_thruster_controller/sr_thruster_controller.c b/src/examples/sr_thruster_controller/sr_thruster_controller.c deleted file mode 100644 index 9ac07dbf34fd..000000000000 --- a/src/examples/sr_thruster_controller/sr_thruster_controller.c +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file sr_thruster_controller.c - * For controlling solenoid cold gas thrusters in space robotics - * - * @author Elias Krantz - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * SR thruster controller app start / stop handling function - * - * @ingroup apps - */ -__EXPORT int sr_thruster_controller_main(int argc, char *argv[]); - -void delay(unsigned int milliseconds) { - struct timespec ts; - ts.tv_sec = milliseconds / 1000; - ts.tv_nsec = (milliseconds % 1000) * 1000000; - nanosleep(&ts, NULL); -} - -int sr_thruster_controller_main(int argc, char *argv[]) -{ - PX4_INFO("Thruster controller started!"); - - // Create publisher for pwm values - struct actuator_motors_s actuator_motors_msg; - memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); - orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); - - /* For publishing debug messages */ - struct my_message_s my_msg; - memset(&my_msg, 0, sizeof(my_msg)); - orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); - - int thrustercmd_sub_fd = orb_subscribe(ORB_ID(thruster_command)); - /* limit the update rate to 10 Hz */ - orb_set_interval(thrustercmd_sub_fd, 100); - - PX4_INFO("Subscriber to thruster command created"); - - /* one could wait for multiple topics with this technique, just using one here */ - px4_pollfd_struct_t fds[] = { - { .fd = thrustercmd_sub_fd, .events = POLLIN }, - }; - - int error_counter = 0; - - // for (int i = 0; i < 100; i++) { - while (true) { - /* wait for sensor update of 1 file descriptor for 10 s (10 second) */ - int poll_ret = px4_poll(fds, 1, 10000); - - /* handle the poll result */ - if (poll_ret == 0) { - /* this means none of our providers is giving us data */ - PX4_INFO("Got no data within a second"); - - } else if (poll_ret < 0) { - /* this is seriously bad - should be an emergency */ - if (error_counter < 10 || error_counter % 50 == 0) { - /* use a counter to prevent flooding (and slowing us down) */ - PX4_ERR("ERROR return value from poll(): %d", poll_ret); - } - - error_counter++; - - } else { - if (fds[0].revents & POLLIN) { - /* obtained data for the first file descriptor */ - struct thruster_command_s thruster_cmd; - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(thruster_command), thrustercmd_sub_fd, &thruster_cmd); - PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", - (double)thruster_cmd.x1, - (double)thruster_cmd.x2, - (double)thruster_cmd.y1, - (double)thruster_cmd.y2); - - actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; - actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; - actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; - actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; - actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; - actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; - actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; - actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; - - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - - strcpy(my_msg.text, "Thruster command received!"); - orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); - } - } - } - - return OK; -} diff --git a/src/examples/sr_thruster_controller/CMakeLists.txt b/src/modules/sr_thruster_controller/CMakeLists.txt similarity index 92% rename from src/examples/sr_thruster_controller/CMakeLists.txt rename to src/modules/sr_thruster_controller/CMakeLists.txt index ba6306be8f58..63721477d728 100644 --- a/src/examples/sr_thruster_controller/CMakeLists.txt +++ b/src/modules/sr_thruster_controller/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2018 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,10 +30,11 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE examples__sr_thruster_controller + MODULE modules__sr_thruster_controller MAIN sr_thruster_controller SRCS - sr_thruster_controller.c - DEPENDS + sr_thruster_controller.cpp ) + diff --git a/src/modules/sr_thruster_controller/Kconfig b/src/modules/sr_thruster_controller/Kconfig new file mode 100644 index 000000000000..5377dd819699 --- /dev/null +++ b/src/modules/sr_thruster_controller/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SR_THRUSTER_CONTROLLER + bool "sr_thruster_controller" + default n + ---help--- + Enable support for sr_thruster_controller diff --git a/src/modules/sr_thruster_controller/sr_thruster_controller.cpp b/src/modules/sr_thruster_controller/sr_thruster_controller.cpp new file mode 100644 index 000000000000..03a35c5b28d0 --- /dev/null +++ b/src/modules/sr_thruster_controller/sr_thruster_controller.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "sr_thruster_controller.hpp" + +#include +#include +#include + +#include +#include + + +int SrThrusterController::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int SrThrusterController::custom_command(int argc, char *argv[]) +{ + + if (!is_running()) { + print_usage("not running"); + return 1; + } + /* + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); +} + + +int SrThrusterController::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("module", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +SrThrusterController *SrThrusterController::instantiate(int argc, char *argv[]) +{ + /* + int example_param = 0; + bool example_flag = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + // parse CLI arguments + while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + example_param = (int)strtol(myoptarg, nullptr, 10); + break; + + case 'f': + example_flag = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + */ + + SrThrusterController *instance = new SrThrusterController(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +SrThrusterController::SrThrusterController(): + ModuleParams(nullptr) +{ +} + +void SrThrusterController::run() +{ + // Subscribe to thruster command + int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); + px4_pollfd_struct_t fds[] = { + { .fd = thrustercmd_sub, .events = POLLIN }, + }; + + // Create publisher for PWM values + struct actuator_motors_s actuator_motors_msg; + memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); + orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); + + // For publishing debug messages + struct my_message_s my_msg; + memset(&my_msg, 0, sizeof(my_msg)); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); + + // initialize parameters + parameters_update(true); + + while (!should_exit()) { + + // wait for up to 1000ms for data + int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + // Timeout: let the loop run anyway, don't do `continue` here + + } else if (pret < 0) { + // this is undesirable but not much we can do + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(50000); + continue; + + } else if (fds[0].revents & POLLIN) { + + /* obtained data for the first file descriptor */ + struct thruster_command_s thruster_cmd; + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", + (double)thruster_cmd.x1, + (double)thruster_cmd.x2, + (double)thruster_cmd.y1, + (double)thruster_cmd.y2); + + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; + + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + + } + + parameters_update(); + } + + orb_unsubscribe(thrustercmd_sub); +} + +void SrThrusterController::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } +} + +int SrThrusterController::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Section that describes the provided module functionality. + +This is a template for a module running as a task in the background with start/stop/status functionality. + +### Implementation +Section describing the high-level implementation of this module. + +### Examples +CLI usage example: +$ module start -f -p 42 + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("module", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int sr_thruster_controller_main(int argc, char *argv[]) +{ + return SrThrusterController::main(argc, argv); +} diff --git a/src/modules/sr_thruster_controller/sr_thruster_controller.hpp b/src/modules/sr_thruster_controller/sr_thruster_controller.hpp new file mode 100644 index 000000000000..8349e3157a69 --- /dev/null +++ b/src/modules/sr_thruster_controller/sr_thruster_controller.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file sr_thruster_controller.c + * For controlling solenoid cold gas thrusters in space robotics + * + * @author Elias Krantz + */ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace time_literals; + +extern "C" __EXPORT int sr_thruster_controller_main(int argc, char *argv[]); + + +class SrThrusterController : public ModuleBase, public ModuleParams +{ +public: + SrThrusterController(); + + virtual ~SrThrusterController() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static SrThrusterController *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); + + + DEFINE_PARAMETERS( + (ParamInt) _param_sys_autostart, /**< example parameter */ + (ParamInt) _param_sys_autoconfig /**< another parameter */ + ) + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + +}; + From 59297384e8182b4086ad536afbad319dc7224a73 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Fri, 14 Jul 2023 18:35:37 +0200 Subject: [PATCH 08/77] Update to Space Ropbot airframe params --- ROMFS/px4fmu_common/init.d/rc.sr_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.sr_defaults | 26 +++++++++++------------ 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index 8ef5de97a8a9..c57355fdaff1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -9,7 +9,7 @@ ekf2 start & # Start MicroDDS Client -microdds_client start -h 192.168.0.1 +# microdds_client start -h 192.168.0.1 # Start Space Robot Thruster Controller sr_thruster_controller start diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_defaults b/ROMFS/px4fmu_common/init.d/rc.sr_defaults index ac0d64afc519..65da7140e62b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sr_defaults @@ -1,7 +1,5 @@ #!/bin/sh # -# UUV default parameters. -# # NOTE: Script variables are declared/initialized/unset in the rcS script. # @@ -10,21 +8,21 @@ set VEHICLE_TYPE sr # MAV_TYPE_QUADROTOR 2 #param set-default MAV_TYPE 12 -# Disable preflight disarm to not interfere with external launching -param set-default COM_DISARM_PRFLT -1 - -param set-default XRCE_DDS_0_CFG 1000 +# Set micro-dds-client to use ethernet and IP-address 192.168.0.1 +param set-default UXRCE_DDS_CFG 1000 +param set-default UXRCE_DDS_AG_IP -1062731775 -# param set-default CP_DIST -1 +# MAVLink +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP 14550 +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 param set-default CBRK_AIRSPD_CHK 162128 param set-default CBRK_SUPPLY_CHK 894281 param set-default COM_ARM_ARSP_EN 0 param set-default COM_ARM_HFLT_CHK 0 -param set-default COM_FLTMODE 0 -param set-default COM_FLTMODE 7 -param set-default COM_FLTMODE 2 -param set-default MAN_ARM_GESTURE 0 -param set-default RTL_LAND_DELAY -1 -param set-default RTL_DESCEND_ALT -1 -param set-default RTL_RETURN_ALT 60 + +#Missing params +param set-default CP_DIST -1.0 From 568da60a4371ca28216cb71a30f79d91c11e05f1 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Fri, 14 Jul 2023 18:45:25 +0200 Subject: [PATCH 09/77] Modified px4_fmu-v6x board defaults to use less FLASH memory --- boards/px4/fmu-v6x/default.px4board | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 484b844f1910..1e381873e9ef 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -41,20 +41,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 -CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_BATTERY_STATUS=y -CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y -CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y -CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_LAND_DETECTOR=y @@ -72,9 +65,9 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SR_THRUSTER_CONTROLLER=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y -CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DMESG=y From 8cf11f86a4470fa22cbb10eeed0e4afea90bf85d Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Mon, 17 Jul 2023 19:19:03 +0200 Subject: [PATCH 10/77] Update to boardconfig for sitl and fmu-v6x --- boards/px4/fmu-v6x/default.px4board | 5 ----- boards/px4/sitl/default.px4board | 18 +----------------- 2 files changed, 1 insertion(+), 22 deletions(-) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 1e381873e9ef..25fb70ab84c0 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -57,11 +57,6 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y -CONFIG_MODULES_MC_ATT_CONTROL=y -CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y -CONFIG_MODULES_MC_POS_CONTROL=y -CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index db358c3e4d0e..b50d1c413d5b 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -5,8 +5,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y -CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -14,11 +12,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y -CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y -CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y @@ -31,24 +24,15 @@ CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MAVLINK_DIALECT="development" -CONFIG_MODULES_MC_ATT_CONTROL=y -CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y -CONFIG_MODULES_MC_POS_CONTROL=y -CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_SR_THRUSTER_CONTROLLER=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_UUV_ATT_CONTROL=y -CONFIG_MODULES_UUV_POS_CONTROL=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y -CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DYN=y From f1a1c287b177e70e03e8496fedb006ee8802a67d Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 19 Jul 2023 12:03:04 +0200 Subject: [PATCH 11/77] fix: set sim build with gazebo 11 option --- Tools/setup/ubuntu.sh | 35 +++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index af7ec2fbffca..05825ac0516e 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -13,6 +13,7 @@ set -e INSTALL_NUTTX="true" INSTALL_SIM="true" +INSTALL_GZ_CLASSIC="false" INSTALL_ARCH=`uname -m` # Parse arguments @@ -25,6 +26,10 @@ do if [[ $arg == "--no-sim-tools" ]]; then INSTALL_SIM="false" fi + + if [[ $arg == "--gazebo-classic" ]]; then + INSTALL_GZ_CLASSIC="true" + fi done # detect if running in docker @@ -217,19 +222,29 @@ if [[ $INSTALL_SIM == "true" ]]; then ; # Set Java 11 as default - sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version") + sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version"); # Gazebo / Gazebo classic installation if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then - echo "Gazebo (Garden) will be installed" - echo "Earlier versions will be removed" - # Add Gazebo binary repository - sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null - sudo apt-get update -y --quiet - - # Install Gazebo - gazebo_packages="gz-garden" + if [[ $INSTALL_GZ_CLASSIC == "true" ]]; then + echo "Gazebo 11 will be installed" + sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - + # Update list, since new gazebo-stable.list has been added + sudo apt-get update -y --quiet + # gazebo_classic_version=11 + gazebo_packages="gazebo libgazebo-dev libgazebo11" + else + echo "Gazebo (Garden) will be installed" + echo "Earlier versions will be removed" + # Add Gazebo binary repository + sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + sudo apt-get update -y --quiet + + # Install Gazebo + gazebo_packages="gz-garden" + fi else sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - From 5abfb92af131e61ee0f4d6e584438d13503c1868 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 19 Jul 2023 12:03:54 +0200 Subject: [PATCH 12/77] fix: updated README with gazebo 11 option --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index dff9bfcd82bf..cb0f7148d2c6 100644 --- a/README.md +++ b/README.md @@ -6,8 +6,8 @@ This repository is a fork of [PX4 Autopilot](https://github.com/PX4/PX4-Autopilo 2. Run the following commands: ``` git submodule update --init --recursive -bash ./Tools/setup/ubuntu.sh -make px4_sitl gazebo-classic +bash ./Tools/setup/ubuntu.sh --gazebo-classic +make px4_sitl gazebo-classic ``` At this point, your simulation should run with the the default Iris quadcopter. From 731775a831ead2cc5deec67d919b1c409dcb3bab Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Thu, 26 Oct 2023 19:54:30 +0200 Subject: [PATCH 13/77] Update to read RC channels --- src/modules/uxrce_dds_client/dds_topics.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index a2478ce6e668..11b625330fed 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -56,6 +56,9 @@ publications: - topic: /fmu/out/my_message type: px4_msgs::msg::MyMessage + - topic: /fmu/out/rc_channels + type: px4_msgs::msg::RcChannels + subscriptions: - topic: /fmu/in/offboard_control_mode From 2eb6e5522aa3bb18f219ef3714d76c5aec364ed7 Mon Sep 17 00:00:00 2001 From: E-Krantz Date: Thu, 26 Oct 2023 19:55:05 +0200 Subject: [PATCH 14/77] Update to use namespace spacebot# --- ROMFS/px4fmu_common/init.d/rc.sr_apps | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index c57355fdaff1..9aa148edcf5b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -9,7 +9,7 @@ ekf2 start & # Start MicroDDS Client -# microdds_client start -h 192.168.0.1 +uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 # Start Space Robot Thruster Controller sr_thruster_controller start From d297a77d28c5d2d0d5601089420564847827a6ee Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 30 Oct 2023 21:31:57 +0100 Subject: [PATCH 15/77] Add space world make target --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- .../simulator_mavlink/sitl_targets_gazebo-classic.cmake | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 0d53638452e6..3c550ad3d365 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 0d53638452e6da4371c12599e1a649608361811e +Subproject commit 3c550ad3d3655869b8c3c09c309397cac3ca0c9e diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 7b73e03f75c5..c733b759138e 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -114,6 +114,7 @@ if(gazebo_FOUND) warehouse windy yosemite + space ) # find corresponding airframes From 788f8638e910af5fbd8049888b941d78e3640de8 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Tue, 7 Nov 2023 07:38:19 +0100 Subject: [PATCH 16/77] Update sitl_gazebo head --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 3c550ad3d365..e27669f7dd8b 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 3c550ad3d3655869b8c3c09c309397cac3ca0c9e +Subproject commit e27669f7dd8bf7d6ca9f993398c4ec04bd58ccbc From 122509fe2ea27c8dc9eae755761c894ccb65727b Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Tue, 7 Nov 2023 08:11:25 +0100 Subject: [PATCH 17/77] Add space systems rate control module This commit adds a space systems rate control module --- .../airframes/10019_gazebo-classic_omnicopter | 2 +- ROMFS/px4fmu_common/init.d/rc.sr_apps | 10 + boards/px4/sitl/default.px4board | 1 + src/modules/ss_rate_control/CMakeLists.txt | 47 ++ src/modules/ss_rate_control/Kconfig | 12 + .../SpacesystemsRateControl.cpp | 355 ++++++++++++++++ .../SpacesystemsRateControl.hpp | 164 +++++++ .../ss_rate_control/ss_rate_control_params.c | 400 ++++++++++++++++++ 8 files changed, 990 insertions(+), 1 deletion(-) create mode 100644 src/modules/ss_rate_control/CMakeLists.txt create mode 100644 src/modules/ss_rate_control/Kconfig create mode 100644 src/modules/ss_rate_control/SpacesystemsRateControl.cpp create mode 100644 src/modules/ss_rate_control/SpacesystemsRateControl.hpp create mode 100644 src/modules/ss_rate_control/ss_rate_control_params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter index 36bed6a03bc6..f065dfe9f388 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter @@ -7,7 +7,7 @@ # @maintainer Jaeyoung Lim # -. ${R}etc/init.d/rc.mc_defaults +. ${R}etc/init.d/rc.sr_apps param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sr_apps index 9aa148edcf5b..2c88919df787 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sr_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sr_apps @@ -11,5 +11,15 @@ ekf2 start & # Start MicroDDS Client uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# +# Start Control Allocator +# +control_allocator start + +# +# Start Multicopter Rate Controller. +# +ss_rate_control start + # Start Space Robot Thruster Controller sr_thruster_controller start diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index b50d1c413d5b..682beccee985 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_SS_RATE_CONTROL=y CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y diff --git a/src/modules/ss_rate_control/CMakeLists.txt b/src/modules/ss_rate_control/CMakeLists.txt new file mode 100644 index 000000000000..1ffbd797db1c --- /dev/null +++ b/src/modules/ss_rate_control/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__ss_rate_control + MAIN ss_rate_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + SpacesystemsRateControl.cpp + SpacesystemsRateControl.hpp + DEPENDS + circuit_breaker + mathlib + RateControl + px4_work_queue + ) diff --git a/src/modules/ss_rate_control/Kconfig b/src/modules/ss_rate_control/Kconfig new file mode 100644 index 000000000000..ed685ceda495 --- /dev/null +++ b/src/modules/ss_rate_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SS_RATE_CONTROL + bool "ss_rate_control" + default n + ---help--- + Enable support for ss_rate_control + +menuconfig USER_SS_RATE_CONTROL + bool "ss_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_SS_RATE_CONTROL + ---help--- + Put ss_rate_control in userspace memory diff --git a/src/modules/ss_rate_control/SpacesystemsRateControl.cpp b/src/modules/ss_rate_control/SpacesystemsRateControl.cpp new file mode 100644 index 000000000000..318c60955335 --- /dev/null +++ b/src/modules/ss_rate_control/SpacesystemsRateControl.cpp @@ -0,0 +1,355 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SpacesystemsRateControl.hpp" + +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +SpacesystemsRateControl::SpacesystemsRateControl(bool vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + parameters_updated(); + _controller_status_pub.advertise(); +} + +SpacesystemsRateControl::~SpacesystemsRateControl() +{ + perf_free(_loop_perf); +} + +bool +SpacesystemsRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void +SpacesystemsRateControl::parameters_updated() +{ + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), + radians(_param_mc_acro_y_max.get())); +} + +void +SpacesystemsRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _thrust_setpoint(2) = -(manual_control_setpoint.throttle + 1.f) * .5f; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_mc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + + } + } + + perf_end(_loop_perf); +} + +void SpacesystemsRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} + +int SpacesystemsRateControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + SpacesystemsRateControl *instance = new SpacesystemsRateControl(vtol); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SpacesystemsRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacesystemsRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int ss_rate_control_main(int argc, char *argv[]) +{ + return SpacesystemsRateControl::main(argc, argv); +} diff --git a/src/modules/ss_rate_control/SpacesystemsRateControl.hpp b/src/modules/ss_rate_control/SpacesystemsRateControl.hpp new file mode 100644 index 000000000000..58d282b6ace9 --- /dev/null +++ b/src/modules/ss_rate_control/SpacesystemsRateControl.hpp @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SpacesystemsRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + SpacesystemsRateControl(bool vtol = false); + ~SpacesystemsRateControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); + + RateControl _rate_control; ///< class for rate control calculations + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub; + uORB::Publication _vehicle_thrust_setpoint_pub; + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + + bool _landed{true}; + bool _maybe_landed{true}; + + hrt_abstime _last_run{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + // keep setpoint values between updates + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + matrix::Vector3f _rates_setpoint{}; + + float _battery_status_scale{0.0f}; + matrix::Vector3f _thrust_setpoint{}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mc_rollrate_p, + (ParamFloat) _param_mc_rollrate_i, + (ParamFloat) _param_mc_rr_int_lim, + (ParamFloat) _param_mc_rollrate_d, + (ParamFloat) _param_mc_rollrate_ff, + (ParamFloat) _param_mc_rollrate_k, + + (ParamFloat) _param_mc_pitchrate_p, + (ParamFloat) _param_mc_pitchrate_i, + (ParamFloat) _param_mc_pr_int_lim, + (ParamFloat) _param_mc_pitchrate_d, + (ParamFloat) _param_mc_pitchrate_ff, + (ParamFloat) _param_mc_pitchrate_k, + + (ParamFloat) _param_mc_yawrate_p, + (ParamFloat) _param_mc_yawrate_i, + (ParamFloat) _param_mc_yr_int_lim, + (ParamFloat) _param_mc_yawrate_d, + (ParamFloat) _param_mc_yawrate_ff, + (ParamFloat) _param_mc_yawrate_k, + + (ParamFloat) _param_mc_acro_r_max, + (ParamFloat) _param_mc_acro_p_max, + (ParamFloat) _param_mc_acro_y_max, + (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamBool) _param_mc_bat_scale_en + ) +}; diff --git a/src/modules/ss_rate_control/ss_rate_control_params.c b/src/modules/ss_rate_control/ss_rate_control_params.c new file mode 100644 index 000000000000..55e92f9be856 --- /dev/null +++ b/src/modules/ss_rate_control/ss_rate_control_params.c @@ -0,0 +1,400 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_rate_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.5 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); + +/** + * Roll rate integrator limit + * + * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @max 0.01 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error + * + MC_ROLLRATE_I * error_integral + * + MC_ROLLRATE_D * error_derivative) + * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.6 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); + +/** + * Pitch rate integrator limit + * + * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error + * + MC_PITCHRATE_I * error_integral + * + MC_PITCHRATE_D * error_derivative) + * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 0.6 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); + +/** + * Yaw rate integrator limit + * + * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_YAWRATE_K * (MC_YAWRATE_P * error + * + MC_YAWRATE_I * error_integral + * + MC_YAWRATE_D * error_derivative) + * Set MC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set MC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); + +/** + * Max acro roll rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); + +/** + * Max acro pitch rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); + +/** + * Max acro yaw rate + * + * default 1.5 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); + +/** + * Acro mode Expo factor for Roll and Pitch. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); + +/** + * Acro mode Expo factor for Yaw. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); + +/** + * Acro mode SuperExpo factor for Roll and Pitch. + * + * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); + +/** + * Acro mode SuperExpo factor for Yaw. + * + * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Multicopter Rate Control + */ +PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); From bb396b45924c53bdd799099f0124d9173917e49a Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Wed, 8 Nov 2023 03:08:25 +0100 Subject: [PATCH 18/77] Use bidirectional thrust on the sticks --- src/modules/ss_rate_control/SpacesystemsRateControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ss_rate_control/SpacesystemsRateControl.cpp b/src/modules/ss_rate_control/SpacesystemsRateControl.cpp index 318c60955335..ff7309ea7a9b 100644 --- a/src/modules/ss_rate_control/SpacesystemsRateControl.cpp +++ b/src/modules/ss_rate_control/SpacesystemsRateControl.cpp @@ -161,7 +161,7 @@ SpacesystemsRateControl::Run() math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -(manual_control_setpoint.throttle + 1.f) * .5f; + _thrust_setpoint(2) = -manual_control_setpoint.throttle; _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; // publish rate setpoint From cbd7a9cd39b970cc3537e788c1c8c68284c9d646 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sun, 14 Jan 2024 01:35:07 +0100 Subject: [PATCH 19/77] fix: builds with sitl gazebo --- .vscode/settings.json | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 35c3146dcf78..8fb46ec23eea 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -127,5 +127,19 @@ "terminal.integrated.scrollback": 15000, "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" - } + }, + "python.autoComplete.extraPaths": [ + "/home/roque/ros2_astrobee_ws/install/ff_msgs/local/lib/python3.10/dist-packages", + "/home/roque/ros2_astrobee_ws/install/ff_hw_msgs/local/lib/python3.10/dist-packages", + "/home/roque/ros2_astrobee_ws/install/astrobee/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/roque/ros2_astrobee_ws/install/ff_msgs/local/lib/python3.10/dist-packages", + "/home/roque/ros2_astrobee_ws/install/ff_hw_msgs/local/lib/python3.10/dist-packages", + "/home/roque/ros2_astrobee_ws/install/astrobee/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] } From 4ac6e6a61aca0cc30680ea5875c1361fff24075c Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sun, 14 Jan 2024 05:02:37 +0100 Subject: [PATCH 20/77] fix: install build folder and ROMFS for ros2 launch sequence --- CMakeLists.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 741e8e574aec..ddd9b6818c85 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -487,3 +487,18 @@ add_custom_target(install_python_requirements if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/finalize.cmake") include(finalize) endif() + +# Install SITL build files +file(GLOB RESULT build/*) +list(LENGTH RESULT RES_LEN) +if(RES_LEN GREATER 0) + install(DIRECTORY build/ + DESTINATION share/${PROJECT_NAME} + PATTERN ".svn" EXCLUDE) + # Install ROMFS + install(DIRECTORY ROMFS/ + DESTINATION share/${PROJECT_NAME} + PATTERN ".svn" EXCLUDE) +else() + message(STATUS "No PX4 build files to install.") +endif() \ No newline at end of file From 2d4b3d3ffefa7bfcf9c7871d0e0e6faeda487c36 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 16 Jan 2024 05:46:51 +0100 Subject: [PATCH 21/77] rft: clean CMakeLists and improved posix rcS for uxrcde connection --- CMakeLists.txt | 6 +++--- ROMFS/px4fmu_common/init.d-posix/rcS | 4 ++++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ddd9b6818c85..b75dd570db7b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -496,9 +496,9 @@ if(RES_LEN GREATER 0) DESTINATION share/${PROJECT_NAME} PATTERN ".svn" EXCLUDE) # Install ROMFS - install(DIRECTORY ROMFS/ - DESTINATION share/${PROJECT_NAME} - PATTERN ".svn" EXCLUDE) +# install(DIRECTORY ROMFS/ +# DESTINATION share/${PROJECT_NAME} +# PATTERN ".svn" EXCLUDE) else() message(STATUS "No PX4 build files to install.") endif() \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 7ecbfc3dcb57..6110215efc0a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -289,6 +289,10 @@ then uxrce_dds_port="$PX4_UXRCE_DDS_PORT" fi +# Refresh uxrce_dds_client if already running +uxrce_dds_client stop + +# Start it for SITL setup uxrce_dds_client start -t udp -h 127.0.0.1 -p $uxrce_dds_port $uxrce_dds_ns if param greater -s MNT_MODE_IN -1 From a8e85f36d6e9a989aaeb6ef5d35e07ab36845510 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sat, 10 Feb 2024 01:51:09 +0100 Subject: [PATCH 22/77] rft: changing default names for space to spacecraft (sc) --- .../airframes/9000_gazebo-classic_spacecraft | 96 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 2 + ROMFS/px4fmu_common/init.d/CMakeLists.txt | 4 +- .../init.d/{rc.sr_apps => rc.sc_apps} | 0 .../init.d/{rc.sr_defaults => rc.sc_defaults} | 0 .../sitl_targets_gazebo-classic.cmake | 1 + 6 files changed, 101 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft rename ROMFS/px4fmu_common/init.d/{rc.sr_apps => rc.sc_apps} (100%) rename ROMFS/px4fmu_common/init.d/{rc.sr_defaults => rc.sc_defaults} (100%) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft new file mode 100644 index 000000000000..0df4395690f4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft @@ -0,0 +1,96 @@ +#!/bin/sh +# +# @name 6DoF Spacecraft Model +# +# @type Freeflyer with 8 thrusters +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_apps + +param set-default CA_AIRFRAME 0 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +param set-default CA_ROTOR0_PX 0.14435 +param set-default CA_ROTOR0_PY -0.14435 +param set-default CA_ROTOR0_PZ -0.14435 +param set-default CA_ROTOR0_KM 0.05 # CCW +param set-default CA_ROTOR0_AX -0.788675 +param set-default CA_ROTOR0_AY -0.211325 +param set-default CA_ROTOR0_AZ -0.57735 + +param set-default CA_ROTOR1_PX -0.14435 +param set-default CA_ROTOR1_PY -0.14435 +param set-default CA_ROTOR1_PZ -0.14435 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_AX 0.211325 +param set-default CA_ROTOR1_AY -0.788675 +param set-default CA_ROTOR1_AZ 0.57735 + +param set-default CA_ROTOR2_PX 0.14435 +param set-default CA_ROTOR2_PY 0.14435 +param set-default CA_ROTOR2_PZ -0.14435 +param set-default CA_ROTOR2_KM 0.05 +param set-default CA_ROTOR2_AX -0.211325 +param set-default CA_ROTOR2_AY 0.788675 +param set-default CA_ROTOR2_AZ 0.57735 + +param set-default CA_ROTOR3_PX -0.14435 +param set-default CA_ROTOR3_PY 0.14435 +param set-default CA_ROTOR3_PZ -0.14435 +param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR3_AX 0.788675 +param set-default CA_ROTOR3_AY 0.211325 +param set-default CA_ROTOR3_AZ -0.57735 + +param set-default CA_ROTOR4_PX 0.14435 +param set-default CA_ROTOR4_PY -0.14435 +param set-default CA_ROTOR4_PZ 0.14435 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 0.788675 +param set-default CA_ROTOR4_AY 0.211325 +param set-default CA_ROTOR4_AZ -0.57735 + +param set-default CA_ROTOR5_PX -0.14435 +param set-default CA_ROTOR5_PY -0.14435 +param set-default CA_ROTOR5_PZ 0.14435 +param set-default CA_ROTOR5_KM 0.05 +param set-default CA_ROTOR5_AX -0.211325 +param set-default CA_ROTOR5_AY 0.788675 +param set-default CA_ROTOR5_AZ 0.57735 + +param set-default CA_ROTOR6_PX 0.14435 +param set-default CA_ROTOR6_PY 0.14435 +param set-default CA_ROTOR6_PZ 0.14435 +param set-default CA_ROTOR6_KM 0.05 +param set-default CA_ROTOR6_AX 0.211325 +param set-default CA_ROTOR6_AY -0.788675 +param set-default CA_ROTOR6_AZ 0.57735 + +param set-default CA_ROTOR7_PX -0.14435 +param set-default CA_ROTOR7_PY 0.14435 +param set-default CA_ROTOR7_PZ 0.14435 +param set-default CA_ROTOR7_KM 0.05 +param set-default CA_ROTOR7_AX -0.788675 +param set-default CA_ROTOR7_AY -0.211325 +param set-default CA_ROTOR7_AZ -0.57735 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 105 +param set-default PWM_MAIN_FUNC6 106 +param set-default PWM_MAIN_FUNC7 107 +param set-default PWM_MAIN_FUNC8 108 + +# disable MC desaturation which improves attitude tracking +param set-default CA_METHOD 0 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 743d54fc702e..0cf65f9cfa87 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -81,6 +81,8 @@ px4_add_romfs_files( 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post + 9000_gazebo-classic_spacecraft + 10015_gazebo-classic_iris 10016_none_iris 10017_jmavsim_iris diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index d59159f1c717..696ea50ceda7 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -48,8 +48,8 @@ px4_add_romfs_files( rc.mc_defaults rcS rc.sensors - rc.sr_apps - rc.sr_defaults + rc.sc_apps + rc.sc_defaults rc.thermal_cal rc.rover_apps rc.rover_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.sr_apps rename to ROMFS/px4fmu_common/init.d/rc.sc_apps diff --git a/ROMFS/px4fmu_common/init.d/rc.sr_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.sr_defaults rename to ROMFS/px4fmu_common/init.d/rc.sc_defaults diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index c733b759138e..48ef825843c8 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -86,6 +86,7 @@ if(gazebo_FOUND) iris_rplidar iris_vision omnicopter + spacecraft plane plane_cam plane_catapult From a7046ec005e7cfa605c256f9d6807db9a8e415a8 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sat, 10 Feb 2024 02:06:30 +0100 Subject: [PATCH 23/77] feat: changing files and filenames to sc_ terminology --- ROMFS/px4fmu_common/init.d/rc.sc_apps | 4 +- boards/px4/fmu-v6x/default.px4board | 2 +- boards/px4/sitl/default.px4board | 4 +- .../CMakeLists.txt | 8 +- src/modules/sc_rate_control/Kconfig | 12 + .../sc_rate_control/SpacecraftRateControl.cpp | 337 +++++++++++++++++ .../SpacecraftRateControl.hpp} | 0 .../sc_rate_control_params.c} | 0 .../CMakeLists.txt | 6 +- src/modules/sc_thruster_controller/Kconfig | 5 + .../sc_thruster_controller.cpp | 230 ++++++++++++ .../sc_thruster_controller.hpp} | 67 ++-- src/modules/sr_thruster_controller/Kconfig | 5 - .../sr_thruster_controller.cpp | 256 ------------- src/modules/ss_rate_control/Kconfig | 12 - .../SpacesystemsRateControl.cpp | 355 ------------------ 16 files changed, 626 insertions(+), 677 deletions(-) rename src/modules/{ss_rate_control => sc_rate_control}/CMakeLists.txt (94%) create mode 100644 src/modules/sc_rate_control/Kconfig create mode 100644 src/modules/sc_rate_control/SpacecraftRateControl.cpp rename src/modules/{ss_rate_control/SpacesystemsRateControl.hpp => sc_rate_control/SpacecraftRateControl.hpp} (100%) rename src/modules/{ss_rate_control/ss_rate_control_params.c => sc_rate_control/sc_rate_control_params.c} (100%) rename src/modules/{sr_thruster_controller => sc_thruster_controller}/CMakeLists.txt (94%) create mode 100644 src/modules/sc_thruster_controller/Kconfig create mode 100644 src/modules/sc_thruster_controller/sc_thruster_controller.cpp rename src/modules/{sr_thruster_controller/sr_thruster_controller.hpp => sc_thruster_controller/sc_thruster_controller.hpp} (65%) delete mode 100644 src/modules/sr_thruster_controller/Kconfig delete mode 100644 src/modules/sr_thruster_controller/sr_thruster_controller.cpp delete mode 100644 src/modules/ss_rate_control/Kconfig delete mode 100644 src/modules/ss_rate_control/SpacesystemsRateControl.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index 2c88919df787..51993870d2bd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -19,7 +19,7 @@ control_allocator start # # Start Multicopter Rate Controller. # -ss_rate_control start +sc_rate_control start # Start Space Robot Thruster Controller -sr_thruster_controller start +sc_thruster_controller start diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 25fb70ab84c0..3fe3dee0d306 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -60,7 +60,7 @@ CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SR_THRUSTER_CONTROLLER=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 682beccee985..c69303ca6e89 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -23,7 +23,7 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y -CONFIG_MODULES_SS_RATE_CONTROL=y +CONFIG_MODULES_SC_RATE_CONTROL=y CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y @@ -31,7 +31,7 @@ CONFIG_MODULES_REPLAY=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y -CONFIG_MODULES_SR_THRUSTER_CONTROLLER=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/src/modules/ss_rate_control/CMakeLists.txt b/src/modules/sc_rate_control/CMakeLists.txt similarity index 94% rename from src/modules/ss_rate_control/CMakeLists.txt rename to src/modules/sc_rate_control/CMakeLists.txt index 1ffbd797db1c..d2f197cb7fe4 100644 --- a/src/modules/ss_rate_control/CMakeLists.txt +++ b/src/modules/sc_rate_control/CMakeLists.txt @@ -32,13 +32,13 @@ ############################################################################ px4_add_module( - MODULE modules__ss_rate_control - MAIN ss_rate_control + MODULE modules__sc_rate_control + MAIN sc_rate_control COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} SRCS - SpacesystemsRateControl.cpp - SpacesystemsRateControl.hpp + SpacecraftRateControl.cpp + SpacecraftRateControl.hpp DEPENDS circuit_breaker mathlib diff --git a/src/modules/sc_rate_control/Kconfig b/src/modules/sc_rate_control/Kconfig new file mode 100644 index 000000000000..0de7129a2bea --- /dev/null +++ b/src/modules/sc_rate_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SC_RATE_CONTROL + bool "sc_rate_control" + default n + ---help--- + Enable support for sc_rate_control + +menuconfig USER_SC_RATE_CONTROL + bool "sc_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_SC_RATE_CONTROL + ---help--- + Put sc_rate_control in userspace memory diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.cpp b/src/modules/sc_rate_control/SpacecraftRateControl.cpp new file mode 100644 index 000000000000..094c134c1d2b --- /dev/null +++ b/src/modules/sc_rate_control/SpacecraftRateControl.cpp @@ -0,0 +1,337 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SpacecraftRateControl.hpp" + +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +SpacesystemsRateControl::SpacesystemsRateControl(bool vtol) + : ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) { + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + parameters_updated(); + _controller_status_pub.advertise(); +} + +SpacesystemsRateControl::~SpacesystemsRateControl() { perf_free(_loop_perf); } + +bool SpacesystemsRateControl::init() { + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void SpacesystemsRateControl::parameters_updated() { + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), + radians(_param_mc_acro_y_max.get())); +} + +void SpacesystemsRateControl::Run() { + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _thrust_setpoint(2) = -manual_control_setpoint.throttle; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || + _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + const Vector3f att_control = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_mc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = + math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = + math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } + + perf_end(_loop_perf); +} + +void SpacesystemsRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s& vehicle_torque_setpoint, + float dt) { + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} + +int SpacesystemsRateControl::task_spawn(int argc, char* argv[]) { + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + SpacesystemsRateControl* instance = new SpacesystemsRateControl(vtol); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SpacesystemsRateControl::custom_command(int argc, char* argv[]) { return print_usage("unknown command"); } + +int SpacesystemsRateControl::print_usage(const char* reason) { + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sc_rate_control_main(int argc, char* argv[]) { + return SpacesystemsRateControl::main(argc, argv); +} diff --git a/src/modules/ss_rate_control/SpacesystemsRateControl.hpp b/src/modules/sc_rate_control/SpacecraftRateControl.hpp similarity index 100% rename from src/modules/ss_rate_control/SpacesystemsRateControl.hpp rename to src/modules/sc_rate_control/SpacecraftRateControl.hpp diff --git a/src/modules/ss_rate_control/ss_rate_control_params.c b/src/modules/sc_rate_control/sc_rate_control_params.c similarity index 100% rename from src/modules/ss_rate_control/ss_rate_control_params.c rename to src/modules/sc_rate_control/sc_rate_control_params.c diff --git a/src/modules/sr_thruster_controller/CMakeLists.txt b/src/modules/sc_thruster_controller/CMakeLists.txt similarity index 94% rename from src/modules/sr_thruster_controller/CMakeLists.txt rename to src/modules/sc_thruster_controller/CMakeLists.txt index 63721477d728..00e843dba0cf 100644 --- a/src/modules/sr_thruster_controller/CMakeLists.txt +++ b/src/modules/sc_thruster_controller/CMakeLists.txt @@ -32,9 +32,9 @@ ############################################################################ px4_add_module( - MODULE modules__sr_thruster_controller - MAIN sr_thruster_controller + MODULE modules__sc_thruster_controller + MAIN sc_thruster_controller SRCS - sr_thruster_controller.cpp + sc_thruster_controller.cpp ) diff --git a/src/modules/sc_thruster_controller/Kconfig b/src/modules/sc_thruster_controller/Kconfig new file mode 100644 index 000000000000..1d7760abd20b --- /dev/null +++ b/src/modules/sc_thruster_controller/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SC_THRUSTER_CONTROLLER + bool "sc_thruster_controller" + default n + ---help--- + Enable support for sc_thruster_controller diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp new file mode 100644 index 000000000000..1ffadcf610b0 --- /dev/null +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "sc_thruster_controller.hpp" + +#include +#include +#include + +#include +#include + +int ScThrusterController::print_status() { + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int ScThrusterController::custom_command(int argc, char* argv[]) { + if (!is_running()) { + print_usage("not running"); + return 1; + } + /* + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); +} + +int ScThrusterController::task_spawn(int argc, char* argv[]) { + _task_id = px4_task_spawn_cmd("module", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, (px4_main_t)&run_trampoline, + (char* const*)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +ScThrusterController* ScThrusterController::instantiate(int argc, char* argv[]) { + /* +int example_param = 0; + bool example_flag = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + // parse CLI arguments + while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + example_param = (int)strtol(myoptarg, nullptr, 10); + break; + + case 'f': + example_flag = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } +*/ + + ScThrusterController* instance = new ScThrusterController(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +ScThrusterController::ScThrusterController() : ModuleParams(nullptr) {} + +void ScThrusterController::run() { + // Subscribe to thruster command + int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); + px4_pollfd_struct_t fds[] = { + {.fd = thrustercmd_sub, .events = POLLIN}, + }; + + // Create publisher for PWM values + struct actuator_motors_s actuator_motors_msg; + memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); + orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); + + // For publishing debug messages + struct my_message_s my_msg; + memset(&my_msg, 0, sizeof(my_msg)); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); + + // initialize parameters + parameters_update(true); + + while (!should_exit()) { + // wait for up to 1000ms for data + int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + // Timeout: let the loop run anyway, don't do `continue` here + + } else if (pret < 0) { + // this is undesirable but not much we can do + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(50000); + continue; + + } else if (fds[0].revents & POLLIN) { + /* obtained data for the first file descriptor */ + struct thruster_command_s thruster_cmd; + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, + (double)thruster_cmd.y1, (double)thruster_cmd.y2); + + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; + + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + } + + parameters_update(); + } + + orb_unsubscribe(thrustercmd_sub); +} + +void ScThrusterController::parameters_update(bool force) { + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } +} + +int ScThrusterController::print_usage(const char* reason) { + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Section that describes the provided module functionality. + +This is a template for a module running as a task in the background with start/stop/status functionality. + +### Implementation +Section describing the high-level implementation of this module. + +### Examples +CLI usage example: +$ module start -f -p 42 + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("module", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int sc_thruster_controller_main(int argc, char* argv[]) { return ScThrusterController::main(argc, argv); } diff --git a/src/modules/sr_thruster_controller/sr_thruster_controller.hpp b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp similarity index 65% rename from src/modules/sr_thruster_controller/sr_thruster_controller.hpp rename to src/modules/sc_thruster_controller/sc_thruster_controller.hpp index 8349e3157a69..8facdcb34cfb 100644 --- a/src/modules/sr_thruster_controller/sr_thruster_controller.hpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp @@ -31,7 +31,7 @@ * ****************************************************************************/ /** - * @file sr_thruster_controller.c + * @file sc_thruster_controller.c * For controlling solenoid cold gas thrusters in space robotics * * @author Elias Krantz @@ -58,51 +58,44 @@ using namespace time_literals; -extern "C" __EXPORT int sr_thruster_controller_main(int argc, char *argv[]); +extern "C" __EXPORT int sc_thruster_controller_main(int argc, char* argv[]); +class ScThrusterController : public ModuleBase, public ModuleParams { + public: + ScThrusterController(); -class SrThrusterController : public ModuleBase, public ModuleParams -{ -public: - SrThrusterController(); + virtual ~ScThrusterController() = default; - virtual ~SrThrusterController() = default; + /** @see ModuleBase */ + static int task_spawn(int argc, char* argv[]); - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); + /** @see ModuleBase */ + static ScThrusterController* instantiate(int argc, char* argv[]); - /** @see ModuleBase */ - static SrThrusterController *instantiate(int argc, char *argv[]); + /** @see ModuleBase */ + static int custom_command(int argc, char* argv[]); - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); + /** @see ModuleBase */ + static int print_usage(const char* reason = nullptr); - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); + /** @see ModuleBase::run() */ + void run() override; - /** @see ModuleBase::run() */ - void run() override; + /** @see ModuleBase::print_status() */ + int print_status() override; - /** @see ModuleBase::print_status() */ - int print_status() override; + private: + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); -private: - - /** - * Check for parameter changes and update them if needed. - * @param parameter_update_sub uorb subscription to parameter_update - * @param force for a parameter update - */ - void parameters_update(bool force = false); - - - DEFINE_PARAMETERS( - (ParamInt) _param_sys_autostart, /**< example parameter */ - (ParamInt) _param_sys_autoconfig /**< another parameter */ - ) - - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + DEFINE_PARAMETERS((ParamInt)_param_sys_autostart, /**< example parameter */ + (ParamInt)_param_sys_autoconfig /**< another parameter */ + ) + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; }; - diff --git a/src/modules/sr_thruster_controller/Kconfig b/src/modules/sr_thruster_controller/Kconfig deleted file mode 100644 index 5377dd819699..000000000000 --- a/src/modules/sr_thruster_controller/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig MODULES_SR_THRUSTER_CONTROLLER - bool "sr_thruster_controller" - default n - ---help--- - Enable support for sr_thruster_controller diff --git a/src/modules/sr_thruster_controller/sr_thruster_controller.cpp b/src/modules/sr_thruster_controller/sr_thruster_controller.cpp deleted file mode 100644 index 03a35c5b28d0..000000000000 --- a/src/modules/sr_thruster_controller/sr_thruster_controller.cpp +++ /dev/null @@ -1,256 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "sr_thruster_controller.hpp" - -#include -#include -#include - -#include -#include - - -int SrThrusterController::print_status() -{ - PX4_INFO("Running"); - // TODO: print additional runtime information about the state of the module - - return 0; -} - -int SrThrusterController::custom_command(int argc, char *argv[]) -{ - - if (!is_running()) { - print_usage("not running"); - return 1; - } - /* - // additional custom commands can be handled like this: - if (!strcmp(argv[0], "do-something")) { - get_instance()->do_something(); - return 0; - } - */ - - return print_usage("unknown command"); -} - - -int SrThrusterController::task_spawn(int argc, char *argv[]) -{ - _task_id = px4_task_spawn_cmd("module", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (px4_main_t)&run_trampoline, - (char *const *)argv); - - if (_task_id < 0) { - _task_id = -1; - return -errno; - } - - return 0; -} - -SrThrusterController *SrThrusterController::instantiate(int argc, char *argv[]) -{ - /* - int example_param = 0; - bool example_flag = false; - bool error_flag = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - // parse CLI arguments - while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'p': - example_param = (int)strtol(myoptarg, nullptr, 10); - break; - - case 'f': - example_flag = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return nullptr; - } - */ - - SrThrusterController *instance = new SrThrusterController(); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - } - - return instance; -} - -SrThrusterController::SrThrusterController(): - ModuleParams(nullptr) -{ -} - -void SrThrusterController::run() -{ - // Subscribe to thruster command - int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); - px4_pollfd_struct_t fds[] = { - { .fd = thrustercmd_sub, .events = POLLIN }, - }; - - // Create publisher for PWM values - struct actuator_motors_s actuator_motors_msg; - memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); - orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); - - // For publishing debug messages - struct my_message_s my_msg; - memset(&my_msg, 0, sizeof(my_msg)); - orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); - - // initialize parameters - parameters_update(true); - - while (!should_exit()) { - - // wait for up to 1000ms for data - int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); - - if (pret == 0) { - // Timeout: let the loop run anyway, don't do `continue` here - - } else if (pret < 0) { - // this is undesirable but not much we can do - PX4_ERR("poll error %d, %d", pret, errno); - px4_usleep(50000); - continue; - - } else if (fds[0].revents & POLLIN) { - - /* obtained data for the first file descriptor */ - struct thruster_command_s thruster_cmd; - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); - PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", - (double)thruster_cmd.x1, - (double)thruster_cmd.x2, - (double)thruster_cmd.y1, - (double)thruster_cmd.y2); - - actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; - actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; - actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; - actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; - actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; - actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; - actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; - actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; - - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - - strcpy(my_msg.text, "Thruster command received!"); - orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); - - } - - parameters_update(); - } - - orb_unsubscribe(thrustercmd_sub); -} - -void SrThrusterController::parameters_update(bool force) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s update; - _parameter_update_sub.copy(&update); - - // update parameters from storage - updateParams(); - } -} - -int SrThrusterController::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Section that describes the provided module functionality. - -This is a template for a module running as a task in the background with start/stop/status functionality. - -### Implementation -Section describing the high-level implementation of this module. - -### Examples -CLI usage example: -$ module start -f -p 42 - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("module", "template"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true); - PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -int sr_thruster_controller_main(int argc, char *argv[]) -{ - return SrThrusterController::main(argc, argv); -} diff --git a/src/modules/ss_rate_control/Kconfig b/src/modules/ss_rate_control/Kconfig deleted file mode 100644 index ed685ceda495..000000000000 --- a/src/modules/ss_rate_control/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_SS_RATE_CONTROL - bool "ss_rate_control" - default n - ---help--- - Enable support for ss_rate_control - -menuconfig USER_SS_RATE_CONTROL - bool "ss_rate_control running as userspace module" - default n - depends on BOARD_PROTECTED && MODULES_SS_RATE_CONTROL - ---help--- - Put ss_rate_control in userspace memory diff --git a/src/modules/ss_rate_control/SpacesystemsRateControl.cpp b/src/modules/ss_rate_control/SpacesystemsRateControl.cpp deleted file mode 100644 index ff7309ea7a9b..000000000000 --- a/src/modules/ss_rate_control/SpacesystemsRateControl.cpp +++ /dev/null @@ -1,355 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "SpacesystemsRateControl.hpp" - -#include -#include -#include -#include -#include - -using namespace matrix; -using namespace time_literals; -using math::radians; - -SpacesystemsRateControl::SpacesystemsRateControl(bool vtol) : - ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), - _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) -{ - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - parameters_updated(); - _controller_status_pub.advertise(); -} - -SpacesystemsRateControl::~SpacesystemsRateControl() -{ - perf_free(_loop_perf); -} - -bool -SpacesystemsRateControl::init() -{ - if (!_vehicle_angular_velocity_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - return true; -} - -void -SpacesystemsRateControl::parameters_updated() -{ - // rate control parameters - // The controller gain K is used to convert the parallel (P + I/s + sD) form - // to the ideal (K * [1 + 1/sTi + sTd]) form - const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); - - _rate_control.setPidGains( - rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), - rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), - rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); - - _rate_control.setIntegratorLimit( - Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); - - _rate_control.setFeedForwardGain( - Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); - - - // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), - radians(_param_mc_acro_y_max.get())); -} - -void -SpacesystemsRateControl::Run() -{ - if (should_exit()) { - _vehicle_angular_velocity_sub.unregisterCallback(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - parameters_updated(); - } - - /* run controller on gyro changes */ - vehicle_angular_velocity_s angular_velocity; - - if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - - const hrt_abstime now = angular_velocity.timestamp_sample; - - // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); - _last_run = now; - - const Vector3f rates{angular_velocity.xyz}; - const Vector3f angular_accel{angular_velocity.xyz_derivative}; - - /* check for updates in other topics */ - _vehicle_control_mode_sub.update(&_vehicle_control_mode); - - if (_vehicle_land_detected_sub.updated()) { - vehicle_land_detected_s vehicle_land_detected; - - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; - _maybe_landed = vehicle_land_detected.maybe_landed; - } - } - - _vehicle_status_sub.update(&_vehicle_status); - - // use rates setpoint topic - vehicle_rates_setpoint_s vehicle_rates_setpoint{}; - - if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { - // generate the rate setpoint from sticks - manual_control_setpoint_s manual_control_setpoint; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - // manual rates control - ACRO mode - const Vector3f man_rate_sp{ - math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; - - _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -manual_control_setpoint.throttle; - _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; - - // publish rate setpoint - vehicle_rates_setpoint.roll = _rates_setpoint(0); - vehicle_rates_setpoint.pitch = _rates_setpoint(1); - vehicle_rates_setpoint.yaw = _rates_setpoint(2); - _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); - vehicle_rates_setpoint.timestamp = hrt_absolute_time(); - - _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); - } - - } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { - if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { - _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); - _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); - _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); - _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); - } - } - - // run the rate controller - if (_vehicle_control_mode.flag_control_rates_enabled) { - - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rate_control.resetIntegral(); - } - - // update saturation status from control allocation feedback - control_allocator_status_s control_allocator_status; - - if (_control_allocator_status_sub.update(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; - - if (!control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; - - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } - } - } - - // TODO: send the unallocated value directly for better anti-windup - _rate_control.setSaturationStatus(saturation_positive, saturation_negative); - } - - // run rate controller - const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); - - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - _rate_control.getRateControlStatus(rate_ctrl_status); - rate_ctrl_status.timestamp = hrt_absolute_time(); - _controller_status_pub.publish(rate_ctrl_status); - - // publish thrust and torque setpoints - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - - _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); - vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; - vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; - vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; - - // scale setpoints by battery status if enabled - if (_param_mc_bat_scale_en.get()) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status; - - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_status_scale = battery_status.scale; - } - } - - if (_battery_status_scale > 0.f) { - for (int i = 0; i < 3; i++) { - vehicle_thrust_setpoint.xyz[i] = math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); - vehicle_torque_setpoint.xyz[i] = math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); - } - } - } - - vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; - vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); - - vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; - vehicle_torque_setpoint.timestamp = hrt_absolute_time(); - _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); - - updateActuatorControlsStatus(vehicle_torque_setpoint, dt); - - } - } - - perf_end(_loop_perf); -} - -void SpacesystemsRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, - float dt) -{ - for (int i = 0; i < 3; i++) { - _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; - } - - _energy_integration_time += dt; - - if (_energy_integration_time > 500e-3f) { - - actuator_controls_status_s status; - status.timestamp = vehicle_torque_setpoint.timestamp; - - for (int i = 0; i < 3; i++) { - status.control_power[i] = _control_energy[i] / _energy_integration_time; - _control_energy[i] = 0.f; - } - - _actuator_controls_status_pub.publish(status); - _energy_integration_time = 0.f; - } -} - -int SpacesystemsRateControl::task_spawn(int argc, char *argv[]) -{ - bool vtol = false; - - if (argc > 1) { - if (strcmp(argv[1], "vtol") == 0) { - vtol = true; - } - } - - SpacesystemsRateControl *instance = new SpacesystemsRateControl(vtol); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int SpacesystemsRateControl::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int SpacesystemsRateControl::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -This implements the multicopter rate controller. It takes rate setpoints (in acro mode -via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. - -The controller has a PID loop for angular rate error. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int ss_rate_control_main(int argc, char *argv[]) -{ - return SpacesystemsRateControl::main(argc, argv); -} From accc043091b00e60bd05756022e7dbaa42e7c9e3 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 3 Apr 2024 19:07:57 +0200 Subject: [PATCH 24/77] feat: update default rc_app and reduce logging. Might break SIM --- .../airframes/10019_gazebo-classic_omnicopter | 2 +- .../init.d/airframes/70000_kth_space_robot | 2 +- ROMFS/px4fmu_common/init.d/rc.sc_apps | 4 +- ROMFS/px4fmu_common/init.d/rc.sc_defaults | 2 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 4 +- boards/px4/fmu-v6x/default.px4board | 2 +- .../sc_thruster_controller.cpp | 62 +++++++++---------- .../sc_thruster_controller.hpp | 5 ++ 8 files changed, 42 insertions(+), 41 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter index f065dfe9f388..37321aae96d3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter @@ -7,7 +7,7 @@ # @maintainer Jaeyoung Lim # -. ${R}etc/init.d/rc.sr_apps +. ${R}etc/init.d/rc.sc_apps param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index 7b352f532eae..12708843098a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -8,7 +8,7 @@ # @maintainer DISCOWER # -. ${R}etc/init.d/rc.sr_defaults +. ${R}etc/init.d/rc.sc_defaults # param set-default CA_AIRFRAME 7 # param set-default CA_ROTOR_COUNT 8 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index 51993870d2bd..e6759d101299 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -14,12 +14,12 @@ uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 # # Start Control Allocator # -control_allocator start +# control_allocator start # # Start Multicopter Rate Controller. # -sc_rate_control start +# sc_rate_control start # Start Space Robot Thruster Controller sc_thruster_controller start diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 65da7140e62b..7d5b1d5b56a6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -3,7 +3,7 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -set VEHICLE_TYPE sr +set VEHICLE_TYPE sc # MAV_TYPE_QUADROTOR 2 #param set-default MAV_TYPE 12 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index a43b00d5f4a9..bafb37aeb5c0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -62,10 +62,10 @@ fi # # SR setup # -if [ $VEHICLE_TYPE = sr ] +if [ $VEHICLE_TYPE = sc ] then # Start standard sr apps. - . ${R}etc/init.d/rc.sr_apps + . ${R}etc/init.d/rc.sc_apps fi # diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 3fe3dee0d306..00197c6691fb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -59,8 +59,8 @@ CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y +CONFIG_MODULES_SENSORS=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp index 1ffadcf610b0..9660b9a47986 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp @@ -76,41 +76,32 @@ int ScThrusterController::task_spawn(int argc, char* argv[]) { } ScThrusterController* ScThrusterController::instantiate(int argc, char* argv[]) { - /* -int example_param = 0; - bool example_flag = false; - bool error_flag = false; - int myoptind = 1; int ch; + bool error_flag = false; const char *myoptarg = nullptr; - // parse CLI arguments - while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'p': - example_param = (int)strtol(myoptarg, nullptr, 10); - break; - - case 'f': - example_flag = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } + // parse CLI arguments + _debug_thruster_print = false; + while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _debug_thruster_print = true; + PX4_INFO("Debugging enabled"); + break; + case '?': + error_flag = true; + break; + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } } if (error_flag) { return nullptr; } -*/ ScThrusterController* instance = new ScThrusterController(); @@ -140,8 +131,13 @@ void ScThrusterController::run() { memset(&my_msg, 0, sizeof(my_msg)); orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); + // Thruster command structure + struct thruster_command_s thruster_cmd; + // initialize parameters parameters_update(true); + bool debugPrint = ScThrusterController::_debug_thruster_print; + PX4_INFO("Debugging enabled: %d", debugPrint); while (!should_exit()) { // wait for up to 1000ms for data @@ -149,7 +145,7 @@ void ScThrusterController::run() { if (pret == 0) { // Timeout: let the loop run anyway, don't do `continue` here - + if (debugPrint) PX4_WARN("Timeout..."); } else if (pret < 0) { // this is undesirable but not much we can do PX4_ERR("poll error %d, %d", pret, errno); @@ -157,11 +153,10 @@ void ScThrusterController::run() { continue; } else if (fds[0].revents & POLLIN) { - /* obtained data for the first file descriptor */ - struct thruster_command_s thruster_cmd; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); - PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, + if (debugPrint) + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, (double)thruster_cmd.y1, (double)thruster_cmd.y2); actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; @@ -175,10 +170,11 @@ void ScThrusterController::run() { orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - strcpy(my_msg.text, "Thruster command received!"); - orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + if (debugPrint) { + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + } } - parameters_update(); } diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp index 8facdcb34cfb..7fc036f82813 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp @@ -96,6 +96,11 @@ class ScThrusterController : public ModuleBase, public Mod (ParamInt)_param_sys_autoconfig /**< another parameter */ ) + // Flags + static bool _debug_thruster_print; + // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; }; + +bool ScThrusterController::_debug_thruster_print = true; From 405a46855be86f5d5979d78501e649dab3cbc42f Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 4 Apr 2024 16:20:12 +0200 Subject: [PATCH 25/77] feat: adding spacecraft airframes --- .../9000_gazebo-classic_2d_spacecraft | 129 ++++++++++++++++++ ...raft => 9001_gazebo-classic_3d_spacecraft} | 25 ++-- 2 files changed, 138 insertions(+), 16 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft rename ROMFS/px4fmu_common/init.d-posix/airframes/{9000_gazebo-classic_spacecraft => 9001_gazebo-classic_3d_spacecraft} (84%) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft new file mode 100644 index 000000000000..d8e386413617 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -0,0 +1,129 @@ +#!/bin/sh +# +# @name 3DoF Spacecraft Model +# +# @type 2D Freeflyer with 8 thrusters - Planar motion +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_apps + +param set-default CA_AIRFRAME 13 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 +param set-default CA_ROTOR0_KM 0.1697 +param set-default CA_ROTOR0_AX 1.0 +param set-default CA_ROTOR0_AY 0.0 +param set-default CA_ROTOR0_AZ 0.0 + +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 +param set-default CA_ROTOR1_KM 0.1697 +param set-default CA_ROTOR1_AX -1.0 +param set-default CA_ROTOR1_AY 0.0 +param set-default CA_ROTOR1_AZ 0.0 + +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 +param set-default CA_ROTOR2_KM 0.1697 +param set-default CA_ROTOR2_AX 1.0 +param set-default CA_ROTOR2_AY 0.0 +param set-default CA_ROTOR2_AZ 0.0 + +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 +param set-default CA_ROTOR3_KM 0.1697 +param set-default CA_ROTOR3_AX -1.0 +param set-default CA_ROTOR3_AY 0.0 +param set-default CA_ROTOR3_AZ 0.0 + +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY -0.12 +param set-default CA_ROTOR4_KM 0.1697 +param set-default CA_ROTOR4_AX 0.0 +param set-default CA_ROTOR4_AY 1.0 +param set-default CA_ROTOR4_AZ 0.0 + +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY 0.12 +param set-default CA_ROTOR5_KM 0.1697 +param set-default CA_ROTOR5_AX 0.0 +param set-default CA_ROTOR5_AY -1.0 +param set-default CA_ROTOR5_AZ 0.0 + +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY -0.12 +param set-default CA_ROTOR6_KM 0.1697 +param set-default CA_ROTOR6_AX 0.0 +param set-default CA_ROTOR6_AY 1.0 +param set-default CA_ROTOR6_AZ 0.0 + +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY 0.12 +param set-default CA_ROTOR7_KM 0.1697 +param set-default CA_ROTOR7_AX 0.0 +param set-default CA_ROTOR7_AY -1.0 +param set-default CA_ROTOR7_AZ 0.0 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 105 +param set-default PWM_MAIN_FUNC6 106 +param set-default PWM_MAIN_FUNC7 107 +param set-default PWM_MAIN_FUNC8 108 + +# From Vehicle Board +param set-default PWM_AUX_TIM0 10 +param set-default PWM_AUX_TIM1 10 +param set-default PWM_AUX_TIM2 10 + +param set-default PWM_AUX_FUNC1 101 +param set-default PWM_AUX_FUNC2 102 +param set-default PWM_AUX_FUNC3 103 +param set-default PWM_AUX_FUNC4 104 +param set-default PWM_AUX_FUNC5 105 +param set-default PWM_AUX_FUNC6 106 +param set-default PWM_AUX_FUNC7 107 +param set-default PWM_AUX_FUNC8 108 + +param set-default PWM_AUX_DIS1 0 +param set-default PWM_AUX_DIS2 0 +param set-default PWM_AUX_DIS3 0 +param set-default PWM_AUX_DIS4 0 +param set-default PWM_AUX_DIS5 0 +param set-default PWM_AUX_DIS6 0 +param set-default PWM_AUX_DIS7 0 +param set-default PWM_AUX_DIS8 0 + +param set-default PWM_AUX_MIN1 0 +param set-default PWM_AUX_MIN2 0 +param set-default PWM_AUX_MIN3 0 +param set-default PWM_AUX_MIN4 0 +param set-default PWM_AUX_MIN5 0 +param set-default PWM_AUX_MIN6 0 +param set-default PWM_AUX_MIN7 0 +param set-default PWM_AUX_MIN8 0 + +# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec +param set-default PWM_AUX_MAX1 10000 +param set-default PWM_AUX_MAX2 10000 +param set-default PWM_AUX_MAX3 10000 +param set-default PWM_AUX_MAX4 10000 +param set-default PWM_AUX_MAX5 10000 +param set-default PWM_AUX_MAX6 10000 +param set-default PWM_AUX_MAX7 10000 +param set-default PWM_AUX_MAX8 10000 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft similarity index 84% rename from ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft rename to ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 0df4395690f4..5d108101d434 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -9,22 +9,27 @@ . ${R}etc/init.d/rc.sc_apps -param set-default CA_AIRFRAME 0 +param set-default CA_AIRFRAME 13 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + param set-default CA_ROTOR0_PX 0.14435 param set-default CA_ROTOR0_PY -0.14435 -param set-default CA_ROTOR0_PZ -0.14435 -param set-default CA_ROTOR0_KM 0.05 # CCW +param set-default CA_ROTOR0_KM 0.01697 param set-default CA_ROTOR0_AX -0.788675 param set-default CA_ROTOR0_AY -0.211325 param set-default CA_ROTOR0_AZ -0.57735 param set-default CA_ROTOR1_PX -0.14435 param set-default CA_ROTOR1_PY -0.14435 -param set-default CA_ROTOR1_PZ -0.14435 param set-default CA_ROTOR1_KM 0.05 param set-default CA_ROTOR1_AX 0.211325 param set-default CA_ROTOR1_AY -0.788675 @@ -32,7 +37,6 @@ param set-default CA_ROTOR1_AZ 0.57735 param set-default CA_ROTOR2_PX 0.14435 param set-default CA_ROTOR2_PY 0.14435 -param set-default CA_ROTOR2_PZ -0.14435 param set-default CA_ROTOR2_KM 0.05 param set-default CA_ROTOR2_AX -0.211325 param set-default CA_ROTOR2_AY 0.788675 @@ -40,7 +44,6 @@ param set-default CA_ROTOR2_AZ 0.57735 param set-default CA_ROTOR3_PX -0.14435 param set-default CA_ROTOR3_PY 0.14435 -param set-default CA_ROTOR3_PZ -0.14435 param set-default CA_ROTOR3_KM 0.05 param set-default CA_ROTOR3_AX 0.788675 param set-default CA_ROTOR3_AY 0.211325 @@ -48,7 +51,6 @@ param set-default CA_ROTOR3_AZ -0.57735 param set-default CA_ROTOR4_PX 0.14435 param set-default CA_ROTOR4_PY -0.14435 -param set-default CA_ROTOR4_PZ 0.14435 param set-default CA_ROTOR4_KM 0.05 param set-default CA_ROTOR4_AX 0.788675 param set-default CA_ROTOR4_AY 0.211325 @@ -56,7 +58,6 @@ param set-default CA_ROTOR4_AZ -0.57735 param set-default CA_ROTOR5_PX -0.14435 param set-default CA_ROTOR5_PY -0.14435 -param set-default CA_ROTOR5_PZ 0.14435 param set-default CA_ROTOR5_KM 0.05 param set-default CA_ROTOR5_AX -0.211325 param set-default CA_ROTOR5_AY 0.788675 @@ -64,7 +65,6 @@ param set-default CA_ROTOR5_AZ 0.57735 param set-default CA_ROTOR6_PX 0.14435 param set-default CA_ROTOR6_PY 0.14435 -param set-default CA_ROTOR6_PZ 0.14435 param set-default CA_ROTOR6_KM 0.05 param set-default CA_ROTOR6_AX 0.211325 param set-default CA_ROTOR6_AY -0.788675 @@ -72,7 +72,6 @@ param set-default CA_ROTOR6_AZ 0.57735 param set-default CA_ROTOR7_PX -0.14435 param set-default CA_ROTOR7_PY 0.14435 -param set-default CA_ROTOR7_PZ 0.14435 param set-default CA_ROTOR7_KM 0.05 param set-default CA_ROTOR7_AX -0.788675 param set-default CA_ROTOR7_AY -0.211325 @@ -87,10 +86,4 @@ param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 107 param set-default PWM_MAIN_FUNC8 108 -# disable MC desaturation which improves attitude tracking -param set-default CA_METHOD 0 - -# disable attitude failure detection -param set-default FD_FAIL_P 0 -param set-default FD_FAIL_R 0 From d0d211b26648b91d105b2bf593e147ed9c1c4638 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 18 Apr 2024 14:41:00 +0200 Subject: [PATCH 26/77] featt: moved everything to sc --- CMakeLists.txt | 15 ++- .../9000_gazebo-classic_2d_spacecraft | 2 +- .../9001_gazebo-classic_3d_spacecraft | 2 +- .../init.d-posix/airframes/CMakeLists.txt | 3 +- ROMFS/px4fmu_common/init.d-posix/rcS | 1 + .../init.d/airframes/70000_kth_space_robot | 92 ++++++++++++++----- ROMFS/px4fmu_common/init.d/rc.sc_apps | 7 +- .../sitl_targets_gazebo-classic.cmake | 3 +- 8 files changed, 90 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b75dd570db7b..7aa5b545ee9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -488,6 +488,15 @@ if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/finalize.cmake") include(finalize) endif() +# Clear target folder +set(INSTALL_DIR "${CMAKE_INSTALL_PREFIX}") +file(GLOB RESULT_BUILD ${INSTALL_DIR}) +list(LENGTH RESULT_BUILD BUILD_RES_LEN) +if(BUILD_RES_LEN GREATER 0) + message("Deleting target folder: ${INSTALL_DIR}") + file(REMOVE_RECURSE ${INSTALL_DIR}) +endif() + # Install SITL build files file(GLOB RESULT build/*) list(LENGTH RESULT RES_LEN) @@ -496,9 +505,9 @@ if(RES_LEN GREATER 0) DESTINATION share/${PROJECT_NAME} PATTERN ".svn" EXCLUDE) # Install ROMFS -# install(DIRECTORY ROMFS/ -# DESTINATION share/${PROJECT_NAME} -# PATTERN ".svn" EXCLUDE) + install(DIRECTORY ROMFS/ + DESTINATION share/${PROJECT_NAME} + PATTERN ".svn" EXCLUDE) else() message(STATUS "No PX4 build files to install.") endif() \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index d8e386413617..45135c7f3dcc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -7,7 +7,7 @@ # @maintainer Pedro Roque # -. ${R}etc/init.d/rc.sc_apps +. ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 13 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 5d108101d434..f4cc53927a94 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -7,7 +7,7 @@ # @maintainer Pedro Roque # -. ${R}etc/init.d/rc.sc_apps +. ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 13 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 0cf65f9cfa87..5893769b229a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -81,7 +81,8 @@ px4_add_romfs_files( 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post - 9000_gazebo-classic_spacecraft + 9000_gazebo-classic_2d_spacecraft + 9001_gazebo-classic_3d_spacecraft 10015_gazebo-classic_iris 10016_none_iris diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 6110215efc0a..faa4a2e62c93 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -224,6 +224,7 @@ done if [ -e "$autostart_file" ] then + echo "INFO [init] using autostart file: $autostart_file" . "$autostart_file" elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ] diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index 12708843098a..e6b157530260 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -10,32 +10,74 @@ . ${R}etc/init.d/rc.sc_defaults -# param set-default CA_AIRFRAME 7 -# param set-default CA_ROTOR_COUNT 8 +param set-default CA_AIRFRAME 13 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 +param set-default CA_ROTOR0_KM 0.1697 +param set-default CA_ROTOR0_AX 1.0 +param set-default CA_ROTOR0_AY 0.0 +param set-default CA_ROTOR0_AZ 0.0 + +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 +param set-default CA_ROTOR1_KM 0.1697 +param set-default CA_ROTOR1_AX -1.0 +param set-default CA_ROTOR1_AY 0.0 +param set-default CA_ROTOR1_AZ 0.0 + +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 +param set-default CA_ROTOR2_KM 0.1697 +param set-default CA_ROTOR2_AX 1.0 +param set-default CA_ROTOR2_AY 0.0 +param set-default CA_ROTOR2_AZ 0.0 + +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 +param set-default CA_ROTOR3_KM 0.1697 +param set-default CA_ROTOR3_AX -1.0 +param set-default CA_ROTOR3_AY 0.0 +param set-default CA_ROTOR3_AZ 0.0 + +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY -0.12 +param set-default CA_ROTOR4_KM 0.1697 +param set-default CA_ROTOR4_AX 0.0 +param set-default CA_ROTOR4_AY 1.0 +param set-default CA_ROTOR4_AZ 0.0 + +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY 0.12 +param set-default CA_ROTOR5_KM 0.1697 +param set-default CA_ROTOR5_AX 0.0 +param set-default CA_ROTOR5_AY -1.0 +param set-default CA_ROTOR5_AZ 0.0 + +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY -0.12 +param set-default CA_ROTOR6_KM 0.1697 +param set-default CA_ROTOR6_AX 0.0 +param set-default CA_ROTOR6_AY 1.0 +param set-default CA_ROTOR6_AZ 0.0 + +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY 0.12 +param set-default CA_ROTOR7_KM 0.1697 +param set-default CA_ROTOR7_AX 0.0 +param set-default CA_ROTOR7_AY -1.0 +param set-default CA_ROTOR7_AZ 0.0 -# param set-default CA_ROTOR0_PX -0.12 -# param set-default CA_ROTOR0_PY -0.12 - -# param set-default CA_ROTOR1_PX 0.12 -# param set-default CA_ROTOR1_PY -0.12 - -# param set-default CA_ROTOR2_PX -0.12 -# param set-default CA_ROTOR2_PY 0.12 - -# param set-default CA_ROTOR3_PX 0.12 -# param set-default CA_ROTOR3_PY 0.12 - -# param set-default CA_ROTOR4_PX -0.12 -# param set-default CA_ROTOR4_PY 0.12 - -# param set-default CA_ROTOR5_PX -0.12 -# param set-default CA_ROTOR5_PY -0.12 - -# param set-default CA_ROTOR6_PX 0.12 -# param set-default CA_ROTOR6_PY 0.12 - -# param set-default CA_ROTOR7_PX 0.12 -# param set-default CA_ROTOR7_PY -0.12 param set-default PWM_AUX_TIM0 10 param set-default PWM_AUX_TIM1 10 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index e6759d101299..bf895cf5e120 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -9,17 +9,18 @@ ekf2 start & # Start MicroDDS Client -uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# uxrce_dds_client start -t udp -p 8888 # # Start Control Allocator # -# control_allocator start +control_allocator start # # Start Multicopter Rate Controller. # -# sc_rate_control start +sc_rate_control start # Start Space Robot Thruster Controller sc_thruster_controller start diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 48ef825843c8..c5ebc862ebde 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -86,7 +86,8 @@ if(gazebo_FOUND) iris_rplidar iris_vision omnicopter - spacecraft + 2d_spacecraft + 3d_spacecraft plane plane_cam plane_catapult From ecf80c54f1f9a1bbf6d73ac62439344b88a1c931 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 11:07:49 +0200 Subject: [PATCH 27/77] feat: adding thruster effectiveness --- .../ActuatorEffectivenessThrusters.cpp | 318 ++++++++++++++++++ .../ActuatorEffectivenessThrusters.hpp | 153 +++++++++ .../ActuatorEffectivenessThrustersTest.cpp | 135 ++++++++ 3 files changed, 606 insertions(+) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp new file mode 100644 index 000000000000..29a2dea9be0f --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -0,0 +1,318 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessRotors.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessRotors.hpp" + +#include "ActuatorEffectivenessTilts.hpp" + +using namespace matrix; + +ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config, + bool tilt_support) + : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) +{ + for (int i = 0; i < NUM_ROTORS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i); + _param_handles[i].position_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i); + _param_handles[i].position_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i); + _param_handles[i].position_z = param_find(buffer); + + if (_axis_config == AxisConfiguration::Configurable) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); + } + + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i); + _param_handles[i].moment_ratio = param_find(buffer); + + if (_tilt_support) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i); + _param_handles[i].tilt_index = param_find(buffer); + } + } + + _count_handle = param_find("CA_ROTOR_COUNT"); + + updateParams(); +} + +void ActuatorEffectivenessRotors::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_count_handle, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_rotors = math::min(NUM_ROTORS_MAX, (int)count); + + for (int i = 0; i < _geometry.num_rotors; ++i) { + Vector3f &position = _geometry.rotors[i].position; + param_get(_param_handles[i].position_x, &position(0)); + param_get(_param_handles[i].position_y, &position(1)); + param_get(_param_handles[i].position_z, &position(2)); + + Vector3f &axis = _geometry.rotors[i].axis; + + switch (_axis_config) { + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + + case AxisConfiguration::FixedForward: + axis = Vector3f(1.f, 0.f, 0.f); + break; + + case AxisConfiguration::FixedUpwards: + axis = Vector3f(0.f, 0.f, -1.f); + break; + } + + param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef); + param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio); + + if (_tilt_support) { + int32_t tilt_param{0}; + param_get(_param_handles[i].tilt_index, &tilt_param); + _geometry.rotors[i].tilt_index = tilt_param - 1; + + } else { + _geometry.rotors[i].tilt_index = -1; + } + } +} + +bool +ActuatorEffectivenessRotors::addActuators(Configuration &configuration) +{ + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; + } + + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[configuration.selected_matrix], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + return true; +} + +int +ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < geometry.num_rotors; i++) { + + if (i + actuator_start_index >= NUM_ACTUATORS) { + break; + } + + ++num_actuators; + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (geometry.propeller_torque_disabled) { + km = 0.f; + } + + if (geometry.propeller_torque_disabled_non_upwards) { + bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f; + + if (!upwards) { + km = 0.f; + } + } + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + effectiveness(j, i + actuator_start_index) = moment(j); + effectiveness(j + 3, i + actuator_start_index) = thrust(j); + } + + if (geometry.yaw_by_differential_thrust_disabled) { + // set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts) + effectiveness(2, i + actuator_start_index) = 0.f; + } + + if (geometry.three_dimensional_thrust_disabled) { + // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), + // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they + // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated + // by the allocator directly. + + effectiveness(0 + 3, i + actuator_start_index) = 0.f; + effectiveness(1 + 3, i + actuator_start_index) = 0.f; + effectiveness(2 + 3, i + actuator_start_index) = -ct; + } + } + + return num_actuators; +} + +uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, + float collective_tilt_control) +{ + if (!PX4_ISFINITE(collective_tilt_control)) { + collective_tilt_control = -1.f; + } + + uint32_t nontilted_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + int tilt_index = _geometry.rotors[i].tilt_index; + + if (tilt_index == -1 || tilt_index >= tilts.count()) { + nontilted_motors |= 1u << i; + continue; + } + + const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); + const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); + const float tilt_direction = math::radians((float)tilt.tilt_direction); + _geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction); + } + + return nontilted_motors; +} + +Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction) +{ + Vector3f axis{0.f, 0.f, -1.f}; + return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; +} + +uint32_t ActuatorEffectivenessRotors::getMotors() const +{ + uint32_t motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + motors |= 1u << i; + } + + return motors; +} + +uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const +{ + uint32_t upwards_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) { + upwards_motors |= 1u << i; + } + } + + return upwards_motors; +} + +uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const +{ + uint32_t forward_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { + forward_motors |= 1u << i; + } + } + + return forward_motors; +} + +bool +ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + return addActuators(configuration); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp new file mode 100644 index 000000000000..14f96b7dab43 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessRotors.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +class ActuatorEffectivenessTilts; + +using namespace time_literals; + +class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness +{ +public: + enum class AxisConfiguration { + Configurable, ///< axis can be configured + FixedForward, ///< axis is fixed, pointing forwards (positive X) + FixedUpwards, ///< axis is fixed, pointing upwards (negative Z) + }; + + static constexpr int NUM_ROTORS_MAX = 12; + + struct RotorGeometry { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + float moment_ratio; + int tilt_index; + }; + + struct Geometry { + RotorGeometry rotors[NUM_ROTORS_MAX]; + int num_rotors{0}; + bool propeller_torque_disabled{false}; + bool yaw_by_differential_thrust_disabled{false}; + bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors + bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt + }; + + ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + bool tilt_support = false); + virtual ~ActuatorEffectivenessRotors() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Rotors"; } + + /** + * Sets the motor axis from tilt configurations and current tilt control. + * @param tilts configured tilt servos + * @param tilt_control current tilt control in [-1, 1] (can be NAN) + * @return the motors as bitset which are not tiltable + */ + uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control); + + const Geometry &geometry() const { return _geometry; } + + /** + * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then + * rotated by tilt_direction around z. + */ + static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction); + + void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; } + + void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; } + + void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; } + + void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } + + uint32_t getMotors() const; + uint32_t getUpwardsMotors() const; + uint32_t getForwardsMotors() const; + +private: + void updateParams() override; + const AxisConfiguration _axis_config; + const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + + struct ParamHandles { + param_t position_x; + param_t position_y; + param_t position_z; + param_t axis_x; + param_t axis_y; + param_t axis_z; + param_t thrust_coef; + param_t moment_ratio; + param_t tilt_index; + }; + ParamHandles _param_handles[NUM_ROTORS_MAX]; + param_t _count_handle; + + Geometry _geometry{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp new file mode 100644 index 000000000000..603248ab9d9b --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessRotors.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include "ActuatorEffectivenessRotors.hpp" + +using namespace matrix; + +TEST(ActuatorEffectivenessRotors, AllZeroCase) +{ + // Quad wide geometry + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} + +TEST(ActuatorEffectivenessRotors, Tilt) +{ + Vector3f axis_expected{0.f, 0.f, -1.f}; + Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{-1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 0.f, -1.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, -1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); +} From 05fa6ff3a9f8bbf08418a326abc3356699dfc6fb Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 11:27:48 +0200 Subject: [PATCH 28/77] feat: updates to module yaml and naming of classes --- .../ActuatorEffectivenessThrusters.cpp | 104 ++++++------ .../ActuatorEffectivenessThrusters.hpp | 8 +- src/modules/control_allocator/module.yaml | 150 ++++++++++++++++++ 3 files changed, 199 insertions(+), 63 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp index 29a2dea9be0f..6c7346fbc2dc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -32,59 +32,54 @@ ****************************************************************************/ /** - * @file ActuatorEffectivenessRotors.hpp + * @file ActuatorEffectivenessThrusters.hpp * - * Actuator effectiveness computed from rotors position and orientation + * Actuator effectiveness computed from thrusters position and orientation * * @author Julien Lecoeur */ -#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessThrusters.hpp" #include "ActuatorEffectivenessTilts.hpp" using namespace matrix; -ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config, +ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config, bool tilt_support) : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) { - for (int i = 0; i < NUM_ROTORS_MAX; ++i) { + for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) { char buffer[17]; - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i); _param_handles[i].position_x = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PY", i); _param_handles[i].position_y = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PZ", i); _param_handles[i].position_z = param_find(buffer); if (_axis_config == AxisConfiguration::Configurable) { - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i); _param_handles[i].axis_x = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i); _param_handles[i].axis_y = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i); _param_handles[i].axis_z = param_find(buffer); } - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i); - _param_handles[i].thrust_coef = param_find(buffer); - - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i); - _param_handles[i].moment_ratio = param_find(buffer); - if (_tilt_support) { - snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i); - _param_handles[i].tilt_index = param_find(buffer); + PX4_ERR("Tilt support not implemented"); + // snprintf(buffer, sizeof(buffer), "SC_CA_THRUSTER%u_TILT", i); + // _param_handles[i].tilt_index = param_find(buffer); } } - _count_handle = param_find("CA_ROTOR_COUNT"); + _count_handle = param_find("CA_THRUSTER_COUNT"); updateParams(); } -void ActuatorEffectivenessRotors::updateParams() +void ActuatorEffectivenessThrusters::updateParams() { ModuleParams::updateParams(); @@ -95,15 +90,15 @@ void ActuatorEffectivenessRotors::updateParams() return; } - _geometry.num_rotors = math::min(NUM_ROTORS_MAX, (int)count); + _geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count); - for (int i = 0; i < _geometry.num_rotors; ++i) { - Vector3f &position = _geometry.rotors[i].position; + for (int i = 0; i < _geometry.num_thrusters; ++i) { + Vector3f &position = _geometry.thrusters[i].position; param_get(_param_handles[i].position_x, &position(0)); param_get(_param_handles[i].position_y, &position(1)); param_get(_param_handles[i].position_z, &position(2)); - Vector3f &axis = _geometry.rotors[i].axis; + Vector3f &axis = _geometry.thrusters[i].axis; switch (_axis_config) { case AxisConfiguration::Configurable: @@ -112,31 +107,22 @@ void ActuatorEffectivenessRotors::updateParams() param_get(_param_handles[i].axis_z, &axis(2)); break; - case AxisConfiguration::FixedForward: - axis = Vector3f(1.f, 0.f, 0.f); - break; - - case AxisConfiguration::FixedUpwards: - axis = Vector3f(0.f, 0.f, -1.f); - break; - } - - param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef); - param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio); + param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); + param_get(_param_handles[i].moment_ratio, &_geometry.thrusters[i].moment_ratio); if (_tilt_support) { int32_t tilt_param{0}; param_get(_param_handles[i].tilt_index, &tilt_param); - _geometry.rotors[i].tilt_index = tilt_param - 1; + _geometry.thrusters[i].tilt_index = tilt_param - 1; } else { - _geometry.rotors[i].tilt_index = -1; + _geometry.thrusters[i].tilt_index = -1; } } } bool -ActuatorEffectivenessRotors::addActuators(Configuration &configuration) +ActuatorEffectivenessThrusters::addActuators(Configuration &configuration) { if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { PX4_ERR("Wrong actuator ordering: servos need to be after motors"); @@ -151,12 +137,12 @@ ActuatorEffectivenessRotors::addActuators(Configuration &configuration) } int -ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry, +ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geometry, EffectivenessMatrix &effectiveness, int actuator_start_index) { int num_actuators = 0; - for (int i = 0; i < geometry.num_rotors; i++) { + for (int i = 0; i < geometry.num_thrusters; i++) { if (i + actuator_start_index >= NUM_ACTUATORS) { break; @@ -165,7 +151,7 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry ++num_actuators; // Get rotor axis - Vector3f axis = geometry.rotors[i].axis; + Vector3f axis = geometry.thrusters[i].axis; // Normalize axis float axis_norm = axis.norm(); @@ -179,11 +165,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry } // Get rotor position - const Vector3f &position = geometry.rotors[i].position; + const Vector3f &position = geometry.thrusters[i].position; // Get coefficients - float ct = geometry.rotors[i].thrust_coef; - float km = geometry.rotors[i].moment_ratio; + float ct = geometry.thrusters[i].thrust_coef; + float km = geometry.thrusters[i].moment_ratio; if (geometry.propeller_torque_disabled) { km = 0.f; @@ -233,7 +219,7 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry return num_actuators; } -uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, +uint32_t ActuatorEffectivenessThrusters::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float collective_tilt_control) { if (!PX4_ISFINITE(collective_tilt_control)) { @@ -242,8 +228,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv uint32_t nontilted_motors = 0; - for (int i = 0; i < _geometry.num_rotors; ++i) { - int tilt_index = _geometry.rotors[i].tilt_index; + for (int i = 0; i < _geometry.num_thrusters; ++i) { + int tilt_index = _geometry.thrusters[i].tilt_index; if (tilt_index == -1 || tilt_index >= tilts.count()) { nontilted_motors |= 1u << i; @@ -253,35 +239,35 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); const float tilt_direction = math::radians((float)tilt.tilt_direction); - _geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction); + _geometry.thrusters[i].axis = tiltedAxis(tilt_angle, tilt_direction); } return nontilted_motors; } -Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction) +Vector3f ActuatorEffectivenessThrusters::tiltedAxis(float tilt_angle, float tilt_direction) { Vector3f axis{0.f, 0.f, -1.f}; return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; } -uint32_t ActuatorEffectivenessRotors::getMotors() const +uint32_t ActuatorEffectivenessThrusters::getMotors() const { uint32_t motors = 0; - for (int i = 0; i < _geometry.num_rotors; ++i) { + for (int i = 0; i < _geometry.num_thrusters; ++i) { motors |= 1u << i; } return motors; } -uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const +uint32_t ActuatorEffectivenessThrusters::getUpwardsMotors() const { uint32_t upwards_motors = 0; - for (int i = 0; i < _geometry.num_rotors; ++i) { - const Vector3f &axis = _geometry.rotors[i].axis; + for (int i = 0; i < _geometry.num_thrusters; ++i) { + const Vector3f &axis = _geometry.thrusters[i].axis; if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) { upwards_motors |= 1u << i; @@ -291,12 +277,12 @@ uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const return upwards_motors; } -uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const +uint32_t ActuatorEffectivenessThrusters::getForwardsMotors() const { uint32_t forward_motors = 0; - for (int i = 0; i < _geometry.num_rotors; ++i) { - const Vector3f &axis = _geometry.rotors[i].axis; + for (int i = 0; i < _geometry.num_thrusters; ++i) { + const Vector3f &axis = _geometry.thrusters[i].axis; if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { forward_motors |= 1u << i; @@ -307,7 +293,7 @@ uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const } bool -ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, +ActuatorEffectivenessThrusters::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp index 14f96b7dab43..a035efb385a9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file ActuatorEffectivenessRotors.hpp + * @file ActuatorEffectivenessThrusters.hpp * * Actuator effectiveness computed from rotors position and orientation * @@ -51,7 +51,7 @@ class ActuatorEffectivenessTilts; using namespace time_literals; -class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness +class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffectiveness { public: enum class AxisConfiguration { @@ -79,9 +79,9 @@ class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiv bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt }; - ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, bool tilt_support = false); - virtual ~ActuatorEffectivenessRotors() = default; + virtual ~ActuatorEffectivenessThrusters() = default; bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index e5467515cb08..5f2831c80c72 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -1,4 +1,5 @@ __max_num_mc_motors: &max_num_mc_motors 12 +__max_num_mc_motors: &max_num_sc_thrusters 12 __max_num_servos: &max_num_servos 8 __max_num_tilts: &max_num_tilts 4 @@ -29,6 +30,8 @@ parameters: 9: Custom 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) + 12: Spacecraft 2D + 13: Spacecraft 3D default: 0 CA_METHOD: @@ -235,6 +238,109 @@ parameters: instance_start: 0 default: 0 + # (SC) Thrusters + CA_THRUSTER_COUNT: + description: + short: Total number of thrusters + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + 9: '9' + 10: '10' + 11: '11' + 12: '12' + default: 0 + CA_THRUSTER${i}_PX: + description: + short: Position of thruster ${i} along X body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PY: + description: + short: Position of thruster ${i} along Y body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER{i}_PZ: + description: + short: Position of thruster ${i} along Z body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + + CA_THRUSTER${i}_AX: + description: + short: Axis of thruster ${i} thrust vector, X body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AY: + description: + short: Axis of thruster ${i} thrust vector, Y body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AZ: + description: + short: Axis of thruster ${i} thrust vector, Z body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: -1.0 + + CA_THRUSTER${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between actuator minimum and maximum) + is the output signal sent to the motor controller. + type: float + decimal: 1 + increment: 1 + num_instances: *max_num_sc_thrusters + min: 0 + max: 100 + default: 6.5 + # control surfaces CA_SV_CS_COUNT: description: @@ -1116,3 +1222,47 @@ mixer: name: CA_HELI_YAW_CCW - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + 12: # Spacecraft 2D + type: 'spacecraft_2d' + title: 'Spacecraft 2D' + actuators: + - actuator_type: 'thruster' + count: 'CA_THRUSTER_COUNT' + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true + + 13: # Sapcecraft 3D + title: 'Spacecraft 3D' + actuators: + - actuator_type: 'thruster' + count: 'CA_THRUSTER_COUNT' + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true \ No newline at end of file From ba910a1dbaf77003f2bdb2945966cc6f1970edcf Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 11:43:18 +0200 Subject: [PATCH 29/77] feat: added actator type and updated thruster effectiveness --- .../ActuatorEffectiveness.hpp | 1 + .../ActuatorEffectivenessThrusters.cpp | 117 ++---------------- .../ActuatorEffectivenessThrusters.hpp | 55 ++------ 3 files changed, 20 insertions(+), 153 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 0ddd4988f335..5f797f55e0aa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -55,6 +55,7 @@ enum class AllocationMethod { enum class ActuatorType { MOTORS = 0, + THRUSTERS, SERVOS, COUNT diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp index 6c7346fbc2dc..0d93cf6d9246 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -36,13 +36,11 @@ * * Actuator effectiveness computed from thrusters position and orientation * - * @author Julien Lecoeur + * @author Pedro Roque, */ #include "ActuatorEffectivenessThrusters.hpp" -#include "ActuatorEffectivenessTilts.hpp" - using namespace matrix; ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config, @@ -69,8 +67,6 @@ ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *par if (_tilt_support) { PX4_ERR("Tilt support not implemented"); - // snprintf(buffer, sizeof(buffer), "SC_CA_THRUSTER%u_TILT", i); - // _param_handles[i].tilt_index = param_find(buffer); } } @@ -109,15 +105,6 @@ void ActuatorEffectivenessThrusters::updateParams() param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); param_get(_param_handles[i].moment_ratio, &_geometry.thrusters[i].moment_ratio); - - if (_tilt_support) { - int32_t tilt_param{0}; - param_get(_param_handles[i].tilt_index, &tilt_param); - _geometry.thrusters[i].tilt_index = tilt_param - 1; - - } else { - _geometry.thrusters[i].tilt_index = -1; - } } } @@ -132,7 +119,7 @@ ActuatorEffectivenessThrusters::addActuators(Configuration &configuration) int num_actuators = computeEffectivenessMatrix(_geometry, configuration.effectiveness_matrices[configuration.selected_matrix], configuration.num_actuators_matrix[configuration.selected_matrix]); - configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + configuration.actuatorsAdded(ActuatorType::THRUSTERS, num_actuators); return true; } @@ -169,19 +156,6 @@ ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geome // Get coefficients float ct = geometry.thrusters[i].thrust_coef; - float km = geometry.thrusters[i].moment_ratio; - - if (geometry.propeller_torque_disabled) { - km = 0.f; - } - - if (geometry.propeller_torque_disabled_non_upwards) { - bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f; - - if (!upwards) { - km = 0.f; - } - } if (fabsf(ct) < FLT_EPSILON) { continue; @@ -191,105 +165,28 @@ ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geome matrix::Vector3f thrust = ct * axis; // Compute moment generated by this rotor - matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + matrix::Vector3f moment = ct * position.cross(axis); // Fill corresponding items in effectiveness matrix for (size_t j = 0; j < 3; j++) { effectiveness(j, i + actuator_start_index) = moment(j); effectiveness(j + 3, i + actuator_start_index) = thrust(j); } - - if (geometry.yaw_by_differential_thrust_disabled) { - // set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts) - effectiveness(2, i + actuator_start_index) = 0.f; - } - - if (geometry.three_dimensional_thrust_disabled) { - // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), - // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they - // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated - // by the allocator directly. - - effectiveness(0 + 3, i + actuator_start_index) = 0.f; - effectiveness(1 + 3, i + actuator_start_index) = 0.f; - effectiveness(2 + 3, i + actuator_start_index) = -ct; - } } return num_actuators; } -uint32_t ActuatorEffectivenessThrusters::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, - float collective_tilt_control) -{ - if (!PX4_ISFINITE(collective_tilt_control)) { - collective_tilt_control = -1.f; - } - - uint32_t nontilted_motors = 0; - for (int i = 0; i < _geometry.num_thrusters; ++i) { - int tilt_index = _geometry.thrusters[i].tilt_index; - - if (tilt_index == -1 || tilt_index >= tilts.count()) { - nontilted_motors |= 1u << i; - continue; - } - - const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); - const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); - const float tilt_direction = math::radians((float)tilt.tilt_direction); - _geometry.thrusters[i].axis = tiltedAxis(tilt_angle, tilt_direction); - } - - return nontilted_motors; -} - -Vector3f ActuatorEffectivenessThrusters::tiltedAxis(float tilt_angle, float tilt_direction) -{ - Vector3f axis{0.f, 0.f, -1.f}; - return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; -} - -uint32_t ActuatorEffectivenessThrusters::getMotors() const -{ - uint32_t motors = 0; - - for (int i = 0; i < _geometry.num_thrusters; ++i) { - motors |= 1u << i; - } - - return motors; -} - -uint32_t ActuatorEffectivenessThrusters::getUpwardsMotors() const -{ - uint32_t upwards_motors = 0; - - for (int i = 0; i < _geometry.num_thrusters; ++i) { - const Vector3f &axis = _geometry.thrusters[i].axis; - - if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) { - upwards_motors |= 1u << i; - } - } - - return upwards_motors; -} - -uint32_t ActuatorEffectivenessThrusters::getForwardsMotors() const +uint32_t ActuatorEffectivenessThrusters::getThrusters() const { - uint32_t forward_motors = 0; + uint32_t thrusters = 0; for (int i = 0; i < _geometry.num_thrusters; ++i) { - const Vector3f &axis = _geometry.thrusters[i].axis; - - if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { - forward_motors |= 1u << i; - } + thrusters |= 1u << i; } - return forward_motors; + return thrusters; } bool diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp index a035efb385a9..845e3b2603ea 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp @@ -34,9 +34,9 @@ /** * @file ActuatorEffectivenessThrusters.hpp * - * Actuator effectiveness computed from rotors position and orientation + * Actuator effectiveness computed from thrusters position and orientation * - * @author Julien Lecoeur + * @author Pedro Roque, */ #pragma once @@ -60,23 +60,17 @@ class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffec FixedUpwards, ///< axis is fixed, pointing upwards (negative Z) }; - static constexpr int NUM_ROTORS_MAX = 12; + static constexpr int NUM_THRUSTERS_MAX = 12; - struct RotorGeometry { + struct ThrusterGeometry { matrix::Vector3f position; matrix::Vector3f axis; float thrust_coef; - float moment_ratio; - int tilt_index; }; struct Geometry { - RotorGeometry rotors[NUM_ROTORS_MAX]; - int num_rotors{0}; - bool propeller_torque_disabled{false}; - bool yaw_by_differential_thrust_disabled{false}; - bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors - bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt + ThrusterGeometry thrusters[NUM_THRUSTERS_MAX]; + int num_thrusters{0}; }; ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, @@ -87,11 +81,12 @@ class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffec void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override { - allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; - } + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer + } void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override { + // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer normalize[0] = true; } @@ -100,35 +95,11 @@ class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffec bool addActuators(Configuration &configuration); - const char *name() const override { return "Rotors"; } - - /** - * Sets the motor axis from tilt configurations and current tilt control. - * @param tilts configured tilt servos - * @param tilt_control current tilt control in [-1, 1] (can be NAN) - * @return the motors as bitset which are not tiltable - */ - uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control); + const char *name() const override { return "Thrusters"; } const Geometry &geometry() const { return _geometry; } - /** - * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then - * rotated by tilt_direction around z. - */ - static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction); - - void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; } - - void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; } - - void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; } - - void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } - - uint32_t getMotors() const; - uint32_t getUpwardsMotors() const; - uint32_t getForwardsMotors() const; + uint32_t getThrusters() const; private: void updateParams() override; @@ -143,10 +114,8 @@ class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffec param_t axis_y; param_t axis_z; param_t thrust_coef; - param_t moment_ratio; - param_t tilt_index; }; - ParamHandles _param_handles[NUM_ROTORS_MAX]; + ParamHandles _param_handles[NUM_THRUSTERS_MAX]; param_t _count_handle; Geometry _geometry{}; From 7f15e731e2c44b109fb72685475f5255b5e11246 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 11:45:33 +0200 Subject: [PATCH 30/77] feat: update cmake lists for added modules --- .../control_allocator/ActuatorEffectiveness/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 18c0679d1067..88f61a1bcc5e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -52,6 +52,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTilts.hpp ActuatorEffectivenessRotors.cpp ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessThrusters.cpp + ActuatorEffectivenessThrusters.hpp ActuatorEffectivenessStandardVTOL.cpp ActuatorEffectivenessStandardVTOL.hpp ActuatorEffectivenessTiltrotorVTOL.cpp @@ -73,3 +75,4 @@ target_link_libraries(ActuatorEffectiveness px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness) \ No newline at end of file From 503ea6d6d8958e95bbfcf7139ce208f33256c5e6 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 14:16:49 +0200 Subject: [PATCH 31/77] fix: compiling with thrusters inside --- Tools/module_config/generate_actuators_metadata.py | 2 ++ src/lib/mixer_module/output_functions.yaml | 3 +++ .../ActuatorEffectivenessThrusters.cpp | 14 +++++++------- .../ActuatorEffectivenessThrusters.hpp | 2 -- src/modules/control_allocator/module.yaml | 12 +++++++++--- 5 files changed, 21 insertions(+), 12 deletions(-) diff --git a/Tools/module_config/generate_actuators_metadata.py b/Tools/module_config/generate_actuators_metadata.py index 196119259baa..bf9096e3823b 100755 --- a/Tools/module_config/generate_actuators_metadata.py +++ b/Tools/module_config/generate_actuators_metadata.py @@ -364,6 +364,8 @@ def get_mixers(yaml_config, output_functions, verbose): actuator['group-label'] = 'Motors' elif actuator_conf['actuator_type'] == 'servo': actuator['group-label'] = 'Servos' + elif actuator_conf['actuator_type'] == 'thruster': + actuator['group-label'] = 'Thrusters' else: raise Exception('Missing group label for actuator type "{}"'.format(actuator_conf['actuator_type'])) diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 1c57ed9d2a63..add976db2381 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -10,6 +10,9 @@ functions: Motor: start: 101 count: 12 + Thruster: + start: 501 + count: 12 Servo: start: 201 count: 8 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp index 0d93cf6d9246..228b916d146c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -70,7 +70,7 @@ ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *par } } - _count_handle = param_find("CA_THRUSTER_COUNT"); + _count_handle = param_find("CA_THRUSTER_CNT"); updateParams(); } @@ -97,14 +97,14 @@ void ActuatorEffectivenessThrusters::updateParams() Vector3f &axis = _geometry.thrusters[i].axis; switch (_axis_config) { - case AxisConfiguration::Configurable: - param_get(_param_handles[i].axis_x, &axis(0)); - param_get(_param_handles[i].axis_y, &axis(1)); - param_get(_param_handles[i].axis_z, &axis(2)); - break; + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + } param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); - param_get(_param_handles[i].moment_ratio, &_geometry.thrusters[i].moment_ratio); } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp index 845e3b2603ea..ae2dd811f7c0 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp @@ -56,8 +56,6 @@ class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffec public: enum class AxisConfiguration { Configurable, ///< axis can be configured - FixedForward, ///< axis is fixed, pointing forwards (positive X) - FixedUpwards, ///< axis is fixed, pointing upwards (negative Z) }; static constexpr int NUM_THRUSTERS_MAX = 12; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 5f2831c80c72..8d5da7686b07 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -239,7 +239,7 @@ parameters: default: 0 # (SC) Thrusters - CA_THRUSTER_COUNT: + CA_THRUSTER_CNT: description: short: Total number of thrusters type: enum @@ -664,6 +664,12 @@ mixer: label: 'Slew Rate' index_offset: 0 advanced: true + thruster: + functions: 'Thruster' + actuator_testing_values: + min: 0 + max: 1 + default_is_nan: true servo: functions: 'Servo' actuator_testing_values: @@ -1227,7 +1233,7 @@ mixer: title: 'Spacecraft 2D' actuators: - actuator_type: 'thruster' - count: 'CA_THRUSTER_COUNT' + count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size per_item_parameters: standard: position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] @@ -1249,7 +1255,7 @@ mixer: title: 'Spacecraft 3D' actuators: - actuator_type: 'thruster' - count: 'CA_THRUSTER_COUNT' + count: 'CA_THRUSTER_CNT' per_item_parameters: standard: position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] From 12dd68e2d6e3c632ce4463aee260e9b16c02dbfa Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 14:48:13 +0200 Subject: [PATCH 32/77] feat: added spacecraft effectiveness - to be validated --- .../ActuatorEffectivenessSpacecraft.cpp | 56 +++++++++++++++++ .../ActuatorEffectivenessSpacecraft.hpp | 63 +++++++++++++++++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../control_allocator/ControlAllocator.cpp | 8 +++ .../control_allocator/ControlAllocator.hpp | 2 + 5 files changed, 131 insertions(+) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp new file mode 100644 index 000000000000..da2ccb4d960a --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ActuatorEffectivenessSpacecraft.hpp" + +using namespace matrix; + +ActuatorEffectivenessSpacecraft::ActuatorEffectivenessSpacecraft(ModuleParams *parent) + : ModuleParams(parent), + _sc_thrusters(this) +{ +} + +bool +ActuatorEffectivenessSpacecraft::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + const bool thrusters_added_successfully = _sc_thrusters.addActuators(configuration); + + return thrusters_added_successfully; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp new file mode 100644 index 000000000000..46664a6d273d --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessThrusters.hpp" + +class ActuatorEffectivenessSpacecraft : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessSpacecraft(ModuleParams *parent); + virtual ~ActuatorEffectivenessSpacecraft() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): to be updated + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): to be updated + normalize[0] = true; + } + + const char *name() const override { return "Spacecraft"; } + +protected: + ActuatorEffectivenessThrusters _sc_thrusters; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 88f61a1bcc5e..8af3690761a4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -54,6 +54,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessRotors.hpp ActuatorEffectivenessThrusters.cpp ActuatorEffectivenessThrusters.hpp + ActuatorEffectivenessSpacecraft.cpp + ActuatorEffectivenessSpacecraft.hpp ActuatorEffectivenessStandardVTOL.cpp ActuatorEffectivenessStandardVTOL.hpp ActuatorEffectivenessTiltrotorVTOL.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 14c3193bfd78..562b544308cb 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -266,6 +266,14 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS); break; + case EffectivenessSource::SPACECRAFT_2D: + tmp = new ActuatorEffectivenessSpacecraft(this); + break; + + case EffectivenessSource::SPACECRAFT_3D: + tmp = new ActuatorEffectivenessSpacecraft(this); + break; + default: PX4_ERR("Unknown airframe"); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 18132cbe55b8..f6c7087e783d 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -156,6 +156,8 @@ class ControlAllocator : public ModuleBase, public ModuleParam CUSTOM = 9, HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, + SPACECRAFT_2D = 12, + SPACECRAFT_3D = 13, }; enum class FailureMode { From f41ee2fc409d367352e45be4f601d1db94de5997 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 17:31:34 +0200 Subject: [PATCH 33/77] feat: fixed allocator bug - need to remove prints --- .../9000_gazebo-classic_2d_spacecraft | 166 +++++++----------- .../ActuatorEffectivenessSpacecraft.cpp | 11 +- .../ActuatorEffectivenessSpacecraft.hpp | 4 + .../ActuatorEffectivenessThrusters.cpp | 29 ++- .../control_allocator/ControlAllocator.cpp | 3 + .../control_allocator/ControlAllocator.hpp | 1 + src/modules/control_allocator/module.yaml | 3 +- 7 files changed, 106 insertions(+), 111 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 45135c7f3dcc..a44a14d33da5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -9,73 +9,81 @@ . ${R}etc/init.d/rc.sc_defaults -param set-default CA_AIRFRAME 13 +param set-default CA_AIRFRAME 12 -param set-default CA_ROTOR_COUNT 8 +param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 # Auto to be provided by Custom Airframe -param set-default CA_METHOD 0 +# param set-default CA_METHOD 0 # disable attitude failure detection param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 -param set-default CA_ROTOR0_PX -0.12 -param set-default CA_ROTOR0_PY -0.12 -param set-default CA_ROTOR0_KM 0.1697 -param set-default CA_ROTOR0_AX 1.0 -param set-default CA_ROTOR0_AY 0.0 -param set-default CA_ROTOR0_AZ 0.0 - -param set-default CA_ROTOR1_PX 0.12 -param set-default CA_ROTOR1_PY -0.12 -param set-default CA_ROTOR1_KM 0.1697 -param set-default CA_ROTOR1_AX -1.0 -param set-default CA_ROTOR1_AY 0.0 -param set-default CA_ROTOR1_AZ 0.0 - -param set-default CA_ROTOR2_PX -0.12 -param set-default CA_ROTOR2_PY 0.12 -param set-default CA_ROTOR2_KM 0.1697 -param set-default CA_ROTOR2_AX 1.0 -param set-default CA_ROTOR2_AY 0.0 -param set-default CA_ROTOR2_AZ 0.0 - -param set-default CA_ROTOR3_PX 0.12 -param set-default CA_ROTOR3_PY 0.12 -param set-default CA_ROTOR3_KM 0.1697 -param set-default CA_ROTOR3_AX -1.0 -param set-default CA_ROTOR3_AY 0.0 -param set-default CA_ROTOR3_AZ 0.0 - -param set-default CA_ROTOR4_PX 0.12 -param set-default CA_ROTOR4_PY -0.12 -param set-default CA_ROTOR4_KM 0.1697 -param set-default CA_ROTOR4_AX 0.0 -param set-default CA_ROTOR4_AY 1.0 -param set-default CA_ROTOR4_AZ 0.0 - -param set-default CA_ROTOR5_PX 0.12 -param set-default CA_ROTOR5_PY 0.12 -param set-default CA_ROTOR5_KM 0.1697 -param set-default CA_ROTOR5_AX 0.0 -param set-default CA_ROTOR5_AY -1.0 -param set-default CA_ROTOR5_AZ 0.0 - -param set-default CA_ROTOR6_PX -0.12 -param set-default CA_ROTOR6_PY -0.12 -param set-default CA_ROTOR6_KM 0.1697 -param set-default CA_ROTOR6_AX 0.0 -param set-default CA_ROTOR6_AY 1.0 -param set-default CA_ROTOR6_AZ 0.0 - -param set-default CA_ROTOR7_PX -0.12 -param set-default CA_ROTOR7_PY 0.12 -param set-default CA_ROTOR7_KM 0.1697 -param set-default CA_ROTOR7_AX 0.0 -param set-default CA_ROTOR7_AY -1.0 -param set-default CA_ROTOR7_AZ 0.0 +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.75 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.75 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.75 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.75 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.75 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.75 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.75 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.75 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 @@ -85,45 +93,3 @@ param set-default PWM_MAIN_FUNC5 105 param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 107 param set-default PWM_MAIN_FUNC8 108 - -# From Vehicle Board -param set-default PWM_AUX_TIM0 10 -param set-default PWM_AUX_TIM1 10 -param set-default PWM_AUX_TIM2 10 - -param set-default PWM_AUX_FUNC1 101 -param set-default PWM_AUX_FUNC2 102 -param set-default PWM_AUX_FUNC3 103 -param set-default PWM_AUX_FUNC4 104 -param set-default PWM_AUX_FUNC5 105 -param set-default PWM_AUX_FUNC6 106 -param set-default PWM_AUX_FUNC7 107 -param set-default PWM_AUX_FUNC8 108 - -param set-default PWM_AUX_DIS1 0 -param set-default PWM_AUX_DIS2 0 -param set-default PWM_AUX_DIS3 0 -param set-default PWM_AUX_DIS4 0 -param set-default PWM_AUX_DIS5 0 -param set-default PWM_AUX_DIS6 0 -param set-default PWM_AUX_DIS7 0 -param set-default PWM_AUX_DIS8 0 - -param set-default PWM_AUX_MIN1 0 -param set-default PWM_AUX_MIN2 0 -param set-default PWM_AUX_MIN3 0 -param set-default PWM_AUX_MIN4 0 -param set-default PWM_AUX_MIN5 0 -param set-default PWM_AUX_MIN6 0 -param set-default PWM_AUX_MIN7 0 -param set-default PWM_AUX_MIN8 0 - -# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec -param set-default PWM_AUX_MAX1 10000 -param set-default PWM_AUX_MAX2 10000 -param set-default PWM_AUX_MAX3 10000 -param set-default PWM_AUX_MAX4 10000 -param set-default PWM_AUX_MAX5 10000 -param set-default PWM_AUX_MAX6 10000 -param set-default PWM_AUX_MAX7 10000 -param set-default PWM_AUX_MAX8 10000 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp index da2ccb4d960a..526fd23e53f6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp @@ -49,8 +49,17 @@ ActuatorEffectivenessSpacecraft::getEffectivenessMatrix(Configuration &configura return false; } - // Motors + // Thrusters const bool thrusters_added_successfully = _sc_thrusters.addActuators(configuration); return thrusters_added_successfully; } + +void ActuatorEffectivenessSpacecraft::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + // TODO(@E-Krantz): Here is where we can add the nonlinearity of multiple thrusters open + // and how to adjust thrust in that time + return; +} \ No newline at end of file diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp index 46664a6d273d..97bc6c8007d3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp @@ -56,6 +56,10 @@ class ActuatorEffectivenessSpacecraft : public ModuleParams, public ActuatorEffe normalize[0] = true; } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + const char *name() const override { return "Spacecraft"; } protected: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp index 228b916d146c..48ffe2635304 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -47,6 +47,7 @@ ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *par bool tilt_support) : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) { + for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) { char buffer[17]; snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i); @@ -57,18 +58,22 @@ ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *par _param_handles[i].position_z = param_find(buffer); if (_axis_config == AxisConfiguration::Configurable) { - snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i); - _param_handles[i].axis_x = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i); - _param_handles[i].axis_y = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i); - _param_handles[i].axis_z = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); } + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + PX4_INFO("Found param for thruster %u", i); + if (_tilt_support) { PX4_ERR("Tilt support not implemented"); + } } - } _count_handle = param_find("CA_THRUSTER_CNT"); @@ -89,22 +94,30 @@ void ActuatorEffectivenessThrusters::updateParams() _geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count); for (int i = 0; i < _geometry.num_thrusters; ++i) { + PX4_INFO("Updating thruster %d", i); Vector3f &position = _geometry.thrusters[i].position; param_get(_param_handles[i].position_x, &position(0)); + PX4_INFO("Position x"); param_get(_param_handles[i].position_y, &position(1)); + PX4_INFO("Position y"); param_get(_param_handles[i].position_z, &position(2)); + PX4_INFO("Position x"); Vector3f &axis = _geometry.thrusters[i].axis; switch (_axis_config) { case AxisConfiguration::Configurable: param_get(_param_handles[i].axis_x, &axis(0)); + PX4_INFO("Axis x"); param_get(_param_handles[i].axis_y, &axis(1)); + PX4_INFO("Axis y"); param_get(_param_handles[i].axis_z, &axis(2)); + PX4_INFO("Axis z"); break; } param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); + PX4_INFO("Thrust coef: "); } } @@ -115,7 +128,7 @@ ActuatorEffectivenessThrusters::addActuators(Configuration &configuration) PX4_ERR("Wrong actuator ordering: servos need to be after motors"); return false; } - + PX4_INFO("Adding thrusters..."); int num_actuators = computeEffectivenessMatrix(_geometry, configuration.effectiveness_matrices[configuration.selected_matrix], configuration.num_actuators_matrix[configuration.selected_matrix]); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 562b544308cb..a7b567d01aca 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -136,6 +136,7 @@ ControlAllocator::parameters_updated() } update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE); + PX4_INFO("Parameters updated!"); } void @@ -267,10 +268,12 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::SPACECRAFT_2D: + PX4_INFO("ActuatorEffectivenessSpacecraft 2D"); tmp = new ActuatorEffectivenessSpacecraft(this); break; case EffectivenessSource::SPACECRAFT_3D: + PX4_INFO("ActuatorEffectivenessSpacecraft 3D"); tmp = new ActuatorEffectivenessSpacecraft(this); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index f6c7087e783d..ab19d488ec9f 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8d5da7686b07..67093d345593 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -280,7 +280,7 @@ parameters: min: -100 max: 100 default: 0.0 - CA_THRUSTER{i}_PZ: + CA_THRUSTER${i}_PZ: description: short: Position of thruster ${i} along Z body axis type: float @@ -291,7 +291,6 @@ parameters: min: -100 max: 100 default: 0.0 - CA_THRUSTER${i}_AX: description: short: Axis of thruster ${i} thrust vector, X body axis component From f00e97bef130655c2ca12d132b9347c36aee2220 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 19:54:24 +0200 Subject: [PATCH 34/77] fix: control allocator running with thrusters! --- .../ActuatorEffectivenessThrusters.cpp | 10 ------- .../control_allocator/ControlAllocator.cpp | 28 +++++++++++++------ .../control_allocator/ControlAllocator.hpp | 1 + 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp index 48ffe2635304..460c3d2c96fa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -68,7 +68,6 @@ ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *par snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i); _param_handles[i].thrust_coef = param_find(buffer); - PX4_INFO("Found param for thruster %u", i); if (_tilt_support) { PX4_ERR("Tilt support not implemented"); @@ -94,30 +93,22 @@ void ActuatorEffectivenessThrusters::updateParams() _geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count); for (int i = 0; i < _geometry.num_thrusters; ++i) { - PX4_INFO("Updating thruster %d", i); Vector3f &position = _geometry.thrusters[i].position; param_get(_param_handles[i].position_x, &position(0)); - PX4_INFO("Position x"); param_get(_param_handles[i].position_y, &position(1)); - PX4_INFO("Position y"); param_get(_param_handles[i].position_z, &position(2)); - PX4_INFO("Position x"); Vector3f &axis = _geometry.thrusters[i].axis; switch (_axis_config) { case AxisConfiguration::Configurable: param_get(_param_handles[i].axis_x, &axis(0)); - PX4_INFO("Axis x"); param_get(_param_handles[i].axis_y, &axis(1)); - PX4_INFO("Axis y"); param_get(_param_handles[i].axis_z, &axis(2)); - PX4_INFO("Axis z"); break; } param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); - PX4_INFO("Thrust coef: "); } } @@ -128,7 +119,6 @@ ActuatorEffectivenessThrusters::addActuators(Configuration &configuration) PX4_ERR("Wrong actuator ordering: servos need to be after motors"); return false; } - PX4_INFO("Adding thrusters..."); int num_actuators = computeEffectivenessMatrix(_geometry, configuration.effectiveness_matrices[configuration.selected_matrix], configuration.num_actuators_matrix[configuration.selected_matrix]); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index a7b567d01aca..7239534c584f 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -268,12 +268,10 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::SPACECRAFT_2D: - PX4_INFO("ActuatorEffectivenessSpacecraft 2D"); tmp = new ActuatorEffectivenessSpacecraft(this); break; case EffectivenessSource::SPACECRAFT_3D: - PX4_INFO("ActuatorEffectivenessSpacecraft 3D"); tmp = new ActuatorEffectivenessSpacecraft(this); break; @@ -434,14 +432,13 @@ ControlAllocator::Run() // Do allocation _control_allocation[i]->allocate(); + // print each actuator setpoint _actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); - if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); } - _control_allocation[i]->clipActuatorSetpoint(); } } @@ -518,6 +515,16 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx]; + } else if ((ActuatorType)actuator_type == ActuatorType::THRUSTERS) { + if (actuator_type_idx >= MAX_NUM_THRUSTERS) { + PX4_ERR("Too many thrusters"); + _num_actuators[actuator_type] = 0; + break; + } + + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f; + + } else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) { if (actuator_type_idx >= MAX_NUM_SERVOS) { PX4_ERR("Too many servos"); @@ -667,14 +674,17 @@ ControlAllocator::publish_actuator_controls() uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; + // Actuator setpoints are shared between Motors or Thrusters. For now, we assume we have only either of them + int actuator_type = 0; + if (_num_actuators[(int)ActuatorType::THRUSTERS] > 0) { + actuator_type = (int)ActuatorType::THRUSTERS; + } // motors int motors_idx; - - for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + for (motors_idx = 0; motors_idx < _num_actuators[actuator_type] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; - if (stopped_motors & (1u << motors_idx)) { actuator_motors.control[motors_idx] = NAN; } @@ -690,10 +700,10 @@ ControlAllocator::publish_actuator_controls() _actuator_motors_pub.publish(actuator_motors); // servos - if (_num_actuators[1] > 0) { + if (_num_actuators[(int)ActuatorType::SERVOS] > 0) { int servos_idx; - for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + for (servos_idx = 0; servos_idx < _num_actuators[(int)ActuatorType::SERVOS] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index ab19d488ec9f..8b1409fc99f9 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -86,6 +86,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam static constexpr int NUM_AXES = ControlAllocation::NUM_AXES; static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS; + static constexpr int MAX_NUM_THRUSTERS = actuator_motors_s::NUM_CONTROLS; static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS; using ActuatorVector = ActuatorEffectiveness::ActuatorVector; From 730d7809dc728e80ba9e5bc9d7753dc9b80e8422 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 20:01:12 +0200 Subject: [PATCH 35/77] rft: removed prints and non-required messages --- src/modules/control_allocator/ControlAllocator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 7239534c584f..456dc4c802a7 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -136,7 +136,6 @@ ControlAllocator::parameters_updated() } update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE); - PX4_INFO("Parameters updated!"); } void @@ -432,7 +431,6 @@ ControlAllocator::Run() // Do allocation _control_allocation[i]->allocate(); - // print each actuator setpoint _actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); From fc99613827d1cfab1ff582e3eb397d3bb32ce4e6 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 22 Apr 2024 20:40:28 +0200 Subject: [PATCH 36/77] feat: renamed MC_ and mc_ to SC_ and sc_, added gains on airframe for tuning --- .../9000_gazebo-classic_2d_spacecraft | 8 ++ .../sc_rate_control/SpacecraftRateControl.cpp | 52 +++++------ .../sc_rate_control/SpacecraftRateControl.hpp | 66 +++++++------- .../sc_rate_control/sc_rate_control_params.c | 86 +++++++++---------- 4 files changed, 110 insertions(+), 102 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index a44a14d33da5..2bf0ec130f0b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -93,3 +93,11 @@ param set-default PWM_MAIN_FUNC5 105 param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 107 param set-default PWM_MAIN_FUNC8 108 + +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 \ No newline at end of file diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.cpp b/src/modules/sc_rate_control/SpacecraftRateControl.cpp index 094c134c1d2b..20572afcf1fe 100644 --- a/src/modules/sc_rate_control/SpacecraftRateControl.cpp +++ b/src/modules/sc_rate_control/SpacecraftRateControl.cpp @@ -43,7 +43,7 @@ using namespace matrix; using namespace time_literals; using math::radians; -SpacesystemsRateControl::SpacesystemsRateControl(bool vtol) +SpacecraftRateControl::SpacecraftRateControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), @@ -55,9 +55,9 @@ SpacesystemsRateControl::SpacesystemsRateControl(bool vtol) _controller_status_pub.advertise(); } -SpacesystemsRateControl::~SpacesystemsRateControl() { perf_free(_loop_perf); } +SpacecraftRateControl::~SpacecraftRateControl() { perf_free(_loop_perf); } -bool SpacesystemsRateControl::init() { +bool SpacecraftRateControl::init() { if (!_vehicle_angular_velocity_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; @@ -66,29 +66,29 @@ bool SpacesystemsRateControl::init() { return true; } -void SpacesystemsRateControl::parameters_updated() { +void SpacecraftRateControl::parameters_updated() { // rate control parameters // The controller gain K is used to convert the parallel (P + I/s + sD) form // to the ideal (K * [1 + 1/sTi + sTd]) form - const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); _rate_control.setPidGains( - rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), - rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), - rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); _rate_control.setIntegratorLimit( - Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); _rate_control.setFeedForwardGain( - Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), - radians(_param_mc_acro_y_max.get())); + _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), + radians(_param_sc_acro_y_max.get())); } -void SpacesystemsRateControl::Run() { +void SpacecraftRateControl::Run() { if (should_exit()) { _vehicle_angular_velocity_sub.unregisterCallback(); exit_and_cleanup(); @@ -144,9 +144,9 @@ void SpacesystemsRateControl::Run() { if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { // manual rates control - ACRO mode const Vector3f man_rate_sp{ - math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + math::superexpo(manual_control_setpoint.roll, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_sc_acro_expo_y.get(), _param_sc_acro_supexpoy.get())}; _rates_setpoint = man_rate_sp.emult(_acro_rate_max); _thrust_setpoint(2) = -manual_control_setpoint.throttle; @@ -221,7 +221,7 @@ void SpacesystemsRateControl::Run() { vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; // scale setpoints by battery status if enabled - if (_param_mc_bat_scale_en.get()) { + if (_param_sc_bat_scale_en.get()) { if (_battery_status_sub.updated()) { battery_status_s battery_status; @@ -255,7 +255,7 @@ void SpacesystemsRateControl::Run() { perf_end(_loop_perf); } -void SpacesystemsRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s& vehicle_torque_setpoint, +void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s& vehicle_torque_setpoint, float dt) { for (int i = 0; i < 3; i++) { _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; @@ -277,7 +277,7 @@ void SpacesystemsRateControl::updateActuatorControlsStatus(const vehicle_torque_ } } -int SpacesystemsRateControl::task_spawn(int argc, char* argv[]) { +int SpacecraftRateControl::task_spawn(int argc, char* argv[]) { bool vtol = false; if (argc > 1) { @@ -286,7 +286,7 @@ int SpacesystemsRateControl::task_spawn(int argc, char* argv[]) { } } - SpacesystemsRateControl* instance = new SpacesystemsRateControl(vtol); + SpacecraftRateControl* instance = new SpacecraftRateControl(vtol); if (instance) { _object.store(instance); @@ -307,9 +307,9 @@ int SpacesystemsRateControl::task_spawn(int argc, char* argv[]) { return PX4_ERROR; } -int SpacesystemsRateControl::custom_command(int argc, char* argv[]) { return print_usage("unknown command"); } +int SpacecraftRateControl::custom_command(int argc, char* argv[]) { return print_usage("unknown command"); } -int SpacesystemsRateControl::print_usage(const char* reason) { +int SpacecraftRateControl::print_usage(const char* reason) { if (reason) { PX4_WARN("%s\n", reason); } @@ -317,14 +317,14 @@ int SpacesystemsRateControl::print_usage(const char* reason) { PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This implements the multicopter rate controller. It takes rate setpoints (in acro mode -via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. +This implements the spacecraft rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs torque and thrust setpoints. The controller has a PID loop for angular rate error. )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller"); + PRINT_MODULE_USAGE_NAME("sc_rate_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); @@ -333,5 +333,5 @@ The controller has a PID loop for angular rate error. } extern "C" __EXPORT int sc_rate_control_main(int argc, char* argv[]) { - return SpacesystemsRateControl::main(argc, argv); + return SpacecraftRateControl::main(argc, argv); } diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.hpp b/src/modules/sc_rate_control/SpacecraftRateControl.hpp index 58d282b6ace9..38945d9d34d5 100644 --- a/src/modules/sc_rate_control/SpacecraftRateControl.hpp +++ b/src/modules/sc_rate_control/SpacecraftRateControl.hpp @@ -62,11 +62,11 @@ using namespace time_literals; -class SpacesystemsRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +class SpacecraftRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - SpacesystemsRateControl(bool vtol = false); - ~SpacesystemsRateControl() override; + SpacecraftRateControl(bool vtol = false); + ~SpacecraftRateControl() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -130,35 +130,35 @@ class SpacesystemsRateControl : public ModuleBase, publ float _control_energy[4] {}; DEFINE_PARAMETERS( - (ParamFloat) _param_mc_rollrate_p, - (ParamFloat) _param_mc_rollrate_i, - (ParamFloat) _param_mc_rr_int_lim, - (ParamFloat) _param_mc_rollrate_d, - (ParamFloat) _param_mc_rollrate_ff, - (ParamFloat) _param_mc_rollrate_k, - - (ParamFloat) _param_mc_pitchrate_p, - (ParamFloat) _param_mc_pitchrate_i, - (ParamFloat) _param_mc_pr_int_lim, - (ParamFloat) _param_mc_pitchrate_d, - (ParamFloat) _param_mc_pitchrate_ff, - (ParamFloat) _param_mc_pitchrate_k, - - (ParamFloat) _param_mc_yawrate_p, - (ParamFloat) _param_mc_yawrate_i, - (ParamFloat) _param_mc_yr_int_lim, - (ParamFloat) _param_mc_yawrate_d, - (ParamFloat) _param_mc_yawrate_ff, - (ParamFloat) _param_mc_yawrate_k, - - (ParamFloat) _param_mc_acro_r_max, - (ParamFloat) _param_mc_acro_p_max, - (ParamFloat) _param_mc_acro_y_max, - (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ - (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ - (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ - (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ - - (ParamBool) _param_mc_bat_scale_en + (ParamFloat) _param_sc_rollrate_p, + (ParamFloat) _param_sc_rollrate_i, + (ParamFloat) _param_sc_rr_int_lim, + (ParamFloat) _param_sc_rollrate_d, + (ParamFloat) _param_sc_rollrate_ff, + (ParamFloat) _param_sc_rollrate_k, + + (ParamFloat) _param_sc_pitchrate_p, + (ParamFloat) _param_sc_pitchrate_i, + (ParamFloat) _param_sc_pr_int_lim, + (ParamFloat) _param_sc_pitchrate_d, + (ParamFloat) _param_sc_pitchrate_ff, + (ParamFloat) _param_sc_pitchrate_k, + + (ParamFloat) _param_sc_yawrate_p, + (ParamFloat) _param_sc_yawrate_i, + (ParamFloat) _param_sc_yr_int_lim, + (ParamFloat) _param_sc_yawrate_d, + (ParamFloat) _param_sc_yawrate_ff, + (ParamFloat) _param_sc_yawrate_k, + + (ParamFloat) _param_sc_acro_r_max, + (ParamFloat) _param_sc_acro_p_max, + (ParamFloat) _param_sc_acro_y_max, + (ParamFloat) _param_sc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_sc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamBool) _param_sc_bat_scale_en ) }; diff --git a/src/modules/sc_rate_control/sc_rate_control_params.c b/src/modules/sc_rate_control/sc_rate_control_params.c index 55e92f9be856..05366e38e7f7 100644 --- a/src/modules/sc_rate_control/sc_rate_control_params.c +++ b/src/modules/sc_rate_control/sc_rate_control_params.c @@ -50,7 +50,7 @@ * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); +PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f); /** * Roll rate I gain @@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); +PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f); /** * Roll rate integrator limit @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f); +PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f); /** * Roll rate D gain @@ -87,7 +87,7 @@ PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f); * @increment 0.0005 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); +PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); /** * Roll rate feedforward @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); * @decimal 4 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); +PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f); /** * Roll rate controller gain @@ -106,11 +106,11 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * Global gain of the controller. * * This gain scales the P, I and D terms of the controller: - * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error - * + MC_ROLLRATE_I * error_integral - * + MC_ROLLRATE_D * error_derivative) - * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form. - * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form. + * output = SC_ROLLRATE_K * (SC_ROLLRATE_P * error + * + SC_ROLLRATE_I * error_integral + * + SC_ROLLRATE_D * error_derivative) + * Set SC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set SC_ROLLRATE_K=1 to implement a PID in the parallel form. * * @min 0.01 * @max 5.0 @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * @increment 0.0005 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); +PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f); /** * Pitch rate P gain @@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); +PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f); /** * Pitch rate I gain @@ -143,7 +143,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); +PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f); /** * Pitch rate integrator limit @@ -155,7 +155,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f); +PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f); /** * Pitch rate D gain @@ -167,7 +167,7 @@ PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f); * @increment 0.0005 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); +PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); /** * Pitch rate feedforward @@ -178,7 +178,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); * @decimal 4 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); +PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f); /** * Pitch rate controller gain @@ -186,11 +186,11 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); * Global gain of the controller. * * This gain scales the P, I and D terms of the controller: - * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error - * + MC_PITCHRATE_I * error_integral - * + MC_PITCHRATE_D * error_derivative) - * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form. - * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form. + * output = SC_PITCHRATE_K * (SC_PITCHRATE_P * error + * + SC_PITCHRATE_I * error_integral + * + SC_PITCHRATE_D * error_derivative) + * Set SC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set SC_PITCHRATE_K=1 to implement a PID in the parallel form. * * @min 0.01 * @max 5.0 @@ -198,7 +198,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); * @increment 0.0005 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); +PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f); /** * Yaw rate P gain @@ -211,7 +211,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); +PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 0.2f); /** * Yaw rate I gain @@ -223,7 +223,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); +PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.1f); /** * Yaw rate integrator limit @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f); +PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.30f); /** * Yaw rate D gain @@ -247,7 +247,7 @@ PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f); /** * Yaw rate feedforward @@ -259,7 +259,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @increment 0.01 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); +PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f); /** * Yaw rate controller gain @@ -267,11 +267,11 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); * Global gain of the controller. * * This gain scales the P, I and D terms of the controller: - * output = MC_YAWRATE_K * (MC_YAWRATE_P * error - * + MC_YAWRATE_I * error_integral - * + MC_YAWRATE_D * error_derivative) - * Set MC_YAWRATE_P=1 to implement a PID in the ideal form. - * Set MC_YAWRATE_K=1 to implement a PID in the parallel form. + * output = SC_YAWRATE_K * (SC_YAWRATE_P * error + * + SC_YAWRATE_I * error_integral + * + SC_YAWRATE_D * error_derivative) + * Set SC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set SC_YAWRATE_K=1 to implement a PID in the parallel form. * * @min 0.0 * @max 5.0 @@ -279,7 +279,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); * @increment 0.0005 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); +PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f); /** * Max acro roll rate @@ -293,7 +293,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); * @increment 5 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); +PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f); /** * Max acro pitch rate @@ -307,7 +307,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); * @increment 5 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); +PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f); /** * Max acro yaw rate @@ -321,7 +321,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); * @increment 5 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); +PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f); /** * Acro mode Expo factor for Roll and Pitch. @@ -336,7 +336,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); * @decimal 2 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f); /** * Acro mode Expo factor for Yaw. @@ -351,12 +351,12 @@ PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); * @decimal 2 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f); /** * Acro mode SuperExpo factor for Roll and Pitch. * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO. * * 0 Pure Expo function * 0.7 reasonable shape enhancement for intuitive stick feel @@ -367,12 +367,12 @@ PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); * @decimal 2 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f); /** * Acro mode SuperExpo factor for Yaw. * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO_Y. * * 0 Pure Expo function * 0.7 reasonable shape enhancement for intuitive stick feel @@ -383,7 +383,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); * @decimal 2 * @group Multicopter Rate Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f); /** * Battery power level scaler @@ -397,4 +397,4 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); * @boolean * @group Multicopter Rate Control */ -PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); +PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); From 2d913e129d2952c01b433d66fc5ea3f11578bbf7 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 23 Apr 2024 14:52:47 +0200 Subject: [PATCH 37/77] feat: adding attitude rate ctl --- .../9000_gazebo-classic_2d_spacecraft | 1 + .../9001_gazebo-classic_3d_spacecraft | 9 +- .../init.d/airframes/70000_kth_space_robot | 132 ++++--- ROMFS/px4fmu_common/init.d/rc.sc_apps | 7 +- boards/px4/sitl/default.px4board | 1 + src/modules/commander/Commander.cpp | 3 + src/modules/commander/commander_helper.cpp | 7 + src/modules/commander/commander_helper.h | 1 + .../AttitudeControl/AttitudeControl.cpp | 110 ++++++ .../AttitudeControl/AttitudeControl.hpp | 110 ++++++ .../AttitudeControl/AttitudeControlMath.hpp | 70 ++++ .../AttitudeControlMathTest.cpp | 92 +++++ .../AttitudeControl/AttitudeControlTest.cpp | 140 +++++++ .../AttitudeControl/CMakeLists.txt | 43 ++ src/modules/sc_att_control/CMakeLists.txt | 48 +++ src/modules/sc_att_control/Kconfig | 12 + src/modules/sc_att_control/sc_att_control.hpp | 149 +++++++ .../sc_att_control/sc_att_control_main.cpp | 369 ++++++++++++++++++ .../sc_att_control/sc_att_control_params.c | 172 ++++++++ .../sc_rate_control/SpacecraftRateControl.cpp | 18 +- .../sc_rate_control/SpacecraftRateControl.hpp | 2 +- 21 files changed, 1422 insertions(+), 74 deletions(-) create mode 100644 src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp create mode 100644 src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp create mode 100644 src/modules/sc_att_control/AttitudeControl/AttitudeControlMath.hpp create mode 100644 src/modules/sc_att_control/AttitudeControl/AttitudeControlMathTest.cpp create mode 100644 src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp create mode 100644 src/modules/sc_att_control/AttitudeControl/CMakeLists.txt create mode 100644 src/modules/sc_att_control/CMakeLists.txt create mode 100644 src/modules/sc_att_control/Kconfig create mode 100644 src/modules/sc_att_control/sc_att_control.hpp create mode 100644 src/modules/sc_att_control/sc_att_control_main.cpp create mode 100644 src/modules/sc_att_control/sc_att_control_params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 2bf0ec130f0b..9e5cc74fd6a5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -10,6 +10,7 @@ . ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 12 +param set-default MAV_TYPE 24 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index f4cc53927a94..946faa285bb5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -10,6 +10,7 @@ . ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 13 +param set-default MAV_TYPE 25 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 @@ -86,4 +87,10 @@ param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 107 param set-default PWM_MAIN_FUNC8 108 - +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index e6b157530260..bf66d71e9a94 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -10,73 +10,82 @@ . ${R}etc/init.d/rc.sc_defaults -param set-default CA_AIRFRAME 13 +param set-default CA_AIRFRAME 12 +param set-default MAV_TYPE 24 -param set-default CA_ROTOR_COUNT 8 +param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 # Auto to be provided by Custom Airframe -param set-default CA_METHOD 0 +# param set-default CA_METHOD 0 # disable attitude failure detection param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 -param set-default CA_ROTOR0_PX -0.12 -param set-default CA_ROTOR0_PY -0.12 -param set-default CA_ROTOR0_KM 0.1697 -param set-default CA_ROTOR0_AX 1.0 -param set-default CA_ROTOR0_AY 0.0 -param set-default CA_ROTOR0_AZ 0.0 - -param set-default CA_ROTOR1_PX 0.12 -param set-default CA_ROTOR1_PY -0.12 -param set-default CA_ROTOR1_KM 0.1697 -param set-default CA_ROTOR1_AX -1.0 -param set-default CA_ROTOR1_AY 0.0 -param set-default CA_ROTOR1_AZ 0.0 - -param set-default CA_ROTOR2_PX -0.12 -param set-default CA_ROTOR2_PY 0.12 -param set-default CA_ROTOR2_KM 0.1697 -param set-default CA_ROTOR2_AX 1.0 -param set-default CA_ROTOR2_AY 0.0 -param set-default CA_ROTOR2_AZ 0.0 - -param set-default CA_ROTOR3_PX 0.12 -param set-default CA_ROTOR3_PY 0.12 -param set-default CA_ROTOR3_KM 0.1697 -param set-default CA_ROTOR3_AX -1.0 -param set-default CA_ROTOR3_AY 0.0 -param set-default CA_ROTOR3_AZ 0.0 - -param set-default CA_ROTOR4_PX 0.12 -param set-default CA_ROTOR4_PY -0.12 -param set-default CA_ROTOR4_KM 0.1697 -param set-default CA_ROTOR4_AX 0.0 -param set-default CA_ROTOR4_AY 1.0 -param set-default CA_ROTOR4_AZ 0.0 - -param set-default CA_ROTOR5_PX 0.12 -param set-default CA_ROTOR5_PY 0.12 -param set-default CA_ROTOR5_KM 0.1697 -param set-default CA_ROTOR5_AX 0.0 -param set-default CA_ROTOR5_AY -1.0 -param set-default CA_ROTOR5_AZ 0.0 - -param set-default CA_ROTOR6_PX -0.12 -param set-default CA_ROTOR6_PY -0.12 -param set-default CA_ROTOR6_KM 0.1697 -param set-default CA_ROTOR6_AX 0.0 -param set-default CA_ROTOR6_AY 1.0 -param set-default CA_ROTOR6_AZ 0.0 - -param set-default CA_ROTOR7_PX -0.12 -param set-default CA_ROTOR7_PY 0.12 -param set-default CA_ROTOR7_KM 0.1697 -param set-default CA_ROTOR7_AX 0.0 -param set-default CA_ROTOR7_AY -1.0 -param set-default CA_ROTOR7_AZ 0.0 +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.75 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.75 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.75 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.75 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.75 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.75 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.75 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.75 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 param set-default PWM_AUX_TIM0 10 @@ -120,3 +129,10 @@ param set-default PWM_AUX_MAX6 10000 param set-default PWM_AUX_MAX7 10000 param set-default PWM_AUX_MAX8 10000 +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index bf895cf5e120..448f2c0ec260 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -18,9 +18,14 @@ ekf2 start & control_allocator start # -# Start Multicopter Rate Controller. +# Start Spacecraft Rate Controller. # sc_rate_control start +# +# Start Spacecraft Attitude Controller. +# +sc_att_control start + # Start Space Robot Thruster Controller sc_thruster_controller start diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index c69303ca6e89..84f34f2705e5 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -24,6 +24,7 @@ CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_SC_RATE_CONTROL=y +CONFIG_MODULES_SC_ATT_CONTROL=y CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4c317f6bdac5..7c3d405b2eec 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1588,6 +1588,7 @@ void Commander::updateParameters() const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status) && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); const bool is_ground = is_ground_vehicle(_vehicle_status); + const bool is_space = is_spacecraft(_vehicle_status); /* disable manual override for all systems that rely on electronic stabilization */ if (is_rotary) { @@ -1598,6 +1599,8 @@ void Commander::updateParameters() } else if (is_ground) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + } else if (is_space) { + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } _vehicle_status.is_vtol = is_vtol(_vehicle_status); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f642541fb74b..20e0d5361b1f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -78,6 +78,8 @@ #define VEHICLE_TYPE_VTOL_TILTROTOR 21 #define VEHICLE_TYPE_VTOL_FIXEDROTOR 22 // VTOL standard #define VEHICLE_TYPE_VTOL_TAILSITTER 23 +#define VEHICLE_TYPE_SPACECRAFT_2D 24 +#define VEHICLE_TYPE_SPACECRAFT_3D 25 #define BLINK_MSG_TIME 700000 // 3 fast blinks (in us) @@ -122,6 +124,11 @@ bool is_ground_vehicle(const vehicle_status_s ¤t_status) return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER); } +bool is_spacecraft(const vehicle_status_s ¤t_status) +{ + return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT_2D || current_status.system_type == VEHICLE_TYPE_SPACECRAFT_3D); +} + // End time for currently blinking LED message, 0 if no blink message static hrt_abstime blink_msg_end = 0; static int fd_leds{-1}; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 3e96a636e575..daa8abdc0056 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -56,6 +56,7 @@ bool is_vtol(const vehicle_status_s ¤t_status); bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); bool is_fixed_wing(const vehicle_status_s ¤t_status); bool is_ground_vehicle(const vehicle_status_s ¤t_status); +bool is_spacecraft(const vehicle_status_s& current_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp new file mode 100644 index 000000000000..6f1585f8015c --- /dev/null +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AttitudeControl.cpp + */ + +#include + +#include + +using namespace matrix; + +void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight) +{ + _proportional_gain = proportional_gain; + _yaw_w = math::constrain(yaw_weight, 0.f, 1.f); + + // compensate for the effect of the yaw weight rescaling the output + if (_yaw_w > 1e-4f) { + _proportional_gain(2) /= _yaw_w; + } +} + +matrix::Vector3f AttitudeControl::update(const Quatf &q) const +{ + Quatf qd = _attitude_setpoint_q; + + // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch + const Vector3f e_z = q.dcm_z(); + const Vector3f e_z_d = qd.dcm_z(); + Quatf qd_red(e_z, e_z_d); + + if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) { + // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, + // full attitude control anyways generates no yaw input and directly takes the combination of + // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. + qd_red = qd; + + } else { + // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + qd_red *= q; + } + + // mix full and reduced desired attitude + Quatf q_mix = qd_red.inversed() * qd; + q_mix.canonicalize(); + // catch numerical problems with the domain of acosf and asinf + q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); + q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); + qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + + // quaternion attitude control law, qe is rotation from q to qd + const Quatf qe = q.inversed() * qd; + + // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) + // also taking care of the antipodal unit quaternion ambiguity + const Vector3f eq = 2.f * qe.canonical().imag(); + + // calculate angular rates setpoint + Vector3f rate_setpoint = eq.emult(_proportional_gain); + + // Feed forward the yaw setpoint rate. + // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis, + // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). + // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) + // and multiply it by the yaw setpoint rate (yawspeed_setpoint). + // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame + // such that it can be added to the rates setpoint. + if (std::isfinite(_yawspeed_setpoint)) { + rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; + } + + // limit rates + for (int i = 0; i < 3; i++) { + rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); + } + + return rate_setpoint; +} diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp new file mode 100644 index 000000000000..e435e91262f8 --- /dev/null +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AttitudeControl.hpp + * + * A quaternion based attitude controller. + * + * @author Matthias Grob + * + * Publication documenting the implemented Quaternion Attitude Control: + * Nonlinear Quadrocopter Attitude Control (2013) + * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea + * Institute for Dynamic Systems and Control (IDSC), ETH Zurich + * + * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + */ + +#pragma once + +#include +#include + +class AttitudeControl +{ +public: + AttitudeControl() = default; + ~AttitudeControl() = default; + + /** + * Set proportional attitude control gain + * @param proportional_gain 3D vector containing gains for roll, pitch, yaw + * @param yaw_weight A fraction [0,1] deprioritizing yaw compared to roll and pitch + */ + void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight); + + /** + * Set hard limit for output rate setpoints + * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw + */ + void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + + /** + * Set a new attitude setpoint replacing the one tracked before + * @param qd desired vehicle attitude setpoint + * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame + */ + void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) + { + _attitude_setpoint_q = qd; + _attitude_setpoint_q.normalize(); + _yawspeed_setpoint = yawspeed_setpoint; + } + + /** + * Adjust last known attitude setpoint by a delta rotation + * Optional use to avoid glitches when attitude estimate reference e.g. heading changes. + * @param q_delta delta rotation to apply + */ + void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) + { + _attitude_setpoint_q = q_delta * _attitude_setpoint_q; + _attitude_setpoint_q.normalize(); + } + + /** + * Run one control loop cycle calculation + * @param q estimation of the current vehicle attitude unit quaternion + * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller + */ + matrix::Vector3f update(const matrix::Quatf &q) const; + +private: + matrix::Vector3f _proportional_gain; + matrix::Vector3f _rate_limit; + float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize caompared to roll and pitch + + matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control + float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint +}; diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControlMath.hpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControlMath.hpp new file mode 100644 index 000000000000..fe0f760ed49c --- /dev/null +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControlMath.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AttitudeControlMath.hpp + */ + +#pragma once + +#include + +namespace AttitudeControlMath +{ +/** + * Rotate a tilt quaternion (without yaw rotation) such that when rotated by a yaw setpoint + * the resulting tilt is the same as if it was rotated by the current yaw of the vehicle + * @param q_sp_tilt pure tilt quaternion (yaw = 0) that needs to be corrected + * @param q_att current attitude of the vehicle + * @param q_sp_yaw pure yaw quaternion of the desired yaw setpoint + */ +void inline correctTiltSetpointForYawError(matrix::Quatf &q_sp_tilt, const matrix::Quatf &q_att, + const matrix::Quatf &q_sp_yaw) +{ + const matrix::Vector3f z_unit(0.f, 0.f, 1.f); + + // Extract yaw from the current attitude + const matrix::Vector3f att_z = q_att.dcm_z(); + const matrix::Quatf q_tilt(z_unit, att_z); + const matrix::Quatf q_yaw = q_tilt.inversed() * q_att; // This is not euler yaw + + // Find the quaternion that creates a tilt aligned with the body frame + // when rotated by the yaw setpoint + // To do so, solve q_yaw * q_tilt_ne = q_sp_yaw * q_sp_rp_compensated + const matrix::Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_tilt; + + // Extract the corrected tilt + const matrix::Vector3f att_sp_z = q_sp_rp_compensated.dcm_z(); + q_sp_tilt = matrix::Quatf(z_unit, att_sp_z); +} +} diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControlMathTest.cpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControlMathTest.cpp new file mode 100644 index 000000000000..b247fce0745e --- /dev/null +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControlMathTest.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "AttitudeControlMath.hpp" + +using namespace matrix; +using namespace AttitudeControlMath; + +static const Vector3f z_unit(0.f, 0.f, 1.f); + +TEST(AttitudeControlMath, tiltCorrectionNoError) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7)); + + // AND: a desired yaw setpoint + const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f); + + // WHEN: the current yaw error is zero (regardless of the tilt) + const Quatf q = q_sp_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt setpoint is unchanged + EXPECT_TRUE(isEqual(q_tilt_sp_ne_before, q_tilt_sp_ne)); +} + +TEST(AttitudeControlMath, tiltCorrectionYaw180) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7)); + const Quatf q_sp_yaw = AxisAnglef(z_unit, -M_PI_F / 2.f); + + // WHEN: there is a yaw error of 180 degrees + const Quatf q_yaw = Quatf(AxisAnglef(z_unit, M_PI_F / 2.f)); + const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt is reversed (the corrected tilt angle is the same but the axis of rotation is opposite) + EXPECT_FLOAT_EQ(AxisAnglef(q_tilt_sp_ne_before).angle(), AxisAnglef(q_tilt_sp_ne).angle()); + EXPECT_TRUE(isEqual(AxisAnglef(q_tilt_sp_ne_before).axis(), -AxisAnglef(q_tilt_sp_ne).axis())); +} + +TEST(AttitudeControlMath, tiltCorrection) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(0.5, -0.1, 0.7)); + const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f); + + // WHEN: there is a some yaw error + const Quatf q_yaw = Quatf(AxisAnglef(z_unit, 3.1f)); + const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt vector obtained by rotating the corrected tilt by the yaw setpoint is the same as + // the one obtained by rotating the initial tilt by the current yaw of the vehicle + EXPECT_TRUE(isEqual((q_sp_yaw * q_tilt_sp_ne).dcm_z(), (q_yaw * q_tilt_sp_ne_before).dcm_z())); +} diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp new file mode 100644 index 000000000000..2803686c12b1 --- /dev/null +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +using namespace matrix; + +TEST(AttitudeControlTest, AllZeroCase) +{ + AttitudeControl attitude_control; + Vector3f rate_setpoint = attitude_control.update(Quatf()); + EXPECT_EQ(rate_setpoint, Vector3f()); +} + +class AttitudeControlConvergenceTest : public ::testing::Test +{ +public: + AttitudeControlConvergenceTest() + { + _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f), .4f); + _attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f)); + } + + void checkConvergence() + { + int i; // need function scope to check how many steps + Vector3f rate_setpoint(1000.f, 1000.f, 1000.f); + + _attitude_control.setAttitudeSetpoint(_quat_goal, 0.f); + + for (i = 100; i > 0; i--) { + // run attitude control to get rate setpoints + const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state); + // rotate the simulated state quaternion according to the rate setpoint + _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); + _quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem + + // expect the error and hence also the output to get smaller with each iteration + if (rate_setpoint_new.norm() >= rate_setpoint.norm()) { + break; + } + + rate_setpoint = rate_setpoint_new; + } + + EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical()); + // it shouldn't have taken longer than an iteration timeout to converge + EXPECT_GT(i, 0); + } + + AttitudeControl _attitude_control; + Quatf _quat_state; + Quatf _quat_goal; +}; + +TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence) +{ + const int inputs = 8; + + const Quatf QArray[inputs] = { + Quatf(), + Quatf(0, 1, 0, 0), + Quatf(0, 0, 1, 0), + Quatf(0, 0, 0, 1), + Quatf(0.698f, 0.024f, -0.681f, -0.220f), + Quatf(-0.820f, -0.313f, 0.225f, -0.423f), + Quatf(0.599f, -0.172f, 0.755f, -0.204f), + Quatf(0.216f, -0.662f, 0.290f, -0.656f) + }; + + for (int i = 0; i < inputs; i++) { + for (int j = 0; j < inputs; j++) { + printf("--- Input combination: %d %d\n", i, j); + _quat_state = QArray[i]; + _quat_goal = QArray[j]; + _quat_state.normalize(); + _quat_goal.normalize(); + checkConvergence(); + } + } +} + +TEST(AttitudeControlTest, YawWeightScaling) +{ + // GIVEN: default tuning and pure yaw turn command + AttitudeControl attitude_control; + const float yaw_gain = 2.8f; + const float yaw_sp = .1f; + Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)); + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f); + attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f)); + attitude_control.setAttitudeSetpoint(pure_yaw_attitude, 0.f); + + // WHEN: we run one iteration of the controller + Vector3f rate_setpoint = attitude_control.update(Quatf()); + + // THEN: no actuation in roll, pitch + EXPECT_EQ(Vector2f(rate_setpoint), Vector2f()); + // THEN: actuation error * gain in yaw + EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f); + + // GIVEN: additional corner case of zero yaw weight + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f); + // WHEN: we run one iteration of the controller + rate_setpoint = attitude_control.update(Quatf()); + // THEN: no actuation (also no NAN) + EXPECT_EQ(rate_setpoint, Vector3f()); +} diff --git a/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt new file mode 100644 index 000000000000..96c4561297a5 --- /dev/null +++ b/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(AttitudeControl + AttitudeControl.cpp + AttitudeControl.hpp + AttitudeControlMath.hpp +) +target_compile_options(AttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(AttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) +px4_add_unit_gtest(SRC AttitudeControlMathTest.cpp LINKLIBS AttitudeControl) diff --git a/src/modules/sc_att_control/CMakeLists.txt b/src/modules/sc_att_control/CMakeLists.txt new file mode 100644 index 000000000000..1e4a62fe0273 --- /dev/null +++ b/src/modules/sc_att_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(AttitudeControl) + +px4_add_module( + MODULE modules__sc_att_control + MAIN sc_att_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + sc_att_control_main.cpp + sc_att_control.hpp + DEPENDS + AttitudeControl + mathlib + px4_work_queue + ) diff --git a/src/modules/sc_att_control/Kconfig b/src/modules/sc_att_control/Kconfig new file mode 100644 index 000000000000..9391e461d326 --- /dev/null +++ b/src/modules/sc_att_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SC_ATT_CONTROL + bool "sc_att_control" + default n + ---help--- + Enable support for sc_att_control + +menuconfig USER_SC_ATT_CONTROL + bool "sc_att_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_SC_ATT_CONTROL + ---help--- + Put sc_att_control in userspace memory diff --git a/src/modules/sc_att_control/sc_att_control.hpp b/src/modules/sc_att_control/sc_att_control.hpp new file mode 100644 index 000000000000..fbb73686bbb2 --- /dev/null +++ b/src/modules/sc_att_control/sc_att_control.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +class SpacecraftAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + SpacecraftAttitudeControl(); + ~SpacecraftAttitudeControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + float throttle_curve(float throttle_stick_input); + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp); + + AttitudeControl _attitude_control; /**< class for attitude control calculations */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _vehicle_attitude_setpoint_pub; + + manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ + vehicle_control_mode_s _vehicle_control_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + + AlphaFilter _man_roll_input_filter; + AlphaFilter _man_pitch_input_filter; + + hrt_abstime _last_run{0}; + hrt_abstime _last_attitude_setpoint{0}; + + bool _reset_yaw_sp{true}; + bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published + bool _vehicle_type_spacecraft{true}; + + uint8_t _quat_reset_counter{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_man_tilt_tau, + + (ParamFloat) _param_sc_roll_p, + (ParamFloat) _param_sc_pitch_p, + (ParamFloat) _param_sc_yaw_p, + (ParamFloat) _param_sc_yaw_weight, + + (ParamFloat) _param_sc_rollrate_max, + (ParamFloat) _param_sc_pitchrate_max, + (ParamFloat) _param_sc_yawrate_max, + + /* Stabilized mode params */ + (ParamFloat) _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */ + ) +}; + diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp new file mode 100644 index 000000000000..8c2db80af977 --- /dev/null +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -0,0 +1,369 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sc_att_control_main.cpp + * Spacecraft attitude controller. + * Based off the multicopter attitude controller module. + * + * @author Pedro Roque, + * + */ + +#include "sc_att_control.hpp" + +#include +#include +#include + +#include "AttitudeControl/AttitudeControlMath.hpp" + +using namespace matrix; + +SpacecraftAttitudeControl::SpacecraftAttitudeControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + + parameters_updated(); +} + +SpacecraftAttitudeControl::~SpacecraftAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +SpacecraftAttitudeControl::init() +{ + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void +SpacecraftAttitudeControl::parameters_updated() +{ + // Store some of the parameters in a more convenient way & precompute often-used values + _attitude_control.setProportionalGain(Vector3f(_param_sc_roll_p.get(), _param_sc_pitch_p.get(), _param_sc_yaw_p.get()), + _param_sc_yaw_weight.get()); + + // angular rate limits + using math::radians; + _attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()), + radians(_param_sc_yawrate_max.get()))); +} + +void +SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp) +{ + vehicle_attitude_setpoint_s attitude_setpoint{}; + const float yaw = Eulerf(q).psi(); + + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_sc_man_y_scale.get()); + + // Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true + if (_manual_control_setpoint.throttle < -.9f) { + reset_yaw_sp = true; + } + + // Make sure not absolute heading error builds up + if (reset_yaw_sp) { + _man_yaw_sp = yaw; + + } else { + _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); + } + + /* + * Input mapping for roll & pitch setpoints + * ---------------------------------------- + * We control the following 2 angles: + * - tilt angle, given by sqrt(roll*roll + pitch*pitch) + * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion + * + * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick + * points to, and changes of the stick input are linear. + */ + _man_roll_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); + _man_pitch_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); + + // we want to fly towards the direction of (roll, pitch) + Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), + -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); + float v_norm = v.norm(); // the norm of v defines the tilt angle + + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle + v *= _man_tilt_max / v_norm; + } + + Quatf q_sp_rp = AxisAnglef(v(0), v(1), 0.f); + // The axis angle can change the yaw as well (noticeable at higher tilt angles). + // This is the formula by how much the yaw changes: + // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) + // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). + const Quatf q_sp_yaw(cosf(_man_yaw_sp / 2.f), 0.f, 0.f, sinf(_man_yaw_sp / 2.f)); + + // Align the desired tilt with the yaw setpoint + Quatf q_sp = q_sp_yaw * q_sp_rp; + + q_sp.copyTo(attitude_setpoint.q_d); + + // Transform to euler angles for logging only + const Eulerf euler_sp(q_sp); + attitude_setpoint.roll_body = euler_sp(0); + attitude_setpoint.pitch_body = euler_sp(1); + attitude_setpoint.yaw_body = euler_sp(2); + + attitude_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + // update attitude controller setpoint immediately + PX4_INFO("Current setpoint: %f %f %f %f", (double)q_sp(0), (double)q_sp(1), (double)q_sp(2), (double)q_sp(3)); + _attitude_control.setAttitudeSetpoint(q_sp, attitude_setpoint.yaw_sp_move_rate); + _thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body); + _last_attitude_setpoint = attitude_setpoint.timestamp; +} + +void +SpacecraftAttitudeControl::Run() +{ + if (should_exit()) { + _vehicle_attitude_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + // run controller on attitude updates + vehicle_attitude_s v_att; + + if (_vehicle_attitude_sub.update(&v_att)) { + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((v_att.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = v_att.timestamp_sample; + + const Quatf q{v_att.q}; + + // Check for new attitude setpoint + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + + if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) + && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { + PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], + (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], + (double)vehicle_attitude_setpoint.q_d[3]); + + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; + } + } + + // Check for a heading reset + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); + + // for stabilized attitude generation only extract the heading change from the delta quaternion + _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); + + if (v_att.timestamp > _last_attitude_setpoint) { + // adapt existing attitude setpoint unless it was generated after the current attitude estimate + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + } + + _quat_reset_counter = v_att.quat_reset_counter; + } + + /* check for updates in other topics */ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _vehicle_type_spacecraft = (vehicle_status.system_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT); + + } + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position; + + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { + _heading_good_for_control = vehicle_local_position.heading_good_for_control; + } + } + + bool attitude_setpoint_generated = false; + if (_vehicle_control_mode.flag_control_attitude_enabled) { + + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_altitude_enabled && + !_vehicle_control_mode.flag_control_velocity_enabled && + !_vehicle_control_mode.flag_control_position_enabled) { + + generate_attitude_setpoint(q, dt, _reset_yaw_sp); + attitude_setpoint_generated = true; + + } else { + _man_roll_input_filter.reset(0.f); + _man_pitch_input_filter.reset(0.f); + } + PX4_INFO("Current state: %f %f %f %f", (double)q(0), (double)q(1), (double)q(2), (double)q(3)); + Vector3f rates_sp = _attitude_control.update(q); + + const hrt_abstime now = hrt_absolute_time(); + autotune_attitude_control_status_s pid_autotune; + + if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) { + if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL + || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH + || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW + || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST) + && ((now - pid_autotune.timestamp) < 1_s)) { + rates_sp += Vector3f(pid_autotune.rate_sp); + } + } + + // publish rate setpoint + vehicle_rates_setpoint_s rates_setpoint{}; + rates_setpoint.roll = rates_sp(0); + rates_setpoint.pitch = rates_sp(1); + rates_setpoint.yaw = rates_sp(2); + _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body); + rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(rates_setpoint); + } + + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control; + } + + perf_end(_loop_perf); +} + +int SpacecraftAttitudeControl::task_spawn(int argc, char *argv[]) +{ + if (argc > 1) { + PX4_INFO("Ignoring extra parameters"); + } + + SpacecraftAttitudeControl *instance = new SpacecraftAttitudeControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SpacecraftAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the spacecraft attitude controller. It takes attitude +setpoints (`vehicle_attitude_setpoint`) as inputs and outputs a rate setpoint. + +The controller has a P loop for angular error + +Publication documenting the implemented Quaternion Attitude Control: +Nonlinear Quadrocopter Attitude Control (2013) +by Dario Brescianini, Markus Hehn and Raffaello D'Andrea +Institute for Dynamic Systems and Control (IDSC), ETH Zurich + +https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sc_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + + +/** + * Multicopter attitude control app start / stop handling function + */ +extern "C" __EXPORT int sc_att_control_main(int argc, char *argv[]) +{ + return SpacecraftAttitudeControl::main(argc, argv); +} diff --git a/src/modules/sc_att_control/sc_att_control_params.c b/src/modules/sc_att_control/sc_att_control_params.c new file mode 100644 index 000000000000..9176a2e818cc --- /dev/null +++ b/src/modules/sc_att_control/sc_att_control_params.c @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_ROLL_P, 6.5f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_PITCH_P, 6.5f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAW_P, 2.8f); + +/** + * Yaw weight + * + * A fraction [0,1] deprioritizing yaw compared to roll and pitch in non-linear attitude control. + * Deprioritizing yaw is necessary because multicopters have much less control authority + * in yaw compared to the other axes and it makes sense because yaw is not critical for + * stable hovering or 3D navigation. + * + * For yaw control tuning use SC_YAW_P. This ratio has no impact on the yaw gain. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAW_WEIGHT, 0.4f); + +/** + * Max roll rate + * + * Limit for roll rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_MAX, 220.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_MAX, 220.0f); + +/** + * Max yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_MAX, 200.0f); + +/** + * Manual tilt input filter time constant + * + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f); \ No newline at end of file diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.cpp b/src/modules/sc_rate_control/SpacecraftRateControl.cpp index 20572afcf1fe..d0691f8940e5 100644 --- a/src/modules/sc_rate_control/SpacecraftRateControl.cpp +++ b/src/modules/sc_rate_control/SpacecraftRateControl.cpp @@ -43,13 +43,13 @@ using namespace matrix; using namespace time_literals; using math::radians; -SpacecraftRateControl::SpacecraftRateControl(bool vtol) +SpacecraftRateControl::SpacecraftRateControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), - _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)), + _vehicle_torque_setpoint_pub(ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(ORB_ID(vehicle_thrust_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) { - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; parameters_updated(); _controller_status_pub.advertise(); @@ -278,15 +278,8 @@ void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_se } int SpacecraftRateControl::task_spawn(int argc, char* argv[]) { - bool vtol = false; - if (argc > 1) { - if (strcmp(argv[1], "vtol") == 0) { - vtol = true; - } - } - - SpacecraftRateControl* instance = new SpacecraftRateControl(vtol); + SpacecraftRateControl* instance = new SpacecraftRateControl(); if (instance) { _object.store(instance); @@ -326,7 +319,6 @@ The controller has a PID loop for angular rate error. PRINT_MODULE_USAGE_NAME("sc_rate_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.hpp b/src/modules/sc_rate_control/SpacecraftRateControl.hpp index 38945d9d34d5..2555814fe784 100644 --- a/src/modules/sc_rate_control/SpacecraftRateControl.hpp +++ b/src/modules/sc_rate_control/SpacecraftRateControl.hpp @@ -65,7 +65,7 @@ using namespace time_literals; class SpacecraftRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - SpacecraftRateControl(bool vtol = false); + SpacecraftRateControl(); ~SpacecraftRateControl() override; /** @see ModuleBase */ From 61b85cd9875d7e67bde978033627da7b1821d035 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 23 Apr 2024 17:48:08 +0200 Subject: [PATCH 38/77] fix: actuator testing and att control. Att control needs further work --- .../init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft | 2 ++ .../init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft | 2 ++ ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot | 2 ++ src/lib/mixer_module/actuator_test.cpp | 1 - src/modules/sc_att_control/sc_att_control_main.cpp | 2 +- src/modules/uxrce_dds_client/dds_topics.yaml | 3 +++ 6 files changed, 10 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 9e5cc74fd6a5..5e98fec65a96 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -15,6 +15,8 @@ param set-default MAV_TYPE 24 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 +param set-default FW_ARSP_MODE 1 + # Auto to be provided by Custom Airframe # param set-default CA_METHOD 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 946faa285bb5..9bf69f648df0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -15,6 +15,8 @@ param set-default MAV_TYPE 25 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 +param set-default FW_ARSP_MODE 1 + # Auto to be provided by Custom Airframe param set-default CA_METHOD 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index bf66d71e9a94..93453f6ce0ab 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -16,6 +16,8 @@ param set-default MAV_TYPE 24 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 +param set-default FW_ARSP_MODE 1 + # Auto to be provided by Custom Airframe # param set-default CA_METHOD 0 diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index 5347be6b1129..4d98fef3a1f6 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -93,7 +93,6 @@ void ActuatorTest::update(int num_outputs, float thrust_curve) value += trim.trim[idx]; } } - _current_outputs[i] = value; } else { diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp index 8c2db80af977..7fe46a871b78 100644 --- a/src/modules/sc_att_control/sc_att_control_main.cpp +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -261,7 +261,7 @@ SpacecraftAttitudeControl::Run() _man_roll_input_filter.reset(0.f); _man_pitch_input_filter.reset(0.f); } - PX4_INFO("Current state: %f %f %f %f", (double)q(0), (double)q(1), (double)q(2), (double)q(3)); + // PX4_INFO("Current state: %f %f %f %f", (double)q(0), (double)q(1), (double)q(2), (double)q(3)); Vector3f rates_sp = _attitude_control.update(q); const hrt_abstime now = hrt_absolute_time(); diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 11b625330fed..f3ba6915b44d 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -50,6 +50,9 @@ publications: - topic: /fmu/out/actuator_motors type: px4_msgs::msg::ActuatorMotors + - topic: /fmu/out/actuator_outputs + type: px4_msgs::msg::ActuatorOutputs + - topic: /fmu/out/log_message type: px4_msgs::msg::LogMessage From cc9750d0d471f055540915b14b5d644f43e592ba Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 24 Apr 2024 10:51:12 +0200 Subject: [PATCH 39/77] chunk: fixes towards full pipeline --- .../9000_gazebo-classic_2d_spacecraft | 2 +- msg/ActuatorMotors.msg | 1 + msg/VehicleStatus.msg | 1 + src/lib/mixer_module/CMakeLists.txt | 1 + .../functions/FunctionThrusters.hpp | 123 ++++++++++++++++++ src/lib/mixer_module/mixer_module.cpp | 4 +- src/lib/mixer_module/mixer_module.hpp | 1 + .../ControlAllocationPseudoInverse.cpp | 1 + .../sc_att_control/sc_att_control_main.cpp | 1 - 9 files changed, 132 insertions(+), 3 deletions(-) create mode 100644 src/lib/mixer_module/functions/FunctionThrusters.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 5e98fec65a96..5a5a1b487f60 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -18,7 +18,7 @@ param set-default CA_R_REV 255 param set-default FW_ARSP_MODE 1 # Auto to be provided by Custom Airframe -# param set-default CA_METHOD 0 +param set-default CA_METHOD 0 # disable attitude failure detection param set-default FD_FAIL_P 0 diff --git a/msg/ActuatorMotors.msg b/msg/ActuatorMotors.msg index e74566f1f75e..bbc908c074a7 100644 --- a/msg/ActuatorMotors.msg +++ b/msg/ActuatorMotors.msg @@ -5,6 +5,7 @@ uint64 timestamp_sample # the timestamp the data this control response is ba uint16 reversible_flags # bitset which motors are configured to be reversible uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 +uint16 ACTUATOR_FUNCTION_THRUSTER1 = 501 uint8 NUM_CONTROLS = 12 float32[12] control # range: [-1, 1], where 1 means maximum positive thrust, diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 8fcb8c2fa951..939bb6ed8b3d 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -82,6 +82,7 @@ uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 uint8 VEHICLE_TYPE_AIRSHIP = 4 +uint8 VEHICLE_TYPE_SPACECRAFT = 4 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3ddef9..c0b58ce45e4c 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_library(mixer_module functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp + functions/FunctionThrusters.hpp functions/FunctionParachute.hpp functions/FunctionServos.hpp diff --git a/src/lib/mixer_module/functions/FunctionThrusters.hpp b/src/lib/mixer_module/functions/FunctionThrusters.hpp new file mode 100644 index 000000000000..012fbdbc658e --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionThrusters.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "FunctionProviderBase.hpp" + +#include + +/** + * Functions: Thruster1 ... ThrusterMax + */ +class FunctionThrusters : public FunctionProviderBase +{ +public: + static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1, + "Unexpected num thrusters"); + + static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, "Unexpected thruster idx"); + + FunctionThrusters(const Context &context) : + _topic(&context.work_item, ORB_ID(actuator_motors)), + _thrust_factor(context.thrust_factor) + { + for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { + _data.control[i] = NAN; + } + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); } + + void update() override + { + if (_topic.update(&_data)) { + updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); + } + } + + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; } + + bool allowPrearmControl() const override { return false; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } + + static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) + { + if (thrust_factor > 0.f && thrust_factor <= 1.f) { + // thrust factor + // rel_thrust = factor * x^2 + (1-factor) * x, + const float a = thrust_factor; + const float b = (1.f - thrust_factor); + + // don't recompute for all values (ax^2+bx+c=0) + const float tmp1 = b / (2.f * a); + const float tmp2 = b * b / (4.f * a * a); + + for (int i = 0; i < num_values; ++i) { + float control = values[i]; + + if (control > 0.f) { + values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); + + } else if (control < -0.f) { + values[i] = tmp1 - sqrtf(tmp2 - (control / a)); + + } else { + values[i] = 0.f; + } + } + } + + for (int i = 0; i < num_values; ++i) { + if ((reversible & (1u << i)) == 0) { + if (values[i] < -FLT_EPSILON) { + values[i] = NAN; + + } else { + // remap from [0, 1] to [-1, 1] + values[i] = values[i] * 2.f - 1.f; + } + } + } + } + + bool reversible(OutputFunction func) const override { return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); } + +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_motors_s _data{}; + const float &_thrust_factor; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 69c9acfb60ff..510651086ff6 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -56,6 +56,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Constant_Min, &FunctionConstantMin::allocate}, {OutputFunction::Constant_Max, &FunctionConstantMax::allocate}, {OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate}, + {OutputFunction::Thruster1, OutputFunction::ThrusterMax, &FunctionThrusters::allocate}, {OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate}, {OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate}, {OutputFunction::Landing_Gear, &FunctionLandingGear::allocate}, @@ -237,7 +238,8 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) int32_t function; if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &function) == 0) { - if (function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) { + if ((function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) || + (function >= (int32_t)OutputFunction::Thruster1 && function <= (int32_t)OutputFunction::ThrusterMax)) { switch_requested = true; } } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 0d8a4952710f..4cefe228ae93 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -44,6 +44,7 @@ #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" #include "functions/FunctionMotors.hpp" +#include "functions/FunctionThrusters.hpp" #include "functions/FunctionParachute.hpp" #include "functions/FunctionServos.hpp" diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index ee1d2dceb2c3..bb8281c1cf73 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -40,6 +40,7 @@ */ #include "ControlAllocationPseudoInverse.hpp" +#include void ControlAllocationPseudoInverse::setEffectivenessMatrix( diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp index 7fe46a871b78..ad3da6d3bcdb 100644 --- a/src/modules/sc_att_control/sc_att_control_main.cpp +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -155,7 +155,6 @@ SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); // update attitude controller setpoint immediately - PX4_INFO("Current setpoint: %f %f %f %f", (double)q_sp(0), (double)q_sp(1), (double)q_sp(2), (double)q_sp(3)); _attitude_control.setAttitudeSetpoint(q_sp, attitude_setpoint.yaw_sp_move_rate); _thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body); _last_attitude_setpoint = attitude_setpoint.timestamp; From 8de434f5d25b8d6c3443e9d37d6794764e98d171 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 24 Apr 2024 14:23:50 +0200 Subject: [PATCH 40/77] feat: functionThrusters in mixer module --- src/lib/mixer_module/functions/FunctionThrusters.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/mixer_module/functions/FunctionThrusters.hpp b/src/lib/mixer_module/functions/FunctionThrusters.hpp index 012fbdbc658e..de7e7871cac8 100644 --- a/src/lib/mixer_module/functions/FunctionThrusters.hpp +++ b/src/lib/mixer_module/functions/FunctionThrusters.hpp @@ -35,8 +35,6 @@ #include "FunctionProviderBase.hpp" -#include - /** * Functions: Thruster1 ... ThrusterMax */ @@ -66,7 +64,7 @@ class FunctionThrusters : public FunctionProviderBase } } - float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; } + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; } bool allowPrearmControl() const override { return false; } @@ -76,7 +74,9 @@ class FunctionThrusters : public FunctionProviderBase static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) { - if (thrust_factor > 0.f && thrust_factor <= 1.f) { + // TODO(@Pedro-Roque): for now bypass this + /*if (thrust_factor > 0.f && thrust_factor <= 1.f) { + // thrust factor // rel_thrust = factor * x^2 + (1-factor) * x, const float a = thrust_factor; @@ -111,10 +111,10 @@ class FunctionThrusters : public FunctionProviderBase values[i] = values[i] * 2.f - 1.f; } } - } + }*/ } - bool reversible(OutputFunction func) const override { return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); } + bool reversible(OutputFunction func) const override { return false; } private: uORB::SubscriptionCallbackWorkItem _topic; From 59df525412a6780549bbac65d317c651c9c4165d Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 25 Apr 2024 22:26:37 +0200 Subject: [PATCH 41/77] init: creating spacecraft position controller --- src/modules/sc_pos_control/CMakeLists.txt | 48 ++ src/modules/sc_pos_control/Kconfig | 12 + .../PositionControl/CMakeLists.txt | 43 ++ .../PositionControl/ControlMath.cpp | 261 ++++++++++ .../PositionControl/ControlMath.hpp | 118 +++++ .../PositionControl/ControlMathTest.cpp | 258 ++++++++++ .../PositionControl/PositionControl.cpp | 263 ++++++++++ .../PositionControl/PositionControl.hpp | 229 +++++++++ .../PositionControl/PositionControlTest.cpp | 382 ++++++++++++++ .../SpacecraftPositionControl.cpp | 481 ++++++++++++++++++ .../SpacecraftPositionControl.hpp | 187 +++++++ .../sc_pos_control/Takeoff/CMakeLists.txt | 41 ++ .../sc_pos_control/Takeoff/Takeoff.cpp | 134 +++++ .../sc_pos_control/Takeoff/Takeoff.hpp | 101 ++++ .../sc_pos_control/Takeoff/TakeoffTest.cpp | 79 +++ .../multicopter_altitude_mode_params.c | 119 +++++ .../multicopter_autonomous_params.c | 162 ++++++ .../multicopter_nudging_params.c | 65 +++ ...multicopter_position_control_gain_params.c | 137 +++++ ...lticopter_position_control_limits_params.c | 140 +++++ .../multicopter_position_control_params.c | 87 ++++ .../multicopter_position_mode_params.c | 198 +++++++ .../multicopter_responsiveness_params.c | 80 +++ .../multicopter_stabilized_mode_params.c | 95 ++++ .../multicopter_takeoff_land_params.c | 124 +++++ src/modules/uxrce_dds_client/dds_topics.yaml | 12 + 26 files changed, 3856 insertions(+) create mode 100644 src/modules/sc_pos_control/CMakeLists.txt create mode 100644 src/modules/sc_pos_control/Kconfig create mode 100644 src/modules/sc_pos_control/PositionControl/CMakeLists.txt create mode 100644 src/modules/sc_pos_control/PositionControl/ControlMath.cpp create mode 100644 src/modules/sc_pos_control/PositionControl/ControlMath.hpp create mode 100644 src/modules/sc_pos_control/PositionControl/ControlMathTest.cpp create mode 100644 src/modules/sc_pos_control/PositionControl/PositionControl.cpp create mode 100644 src/modules/sc_pos_control/PositionControl/PositionControl.hpp create mode 100644 src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp create mode 100644 src/modules/sc_pos_control/SpacecraftPositionControl.cpp create mode 100644 src/modules/sc_pos_control/SpacecraftPositionControl.hpp create mode 100644 src/modules/sc_pos_control/Takeoff/CMakeLists.txt create mode 100644 src/modules/sc_pos_control/Takeoff/Takeoff.cpp create mode 100644 src/modules/sc_pos_control/Takeoff/Takeoff.hpp create mode 100644 src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp create mode 100644 src/modules/sc_pos_control/multicopter_altitude_mode_params.c create mode 100644 src/modules/sc_pos_control/multicopter_autonomous_params.c create mode 100644 src/modules/sc_pos_control/multicopter_nudging_params.c create mode 100644 src/modules/sc_pos_control/multicopter_position_control_gain_params.c create mode 100644 src/modules/sc_pos_control/multicopter_position_control_limits_params.c create mode 100644 src/modules/sc_pos_control/multicopter_position_control_params.c create mode 100644 src/modules/sc_pos_control/multicopter_position_mode_params.c create mode 100644 src/modules/sc_pos_control/multicopter_responsiveness_params.c create mode 100644 src/modules/sc_pos_control/multicopter_stabilized_mode_params.c create mode 100644 src/modules/sc_pos_control/multicopter_takeoff_land_params.c diff --git a/src/modules/sc_pos_control/CMakeLists.txt b/src/modules/sc_pos_control/CMakeLists.txt new file mode 100644 index 000000000000..5830535907b1 --- /dev/null +++ b/src/modules/sc_pos_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(PositionControl) + +px4_add_module( + MODULE modules__sc_pos_control + MAIN sc_pos_control + COMPILE_FLAGS + SRCS + SpacecraftPositionControl.cpp + SpacecraftPositionControl.hpp + DEPENDS + PositionControl + controllib + geo + SlewRate + ) diff --git a/src/modules/sc_pos_control/Kconfig b/src/modules/sc_pos_control/Kconfig new file mode 100644 index 000000000000..57f36b74b126 --- /dev/null +++ b/src/modules/sc_pos_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SC_POS_CONTROL + bool "sc_pos_control" + default n + ---help--- + Enable support for sc_pos_control + +menuconfig USER_SC_POS_CONTROL + bool "sc_pos_control running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_SC_POS_CONTROL + ---help--- + Put sc_pos_control in userspace memory diff --git a/src/modules/sc_pos_control/PositionControl/CMakeLists.txt b/src/modules/sc_pos_control/PositionControl/CMakeLists.txt new file mode 100644 index 000000000000..813631737681 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(PositionControl + ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp +) +target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl) +px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl) diff --git a/src/modules/sc_pos_control/PositionControl/ControlMath.cpp b/src/modules/sc_pos_control/PositionControl/ControlMath.cpp new file mode 100644 index 000000000000..b4eba96115cd --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ControlMath.cpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlMath.cpp + */ + +#include "ControlMath.hpp" +#include +#include +#include + +using namespace matrix; + +namespace ControlMath +{ +void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + bodyzToAttitude(-thr_sp, yaw_sp, att_sp); + att_sp.thrust_body[2] = -thr_sp.length(); +} + +void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle) +{ + // determine tilt + const float dot_product_unit = body_unit.dot(world_unit); + float angle = acosf(dot_product_unit); + // limit tilt + angle = math::min(angle, max_angle); + Vector3f rejection = body_unit - (dot_product_unit * world_unit); + + // corner case exactly parallel vectors + if (rejection.norm_squared() < FLT_EPSILON) { + rejection(0) = 1.f; + } + + body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit(); +} + +void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + // zero vector, no direction, set safe level value + if (body_z.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + } + + body_z.normalize(); + + // vector of desired yaw direction in XY plane, rotated by PI/2 + const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f}; + + // desired body_x axis, orthogonal to body_z + Vector3f body_x = y_C % body_z; + + // keep nose to front while inverted upside down + if (body_z(2) < 0.f) { + body_x = -body_x; + } + + if (fabsf(body_z(2)) < 0.000001f) { + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually + body_x.zero(); + body_x(2) = 1.f; + } + + body_x.normalize(); + + // desired body_y axis + const Vector3f body_y = body_z % body_x; + + Dcmf R_sp; + + // fill rotation matrix + for (int i = 0; i < 3; i++) { + R_sp(i, 0) = body_x(i); + R_sp(i, 1) = body_y(i); + R_sp(i, 2) = body_z(i); + } + + // copy quaternion setpoint to attitude setpoint topic + const Quatf q_sp{R_sp}; + q_sp.copyTo(att_sp.q_d); + + // calculate euler angles, for logging only, must not be used for control + const Eulerf euler{R_sp}; + att_sp.roll_body = euler.phi(); + att_sp.pitch_body = euler.theta(); + att_sp.yaw_body = euler.psi(); +} + +Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) +{ + if (Vector2f(v0 + v1).norm() <= max) { + // vector does not exceed maximum magnitude + return v0 + v1; + + } else if (v0.length() >= max) { + // the magnitude along v0, which has priority, already exceeds maximum. + return v0.normalized() * max; + + } else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) { + // the two vectors are equal + return v0.normalized() * max; + + } else if (v0.length() < 0.001f) { + // the first vector is 0. + return v1.normalized() * max; + + } else { + // vf = final vector with ||vf|| <= max + // s = scaling factor + // u1 = unit of v1 + // vf = v0 + v1 = v0 + s * u1 + // constraint: ||vf|| <= max + // + // solve for s: ||vf|| = ||v0 + s * u1|| <= max + // + // Derivation: + // For simplicity, replace v0 -> v, u1 -> u + // v0(0/1/2) -> v0/1/2 + // u1(0/1/2) -> u0/1/2 + // + // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2 + // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2 + // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0 + // + // quadratic equation: + // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a) + // + // b = 2 * u.dot(v) + // a = 1 (because u is normalized) + // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2 + // + // sqrt(b^2 - 4*a*c) = + // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2)) + // + // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2 + // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2)) + // m = u.dot(v) + // s = -m + sqrt(m^2 - c) + // + // + // + // notes: + // - s (=scaling factor) needs to be positive + // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement + Vector2f u1 = v1.normalized(); + float m = u1.dot(v0); + float c = v0.dot(v0) - max * max; + float s = -m + sqrtf(m * m - c); + return v0 + u1 * s; + } +} + +bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, + const Vector3f &line_a, const Vector3f &line_b, Vector3f &res) +{ + // project center of sphere on line normalized AB + Vector3f ab_norm = line_b - line_a; + + if (ab_norm.length() < 0.01f) { + return true; + } + + ab_norm.normalize(); + Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + if (sphere_r > cd_len) { + // we have triangle CDX with known CD and CX = R, find DX + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + + if ((sphere_c - line_b) * ab_norm > 0.f) { + // target waypoint is already behind us + res = line_b; + + } else { + // target is in front of us + res = d + ab_norm * dx_len; // vector A->B on line + } + + return true; + + } else { + + // have no roots, return D + res = d; // go directly to line + + // previous waypoint is still in front of us + if ((sphere_c - line_a) * ab_norm < 0.f) { + res = line_a; + } + + // target waypoint is already behind us + if ((sphere_c - line_b) * ab_norm > 0.f) { + res = line_b; + } + + return false; + } +} + +void addIfNotNan(float &setpoint, const float addition) +{ + if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) { + // No NAN, add to the setpoint + setpoint += addition; + + } else if (!PX4_ISFINITE(setpoint)) { + // Setpoint NAN, take addition + setpoint = addition; + } + + // Addition is NAN or both are NAN, nothing to do +} + +void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition) +{ + for (int i = 0; i < 3; i++) { + addIfNotNan(setpoint(i), addition(i)); + } +} + +void setZeroIfNanVector3f(Vector3f &vector) +{ + // Adding zero vector overwrites elements that are NaN with zero + addIfNotNanVector3f(vector, Vector3f()); +} + +} // ControlMath diff --git a/src/modules/sc_pos_control/PositionControl/ControlMath.hpp b/src/modules/sc_pos_control/PositionControl/ControlMath.hpp new file mode 100644 index 000000000000..c2d9b8b7f8bb --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ControlMath.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlMath.hpp + * + * Simple functions for vector manipulation that do not fit into matrix lib. + * These functions are specific for controls. + */ + +#pragma once + +#include +#include + +namespace ControlMath +{ +/** + * Converts thrust vector and yaw set-point to a desired attitude. + * @param thr_sp desired 3D thrust vector + * @param yaw_sp the desired yaw + * @param att_sp attitude setpoint to fill + */ +void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Limits the tilt angle between two unit vectors + * @param body_unit unit vector that will get adjusted if angle is too big + * @param world_unit fixed vector to measure the angle against + * @param max_angle maximum tilt angle between vectors in radians + */ +void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle); + +/** + * Converts a body z vector and yaw set-point to a desired attitude. + * @param body_z a world frame 3D vector in direction of the desired body z axis + * @param yaw_sp the desired yaw setpoint + * @param att_sp attitude setpoint to fill + */ +void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Outputs the sum of two vectors but respecting the limits and priority. + * The sum of two vectors are constraint such that v0 has priority over v1. + * This means that if the length of (v0+v1) exceeds max, then it is constraint such + * that v0 has priority. + * + * @param v0 a 2D vector that has priority given the maximum available magnitude. + * @param v1 a 2D vector that less priority given the maximum available magnitude. + * @return 2D vector + */ +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max); + +/** + * This method was used for smoothing the corners along two lines. + * + * @param sphere_c + * @param sphere_r + * @param line_a + * @param line_b + * @param res + * return boolean + * + * Note: this method is not used anywhere and first requires review before usage. + */ +bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a, + const matrix::Vector3f &line_b, matrix::Vector3f &res); + +/** + * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control. + * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommitted value. + * @param setpoint existing possibly NAN setpoint to add to + * @param addition value/NAN to add to the setpoint + */ +void addIfNotNan(float &setpoint, const float addition); + +/** + * _addIfNotNan for Vector3f treating each element individually + * @see _addIfNotNan + */ +void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); + +/** + * Overwrites elements of a Vector3f which are NaN with zero + * @param vector possibly containing NAN elements + */ +void setZeroIfNanVector3f(matrix::Vector3f &vector); +} diff --git a/src/modules/sc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/sc_pos_control/PositionControl/ControlMathTest.cpp new file mode 100644 index 000000000000..e9b6fba12fb4 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ControlMathTest.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include + +using namespace matrix; +using namespace ControlMath; + +TEST(ControlMathTest, LimitTiltUnchanged) +{ + Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized(); + Vector3f body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); + + body = Vector3f(0.f, .1f, 1.f).normalized(); + body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); +} + +TEST(ControlMathTest, LimitTiltOpposite) +{ + Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTiltAlmostOpposite) +{ + // This case doesn't trigger corner case handling but is very close to it + Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTilt45degree) +{ + Vector3f body = Vector3f(1.f, 0.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F)); + + body = Vector3f(0.f, 1.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F)); +} + +TEST(ControlMathTest, LimitTilt10degree) +{ + Vector3f body = Vector3f(1.f, 1.f, .1f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(body(0), body(1)); + + body = Vector3f(1, 2, .2f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(2.f * body(0), body(1)); +} + +TEST(ControlMathTest, ThrottleAttitudeMapping) +{ + /* expected: zero roll, zero pitch, zero yaw, full thr mag + * reason: thrust pointing full upward */ + Vector3f thr{0.f, 0.f, -1.f}; + float yaw = 0.f; + vehicle_attitude_setpoint_s att{}; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but with 90 yaw + * reason: only yaw changed */ + yaw = M_PI_2_F; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but roll 180 + * reason: thrust points straight down and order Euler + * order is: 1. roll, 2. pitch, 3. yaw */ + thr = Vector3f(0.f, 0.f, 1.f); + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); +} + +TEST(ControlMathTest, ConstrainXYPriorities) +{ + const float max = 5.f; + // v0 already at max + Vector2f v0(max, 0.f); + Vector2f v1(v0(1), -v0(0)); + + Vector2f v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), max); + EXPECT_FLOAT_EQ(v_r(1), 0.f); + + // norm of v1 exceeds max but v0 is zero + v0.zero(); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(1), -max); + EXPECT_FLOAT_EQ(v_r(0), 0.f); + + v0 = Vector2f(.5f, .5f); + v1 = Vector2f(.5f, -.5f); + v_r = constrainXY(v0, v1, max); + const float diff = Vector2f(v_r - (v0 + v1)).length(); + EXPECT_FLOAT_EQ(diff, 0.f); + + // v0 and v1 exceed max and are perpendicular + v0 = Vector2f(4.f, 0.f); + v1 = Vector2f(0.f, -4.f); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), v0(0)); + EXPECT_GT(v_r(0), 0.f); + const float remaining = sqrtf(max * max - (v0(0) * v0(0))); + EXPECT_FLOAT_EQ(v_r(1), -remaining); +} + +TEST(ControlMathTest, CrossSphereLine) +{ + /* Testing 9 positions (+) around waypoints (o): + * + * Far + + + + * + * Near + + + + * On trajectory --+----o---------+---------o----+-- + * prev curr + * + * Expected targets (1, 2, 3): + * Far + + + + * + * + * On trajectory -------1---------2---------3------- + * + * + * Near + + + + * On trajectory -------o---1---------2-----3------- + * + * + * On trajectory --+----o----1----+--------2/3---+-- */ + const Vector3f prev = Vector3f(0.f, 0.f, 0.f); + const Vector3f curr = Vector3f(0.f, 0.f, 2.f); + Vector3f res; + bool retval = false; + + // on line, near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f)); + + // on line, near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // on line, near, after target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f)); + + // near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f)); + + // near, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // far, before previous waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f()); + + // far, before target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f)); + + // far, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); +} + +TEST(ControlMathTest, addIfNotNan) +{ + float v = 1.f; + // regular addition + ControlMath::addIfNotNan(v, 2.f); + EXPECT_EQ(v, 3.f); + // addition is NAN and has no influence + ControlMath::addIfNotNan(v, NAN); + EXPECT_EQ(v, 3.f); + v = NAN; + // both summands are NAN + ControlMath::addIfNotNan(v, NAN); + EXPECT_TRUE(std::isnan(v)); + // regular value gets added to NAN and overwrites it + ControlMath::addIfNotNan(v, 3.f); + EXPECT_EQ(v, 3.f); +} diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp new file mode 100644 index 000000000000..1371523c8d5d --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -0,0 +1,263 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PositionControl.cpp + */ + +#include "PositionControl.hpp" +#include "ControlMath.hpp" +#include +#include +#include +#include + +using namespace matrix; + +const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; + +void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_vel_p = P; + _gain_vel_i = I; + _gain_vel_d = D; +} + +void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down) +{ + _lim_vel_horizontal = vel_horizontal; + _lim_vel_up = vel_up; + _lim_vel_down = vel_down; +} + +void PositionControl::setThrustLimits(const float min, const float max) +{ + // make sure there's always enough thrust vector length to infer the attitude + _lim_thr_min = math::max(min, 10e-4f); + _lim_thr_max = max; +} + +void PositionControl::setHorizontalThrustMargin(const float margin) +{ + _lim_thr_xy_margin = margin; +} + +void PositionControl::updateHoverThrust(const float hover_thrust_new) +{ + // Given that the equation for thrust is T = a_sp * Th / g - Th + // with a_sp = desired acceleration, Th = hover thrust and g = gravity constant, + // we want to find the acceleration that needs to be added to the integrator in order obtain + // the same thrust after replacing the current hover thrust by the new one. + // T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th + // so a_sp' = (a_sp - g) * Th / Th' + g + // we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th' + const float previous_hover_thrust = _hover_thrust; + setHoverThrust(hover_thrust_new); + + _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust + + CONSTANTS_ONE_G - _acc_sp(2); +} + +void PositionControl::setState(const PositionControlStates &states) +{ + _pos = states.position; + _vel = states.velocity; + _yaw = states.yaw; + _vel_dot = states.acceleration; +} + +void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) +{ + _pos_sp = Vector3f(setpoint.position); + _vel_sp = Vector3f(setpoint.velocity); + _acc_sp = Vector3f(setpoint.acceleration); + _yaw_sp = setpoint.yaw; + _yawspeed_sp = setpoint.yawspeed; +} + +bool PositionControl::update(const float dt) +{ + bool valid = _inputValid(); + + if (valid) { + _positionControl(); + _velocityControl(dt); + + _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; + _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control + } + + // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong + return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite(); +} + +void PositionControl::_positionControl() +{ + // P-position controller + Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); + // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence + ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); + // make sure there are no NAN elements for further reference while constraining + ControlMath::setZeroIfNanVector3f(vel_sp_position); + + // Constrain horizontal velocity by prioritizing the velocity component along the + // the desired position setpoint over the feed-forward term. + _vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal); + // Constrain velocity in z-direction. + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down); +} + +void PositionControl::_velocityControl(const float dt) +{ + // Constrain vertical velocity integral + _vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G); + + // PID velocity control + Vector3f vel_error = _vel_sp - _vel; + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + + // No control input from setpoints or corresponding states which are NAN + ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); + + _accelerationControl(); + + // Integrator anti-windup in vertical direction + if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) || + (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) { + vel_error(2) = 0.f; + } + + // Prioritize vertical control while keeping a horizontal margin + const Vector2f thrust_sp_xy(_thr_sp); + const float thrust_sp_xy_norm = thrust_sp_xy.norm(); + const float thrust_max_squared = math::sq(_lim_thr_max); + + // Determine how much vertical thrust is left keeping horizontal margin + const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin); + const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust); + + // Saturate maximal vertical thrust + _thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared)); + + // Determine how much horizontal thrust is left after prioritizing vertical control + const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2)); + float thrust_max_xy = 0; + + if (thrust_max_xy_squared > 0) { + thrust_max_xy = sqrtf(thrust_max_xy_squared); + } + + // Saturate thrust in horizontal direction + if (thrust_sp_xy_norm > thrust_max_xy) { + _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; + } + + // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output + // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 + const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); + const float arw_gain = 2.f / _gain_vel_p(0); + + // The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently). + // The ARW loop needs to run if the signal is saturated only. + const Vector2f acc_sp_xy = _acc_sp.xy(); + const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared()) + ? acc_sp_xy_produced + : acc_sp_xy; + vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy); + + // Make sure integral doesn't get NAN + ControlMath::setZeroIfNanVector3f(vel_error); + // Update integral part of velocity control + _vel_int += vel_error.emult(_gain_vel_i) * dt; +} + +void PositionControl::_accelerationControl() +{ + // Assume standard acceleration due to gravity in vertical direction for attitude generation + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); + // Scale thrust assuming hover thrust produces standard gravity + float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Project thrust to planned body attitude + collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); + collective_thrust = math::min(collective_thrust, -_lim_thr_min); + _thr_sp = body_z * collective_thrust; +} + +bool PositionControl::_inputValid() +{ + bool valid = true; + + // Every axis x, y, z needs to have some setpoint + for (int i = 0; i <= 2; i++) { + valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i))); + } + + // x and y input setpoints always have to come in pairs + valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))); + valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))); + valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); + + // For each controlled state the estimate has to be valid + for (int i = 0; i <= 2; i++) { + if (PX4_ISFINITE(_pos_sp(i))) { + valid = valid && PX4_ISFINITE(_pos(i)); + } + + if (PX4_ISFINITE(_vel_sp(i))) { + valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i)); + } + } + + return valid; +} + +void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const +{ + local_position_setpoint.x = _pos_sp(0); + local_position_setpoint.y = _pos_sp(1); + local_position_setpoint.z = _pos_sp(2); + local_position_setpoint.yaw = _yaw_sp; + local_position_setpoint.yawspeed = _yawspeed_sp; + local_position_setpoint.vx = _vel_sp(0); + local_position_setpoint.vy = _vel_sp(1); + local_position_setpoint.vz = _vel_sp(2); + _acc_sp.copyTo(local_position_setpoint.acceleration); + _thr_sp.copyTo(local_position_setpoint.thrust); +} + +void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const +{ + ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); + attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; +} diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.hpp b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp new file mode 100644 index 000000000000..4eb909e1fa3e --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PositionControl.hpp + * + * A cascaded position controller for position/velocity control only. + */ + +#pragma once + +#include +#include +#include +#include +#include + +struct PositionControlStates { + matrix::Vector3f position; + matrix::Vector3f velocity; + matrix::Vector3f acceleration; + float yaw; +}; + +/** + * Core Position-Control for MC. + * This class contains P-controller for position and + * PID-controller for velocity. + * Inputs: + * vehicle position/velocity/yaw + * desired set-point position/velocity/thrust/yaw/yaw-speed + * constraints that are stricter than global limits + * Output + * thrust vector and a yaw-setpoint + * + * If there is a position and a velocity set-point present, then + * the velocity set-point is used as feed-forward. If feed-forward is + * active, then the velocity component of the P-controller output has + * priority over the feed-forward component. + * + * A setpoint that is NAN is considered as not set. + * If there is a position/velocity- and thrust-setpoint present, then + * the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop. + */ +class PositionControl +{ +public: + + PositionControl() = default; + ~PositionControl() = default; + + /** + * Set the position control gains + * @param P 3D vector of proportional gains for x,y,z axis + */ + void setPositionGains(const matrix::Vector3f &P) { _gain_pos_p = P; } + + /** + * Set the velocity control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the maximum velocity to execute with feed forward and position control + * @param vel_horizontal horizontal velocity limit + * @param vel_up upwards velocity limit + * @param vel_down downwards velocity limit + */ + void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down); + + /** + * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller + * @param min minimum thrust e.g. 0.1 or 0 + * @param max maximum thrust e.g. 0.9 or 1 + */ + void setThrustLimits(const float min, const float max); + + /** + * Set margin that is kept for horizontal control when prioritizing vertical thrust + * @param margin of normalized thrust that is kept for horizontal control e.g. 0.3 + */ + void setHorizontalThrustMargin(const float margin); + + /** + * Set the maximum tilt angle in radians the output attitude is allowed to have + * @param tilt angle in radians from level orientation + */ + void setTiltLimit(const float tilt) { _lim_tilt = tilt; } + + /** + * Set the normalized hover thrust + * @param hover_thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation + */ + void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, HOVER_THRUST_MIN, HOVER_THRUST_MAX); } + + /** + * Update the hover thrust without immediately affecting the output + * by adjusting the integrator. This prevents propagating the dynamics + * of the hover thrust signal directly to the output of the controller. + */ + void updateHoverThrust(const float hover_thrust_new); + + /** + * Pass the current vehicle state to the controller + * @param PositionControlStates structure + */ + void setState(const PositionControlStates &states); + + /** + * Pass the desired setpoints + * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. + * @param setpoint setpoints including feed-forwards to execute in update() + */ + void setInputSetpoint(const trajectory_setpoint_s &setpoint); + + /** + * Apply P-position and PID-velocity controller that updates the member + * thrust, yaw- and yawspeed-setpoints. + * @see _thr_sp + * @see _yaw_sp + * @see _yawspeed_sp + * @param dt time in seconds since last iteration + * @return true if update succeeded and output setpoint is executable, false if not + */ + bool update(const float dt); + + /** + * Set the integral term in xy to 0. + * @see _vel_int + */ + void resetIntegral() { _vel_int.setZero(); } + + /** + * Get the controllers output local position setpoint + * These setpoints are the ones which were executed on including PID output and feed-forward. + * The acceleration or thrust setpoints can be used for attitude control. + * @param local_position_setpoint reference to struct to fill up + */ + void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const; + + /** + * Get the controllers output attitude setpoint + * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. + * It needs to be executed by the attitude controller to achieve velocity and position tracking. + * @param attitude_setpoint reference to struct to fill up + */ + void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; + + /** + * All setpoints are set to NAN (uncontrolled). Timestampt zero. + */ + static const trajectory_setpoint_s empty_trajectory_setpoint; + +private: + // The range limits of the hover thrust configuration/estimate + static constexpr float HOVER_THRUST_MIN = 0.05f; + static constexpr float HOVER_THRUST_MAX = 0.9f; + + bool _inputValid(); + + void _positionControl(); ///< Position proportional control + void _velocityControl(const float dt); ///< Velocity PID control + void _accelerationControl(); ///< Acceleration setpoint processing + + // Gains + matrix::Vector3f _gain_pos_p; ///< Position control proportional gain + matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain + matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain + matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain + + // Limits + float _lim_vel_horizontal{}; ///< Horizontal velocity limit with feed forward and position control + float _lim_vel_up{}; ///< Upwards velocity limit with feed forward and position control + float _lim_vel_down{}; ///< Downwards velocity limit with feed forward and position control + float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 + float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 + float _lim_thr_xy_margin{}; ///< Margin to keep for horizontal control when saturating prioritized vertical thrust + float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have + + float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation + + // States + matrix::Vector3f _pos; /**< current position */ + matrix::Vector3f _vel; /**< current velocity */ + matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ + matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ + float _yaw{}; /**< current heading */ + + // Setpoints + matrix::Vector3f _pos_sp; /**< desired position */ + matrix::Vector3f _vel_sp; /**< desired velocity */ + matrix::Vector3f _acc_sp; /**< desired acceleration */ + matrix::Vector3f _thr_sp; /**< desired thrust */ + float _yaw_sp{}; /**< desired heading */ + float _yawspeed_sp{}; /** desired yaw-speed */ +}; diff --git a/src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp new file mode 100644 index 000000000000..0e5a60619a44 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +using namespace matrix; + +TEST(PositionControlTest, EmptySetpoint) +{ + PositionControl position_control; + + vehicle_local_position_setpoint_s output_setpoint{}; + position_control.getLocalPositionSetpoint(output_setpoint); + EXPECT_FLOAT_EQ(output_setpoint.x, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.y, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.z, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.yaw, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.yawspeed, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.vx, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.vy, 0.f); + EXPECT_FLOAT_EQ(output_setpoint.vz, 0.f); + EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0)); + + vehicle_attitude_setpoint_s attitude{}; + position_control.getAttitudeSetpoint(attitude); + EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); + EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(attitude.reset_integral, false); + EXPECT_EQ(attitude.fw_control_yaw_wheel, false); +} + +class PositionControlBasicTest : public ::testing::Test +{ +public: + PositionControlBasicTest() + { + _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); + _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); + _position_control.setVelocityLimits(1.f, 1.f, 1.f); + _position_control.setThrustLimits(0.1f, MAXIMUM_THRUST); + _position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN); + _position_control.setTiltLimit(1.f); + _position_control.setHoverThrust(.5f); + } + + bool runController() + { + _position_control.setInputSetpoint(_input_setpoint); + const bool ret = _position_control.update(.1f); + _position_control.getLocalPositionSetpoint(_output_setpoint); + _position_control.getAttitudeSetpoint(_attitude); + return ret; + } + + PositionControl _position_control; + trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; + vehicle_local_position_setpoint_s _output_setpoint{}; + vehicle_attitude_setpoint_s _attitude{}; + + static constexpr float MAXIMUM_THRUST = 0.9f; + static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f; +}; + +class PositionControlBasicDirectionTest : public PositionControlBasicTest +{ +public: + void checkDirection() + { + Vector3f thrust(_output_setpoint.thrust); + EXPECT_GT(thrust(0), 0.f); + EXPECT_GT(thrust(1), 0.f); + EXPECT_LT(thrust(2), 0.f); + + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + EXPECT_LT(body_z(0), 0.f); + EXPECT_LT(body_z(1), 0.f); + EXPECT_GT(body_z(2), 0.f); + } +}; + +TEST_F(PositionControlBasicDirectionTest, PositionDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicDirectionTest, VelocityDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicTest, TiltLimit) +{ + Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, 1.f); + + _position_control.setTiltLimit(0.5f); + EXPECT_TRUE(runController()); + body_z = Quatf(_attitude.q_d).dcm_z(); + angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, .50001f); + + _position_control.setTiltLimit(1.f); // restore original +} + +TEST_F(PositionControlBasicTest, VelocityLimit) +{ + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); + EXPECT_LE(velocity_xy.norm(), 1.f); + EXPECT_LE(abs(_output_setpoint.vz), 1.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) +{ + // Given a setpoint that drives the controller into vertical and horizontal saturation + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + // When you run it for one iteration + runController(); + Vector3f thrust(_output_setpoint.thrust); + + // Then the thrust vector length is limited by the maximum + EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST); + + // Then the horizontal thrust is limited by its margin + EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(2), + -sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN)); + thrust.print(); + + // Then the collective thrust is limited by the maximum + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); + + // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust + EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore + // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); +} + +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) +{ + Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position); + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); +} + +TEST_F(PositionControlBasicTest, FailsafeInput) +{ + _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + _input_setpoint.velocity[2] = .1f; + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_LT(_output_setpoint.thrust[2], -.1f); + EXPECT_GT(_output_setpoint.thrust[2], -.5f); + EXPECT_GT(_attitude.thrust_body[2], -.5f); + EXPECT_LE(_attitude.thrust_body[2], -.1f); +} + +TEST_F(PositionControlBasicTest, IdleThrustInput) +{ + // High downwards acceleration to make sure there's no thrust + Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust +} + +TEST_F(PositionControlBasicTest, InputCombinationsPosition) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FALSE(isnan(_output_setpoint.vx)); + EXPECT_FALSE(isnan(_output_setpoint.vy)); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +{ + _input_setpoint.velocity[0] = .1f; + _input_setpoint.velocity[1] = .2f; + _input_setpoint.position[2] = .3f; // altitude + + EXPECT_TRUE(runController()); + EXPECT_TRUE(isnan(_output_setpoint.x)); + EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, SetpointValiditySimple) +{ + EXPECT_FALSE(runController()); + _input_setpoint.position[0] = .1f; + EXPECT_FALSE(runController()); + _input_setpoint.position[1] = .2f; + EXPECT_FALSE(runController()); + _input_setpoint.acceleration[2] = .3f; + EXPECT_TRUE(runController()); +} + +TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) +{ + // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly + float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0], + &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1], + &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2] + }; + + for (int combination = 0; combination < 512; combination++) { + _input_setpoint = PositionControl::empty_trajectory_setpoint; + + for (int j = 0; j < 9; j++) { + if (combination & (1 << j)) { + // Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations + *(setpoint_loop_access_map[j]) = static_cast(combination) / static_cast(j + 1); + } + } + + // Expect at least one setpoint per axis + const bool has_x_setpoint = ((combination & 7) != 0); + const bool has_y_setpoint = (((combination >> 3) & 7) != 0); + const bool has_z_setpoint = (((combination >> 6) & 7) != 0); + // Expect xy setpoints to come in pairs + const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); + const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; + + EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl + << "input" << std::endl + << "position " << _input_setpoint.position[0] << ", " + << _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl + << "velocity " << _input_setpoint.velocity[0] << ", " + << _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl + << "acceleration " << _input_setpoint.acceleration[0] << ", " + << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl + << "output" << std::endl + << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl + << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl + << "acceleration " << _output_setpoint.acceleration[0] << ", " + << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; + } +} + +TEST_F(PositionControlBasicTest, InvalidState) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + PositionControlStates states{}; + states.position(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.position(0) = 0.f; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = 0.f; + states.acceleration(1) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); +} + + +TEST_F(PositionControlBasicTest, UpdateHoverThrust) +{ + // GIVEN: some hover thrust and 0 velocity change + const float hover_thrust = 0.6f; + _position_control.setHoverThrust(hover_thrust); + + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + + // WHEN: we run the controller + EXPECT_TRUE(runController()); + + // THEN: the output thrust equals the hover thrust + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + + // HOWEVER WHEN: we set a new hover thrust through the update function + const float hover_thrust_new = 0.7f; + _position_control.updateHoverThrust(hover_thrust_new); + EXPECT_TRUE(runController()); + + // THEN: the integral is updated to avoid discontinuities and + // the output is still the same + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); +} + +TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) +{ + // GIVEN: the controller was ran with an invalid setpoint containing some valid values + _input_setpoint.position[0] = .1f; + _input_setpoint.position[1] = .2f; + // all z-axis setpoints stay NAN + EXPECT_FALSE(runController()); + + // WHEN: we run the controller with a valid setpoint + _input_setpoint = PositionControl::empty_trajectory_setpoint; + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + + // THEN: the integral did not wind up and produce unexpected deviation + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +} diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp new file mode 100644 index 000000000000..986b79e59dc4 --- /dev/null +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -0,0 +1,481 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SpacecraftPositionControl.hpp" + +#include +#include +#include +#include +#include "PositionControl/ControlMath.hpp" + +using namespace matrix; + +SpacecraftPositionControl::SpacecraftPositionControl() : + SuperBlock(nullptr, "MPC"), + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD") +{ + parameters_update(true); + _tilt_limit_slew_rate.setSlewRate(.2f); +} + +SpacecraftPositionControl::~SpacecraftPositionControl() +{ + perf_free(_cycle_perf); +} + +bool SpacecraftPositionControl::init() +{ + if (!_local_pos_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + _time_stamp_last_loop = hrt_absolute_time(); + ScheduleNow(); + + return true; +} + +void SpacecraftPositionControl::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + SuperBlock::updateParams(); + + int num_changed = 0; + + if (_param_sys_vehicle_resp.get() >= 0.f) { + // make it less sensitive at the lower end + float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); + + num_changed += _param_mpc_acc_hor.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_hor_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); + + if (responsiveness > 0.6f) { + num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + + } else { + num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); + } + + num_changed += _param_mpc_acc_down_max.commit_no_notification(math::lerp(0.8f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_up_max.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); + num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); + } + + if (_param_mpc_xy_vel_all.get() >= 0.f) { + float xy_vel = _param_mpc_xy_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel); + num_changed += _param_mpc_vel_man_back.commit_no_notification(-1.f); + num_changed += _param_mpc_vel_man_side.commit_no_notification(-1.f); + num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel); + } + + if (_param_mpc_z_vel_all.get() >= 0.f) { + float z_vel = _param_mpc_z_vel_all.get(); + num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f); + num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f); + } + + if (num_changed > 0) { + param_notify_changes(); + } + + _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); + _control.setVelocityGains( + Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), + Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), + Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); + + // Check that the design parameters are inside the absolute maximum constraints + if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { + _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get()); + _param_mpc_xy_cruise.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_XY_CRUISE is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_cruise_set"), events::Log::Warning, + "Cruise speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get()); + } + + if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get()); + _param_mpc_vel_manual.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_VEL_MANUAL is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_man_vel_set"), events::Log::Warning, + "Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get()); + } + + if (_param_mpc_vel_man_back.get() > _param_mpc_vel_manual.get()) { + _param_mpc_vel_man_back.set(_param_mpc_vel_manual.get()); + _param_mpc_vel_man_back.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual backward speed has been constrained by forward speed\t"); + /* EVENT + * @description MPC_VEL_MAN_BACK is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_man_vel_back_set"), events::Log::Warning, + "Manual backward speed has been constrained by forward speed", _param_mpc_vel_manual.get()); + } + + if (_param_mpc_vel_man_side.get() > _param_mpc_vel_manual.get()) { + _param_mpc_vel_man_side.set(_param_mpc_vel_manual.get()); + _param_mpc_vel_man_side.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual sideways speed has been constrained by forward speed\t"); + /* EVENT + * @description MPC_VEL_MAN_SIDE is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_man_vel_side_set"), events::Log::Warning, + "Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.get()); + } + + if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) { + _param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get()); + _param_mpc_z_v_auto_up.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_AUTO_UP is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning, + "Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); + } + + if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) { + _param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get()); + _param_mpc_z_v_auto_dn.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_AUTO_DN is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning, + "Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); + } + } +} + +PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicle_local_position_s + &vehicle_local_position) +{ + PositionControlStates states; + + const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y); + + // only set position states if valid and finite + if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) { + states.position.xy() = position_xy; + + } else { + states.position(0) = states.position(1) = NAN; + } + + if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) { + states.position(2) = vehicle_local_position.z; + + } else { + states.position(2) = NAN; + } + + const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy); + + if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { + states.velocity.xy() = velocity_xy; + states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0)); + states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1)); + + } else { + states.velocity(0) = states.velocity(1) = NAN; + states.acceleration(0) = states.acceleration(1) = NAN; + + // reset derivatives to prevent acceleration spikes when regaining velocity + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) { + states.velocity(2) = vehicle_local_position.vz; + states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); + + } else { + states.velocity(2) = NAN; + states.acceleration(2) = NAN; + + // reset derivative to prevent acceleration spikes when regaining velocity + _vel_z_deriv.reset(); + } + + states.yaw = vehicle_local_position.heading; + + return states; +} + +void SpacecraftPositionControl::Run() +{ + if (should_exit()) { + _local_pos_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // reschedule backup + ScheduleDelayed(100_ms); + + parameters_update(false); + + perf_begin(_cycle_perf); + vehicle_local_position_s vehicle_local_position; + + if (_local_pos_sub.update(&vehicle_local_position)) { + const float dt = + math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = vehicle_local_position.timestamp_sample; + + // set _dt in controllib Block for BlockDerivative + setDt(dt); + + if (_vehicle_control_mode_sub.updated()) { + const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled; + + if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) { + if (!previous_position_control_enabled && _vehicle_control_mode.flag_control_position_enabled) { + _time_position_control_enabled = _vehicle_control_mode.timestamp; + + } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_control_position_enabled) { + // clear existing setpoint when controller is no longer active + _setpoint = PositionControl::empty_trajectory_setpoint; + } + } + } + + // TODO: check if setpoint is different than the previous one and reset integral then + // _control.resetIntegral(); + _trajectory_setpoint_sub.update(&_setpoint); + + // adjust existing (or older) setpoint with any EKF reset deltas + if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.velocity[0] += vehicle_local_position.delta_vxy[0]; + _setpoint.velocity[1] += vehicle_local_position.delta_vxy[1]; + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _setpoint.velocity[2] += vehicle_local_position.delta_vz; + } + + if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) { + _setpoint.position[0] += vehicle_local_position.delta_xy[0]; + _setpoint.position[1] += vehicle_local_position.delta_xy[1]; + } + + if (vehicle_local_position.z_reset_counter != _z_reset_counter) { + _setpoint.position[2] += vehicle_local_position.delta_z; + } + + if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) { + _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading); + } + } + + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _vel_z_deriv.reset(); + } + + // save latest reset counters + _vxy_reset_counter = vehicle_local_position.vxy_reset_counter; + _vz_reset_counter = vehicle_local_position.vz_reset_counter; + _xy_reset_counter = vehicle_local_position.xy_reset_counter; + _z_reset_counter = vehicle_local_position.z_reset_counter; + _heading_reset_counter = vehicle_local_position.heading_reset_counter; + + + PositionControlStates states{set_vehicle_states(vehicle_local_position)}; + + + if (_vehicle_control_mode.flag_control_position_enabled) { + // set failsafe setpoint if there hasn't been a new + // trajectory setpoint since position control started + if ((_setpoint.timestamp < _time_position_control_enabled) + && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { + + _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); + } + } + + if (_vehicle_control_mode.flag_control_position_enabled + && (_setpoint.timestamp >= _time_position_control_enabled)) { + + // Allow ramping from zero thrust on takeoff + _control.setThrustLimits(0.0, _param_mpc_thr_max.get()); + + _control.setVelocityLimits( + max_speed_xy, + math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit + math::max(speed_down, 0.f)); + + _control.setInputSetpoint(_setpoint); + + _control.setState(states); + + // Run position control + if (!_control.update(dt)) { + // Failsafe + _vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints + + _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true)); + _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); + _control.update(dt); + } + + // Publish internal position control setpoints + // on top of the input/feed-forward setpoints these containt the PID corrections + // This message is used by other modules (such as Landdetector) to determine vehicle intention. + vehicle_local_position_setpoint_s local_pos_sp{}; + _control.getLocalPositionSetpoint(local_pos_sp); + local_pos_sp.timestamp = hrt_absolute_time(); + _local_pos_sp_pub.publish(local_pos_sp); + + // Publish attitude setpoint output + vehicle_attitude_setpoint_s attitude_setpoint{}; + _control.getAttitudeSetpoint(attitude_setpoint); + attitude_setpoint.timestamp = hrt_absolute_time(); + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + } + } + + perf_end(_cycle_perf); +} + +trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, + const PositionControlStates &states, bool warn) +{ + // rate limit the warnings + warn = warn && (now - _last_warn) > 2_s; + + if (warn) { + PX4_WARN("invalid setpoints"); + _last_warn = now; + } + + trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint; + failsafe_setpoint.timestamp = now; + + failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f; + + if (warn) { + PX4_WARN("Failsafe: stop and wait"); + } + + return failsafe_setpoint; +} + +int SpacecraftPositionControl::task_spawn(int argc, char *argv[]) +{ + SpacecraftPositionControl *instance = new SpacecraftPositionControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SpacecraftPositionControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftPositionControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + The controller has two loops: a P loop for position error and a PID loop for velocity error. + Output of the velocity controller is thrust vector that is split to thrust direction + (i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself). + + The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and + logging. + )DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sc_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sc_pos_control_main(int argc, char *argv[]) +{ + return SpacecraftPositionControl::main(argc, argv); +} diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp new file mode 100644 index 000000000000..0b95f2fd95f1 --- /dev/null +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Multicopter position controller. + */ + +#pragma once + +#include "PositionControl/PositionControl.hpp" +#include "Takeoff/Takeoff.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SpacecraftPositionControl : public ModuleBase, public control::SuperBlock, + public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SpacecraftPositionControl(); + ~SpacecraftPositionControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + hrt_abstime _time_position_control_enabled{0}; + + trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint}; + vehicle_control_mode_s _vehicle_control_mode{}; + + DEFINE_PARAMETERS( + // Position Control + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_i_acc, + (ParamFloat) _param_mpc_xy_vel_d_acc, + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_z_vel_i_acc, + (ParamFloat) _param_mpc_z_vel_d_acc, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_v_auto_dn, + (ParamFloat) _param_mpc_z_vel_max_dn, + + // Takeoff / Land + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_vel_man_back, + (ParamFloat) _param_mpc_vel_man_side, + (ParamFloat) _param_mpc_xy_cruise, + (ParamInt) _param_mpc_pos_mode, + (ParamFloat) _param_mpc_thr_max, + + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all + ); + + control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ + control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ + control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + + PositionControl _control; /**< class for core PID position control */ + + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + + static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf + + SlewRate _tilt_limit_slew_rate; + + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + /** + * Update our local parameter cache. + * Parameter update can be forced when argument is true. + * @param force forces parameter update. + */ + void parameters_update(bool force); + + /** + * Check for validity of positon/velocity states. + */ + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); + + /** + * Generate setpoint to bridge no executable setpoint being available. + * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. + * This should only happen briefly when transitioning and never during mode operation or by design. + */ + trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn); +}; diff --git a/src/modules/sc_pos_control/Takeoff/CMakeLists.txt b/src/modules/sc_pos_control/Takeoff/CMakeLists.txt new file mode 100644 index 000000000000..a4c129aed661 --- /dev/null +++ b/src/modules/sc_pos_control/Takeoff/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(Takeoff + Takeoff.cpp + Takeoff.hpp +) +target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(Takeoff PUBLIC hysteresis) + +px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/sc_pos_control/Takeoff/Takeoff.cpp b/src/modules/sc_pos_control/Takeoff/Takeoff.cpp new file mode 100644 index 000000000000..2e4fbc1f4047 --- /dev/null +++ b/src/modules/sc_pos_control/Takeoff/Takeoff.cpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Takeoff.cpp + */ + +#include "Takeoff.hpp" +#include +#include + +void TakeoffHandling::generateInitialRampValue(float velocity_p_gain) +{ + velocity_p_gain = math::max(velocity_p_gain, 0.01f); + _takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain; +} + +void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us) +{ + _spoolup_time_hysteresis.set_state_and_update(armed, now_us); + + switch (_takeoff_state) { + case TakeoffState::disarmed: + if (armed) { + _takeoff_state = TakeoffState::spoolup; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::spoolup: + if (_spoolup_time_hysteresis.get_state()) { + _takeoff_state = TakeoffState::ready_for_takeoff; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::ready_for_takeoff: + if (want_takeoff) { + _takeoff_state = TakeoffState::rampup; + _takeoff_ramp_progress = 0.f; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::rampup: + if (_takeoff_ramp_progress >= 1.f) { + _takeoff_state = TakeoffState::flight; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::flight: + if (landed) { + _takeoff_state = TakeoffState::ready_for_takeoff; + } + + break; + + default: + break; + } + + if (armed && skip_takeoff) { + _takeoff_state = TakeoffState::flight; + } + + // TODO: need to consider free fall here + if (!armed) { + _takeoff_state = TakeoffState::disarmed; + } +} + +float TakeoffHandling::updateRamp(const float dt, const float takeoff_desired_vz) +{ + float upwards_velocity_limit = takeoff_desired_vz; + + if (_takeoff_state < TakeoffState::rampup) { + upwards_velocity_limit = _takeoff_ramp_vz_init; + } + + if (_takeoff_state == TakeoffState::rampup) { + if (_takeoff_ramp_time > dt) { + _takeoff_ramp_progress += dt / _takeoff_ramp_time; + + } else { + _takeoff_ramp_progress = 1.f; + } + + if (_takeoff_ramp_progress < 1.f) { + upwards_velocity_limit = _takeoff_ramp_vz_init + _takeoff_ramp_progress * (takeoff_desired_vz - _takeoff_ramp_vz_init); + } + } + + return upwards_velocity_limit; +} diff --git a/src/modules/sc_pos_control/Takeoff/Takeoff.hpp b/src/modules/sc_pos_control/Takeoff/Takeoff.hpp new file mode 100644 index 000000000000..1de486e3253b --- /dev/null +++ b/src/modules/sc_pos_control/Takeoff/Takeoff.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Takeoff.hpp + * + * A class handling all takeoff states and a smooth ramp up of the motors. + */ + +#pragma once + +#include +#include +#include + +using namespace time_literals; + +enum class TakeoffState { + disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED, + spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP, + ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF, + rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP, + flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT +}; + +class TakeoffHandling +{ +public: + TakeoffHandling() = default; + ~TakeoffHandling() = default; + + // initialize parameters + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + + /** + * Calculate a vertical velocity to initialize the takeoff ramp + * that when passed to the velocity controller results in a zero throttle setpoint. + * @param hover_thrust normalized thrsut value with which the vehicle hovers + * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust + */ + void generateInitialRampValue(const float velocity_p_gain); + + /** + * Update the state for the takeoff. + * Has to be called also when not flying altitude controlled to skip the takeoff and not do it in flight when switching mode. + */ + void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us); + + /** + * Update and return the velocity constraint ramp value during takeoff. + * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved. + * Returns zero on the ground and takeoff_desired_vz in flight. + * @param dt time in seconds since the last call/loop iteration + * @param takeoff_desired_vz end value for the velocity ramp + * @return true if setpoint has updated correctly + */ + float updateRamp(const float dt, const float takeoff_desired_vz); + + TakeoffState getTakeoffState() { return _takeoff_state; } + +private: + TakeoffState _takeoff_state = TakeoffState::disarmed; + + systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true COM_SPOOLUP_TIME seconds after the vehicle was armed + + float _takeoff_ramp_time{0.f}; + float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust + float _takeoff_ramp_progress{0.f}; ///< asecnding from 0 to 1 +}; diff --git a/src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp new file mode 100644 index 000000000000..d718ef687bbe --- /dev/null +++ b/src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +TEST(TakeoffTest, Initialization) +{ + TakeoffHandling takeoff; + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +} + +TEST(TakeoffTest, RegularTakeoffRamp) +{ + TakeoffHandling takeoff; + takeoff.setSpoolupTime(1.f); + takeoff.setTakeoffRampTime(2.f); + takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); + + // disarmed, landed, don't want takeoff + takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); + + // armed, not landed anymore, don't want takeoff + takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); + + // armed, not landed, don't want takeoff yet, spoolup time passed + takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); + + // armed, not landed, want takeoff + takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup); + + // armed, not landed, want takeoff, ramping up + takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); + + // armed, not landed, want takeoff, rampup time passed + takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight); +} diff --git a/src/modules/sc_pos_control/multicopter_altitude_mode_params.c b/src/modules/sc_pos_control/multicopter_altitude_mode_params.c new file mode 100644 index 000000000000..ecebaa9e01f9 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_altitude_mode_params.c @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Maximum upwards acceleration in climb rate controlled modes + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.f); + +/** + * Maximum downwards acceleration in climb rate controlled modes + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.f); + +/** + * Manual yaw rate input filter time constant + * + * Not used in Stabilized mode + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); + +/** + * Altitude reference mode + * + * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in + * flight due to sensor drift. + * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down + * with terrain height variation. Requires a distance to ground sensor. The height controller will + * revert to using height above origin if the distance to ground estimate becomes invalid as indicated + * by the local_position.distance_bottom_valid message being false. + * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative + * to earth frame origin when moving horizontally. + * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. + * + * @min 0 + * @max 2 + * @value 0 Altitude following + * @value 1 Terrain following + * @value 2 Terrain hold + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * Only used with MPC_POS_MODE 0 or MPC_ALT_MODE 2 + * + * @unit m/s + * @min 0 + * @max 3 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * Only used with MPC_ALT_MODE 1 + * + * @unit m/s + * @min 0 + * @max 3 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f); diff --git a/src/modules/sc_pos_control/multicopter_autonomous_params.c b/src/modules/sc_pos_control/multicopter_autonomous_params.c new file mode 100644 index 000000000000..8596cedd8b7b --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_autonomous_params.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Default horizontal velocity in autonomous modes + * + * e.g. in Missions, RTL, Goto if the waypoint does not specify differently + * + * @unit m/s + * @min 3 + * @max 20 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.f); + +/** + * Ascent velocity in autonomous modes + * + * For manually controlled modes and offboard see MPC_Z_VEL_MAX_UP + * + * @unit m/s + * @min 0.5 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f); + +/** + * Descent velocity in autonomous modes + * + * For manual modes and offboard, see MPC_Z_VEL_MAX_DN + * + * @unit m/s + * @min 0.5 + * @max 4 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f); + +/** + * Acceleration for autonomous and for manual modes + * + * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.f); + +/** + * Jerk limit in autonomous modes + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions but also limited agility. + * + * @unit m/s^3 + * @min 1 + * @max 80 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.f); + +/** + * Proportional gain for horizontal trajectory position error + * + * @min 0.1 + * @max 1 + * @decimal 1 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); + +/** + * Maximum horizontal error allowed by the trajectory generator + * + * The integration speed of the trajectory setpoint is linearly + * reduced with the horizontal position tracking error. When the + * error is above this parameter, the integration of the + * trajectory is stopped to wait for the drone. + * + * This value can be adjusted depending on the tracking + * capabilities of the vehicle. + * + * @min 0.1 + * @max 10 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f); + +/** + * Max yaw rate in autonomous modes + * + * Limits the rate of change of the yaw setpoint to avoid large + * control output and mixer saturation. + * + * @unit deg/s + * @min 0 + * @max 360 + * @decimal 0 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f); + +/** + * Heading behavior in autonomous modes + * + * @min 0 + * @max 4 + * @value 0 towards waypoint + * @value 1 towards home + * @value 2 away from home + * @value 3 along trajectory + * @value 4 towards waypoint (yaw first) + * @group Mission + */ +PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); diff --git a/src/modules/sc_pos_control/multicopter_nudging_params.c b/src/modules/sc_pos_control/multicopter_nudging_params.c new file mode 100644 index 000000000000..c431e88fd04c --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_nudging_params.c @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Enable nudging based on user input during autonomous land routine + * + * Using stick input the vehicle can be moved horizontally and yawed. + * The descend speed is amended: + * stick full up - 0 + * stick centered - MPC_LAND_SPEED + * stick full down - 2 * MPC_LAND_SPEED + * + * Manual override during auto modes has to be disabled to use this feature (see COM_RC_OVERRIDE). + * + * @min 0 + * @max 1 + * @value 0 Nudging disabled + * @value 1 Nudging enabled + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); + +/** + * User assisted landing radius + * + * When nudging is enabled (see MPC_LAND_RC_HELP), this controls + * the maximum allowed horizontal displacement from the original landing point. + * + * @unit m + * @min 0 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); diff --git a/src/modules/sc_pos_control/multicopter_position_control_gain_params.c b/src/modules/sc_pos_control/multicopter_position_control_gain_params.c new file mode 100644 index 000000000000..df36d1f733f9 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_position_control_gain_params.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Proportional gain for vertical position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0.1 + * @max 1.5 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.f); + +/** + * Proportional gain for horizontal position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); + +/** + * Proportional gain for vertical velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.f); + +/** + * Proportional gain for horizontal velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 1.2 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); + +/** + * Integral gain for vertical velocity error + * + * Defined as corrective acceleration in m/s^2 per m velocity integral + * + * @min 0.2 + * @max 3 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.f); + +/** + * Integral gain for horizontal velocity error + * + * Defined as correction acceleration in m/s^2 per m velocity integral + * Allows to eliminate steady state errors in disturbances like wind. + * + * @min 0 + * @max 60 + * @decimal 2 + * @increment 0.02 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); + +/** + * Differential gain for vertical velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.02 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f); + +/** + * Differential gain for horizontal velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.1 + * @max 2 + * @decimal 2 + * @increment 0.02 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); diff --git a/src/modules/sc_pos_control/multicopter_position_control_limits_params.c b/src/modules/sc_pos_control/multicopter_position_control_limits_params.c new file mode 100644 index 000000000000..a60732cfa8c2 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_position_control_limits_params.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Maximum horizontal velocity + * + * Absolute maximum for all velocity controlled modes. + * Any higher value is truncated. + * + * @unit m/s + * @min 0 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.f); + +/** + * Maximum ascent velocity + * + * Absolute maximum for all climb rate controlled modes. + * In manually piloted modes full stick deflection commands this velocity. + * + * For default autonomous velocity see MPC_Z_V_AUTO_UP + * + * @unit m/s + * @min 0.5 + * @max 8 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f); + +/** + * Maximum descent velocity + * + * Absolute maximum for all climb rate controlled modes. + * In manually piloted modes full stick deflection commands this velocity. + * + * For default autonomous velocity see MPC_Z_V_AUTO_UP + * + * @unit m/s + * @min 0.5 + * @max 4 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.5f); + +/** + * Maximum tilt angle in air + * + * Absolute maximum for all velocity or acceleration controlled modes. + * Any higher value is truncated. + * + * @unit deg + * @min 20 + * @max 89 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.f); + +/** + * Maximum tilt during inital takeoff ramp + * + * Tighter tilt limit during takeoff to avoid tip over. + * + * @unit deg + * @min 5 + * @max 89 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.f); + +/** + * Minimum collective thrust in climb rate controlled modes + * + * Too low thrust leads to loss of roll/pitch/yaw torque control authority. + * With airmode enabled this parameters can be set to 0 + * while still keeping torque authority (see MC_AIRMODE). + * + * @unit norm + * @min 0.05 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); + +/** + * Maximum collective thrust in climb rate controlled modes + * + * Limit allowed thrust e.g. for indoor test of overpowered vehicle. + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); diff --git a/src/modules/sc_pos_control/multicopter_position_control_params.c b/src/modules/sc_pos_control/multicopter_position_control_params.c new file mode 100644 index 000000000000..a8c00fb16738 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_position_control_params.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Vertical thrust required to hover + * + * Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). + * Used for initialization of the hover thrust estimator (see MPC_USE_HTE). + * The estimated hover thrust is used as base for zero vertical acceleration in altitude control. + * The hover thrust is important for land detection to work correctly. + * + * @unit norm + * @min 0.1 + * @max 0.8 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); + +/** + * Hover thrust estimator + * + * Disable to use the fixed parameter MPC_THR_HOVER + * Enable to use the hover thrust estimator + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_USE_HTE, 1); + +/** + * Horizontal thrust margin + * + * Margin that is kept for horizontal control when higher priority vertical thrust is saturated. + * To avoid completely starving horizontal control with high vertical error. + * + * @unit norm + * @min 0 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); + +/** + * Numerical velocity derivative low pass cutoff frequency + * + * @unit Hz + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/modules/sc_pos_control/multicopter_position_mode_params.c b/src/modules/sc_pos_control/multicopter_position_mode_params.c new file mode 100644 index 000000000000..56adad1144bf --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_position_mode_params.c @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Position/Altitude mode variant + * + * The supported sub-modes are: + * 0 Sticks directly map to velocity setpoints without smoothing. + * Also applies to vertical direction and Altitude mode. + * Useful for velocity control tuning. + * 3 Sticks map to velocity but with maximum acceleration and jerk limits based on + * jerk optimized trajectory generator (different algorithm than 1). + * 4 Sticks map to acceleration and there's a virtual brake drag + * + * @value 0 Direct velocity + * @value 3 Smoothed velocity + * @value 4 Acceleration based + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_POS_MODE, 4); + +/** + * Maximum horizontal velocity setpoint in Position mode + * + * Must be smaller than MPC_XY_VEL_MAX. + * + * The maximum sideways and backward speed can be set differently + * using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively. + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.f); + +/** + * Maximum sideways velocity in Position mode + * + * If set to a negative value or larger than + * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. + * + * @unit m/s + * @min -1 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.f); + +/** + * Maximum backward velocity in Position mode + * + * If set to a negative value or larger than + * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. + * + * @unit m/s + * @min -1 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); + +/** + * Maximum horizontal acceleration + * + * MPC_POS_MODE + * 1 just deceleration + * 3 acceleration and deceleration + * 4 not used, use MPC_ACC_HOR instead + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); + +/** + * Maximum horizontal and vertical jerk in Position/Altitude mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother motions but limits agility + * (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Only used with smooth MPC_POS_MODE 3 and 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.f); + +/** + * Deadzone for sticks in manual piloted modes + * + * Does not apply to manual throttle and direct attitude piloting by stick. + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); + +/** + * Manual position control stick exponential curve sensitivity + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.6f); + +/** + * Manual control stick vertical exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.6f); + +/** + * Manual control stick yaw rotation exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.6f); diff --git a/src/modules/sc_pos_control/multicopter_responsiveness_params.c b/src/modules/sc_pos_control/multicopter_responsiveness_params.c new file mode 100644 index 000000000000..e0097f6ca015 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_responsiveness_params.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Responsiveness + * + * Changes the overall responsiveness of the vehicle. + * The higher the value, the faster the vehicle will react. + * + * If set to a value greater than zero, other parameters are automatically set (such as + * the acceleration or jerk limits). + * If set to a negative value, the existing individual parameters are used. + * + * @min -1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); + +/** + * Overall Horizontal Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.f); + +/** + * Overall Vertical Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). + * If set to a negative value, the existing individual parameters are used. + * + * @min -3 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.f); diff --git a/src/modules/sc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/sc_pos_control/multicopter_stabilized_mode_params.c new file mode 100644 index 000000000000..f478f9938e27 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_stabilized_mode_params.c @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Maximal tilt angle in Stabilized or Altitude mode + * + * @unit deg + * @min 0 + * @max 90 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.f); + +/** + * Minimum collective thrust in Stabilized mode + * + * The value is mapped to the lowest throttle stick position in Stabilized mode. + * + * Too low collective thrust leads to loss of roll/pitch/yaw torque control authority. + * Airmode is used to keep torque authority with zero thrust (see MC_AIRMODE). + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); + +/** + * Thrust curve mapping in Stabilized Mode + * + * This parameter defines how the throttle stick input is mapped to collective thrust + * in Stabilized mode. + * + * In case the default is used ('Rescale to hover thrust'), the stick input is linearly + * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * + * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful + * in case the hover thrust is very low and the default would lead to too much distortion + * (e.g. if hover thrust is set to 20%, then 80% of the upper thrust range is squeezed into the + * upper half of the stick range). + * + * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * + * @value 0 Rescale to hover thrust + * @value 1 No Rescale + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); diff --git a/src/modules/sc_pos_control/multicopter_takeoff_land_params.c b/src/modules/sc_pos_control/multicopter_takeoff_land_params.c new file mode 100644 index 000000000000..328a8531ad93 --- /dev/null +++ b/src/modules/sc_pos_control/multicopter_takeoff_land_params.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Smooth takeoff ramp time constant + * + * Increasing this value will make climb rate controlled takeoff slower. + * If it's too slow the drone might scratch the ground and tip over. + * A time constant of 0 disables the ramp + * + * @unit s + * @min 0 + * @max 5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.f); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 1 + * @max 5 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); + +/** + * Altitude for 1. step of slow landing (descend) + * + * Below this altitude descending velocity gets limited to a value + * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED" + * Value needs to be higher than "MPC_LAND_ALT2" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.f); + +/** + * Altitude for 2. step of slow landing (landing) + * + * Below this altitude descending velocity gets + * limited to "MPC_LAND_SPEED" + * Value needs to be lower than "MPC_LAND_ALT1" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.f); + +/** + * Altitude for 3. step of slow landing + * + * Below this altitude descending velocity gets + * limited to "MPC_LAND_CRWL", if LIDAR available. + * No effect if LIDAR not available + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.f); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.6 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); + +/** + * Land crawl descend rate + * + * Used below MPC_LAND_ALT3 if distance sensor data is availabe. + * + * @unit m/s + * @min 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f); diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index f3ba6915b44d..a60bd84ce35f 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -53,6 +53,12 @@ publications: - topic: /fmu/out/actuator_outputs type: px4_msgs::msg::ActuatorOutputs + - topic: /fmu/out/vehicle_torque_setpoint + type: px4_msgs::msg::VehicleTorqueSetpoint + + - topic: /fmu/out/vehicle_torque_setpoint + type: px4_msgs::msg::VehicleThrustSetpoint + - topic: /fmu/out/log_message type: px4_msgs::msg::LogMessage @@ -108,3 +114,9 @@ subscriptions: - topic: /fmu/in/thruster_command type: px4_msgs::msg::ThrusterCommand + + - topic: /fmu/in/vehicle_torque_setpoint + type: px4_msgs::msg::VehicleTorqueSetpoint + + - topic: /fmu/in/vehicle_thrust_setpoint + type: px4_msgs::msg::VehicleThrustSetpoint From 2337f2581ae158b87e641cb82e7fa4da726fc9f6 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 25 Apr 2024 22:43:43 +0200 Subject: [PATCH 42/77] add: params --- .../SpacecraftPositionControl.cpp | 1 - .../SpacecraftPositionControl.hpp | 56 +++----- .../sc_pos_control/sc_pos_control_params.c | 128 ++++++++++++++++++ 3 files changed, 144 insertions(+), 41 deletions(-) create mode 100644 src/modules/sc_pos_control/sc_pos_control_params.c diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index 986b79e59dc4..f5aa4d428370 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -358,7 +358,6 @@ void SpacecraftPositionControl::Run() if (_vehicle_control_mode.flag_control_position_enabled && (_setpoint.timestamp >= _time_position_control_enabled)) { - // Allow ramping from zero thrust on takeoff _control.setThrustLimits(0.0, _param_mpc_thr_max.get()); _control.setVelocityLimits( diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index 0b95f2fd95f1..e4e7189a7f4e 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -55,13 +55,10 @@ #include #include #include -#include #include #include #include -#include #include -#include #include #include @@ -107,40 +104,23 @@ class SpacecraftPositionControl : public ModuleBase, DEFINE_PARAMETERS( // Position Control - (ParamFloat) _param_mpc_xy_p, - (ParamFloat) _param_mpc_z_p, - (ParamFloat) _param_mpc_xy_vel_p_acc, - (ParamFloat) _param_mpc_xy_vel_i_acc, - (ParamFloat) _param_mpc_xy_vel_d_acc, - (ParamFloat) _param_mpc_z_vel_p_acc, - (ParamFloat) _param_mpc_z_vel_i_acc, - (ParamFloat) _param_mpc_z_vel_d_acc, - (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_z_v_auto_up, - (ParamFloat) _param_mpc_z_vel_max_up, - (ParamFloat) _param_mpc_z_v_auto_dn, - (ParamFloat) _param_mpc_z_vel_max_dn, - - // Takeoff / Land - (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_vel_man_back, - (ParamFloat) _param_mpc_vel_man_side, - (ParamFloat) _param_mpc_xy_cruise, - (ParamInt) _param_mpc_pos_mode, - (ParamFloat) _param_mpc_thr_max, - + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_sys_vehicle_resp, - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_acc_down_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_hor_max, - (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_man_y_max, - (ParamFloat) _param_mpc_man_y_tau, - - (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_thr_max, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -154,10 +134,6 @@ class SpacecraftPositionControl : public ModuleBase, /** Timeout in us for trajectory data to get considered invalid */ static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; - static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf - - SlewRate _tilt_limit_slew_rate; - uint8_t _vxy_reset_counter{0}; uint8_t _vz_reset_counter{0}; uint8_t _xy_reset_counter{0}; diff --git a/src/modules/sc_pos_control/sc_pos_control_params.c b/src/modules/sc_pos_control/sc_pos_control_params.c new file mode 100644 index 000000000000..ef0748748630 --- /dev/null +++ b/src/modules/sc_pos_control/sc_pos_control_params.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Proportional gain for position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_P, 0.95f); + +/** + * Proportional gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_P, 4.f); + +/** + * Integral gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_I, 4.f); + +/** + * Derivative gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_D, 4.f); + +/** + * Maximum velocity + * + * Absolute maximum for all velocity controlled modes. + * Any higher value is truncated. + * + * @unit m/s + * @min 0 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MAX, 12.f); + +/** + * Default velocity in autonomous modes + * + * e.g. in Missions, RTL, Goto if the waypoint does not specify differently + * + * @unit m/s + * @min 3 + * @max 20 + * @decimal 0 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_CRUISE, 5.f); + +/* + TODO: + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_thr_max, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all +*/ From f246f84963fb64c4e855f1c9d4098ffa6c284878 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 26 Apr 2024 16:45:54 +0200 Subject: [PATCH 43/77] fix: major changes to position control for compatibility with spacecraft --- boards/px4/sitl/default.px4board | 3 +- msg/TrajectorySetpoint.msg | 3 + .../PositionControl/PositionControl.cpp | 121 +++-------- .../PositionControl/PositionControl.hpp | 41 +--- .../SpacecraftPositionControl.cpp | 117 +++-------- .../SpacecraftPositionControl.hpp | 28 ++- .../sc_pos_control/Takeoff/CMakeLists.txt | 41 ---- .../sc_pos_control/Takeoff/Takeoff.cpp | 134 ------------ .../sc_pos_control/Takeoff/Takeoff.hpp | 101 --------- .../sc_pos_control/Takeoff/TakeoffTest.cpp | 79 ------- .../multicopter_altitude_mode_params.c | 119 ----------- .../multicopter_autonomous_params.c | 162 -------------- .../multicopter_nudging_params.c | 65 ------ ...multicopter_position_control_gain_params.c | 137 ------------ ...lticopter_position_control_limits_params.c | 140 ------------- .../multicopter_position_control_params.c | 87 -------- .../multicopter_position_mode_params.c | 198 ------------------ .../multicopter_responsiveness_params.c | 80 ------- .../multicopter_stabilized_mode_params.c | 95 --------- .../multicopter_takeoff_land_params.c | 124 ----------- .../sc_pos_control/sc_pos_control_params.c | 185 ++++++++++++++-- 21 files changed, 243 insertions(+), 1817 deletions(-) delete mode 100644 src/modules/sc_pos_control/Takeoff/CMakeLists.txt delete mode 100644 src/modules/sc_pos_control/Takeoff/Takeoff.cpp delete mode 100644 src/modules/sc_pos_control/Takeoff/Takeoff.hpp delete mode 100644 src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp delete mode 100644 src/modules/sc_pos_control/multicopter_altitude_mode_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_autonomous_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_nudging_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_position_control_gain_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_position_control_limits_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_position_control_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_position_mode_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_responsiveness_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_stabilized_mode_params.c delete mode 100644 src/modules/sc_pos_control/multicopter_takeoff_land_params.c diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 84f34f2705e5..b9d2ba33a21a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -23,8 +23,10 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y CONFIG_MODULES_SC_RATE_CONTROL=y CONFIG_MODULES_SC_ATT_CONTROL=y +CONFIG_MODULES_SC_POS_CONTROL=y CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y @@ -32,7 +34,6 @@ CONFIG_MODULES_REPLAY=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y -CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/msg/TrajectorySetpoint.msg b/msg/TrajectorySetpoint.msg index 4a88c86769a6..565b14de561e 100644 --- a/msg/TrajectorySetpoint.msg +++ b/msg/TrajectorySetpoint.msg @@ -10,6 +10,9 @@ float32[3] position # in meters float32[3] velocity # in meters/second float32[3] acceleration # in meters/second^2 float32[3] jerk # in meters/second^3 (for logging only) +float32[4] attitude # in quaternions +float32[3] angular_velocity # in rad/s^2 float32 yaw # euler angle of desired attitude in radians -PI..+PI float32 yawspeed # angular velocity around NED frame z-axis in radians/second + diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index 1371523c8d5d..aaf4809ed0f4 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -53,46 +53,22 @@ void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, con _gain_vel_d = D; } -void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down) +void PositionControl::setVelocityLimits(const float vel_limit) { - _lim_vel_horizontal = vel_horizontal; - _lim_vel_up = vel_up; - _lim_vel_down = vel_down; + _lim_vel = vel_limit; } void PositionControl::setThrustLimits(const float min, const float max) { // make sure there's always enough thrust vector length to infer the attitude - _lim_thr_min = math::max(min, 10e-4f); + _lim_thr_min = min; _lim_thr_max = max; } -void PositionControl::setHorizontalThrustMargin(const float margin) -{ - _lim_thr_xy_margin = margin; -} - -void PositionControl::updateHoverThrust(const float hover_thrust_new) -{ - // Given that the equation for thrust is T = a_sp * Th / g - Th - // with a_sp = desired acceleration, Th = hover thrust and g = gravity constant, - // we want to find the acceleration that needs to be added to the integrator in order obtain - // the same thrust after replacing the current hover thrust by the new one. - // T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th - // so a_sp' = (a_sp - g) * Th / Th' + g - // we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th' - const float previous_hover_thrust = _hover_thrust; - setHoverThrust(hover_thrust_new); - - _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust - + CONSTANTS_ONE_G - _acc_sp(2); -} - void PositionControl::setState(const PositionControlStates &states) { _pos = states.position; _vel = states.velocity; - _yaw = states.yaw; _vel_dot = states.acceleration; } @@ -101,8 +77,7 @@ void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) _pos_sp = Vector3f(setpoint.position); _vel_sp = Vector3f(setpoint.velocity); _acc_sp = Vector3f(setpoint.acceleration); - _yaw_sp = setpoint.yaw; - _yawspeed_sp = setpoint.yawspeed; + _quat_sp = Quatf(setpoint.attitude); } bool PositionControl::update(const float dt) @@ -112,9 +87,6 @@ bool PositionControl::update(const float dt) if (valid) { _positionControl(); _velocityControl(dt); - - _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; - _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control } // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong @@ -130,11 +102,10 @@ void PositionControl::_positionControl() // make sure there are no NAN elements for further reference while constraining ControlMath::setZeroIfNanVector3f(vel_sp_position); - // Constrain horizontal velocity by prioritizing the velocity component along the - // the desired position setpoint over the feed-forward term. - _vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal); - // Constrain velocity in z-direction. - _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down); + // Constrain velocity setpoints + _vel_sp(0) = math::constrain(_vel_sp(0), -_lim_vel, _lim_vel); + _vel_sp(1) = math::constrain(_vel_sp(1), -_lim_vel, _lim_vel); + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel, _lim_vel); } void PositionControl::_velocityControl(const float dt) @@ -149,71 +120,19 @@ void PositionControl::_velocityControl(const float dt) // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); - _accelerationControl(); - - // Integrator anti-windup in vertical direction - if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) || - (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) { - vel_error(2) = 0.f; - } - - // Prioritize vertical control while keeping a horizontal margin - const Vector2f thrust_sp_xy(_thr_sp); - const float thrust_sp_xy_norm = thrust_sp_xy.norm(); - const float thrust_max_squared = math::sq(_lim_thr_max); - - // Determine how much vertical thrust is left keeping horizontal margin - const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin); - const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust); - - // Saturate maximal vertical thrust - _thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared)); - - // Determine how much horizontal thrust is left after prioritizing vertical control - const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2)); - float thrust_max_xy = 0; - - if (thrust_max_xy_squared > 0) { - thrust_max_xy = sqrtf(thrust_max_xy_squared); - } - - // Saturate thrust in horizontal direction - if (thrust_sp_xy_norm > thrust_max_xy) { - _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; - } - - // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output - // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 - const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); - const float arw_gain = 2.f / _gain_vel_p(0); - - // The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently). - // The ARW loop needs to run if the signal is saturated only. - const Vector2f acc_sp_xy = _acc_sp.xy(); - const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared()) - ? acc_sp_xy_produced - : acc_sp_xy; - vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy); + // Accelaration to Thrust + _thr_sp = _acc_sp; + _thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max); + _thr_sp(1) = math::constrain(_thr_sp(1), -_lim_thr_max, _lim_thr_max); + _thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, _lim_thr_max); // Make sure integral doesn't get NAN ControlMath::setZeroIfNanVector3f(vel_error); + // Update integral part of velocity control _vel_int += vel_error.emult(_gain_vel_i) * dt; } -void PositionControl::_accelerationControl() -{ - // Assume standard acceleration due to gravity in vertical direction for attitude generation - Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); - ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); - // Scale thrust assuming hover thrust produces standard gravity - float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; - // Project thrust to planned body attitude - collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); - collective_thrust = math::min(collective_thrust, -_lim_thr_min); - _thr_sp = body_z * collective_thrust; -} - bool PositionControl::_inputValid() { bool valid = true; @@ -258,6 +177,14 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const { - ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); - attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; + // Set thrust setpoint + attitude_setpoint.thrust_body[0] = _thr_sp(0); + attitude_setpoint.thrust_body[1] = _thr_sp(1); + attitude_setpoint.thrust_body[2] = _thr_sp(2); + + // Bypass attitude control by giving same attitude setpoint to att control + attitude_setpoint.q_d[0] = _quat_sp(0); + attitude_setpoint.q_d[1] = _quat_sp(1); + attitude_setpoint.q_d[2] = _quat_sp(2); + attitude_setpoint.q_d[3] = _quat_sp(3); } diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.hpp b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp index 4eb909e1fa3e..a3d0eaa9fe6f 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp @@ -95,11 +95,9 @@ class PositionControl /** * Set the maximum velocity to execute with feed forward and position control - * @param vel_horizontal horizontal velocity limit - * @param vel_up upwards velocity limit - * @param vel_down downwards velocity limit + * @param vel_limit velocity limit */ - void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down); + void setVelocityLimits(const float vel_limit); /** * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller @@ -108,31 +106,6 @@ class PositionControl */ void setThrustLimits(const float min, const float max); - /** - * Set margin that is kept for horizontal control when prioritizing vertical thrust - * @param margin of normalized thrust that is kept for horizontal control e.g. 0.3 - */ - void setHorizontalThrustMargin(const float margin); - - /** - * Set the maximum tilt angle in radians the output attitude is allowed to have - * @param tilt angle in radians from level orientation - */ - void setTiltLimit(const float tilt) { _lim_tilt = tilt; } - - /** - * Set the normalized hover thrust - * @param hover_thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation - */ - void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, HOVER_THRUST_MIN, HOVER_THRUST_MAX); } - - /** - * Update the hover thrust without immediately affecting the output - * by adjusting the integrator. This prevents propagating the dynamics - * of the hover thrust signal directly to the output of the controller. - */ - void updateHoverThrust(const float hover_thrust_new); - /** * Pass the current vehicle state to the controller * @param PositionControlStates structure @@ -193,7 +166,6 @@ class PositionControl void _positionControl(); ///< Position proportional control void _velocityControl(const float dt); ///< Velocity PID control - void _accelerationControl(); ///< Acceleration setpoint processing // Gains matrix::Vector3f _gain_pos_p; ///< Position control proportional gain @@ -202,15 +174,9 @@ class PositionControl matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain // Limits - float _lim_vel_horizontal{}; ///< Horizontal velocity limit with feed forward and position control - float _lim_vel_up{}; ///< Upwards velocity limit with feed forward and position control - float _lim_vel_down{}; ///< Downwards velocity limit with feed forward and position control + float _lim_vel{}; ///< Horizontal velocity limit with feed forward and position control float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 - float _lim_thr_xy_margin{}; ///< Margin to keep for horizontal control when saturating prioritized vertical thrust - float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have - - float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation // States matrix::Vector3f _pos; /**< current position */ @@ -224,6 +190,7 @@ class PositionControl matrix::Vector3f _vel_sp; /**< desired velocity */ matrix::Vector3f _acc_sp; /**< desired acceleration */ matrix::Vector3f _thr_sp; /**< desired thrust */ + matrix::Quatf _quat_sp; /**< desired attitude */ float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ }; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index f5aa4d428370..ace1e9d6c84d 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -51,7 +51,6 @@ SpacecraftPositionControl::SpacecraftPositionControl() : _vel_z_deriv(this, "VELD") { parameters_update(true); - _tilt_limit_slew_rate.setSlewRate(.2f); } SpacecraftPositionControl::~SpacecraftPositionControl() @@ -90,115 +89,57 @@ void SpacecraftPositionControl::parameters_update(bool force) // make it less sensitive at the lower end float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); - num_changed += _param_mpc_acc_hor.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); - num_changed += _param_mpc_acc_hor_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); if (responsiveness > 0.6f) { num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); - } else { num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); } - - num_changed += _param_mpc_acc_down_max.commit_no_notification(math::lerp(0.8f, 15.f, responsiveness)); - num_changed += _param_mpc_acc_up_max.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); } - if (_param_mpc_xy_vel_all.get() >= 0.f) { - float xy_vel = _param_mpc_xy_vel_all.get(); - num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel); - num_changed += _param_mpc_vel_man_back.commit_no_notification(-1.f); - num_changed += _param_mpc_vel_man_side.commit_no_notification(-1.f); - num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel); - num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel); - } - - if (_param_mpc_z_vel_all.get() >= 0.f) { - float z_vel = _param_mpc_z_vel_all.get(); - num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel); - num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel); - num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f); - num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f); + if (_param_mpc_vel_all.get() >= 0.f) { + float all_vel = _param_mpc_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_cruise.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_max.commit_no_notification(all_vel); } if (num_changed > 0) { param_notify_changes(); } - _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); + _control.setPositionGains(Vector3f(_param_mpc_pos_p.get(), _param_mpc_pos_p.get(), _param_mpc_pos_p.get())); _control.setVelocityGains( - Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), - Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), - Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); + Vector3f(_param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get()), + Vector3f(_param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get()), + Vector3f(_param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get())); // Check that the design parameters are inside the absolute maximum constraints - if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { - _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get()); - _param_mpc_xy_cruise.commit(); + if (_param_mpc_vel_cruise.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_cruise.set(_param_mpc_vel_max.get()); + _param_mpc_vel_cruise.commit(); mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t"); /* EVENT - * @description MPC_XY_CRUISE is set to {1:.0}. + * @description SPC_VEL_CRUISE is set to {1:.0}. */ - events::send(events::ID("mc_pos_ctrl_cruise_set"), events::Log::Warning, - "Cruise speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get()); + events::send(events::ID("sc_pos_ctrl_cruise_set"), events::Log::Warning, + "Cruise speed has been constrained by maximum speed", _param_mpc_vel_max.get()); } - if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) { - _param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get()); + if (_param_mpc_vel_manual.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_vel_max.get()); _param_mpc_vel_manual.commit(); mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t"); /* EVENT - * @description MPC_VEL_MANUAL is set to {1:.0}. + * @description SPC_VEL_MANUAL is set to {1:.0}. */ - events::send(events::ID("mc_pos_ctrl_man_vel_set"), events::Log::Warning, - "Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get()); - } - - if (_param_mpc_vel_man_back.get() > _param_mpc_vel_manual.get()) { - _param_mpc_vel_man_back.set(_param_mpc_vel_manual.get()); - _param_mpc_vel_man_back.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Manual backward speed has been constrained by forward speed\t"); - /* EVENT - * @description MPC_VEL_MAN_BACK is set to {1:.0}. - */ - events::send(events::ID("mc_pos_ctrl_man_vel_back_set"), events::Log::Warning, - "Manual backward speed has been constrained by forward speed", _param_mpc_vel_manual.get()); - } - - if (_param_mpc_vel_man_side.get() > _param_mpc_vel_manual.get()) { - _param_mpc_vel_man_side.set(_param_mpc_vel_manual.get()); - _param_mpc_vel_man_side.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Manual sideways speed has been constrained by forward speed\t"); - /* EVENT - * @description MPC_VEL_MAN_SIDE is set to {1:.0}. - */ - events::send(events::ID("mc_pos_ctrl_man_vel_side_set"), events::Log::Warning, - "Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.get()); - } - - if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) { - _param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get()); - _param_mpc_z_v_auto_up.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t"); - /* EVENT - * @description MPC_Z_V_AUTO_UP is set to {1:.0}. - */ - events::send(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning, - "Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); - } - - if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) { - _param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get()); - _param_mpc_z_v_auto_dn.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t"); - /* EVENT - * @description MPC_Z_V_AUTO_DN is set to {1:.0}. - */ - events::send(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning, - "Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); + events::send(events::ID("sc_pos_ctrl_man_vel_set"), events::Log::Warning, + "Manual speed has been constrained by maximum speed", _param_mpc_vel_max.get()); } } } @@ -360,10 +301,7 @@ void SpacecraftPositionControl::Run() _control.setThrustLimits(0.0, _param_mpc_thr_max.get()); - _control.setVelocityLimits( - max_speed_xy, - math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit - math::max(speed_down, 0.f)); + _control.setVelocityLimits(_param_mpc_vel_max.get()); _control.setInputSetpoint(_setpoint); @@ -371,11 +309,8 @@ void SpacecraftPositionControl::Run() // Run position control if (!_control.update(dt)) { - // Failsafe - _vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints - _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true)); - _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); + _control.setVelocityLimits(_param_mpc_vel_max.get()); _control.update(dt); } @@ -460,8 +395,8 @@ int SpacecraftPositionControl::print_usage(const char *reason) R"DESCR_STR( ### Description The controller has two loops: a P loop for position error and a PID loop for velocity error. - Output of the velocity controller is thrust vector that is split to thrust direction - (i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself). + Output of the velocity controller is thrust vector in the body frame, and the same target attitude + received on the trajectory setpoint as quaternion. The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and logging. diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index e4e7189a7f4e..b09060fec2a7 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -38,7 +38,6 @@ #pragma once #include "PositionControl/PositionControl.hpp" -#include "Takeoff/Takeoff.hpp" #include #include @@ -104,23 +103,22 @@ class SpacecraftPositionControl : public ModuleBase, DEFINE_PARAMETERS( // Position Control - (ParamFloat) _param_mpc_xy_p, - (ParamFloat) _param_mpc_xy_vel_p_acc, - (ParamFloat) _param_mpc_xy_vel_p_acc, - (ParamFloat) _param_mpc_xy_vel_p_acc, - (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_xy_cruise, - (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_pos_p, + (ParamFloat) _param_mpc_vel_p_acc, + (ParamFloat) _param_mpc_vel_i_acc, + (ParamFloat) _param_mpc_vel_d_acc, + (ParamFloat) _param_mpc_vel_all, + (ParamFloat) _param_mpc_vel_max, + (ParamFloat) _param_mpc_vel_cruise, (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_thr_max, - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc, + (ParamFloat) _param_mpc_acc_max, (ParamFloat) _param_mpc_man_y_max, (ParamFloat) _param_mpc_man_y_tau, - (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_thr_max ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ diff --git a/src/modules/sc_pos_control/Takeoff/CMakeLists.txt b/src/modules/sc_pos_control/Takeoff/CMakeLists.txt deleted file mode 100644 index a4c129aed661..000000000000 --- a/src/modules/sc_pos_control/Takeoff/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(Takeoff - Takeoff.cpp - Takeoff.hpp -) -target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(Takeoff PUBLIC hysteresis) - -px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/sc_pos_control/Takeoff/Takeoff.cpp b/src/modules/sc_pos_control/Takeoff/Takeoff.cpp deleted file mode 100644 index 2e4fbc1f4047..000000000000 --- a/src/modules/sc_pos_control/Takeoff/Takeoff.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Takeoff.cpp - */ - -#include "Takeoff.hpp" -#include -#include - -void TakeoffHandling::generateInitialRampValue(float velocity_p_gain) -{ - velocity_p_gain = math::max(velocity_p_gain, 0.01f); - _takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain; -} - -void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us) -{ - _spoolup_time_hysteresis.set_state_and_update(armed, now_us); - - switch (_takeoff_state) { - case TakeoffState::disarmed: - if (armed) { - _takeoff_state = TakeoffState::spoolup; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::spoolup: - if (_spoolup_time_hysteresis.get_state()) { - _takeoff_state = TakeoffState::ready_for_takeoff; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::ready_for_takeoff: - if (want_takeoff) { - _takeoff_state = TakeoffState::rampup; - _takeoff_ramp_progress = 0.f; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::rampup: - if (_takeoff_ramp_progress >= 1.f) { - _takeoff_state = TakeoffState::flight; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::flight: - if (landed) { - _takeoff_state = TakeoffState::ready_for_takeoff; - } - - break; - - default: - break; - } - - if (armed && skip_takeoff) { - _takeoff_state = TakeoffState::flight; - } - - // TODO: need to consider free fall here - if (!armed) { - _takeoff_state = TakeoffState::disarmed; - } -} - -float TakeoffHandling::updateRamp(const float dt, const float takeoff_desired_vz) -{ - float upwards_velocity_limit = takeoff_desired_vz; - - if (_takeoff_state < TakeoffState::rampup) { - upwards_velocity_limit = _takeoff_ramp_vz_init; - } - - if (_takeoff_state == TakeoffState::rampup) { - if (_takeoff_ramp_time > dt) { - _takeoff_ramp_progress += dt / _takeoff_ramp_time; - - } else { - _takeoff_ramp_progress = 1.f; - } - - if (_takeoff_ramp_progress < 1.f) { - upwards_velocity_limit = _takeoff_ramp_vz_init + _takeoff_ramp_progress * (takeoff_desired_vz - _takeoff_ramp_vz_init); - } - } - - return upwards_velocity_limit; -} diff --git a/src/modules/sc_pos_control/Takeoff/Takeoff.hpp b/src/modules/sc_pos_control/Takeoff/Takeoff.hpp deleted file mode 100644 index 1de486e3253b..000000000000 --- a/src/modules/sc_pos_control/Takeoff/Takeoff.hpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Takeoff.hpp - * - * A class handling all takeoff states and a smooth ramp up of the motors. - */ - -#pragma once - -#include -#include -#include - -using namespace time_literals; - -enum class TakeoffState { - disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED, - spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP, - ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF, - rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP, - flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT -}; - -class TakeoffHandling -{ -public: - TakeoffHandling() = default; - ~TakeoffHandling() = default; - - // initialize parameters - void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } - void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } - - /** - * Calculate a vertical velocity to initialize the takeoff ramp - * that when passed to the velocity controller results in a zero throttle setpoint. - * @param hover_thrust normalized thrsut value with which the vehicle hovers - * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust - */ - void generateInitialRampValue(const float velocity_p_gain); - - /** - * Update the state for the takeoff. - * Has to be called also when not flying altitude controlled to skip the takeoff and not do it in flight when switching mode. - */ - void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us); - - /** - * Update and return the velocity constraint ramp value during takeoff. - * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved. - * Returns zero on the ground and takeoff_desired_vz in flight. - * @param dt time in seconds since the last call/loop iteration - * @param takeoff_desired_vz end value for the velocity ramp - * @return true if setpoint has updated correctly - */ - float updateRamp(const float dt, const float takeoff_desired_vz); - - TakeoffState getTakeoffState() { return _takeoff_state; } - -private: - TakeoffState _takeoff_state = TakeoffState::disarmed; - - systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true COM_SPOOLUP_TIME seconds after the vehicle was armed - - float _takeoff_ramp_time{0.f}; - float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust - float _takeoff_ramp_progress{0.f}; ///< asecnding from 0 to 1 -}; diff --git a/src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp deleted file mode 100644 index d718ef687bbe..000000000000 --- a/src/modules/sc_pos_control/Takeoff/TakeoffTest.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include - -TEST(TakeoffTest, Initialization) -{ - TakeoffHandling takeoff; - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); -} - -TEST(TakeoffTest, RegularTakeoffRamp) -{ - TakeoffHandling takeoff; - takeoff.setSpoolupTime(1.f); - takeoff.setTakeoffRampTime(2.f); - takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); - - // disarmed, landed, don't want takeoff - takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); - - // armed, not landed anymore, don't want takeoff - takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); - - // armed, not landed, don't want takeoff yet, spoolup time passed - takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); - - // armed, not landed, want takeoff - takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup); - - // armed, not landed, want takeoff, ramping up - takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); - - // armed, not landed, want takeoff, rampup time passed - takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight); -} diff --git a/src/modules/sc_pos_control/multicopter_altitude_mode_params.c b/src/modules/sc_pos_control/multicopter_altitude_mode_params.c deleted file mode 100644 index ecebaa9e01f9..000000000000 --- a/src/modules/sc_pos_control/multicopter_altitude_mode_params.c +++ /dev/null @@ -1,119 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Maximum upwards acceleration in climb rate controlled modes - * - * @unit m/s^2 - * @min 2 - * @max 15 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.f); - -/** - * Maximum downwards acceleration in climb rate controlled modes - * - * @unit m/s^2 - * @min 2 - * @max 15 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.f); - -/** - * Manual yaw rate input filter time constant - * - * Not used in Stabilized mode - * Setting this parameter to 0 disables the filter - * - * @unit s - * @min 0 - * @max 5 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); - -/** - * Altitude reference mode - * - * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in - * flight due to sensor drift. - * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down - * with terrain height variation. Requires a distance to ground sensor. The height controller will - * revert to using height above origin if the distance to ground estimate becomes invalid as indicated - * by the local_position.distance_bottom_valid message being false. - * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative - * to earth frame origin when moving horizontally. - * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. - * - * @min 0 - * @max 2 - * @value 0 Altitude following - * @value 1 Terrain following - * @value 2 Terrain hold - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); - -/** - * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) - * - * Only used with MPC_POS_MODE 0 or MPC_ALT_MODE 2 - * - * @unit m/s - * @min 0 - * @max 3 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f); - -/** - * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) - * - * Only used with MPC_ALT_MODE 1 - * - * @unit m/s - * @min 0 - * @max 3 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f); diff --git a/src/modules/sc_pos_control/multicopter_autonomous_params.c b/src/modules/sc_pos_control/multicopter_autonomous_params.c deleted file mode 100644 index 8596cedd8b7b..000000000000 --- a/src/modules/sc_pos_control/multicopter_autonomous_params.c +++ /dev/null @@ -1,162 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Default horizontal velocity in autonomous modes - * - * e.g. in Missions, RTL, Goto if the waypoint does not specify differently - * - * @unit m/s - * @min 3 - * @max 20 - * @decimal 0 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.f); - -/** - * Ascent velocity in autonomous modes - * - * For manually controlled modes and offboard see MPC_Z_VEL_MAX_UP - * - * @unit m/s - * @min 0.5 - * @max 8 - * @decimal 1 - * @increment 0.5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f); - -/** - * Descent velocity in autonomous modes - * - * For manual modes and offboard, see MPC_Z_VEL_MAX_DN - * - * @unit m/s - * @min 0.5 - * @max 4 - * @decimal 1 - * @increment 0.5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f); - -/** - * Acceleration for autonomous and for manual modes - * - * When piloting manually, this parameter is only used in MPC_POS_MODE 4. - * - * @unit m/s^2 - * @min 2 - * @max 15 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.f); - -/** - * Jerk limit in autonomous modes - * - * Limit the maximum jerk of the vehicle (how fast the acceleration can change). - * A lower value leads to smoother vehicle motions but also limited agility. - * - * @unit m/s^3 - * @min 1 - * @max 80 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.f); - -/** - * Proportional gain for horizontal trajectory position error - * - * @min 0.1 - * @max 1 - * @decimal 1 - * @increment 0.1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); - -/** - * Maximum horizontal error allowed by the trajectory generator - * - * The integration speed of the trajectory setpoint is linearly - * reduced with the horizontal position tracking error. When the - * error is above this parameter, the integration of the - * trajectory is stopped to wait for the drone. - * - * This value can be adjusted depending on the tracking - * capabilities of the vehicle. - * - * @min 0.1 - * @max 10 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f); - -/** - * Max yaw rate in autonomous modes - * - * Limits the rate of change of the yaw setpoint to avoid large - * control output and mixer saturation. - * - * @unit deg/s - * @min 0 - * @max 360 - * @decimal 0 - * @increment 5 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f); - -/** - * Heading behavior in autonomous modes - * - * @min 0 - * @max 4 - * @value 0 towards waypoint - * @value 1 towards home - * @value 2 away from home - * @value 3 along trajectory - * @value 4 towards waypoint (yaw first) - * @group Mission - */ -PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); diff --git a/src/modules/sc_pos_control/multicopter_nudging_params.c b/src/modules/sc_pos_control/multicopter_nudging_params.c deleted file mode 100644 index c431e88fd04c..000000000000 --- a/src/modules/sc_pos_control/multicopter_nudging_params.c +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Enable nudging based on user input during autonomous land routine - * - * Using stick input the vehicle can be moved horizontally and yawed. - * The descend speed is amended: - * stick full up - 0 - * stick centered - MPC_LAND_SPEED - * stick full down - 2 * MPC_LAND_SPEED - * - * Manual override during auto modes has to be disabled to use this feature (see COM_RC_OVERRIDE). - * - * @min 0 - * @max 1 - * @value 0 Nudging disabled - * @value 1 Nudging enabled - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); - -/** - * User assisted landing radius - * - * When nudging is enabled (see MPC_LAND_RC_HELP), this controls - * the maximum allowed horizontal displacement from the original landing point. - * - * @unit m - * @min 0 - * @decimal 0 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); diff --git a/src/modules/sc_pos_control/multicopter_position_control_gain_params.c b/src/modules/sc_pos_control/multicopter_position_control_gain_params.c deleted file mode 100644 index df36d1f733f9..000000000000 --- a/src/modules/sc_pos_control/multicopter_position_control_gain_params.c +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Proportional gain for vertical position error - * - * Defined as corrective velocity in m/s per m position error - * - * @min 0.1 - * @max 1.5 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_P, 1.f); - -/** - * Proportional gain for horizontal position error - * - * Defined as corrective velocity in m/s per m position error - * - * @min 0 - * @max 2 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); - -/** - * Proportional gain for vertical velocity error - * - * Defined as corrective acceleration in m/s^2 per m/s velocity error - * - * @min 2 - * @max 15 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.f); - -/** - * Proportional gain for horizontal velocity error - * - * Defined as corrective acceleration in m/s^2 per m/s velocity error - * - * @min 1.2 - * @max 5 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); - -/** - * Integral gain for vertical velocity error - * - * Defined as corrective acceleration in m/s^2 per m velocity integral - * - * @min 0.2 - * @max 3 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.f); - -/** - * Integral gain for horizontal velocity error - * - * Defined as correction acceleration in m/s^2 per m velocity integral - * Allows to eliminate steady state errors in disturbances like wind. - * - * @min 0 - * @max 60 - * @decimal 2 - * @increment 0.02 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); - -/** - * Differential gain for vertical velocity error - * - * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative - * - * @min 0 - * @max 2 - * @decimal 2 - * @increment 0.02 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f); - -/** - * Differential gain for horizontal velocity error - * - * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative - * - * @min 0.1 - * @max 2 - * @decimal 2 - * @increment 0.02 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); diff --git a/src/modules/sc_pos_control/multicopter_position_control_limits_params.c b/src/modules/sc_pos_control/multicopter_position_control_limits_params.c deleted file mode 100644 index a60732cfa8c2..000000000000 --- a/src/modules/sc_pos_control/multicopter_position_control_limits_params.c +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Maximum horizontal velocity - * - * Absolute maximum for all velocity controlled modes. - * Any higher value is truncated. - * - * @unit m/s - * @min 0 - * @max 20 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.f); - -/** - * Maximum ascent velocity - * - * Absolute maximum for all climb rate controlled modes. - * In manually piloted modes full stick deflection commands this velocity. - * - * For default autonomous velocity see MPC_Z_V_AUTO_UP - * - * @unit m/s - * @min 0.5 - * @max 8 - * @increment 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f); - -/** - * Maximum descent velocity - * - * Absolute maximum for all climb rate controlled modes. - * In manually piloted modes full stick deflection commands this velocity. - * - * For default autonomous velocity see MPC_Z_V_AUTO_UP - * - * @unit m/s - * @min 0.5 - * @max 4 - * @increment 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.5f); - -/** - * Maximum tilt angle in air - * - * Absolute maximum for all velocity or acceleration controlled modes. - * Any higher value is truncated. - * - * @unit deg - * @min 20 - * @max 89 - * @decimal 0 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.f); - -/** - * Maximum tilt during inital takeoff ramp - * - * Tighter tilt limit during takeoff to avoid tip over. - * - * @unit deg - * @min 5 - * @max 89 - * @decimal 0 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.f); - -/** - * Minimum collective thrust in climb rate controlled modes - * - * Too low thrust leads to loss of roll/pitch/yaw torque control authority. - * With airmode enabled this parameters can be set to 0 - * while still keeping torque authority (see MC_AIRMODE). - * - * @unit norm - * @min 0.05 - * @max 0.5 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); - -/** - * Maximum collective thrust in climb rate controlled modes - * - * Limit allowed thrust e.g. for indoor test of overpowered vehicle. - * - * @unit norm - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.05 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); diff --git a/src/modules/sc_pos_control/multicopter_position_control_params.c b/src/modules/sc_pos_control/multicopter_position_control_params.c deleted file mode 100644 index a8c00fb16738..000000000000 --- a/src/modules/sc_pos_control/multicopter_position_control_params.c +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Vertical thrust required to hover - * - * Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). - * Used for initialization of the hover thrust estimator (see MPC_USE_HTE). - * The estimated hover thrust is used as base for zero vertical acceleration in altitude control. - * The hover thrust is important for land detection to work correctly. - * - * @unit norm - * @min 0.1 - * @max 0.8 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); - -/** - * Hover thrust estimator - * - * Disable to use the fixed parameter MPC_THR_HOVER - * Enable to use the hover thrust estimator - * - * @boolean - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_USE_HTE, 1); - -/** - * Horizontal thrust margin - * - * Margin that is kept for horizontal control when higher priority vertical thrust is saturated. - * To avoid completely starving horizontal control with high vertical error. - * - * @unit norm - * @min 0 - * @max 0.5 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); - -/** - * Numerical velocity derivative low pass cutoff frequency - * - * @unit Hz - * @min 0 - * @max 10 - * @decimal 1 - * @increment 0.5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/modules/sc_pos_control/multicopter_position_mode_params.c b/src/modules/sc_pos_control/multicopter_position_mode_params.c deleted file mode 100644 index 56adad1144bf..000000000000 --- a/src/modules/sc_pos_control/multicopter_position_mode_params.c +++ /dev/null @@ -1,198 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Position/Altitude mode variant - * - * The supported sub-modes are: - * 0 Sticks directly map to velocity setpoints without smoothing. - * Also applies to vertical direction and Altitude mode. - * Useful for velocity control tuning. - * 3 Sticks map to velocity but with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). - * 4 Sticks map to acceleration and there's a virtual brake drag - * - * @value 0 Direct velocity - * @value 3 Smoothed velocity - * @value 4 Acceleration based - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_POS_MODE, 4); - -/** - * Maximum horizontal velocity setpoint in Position mode - * - * Must be smaller than MPC_XY_VEL_MAX. - * - * The maximum sideways and backward speed can be set differently - * using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively. - * - * @unit m/s - * @min 3 - * @max 20 - * @increment 1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.f); - -/** - * Maximum sideways velocity in Position mode - * - * If set to a negative value or larger than - * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. - * - * @unit m/s - * @min -1 - * @max 20 - * @increment 1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.f); - -/** - * Maximum backward velocity in Position mode - * - * If set to a negative value or larger than - * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. - * - * @unit m/s - * @min -1 - * @max 20 - * @increment 1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); - -/** - * Maximum horizontal acceleration - * - * MPC_POS_MODE - * 1 just deceleration - * 3 acceleration and deceleration - * 4 not used, use MPC_ACC_HOR instead - * - * @unit m/s^2 - * @min 2 - * @max 15 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); - -/** - * Maximum horizontal and vertical jerk in Position/Altitude mode - * - * Limit the maximum jerk of the vehicle (how fast the acceleration can change). - * A lower value leads to smoother motions but limits agility - * (how fast it can change directions or break). - * - * Setting this to the maximum value essentially disables the limit. - * - * Only used with smooth MPC_POS_MODE 3 and 4. - * - * @unit m/s^3 - * @min 0.5 - * @max 500 - * @decimal 0 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.f); - -/** - * Deadzone for sticks in manual piloted modes - * - * Does not apply to manual throttle and direct attitude piloting by stick. - * - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); - -/** - * Manual position control stick exponential curve sensitivity - * - * The higher the value the less sensitivity the stick has around zero - * while still reaching the maximum value with full stick deflection. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.6f); - -/** - * Manual control stick vertical exponential curve - * - * The higher the value the less sensitivity the stick has around zero - * while still reaching the maximum value with full stick deflection. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.6f); - -/** - * Manual control stick yaw rotation exponential curve - * - * The higher the value the less sensitivity the stick has around zero - * while still reaching the maximum value with full stick deflection. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.6f); diff --git a/src/modules/sc_pos_control/multicopter_responsiveness_params.c b/src/modules/sc_pos_control/multicopter_responsiveness_params.c deleted file mode 100644 index e0097f6ca015..000000000000 --- a/src/modules/sc_pos_control/multicopter_responsiveness_params.c +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Responsiveness - * - * Changes the overall responsiveness of the vehicle. - * The higher the value, the faster the vehicle will react. - * - * If set to a value greater than zero, other parameters are automatically set (such as - * the acceleration or jerk limits). - * If set to a negative value, the existing individual parameters are used. - * - * @min -1 - * @max 1 - * @decimal 2 - * @increment 0.05 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); - -/** - * Overall Horizontal Velocity Limit - * - * If set to a value greater than zero, other parameters are automatically set (such as - * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). - * If set to a negative value, the existing individual parameters are used. - * - * @min -20 - * @max 20 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.f); - -/** - * Overall Vertical Velocity Limit - * - * If set to a value greater than zero, other parameters are automatically set (such as - * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). - * If set to a negative value, the existing individual parameters are used. - * - * @min -3 - * @max 8 - * @decimal 1 - * @increment 0.5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.f); diff --git a/src/modules/sc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/sc_pos_control/multicopter_stabilized_mode_params.c deleted file mode 100644 index f478f9938e27..000000000000 --- a/src/modules/sc_pos_control/multicopter_stabilized_mode_params.c +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Maximal tilt angle in Stabilized or Altitude mode - * - * @unit deg - * @min 0 - * @max 90 - * @decimal 0 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.f); - -/** - * Max manual yaw rate for Stabilized, Altitude, Position mode - * - * @unit deg/s - * @min 0 - * @max 400 - * @decimal 0 - * @increment 10 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.f); - -/** - * Minimum collective thrust in Stabilized mode - * - * The value is mapped to the lowest throttle stick position in Stabilized mode. - * - * Too low collective thrust leads to loss of roll/pitch/yaw torque control authority. - * Airmode is used to keep torque authority with zero thrust (see MC_AIRMODE). - * - * @unit norm - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); - -/** - * Thrust curve mapping in Stabilized Mode - * - * This parameter defines how the throttle stick input is mapped to collective thrust - * in Stabilized mode. - * - * In case the default is used ('Rescale to hover thrust'), the stick input is linearly - * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). - * - * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful - * in case the hover thrust is very low and the default would lead to too much distortion - * (e.g. if hover thrust is set to 20%, then 80% of the upper thrust range is squeezed into the - * upper half of the stick range). - * - * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. - * - * @value 0 Rescale to hover thrust - * @value 1 No Rescale - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); diff --git a/src/modules/sc_pos_control/multicopter_takeoff_land_params.c b/src/modules/sc_pos_control/multicopter_takeoff_land_params.c deleted file mode 100644 index 328a8531ad93..000000000000 --- a/src/modules/sc_pos_control/multicopter_takeoff_land_params.c +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Smooth takeoff ramp time constant - * - * Increasing this value will make climb rate controlled takeoff slower. - * If it's too slow the drone might scratch the ground and tip over. - * A time constant of 0 disables the ramp - * - * @unit s - * @min 0 - * @max 5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.f); - -/** - * Takeoff climb rate - * - * @unit m/s - * @min 1 - * @max 5 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); - -/** - * Altitude for 1. step of slow landing (descend) - * - * Below this altitude descending velocity gets limited to a value - * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED" - * Value needs to be higher than "MPC_LAND_ALT2" - * - * @unit m - * @min 0 - * @max 122 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.f); - -/** - * Altitude for 2. step of slow landing (landing) - * - * Below this altitude descending velocity gets - * limited to "MPC_LAND_SPEED" - * Value needs to be lower than "MPC_LAND_ALT1" - * - * @unit m - * @min 0 - * @max 122 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.f); - -/** - * Altitude for 3. step of slow landing - * - * Below this altitude descending velocity gets - * limited to "MPC_LAND_CRWL", if LIDAR available. - * No effect if LIDAR not available - * - * @unit m - * @min 0 - * @max 122 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.f); - -/** - * Landing descend rate - * - * @unit m/s - * @min 0.6 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); - -/** - * Land crawl descend rate - * - * Used below MPC_LAND_ALT3 if distance sensor data is availabe. - * - * @unit m/s - * @min 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f); diff --git a/src/modules/sc_pos_control/sc_pos_control_params.c b/src/modules/sc_pos_control/sc_pos_control_params.c index ef0748748630..45c7dff44383 100644 --- a/src/modules/sc_pos_control/sc_pos_control_params.c +++ b/src/modules/sc_pos_control/sc_pos_control_params.c @@ -112,17 +112,174 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAX, 12.f); */ PARAM_DEFINE_FLOAT(MPC_VEL_CRUISE, 5.f); -/* - TODO: - (ParamFloat) _param_sys_vehicle_resp, - (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_thr_max, - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_man_y_max, - (ParamFloat) _param_mpc_man_y_tau, - (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all -*/ +/** + * Responsiveness + * + * Changes the overall responsiveness of the vehicle. + * The higher the value, the faster the vehicle will react. + * + * If set to a value greater than zero, other parameters are automatically set (such as + * the acceleration or jerk limits). + * If set to a negative value, the existing individual parameters are used. + * + * @min -1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); + +/** + * Overall Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_ALL, -10.f); + +/** + * Maximum velocity setpoint in autonomous modes + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MAX, 10.f); + +/** + * Cruising elocity setpoint in autonomous modes + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_CRUISE, 10.f); + +/** + * Maximum velocity setpoint in Position mode + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MANUAL, 10.f); + +/** + * Maximum collective thrust + * + * Limit allowed thrust + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_THR_MAX, 1.f); + +/** + * Acceleration for autonomous and for manual modes + * + * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC, 3.f); + +/** + * Maximum accelaration in autonomous modes + * + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC_MAX, 5.f); + +/** + * Jerk limit in autonomous modes + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions but also limited agility. + * + * @unit m/s^3 + * @min 1 + * @max 80 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_AUTO, 4.f); + +/** + * Maximum jerk in Position/Altitude mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother motions but limits agility + * (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Only used with smooth MPC_POS_MODE 3 and 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500 + * @decimal 0 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_MAX, 8.f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_MAX, 150.f); + +/** + * Manual yaw rate input filter time constant + * + * Not used in Stabilized mode + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_TAU, 0.08f); From e09783366ff2da8e8b2c1b3414ed2e57a0b82fdc Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 26 Apr 2024 17:03:53 +0200 Subject: [PATCH 44/77] fix: nan on setpoints for att control --- .../PositionControl/ControlMath.cpp | 3 +-- .../PositionControl/PositionControl.cpp | 18 +++++++++++++----- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/modules/sc_pos_control/PositionControl/ControlMath.cpp b/src/modules/sc_pos_control/PositionControl/ControlMath.cpp index b4eba96115cd..d2228d559743 100644 --- a/src/modules/sc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/sc_pos_control/PositionControl/ControlMath.cpp @@ -257,5 +257,4 @@ void setZeroIfNanVector3f(Vector3f &vector) // Adding zero vector overwrites elements that are NaN with zero addIfNotNanVector3f(vector, Vector3f()); } - -} // ControlMath +} \ No newline at end of file diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index aaf4809ed0f4..5fe5b6f44851 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -44,7 +44,7 @@ using namespace matrix; -const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; +const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { @@ -183,8 +183,16 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_ attitude_setpoint.thrust_body[2] = _thr_sp(2); // Bypass attitude control by giving same attitude setpoint to att control - attitude_setpoint.q_d[0] = _quat_sp(0); - attitude_setpoint.q_d[1] = _quat_sp(1); - attitude_setpoint.q_d[2] = _quat_sp(2); - attitude_setpoint.q_d[3] = _quat_sp(3); + if(PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3)) ) { + attitude_setpoint.q_d[0] = _quat_sp(0); + attitude_setpoint.q_d[1] = _quat_sp(1); + attitude_setpoint.q_d[2] = _quat_sp(2); + attitude_setpoint.q_d[3] = _quat_sp(3); + } else { + // We probably dont want this but will leave it for now + attitude_setpoint.q_d[0] = 0; + attitude_setpoint.q_d[1] = 0; + attitude_setpoint.q_d[2] = 0; + attitude_setpoint.q_d[3] = 1; + } } From 6207209955c637b2da1725cf21dd3c50ac181199 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sat, 27 Apr 2024 14:15:11 +0200 Subject: [PATCH 45/77] feat: cleaning position controller --- .../PositionControl/PositionControl.cpp | 30 ++++------------- .../PositionControl/PositionControl.hpp | 33 ++++++------------- .../SpacecraftPositionControl.cpp | 24 +++++++------- .../SpacecraftPositionControl.hpp | 4 ++- 4 files changed, 31 insertions(+), 60 deletions(-) diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index 5fe5b6f44851..906abaf43f2b 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -58,10 +58,8 @@ void PositionControl::setVelocityLimits(const float vel_limit) _lim_vel = vel_limit; } -void PositionControl::setThrustLimits(const float min, const float max) +void PositionControl::setThrustLimit(const float max) { - // make sure there's always enough thrust vector length to infer the attitude - _lim_thr_min = min; _lim_thr_max = max; } @@ -70,6 +68,7 @@ void PositionControl::setState(const PositionControlStates &states) _pos = states.position; _vel = states.velocity; _vel_dot = states.acceleration; + _att_q = states.quaternion; } void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) @@ -161,21 +160,7 @@ bool PositionControl::_inputValid() return valid; } -void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const -{ - local_position_setpoint.x = _pos_sp(0); - local_position_setpoint.y = _pos_sp(1); - local_position_setpoint.z = _pos_sp(2); - local_position_setpoint.yaw = _yaw_sp; - local_position_setpoint.yawspeed = _yawspeed_sp; - local_position_setpoint.vx = _vel_sp(0); - local_position_setpoint.vy = _vel_sp(1); - local_position_setpoint.vz = _vel_sp(2); - _acc_sp.copyTo(local_position_setpoint.acceleration); - _thr_sp.copyTo(local_position_setpoint.thrust); -} - -void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const +void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const { // Set thrust setpoint attitude_setpoint.thrust_body[0] = _thr_sp(0); @@ -189,10 +174,9 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_ attitude_setpoint.q_d[2] = _quat_sp(2); attitude_setpoint.q_d[3] = _quat_sp(3); } else { - // We probably dont want this but will leave it for now - attitude_setpoint.q_d[0] = 0; - attitude_setpoint.q_d[1] = 0; - attitude_setpoint.q_d[2] = 0; - attitude_setpoint.q_d[3] = 1; + attitude_setpoint.q_d[0] = v_att.q[0]; + attitude_setpoint.q_d[1] = v_att.q[1]; + attitude_setpoint.q_d[2] = v_att.q[2]; + attitude_setpoint.q_d[3] = v_att.q[3]; } } diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.hpp b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp index a3d0eaa9fe6f..c5e95ccc54d1 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -49,24 +50,19 @@ struct PositionControlStates { matrix::Vector3f position; matrix::Vector3f velocity; matrix::Vector3f acceleration; - float yaw; + matrix::Quatf quaternion; // bypassed to attitude controller }; /** - * Core Position-Control for MC. + * Core Position-Control for spacecrafts. * This class contains P-controller for position and * PID-controller for velocity. + * * Inputs: - * vehicle position/velocity/yaw - * desired set-point position/velocity/thrust/yaw/yaw-speed - * constraints that are stricter than global limits + * vehicle position/velocity/attitude + * desired set-point position/velocity/thrust/attitude * Output - * thrust vector and a yaw-setpoint - * - * If there is a position and a velocity set-point present, then - * the velocity set-point is used as feed-forward. If feed-forward is - * active, then the velocity component of the P-controller output has - * priority over the feed-forward component. + * thrust vector and quaternion for attitude control * * A setpoint that is NAN is considered as not set. * If there is a position/velocity- and thrust-setpoint present, then @@ -104,7 +100,7 @@ class PositionControl * @param min minimum thrust e.g. 0.1 or 0 * @param max maximum thrust e.g. 0.9 or 1 */ - void setThrustLimits(const float min, const float max); + void setThrustLimit(const float max); /** * Pass the current vehicle state to the controller @@ -136,21 +132,13 @@ class PositionControl */ void resetIntegral() { _vel_int.setZero(); } - /** - * Get the controllers output local position setpoint - * These setpoints are the ones which were executed on including PID output and feed-forward. - * The acceleration or thrust setpoints can be used for attitude control. - * @param local_position_setpoint reference to struct to fill up - */ - void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const; - /** * Get the controllers output attitude setpoint * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. * It needs to be executed by the attitude controller to achieve velocity and position tracking. * @param attitude_setpoint reference to struct to fill up */ - void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; + void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const; /** * All setpoints are set to NAN (uncontrolled). Timestampt zero. @@ -183,6 +171,7 @@ class PositionControl matrix::Vector3f _vel; /**< current velocity */ matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ + matrix::Quatf _att_q; /**< current attitude */ float _yaw{}; /**< current heading */ // Setpoints @@ -191,6 +180,4 @@ class PositionControl matrix::Vector3f _acc_sp; /**< desired acceleration */ matrix::Vector3f _thr_sp; /**< desired thrust */ matrix::Quatf _quat_sp; /**< desired attitude */ - float _yaw_sp{}; /**< desired heading */ - float _yawspeed_sp{}; /** desired yaw-speed */ }; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index ace1e9d6c84d..b69f6577a2de 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -145,7 +145,7 @@ void SpacecraftPositionControl::parameters_update(bool force) } PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicle_local_position_s - &vehicle_local_position) + &vehicle_local_position, const vehicle_attitude_s &vehicle_attitude) { PositionControlStates states; @@ -194,7 +194,11 @@ PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicl _vel_z_deriv.reset(); } - states.yaw = vehicle_local_position.heading; + if(PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) && PX4_ISFINITE(vehicle_attitude.q[3])) { + states.quaternion = Quatf(vehicle_attitude.q); + } else { + states.quaternion = Quatf(); + } return states; } @@ -214,6 +218,7 @@ void SpacecraftPositionControl::Run() perf_begin(_cycle_perf); vehicle_local_position_s vehicle_local_position; + vehicle_attitude_s v_att; if (_local_pos_sub.update(&vehicle_local_position)) { const float dt = @@ -240,6 +245,7 @@ void SpacecraftPositionControl::Run() // TODO: check if setpoint is different than the previous one and reset integral then // _control.resetIntegral(); _trajectory_setpoint_sub.update(&_setpoint); + _vehicle_attitude_sub.update(&v_att); // adjust existing (or older) setpoint with any EKF reset deltas if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { @@ -283,7 +289,7 @@ void SpacecraftPositionControl::Run() _heading_reset_counter = vehicle_local_position.heading_reset_counter; - PositionControlStates states{set_vehicle_states(vehicle_local_position)}; + PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; if (_vehicle_control_mode.flag_control_position_enabled) { @@ -299,7 +305,7 @@ void SpacecraftPositionControl::Run() if (_vehicle_control_mode.flag_control_position_enabled && (_setpoint.timestamp >= _time_position_control_enabled)) { - _control.setThrustLimits(0.0, _param_mpc_thr_max.get()); + _control.setThrustLimit(_param_mpc_thr_max.get()); _control.setVelocityLimits(_param_mpc_vel_max.get()); @@ -314,17 +320,9 @@ void SpacecraftPositionControl::Run() _control.update(dt); } - // Publish internal position control setpoints - // on top of the input/feed-forward setpoints these containt the PID corrections - // This message is used by other modules (such as Landdetector) to determine vehicle intention. - vehicle_local_position_setpoint_s local_pos_sp{}; - _control.getLocalPositionSetpoint(local_pos_sp); - local_pos_sp.timestamp = hrt_absolute_time(); - _local_pos_sp_pub.publish(local_pos_sp); - // Publish attitude setpoint output vehicle_attitude_setpoint_s attitude_setpoint{}; - _control.getAttitudeSetpoint(attitude_setpoint); + _control.getAttitudeSetpoint(attitude_setpoint, v_att); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index b09060fec2a7..bca05daefedc 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -94,6 +95,7 @@ class SpacecraftPositionControl : public ModuleBase, uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ hrt_abstime _time_position_control_enabled{0}; @@ -150,7 +152,7 @@ class SpacecraftPositionControl : public ModuleBase, /** * Check for validity of positon/velocity states. */ - PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &att); /** * Generate setpoint to bridge no executable setpoint being available. From 71cd6ffaa08bb4aeb13f6af54e001b2721fb2d9b Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sat, 27 Apr 2024 14:19:26 +0200 Subject: [PATCH 46/77] fix: updated empty trajectories --- src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp | 2 +- src/modules/mc_pos_control/PositionControl/PositionControl.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index c4b964904637..0efc8919f767 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -3,7 +3,7 @@ #include constexpr uint64_t FlightTask::_timeout; -const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; +const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 1371523c8d5d..74e492331742 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -44,7 +44,7 @@ using namespace matrix; -const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; +const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { From ddad102e631eb6b9ee02f7aa13f27e413eb101bc Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sat, 27 Apr 2024 14:39:48 +0200 Subject: [PATCH 47/77] doc: small changes to att control - will move to different branch --- .../sc_att_control/sc_att_control_params.c | 23 ++++++++++--------- .../SpacecraftPositionControl.cpp | 2 -- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/src/modules/sc_att_control/sc_att_control_params.c b/src/modules/sc_att_control/sc_att_control_params.c index 9176a2e818cc..a8b276833264 100644 --- a/src/modules/sc_att_control/sc_att_control_params.c +++ b/src/modules/sc_att_control/sc_att_control_params.c @@ -32,11 +32,12 @@ ****************************************************************************/ /** - * @file mc_att_control_params.c - * Parameters for multicopter attitude controller. + * @file sc_att_control_params.c + * Parameters for spacecraft attitude controller. * * @author Lorenz Meier * @author Anton Babushkin + * @author Pedro Roque, */ /** @@ -48,7 +49,7 @@ * @max 12 * @decimal 2 * @increment 0.1 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_ROLL_P, 6.5f); @@ -61,7 +62,7 @@ PARAM_DEFINE_FLOAT(SC_ROLL_P, 6.5f); * @max 12 * @decimal 2 * @increment 0.1 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_PITCH_P, 6.5f); @@ -74,7 +75,7 @@ PARAM_DEFINE_FLOAT(SC_PITCH_P, 6.5f); * @max 5 * @decimal 2 * @increment 0.1 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_YAW_P, 2.8f); @@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(SC_YAW_P, 2.8f); * @max 1.0 * @decimal 2 * @increment 0.1 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_YAW_WEIGHT, 0.4f); @@ -111,7 +112,7 @@ PARAM_DEFINE_FLOAT(SC_YAW_WEIGHT, 0.4f); * @max 1800.0 * @decimal 1 * @increment 5 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_ROLLRATE_MAX, 220.0f); @@ -130,7 +131,7 @@ PARAM_DEFINE_FLOAT(SC_ROLLRATE_MAX, 220.0f); * @max 1800.0 * @decimal 1 * @increment 5 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_PITCHRATE_MAX, 220.0f); @@ -142,7 +143,7 @@ PARAM_DEFINE_FLOAT(SC_PITCHRATE_MAX, 220.0f); * @max 1800.0 * @decimal 1 * @increment 5 - * @group Multicopter Attitude Control + * @group Spacecraft Attitude Control */ PARAM_DEFINE_FLOAT(SC_YAWRATE_MAX, 200.0f); @@ -155,7 +156,7 @@ PARAM_DEFINE_FLOAT(SC_YAWRATE_MAX, 200.0f); * @min 0.0 * @max 2.0 * @decimal 2 - * @group Multicopter Position Control + * @group Spacecraft Position Control */ PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f); @@ -167,6 +168,6 @@ PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f); * @max 400 * @decimal 0 * @increment 10 - * @group Multicopter Position Control + * @group Spacecraft Position Control */ PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f); \ No newline at end of file diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index b69f6577a2de..5961aa15f5e4 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -288,10 +288,8 @@ void SpacecraftPositionControl::Run() _z_reset_counter = vehicle_local_position.z_reset_counter; _heading_reset_counter = vehicle_local_position.heading_reset_counter; - PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; - if (_vehicle_control_mode.flag_control_position_enabled) { // set failsafe setpoint if there hasn't been a new // trajectory setpoint since position control started From 89e5b11c1011ce7d0badefe96a5c2e773314b705 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Sat, 27 Apr 2024 20:05:52 +0200 Subject: [PATCH 48/77] fix: several fixes on attitude controller cleaning --- ROMFS/px4fmu_common/init.d/rc.sc_apps | 7 ++++- .../AttitudeControl/AttitudeControl.cpp | 22 ++------------ .../AttitudeControl/AttitudeControl.hpp | 9 ++---- src/modules/sc_att_control/sc_att_control.hpp | 14 ++++++--- .../sc_att_control/sc_att_control_main.cpp | 29 +++++-------------- .../SpacecraftPositionControl.cpp | 2 +- .../sc_pos_control/sc_pos_control_params.c | 13 +++++++++ 7 files changed, 42 insertions(+), 54 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index 448f2c0ec260..dcbaf87b0b3b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -27,5 +27,10 @@ sc_rate_control start # sc_att_control start +# +# Start Spacecraft Position Controller. +# +sc_pos_control start + # Start Space Robot Thruster Controller -sc_thruster_controller start +sc_thruster_controller start \ No newline at end of file diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp index 6f1585f8015c..ba88de2fcd16 100644 --- a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp @@ -41,15 +41,9 @@ using namespace matrix; -void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight) +void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain) { _proportional_gain = proportional_gain; - _yaw_w = math::constrain(yaw_weight, 0.f, 1.f); - - // compensate for the effect of the yaw weight rescaling the output - if (_yaw_w > 1e-4f) { - _proportional_gain(2) /= _yaw_w; - } } matrix::Vector3f AttitudeControl::update(const Quatf &q) const @@ -75,10 +69,11 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // mix full and reduced desired attitude Quatf q_mix = qd_red.inversed() * qd; q_mix.canonicalize(); + // catch numerical problems with the domain of acosf and asinf q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); - qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + qd = qd_red * Quatf(q_mix(0), 0, 0, q_mix(3)); // quaternion attitude control law, qe is rotation from q to qd const Quatf qe = q.inversed() * qd; @@ -90,17 +85,6 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // calculate angular rates setpoint Vector3f rate_setpoint = eq.emult(_proportional_gain); - // Feed forward the yaw setpoint rate. - // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis, - // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). - // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) - // and multiply it by the yaw setpoint rate (yawspeed_setpoint). - // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame - // such that it can be added to the rates setpoint. - if (std::isfinite(_yawspeed_setpoint)) { - rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; - } - // limit rates for (int i = 0; i < 3; i++) { rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp index e435e91262f8..9590d3ebfdfc 100644 --- a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp @@ -60,9 +60,8 @@ class AttitudeControl /** * Set proportional attitude control gain * @param proportional_gain 3D vector containing gains for roll, pitch, yaw - * @param yaw_weight A fraction [0,1] deprioritizing yaw compared to roll and pitch */ - void setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight); + void setProportionalGain(const matrix::Vector3f &proportional_gain); /** * Set hard limit for output rate setpoints @@ -73,13 +72,11 @@ class AttitudeControl /** * Set a new attitude setpoint replacing the one tracked before * @param qd desired vehicle attitude setpoint - * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame */ - void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) + void setAttitudeSetpoint(const matrix::Quatf &qd) { _attitude_setpoint_q = qd; _attitude_setpoint_q.normalize(); - _yawspeed_setpoint = yawspeed_setpoint; } /** @@ -103,8 +100,6 @@ class AttitudeControl private: matrix::Vector3f _proportional_gain; matrix::Vector3f _rate_limit; - float _yaw_w{0.f}; ///< yaw weight [0,1] to deprioritize caompared to roll and pitch matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control - float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint }; diff --git a/src/modules/sc_att_control/sc_att_control.hpp b/src/modules/sc_att_control/sc_att_control.hpp index fbb73686bbb2..3f12198ffd75 100644 --- a/src/modules/sc_att_control/sc_att_control.hpp +++ b/src/modules/sc_att_control/sc_att_control.hpp @@ -96,15 +96,21 @@ class SpacecraftAttitudeControl : public ModuleBase, uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + // Attitude setpoint and current attitude sub + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + + // Manual control setpoint sub + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + + // Vehicle control mode sub and status uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + // Vehicle local position sub + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + // Publish rate setpoint for rate controller and att_control status uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ uORB::Publication _vehicle_attitude_setpoint_pub; diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp index ad3da6d3bcdb..9feb8e8f4ecb 100644 --- a/src/modules/sc_att_control/sc_att_control_main.cpp +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -80,8 +80,7 @@ void SpacecraftAttitudeControl::parameters_updated() { // Store some of the parameters in a more convenient way & precompute often-used values - _attitude_control.setProportionalGain(Vector3f(_param_sc_roll_p.get(), _param_sc_pitch_p.get(), _param_sc_yaw_p.get()), - _param_sc_yaw_weight.get()); + _attitude_control.setProportionalGain(Vector3f(_param_sc_roll_p.get(), _param_sc_pitch_p.get(), _param_sc_yaw_p.get())); // angular rate limits using math::radians; @@ -155,7 +154,7 @@ SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); // update attitude controller setpoint immediately - _attitude_control.setAttitudeSetpoint(q_sp, attitude_setpoint.yaw_sp_move_rate); + _attitude_control.setAttitudeSetpoint(q_sp); _thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body); _last_attitude_setpoint = attitude_setpoint.timestamp; } @@ -198,11 +197,12 @@ SpacecraftAttitudeControl::Run() if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { + /* Removing print PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], - (double)vehicle_attitude_setpoint.q_d[3]); + (double)vehicle_attitude_setpoint.q_d[3]);*/ - _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d)); _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; } @@ -260,25 +260,11 @@ SpacecraftAttitudeControl::Run() _man_roll_input_filter.reset(0.f); _man_pitch_input_filter.reset(0.f); } - // PX4_INFO("Current state: %f %f %f %f", (double)q(0), (double)q(1), (double)q(2), (double)q(3)); Vector3f rates_sp = _attitude_control.update(q); - - const hrt_abstime now = hrt_absolute_time(); - autotune_attitude_control_status_s pid_autotune; - - if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) { - if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL - || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH - || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW - || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST) - && ((now - pid_autotune.timestamp) < 1_s)) { - rates_sp += Vector3f(pid_autotune.rate_sp); - } - } - + // publish rate setpoint vehicle_rates_setpoint_s rates_setpoint{}; - rates_setpoint.roll = rates_sp(0); + rates_setpoint.roll = rates_sp(0); rates_setpoint.pitch = rates_sp(1); rates_setpoint.yaw = rates_sp(2); _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body); @@ -352,7 +338,6 @@ Institute for Dynamic Systems and Control (IDSC), ETH Zurich PRINT_MODULE_USAGE_NAME("sc_att_control", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index 5961aa15f5e4..ec0dd218ecea 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -42,7 +42,7 @@ using namespace matrix; SpacecraftPositionControl::SpacecraftPositionControl() : - SuperBlock(nullptr, "MPC"), + SuperBlock(nullptr, "SPC"), ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), diff --git a/src/modules/sc_pos_control/sc_pos_control_params.c b/src/modules/sc_pos_control/sc_pos_control_params.c index 45c7dff44383..654d20af0f53 100644 --- a/src/modules/sc_pos_control/sc_pos_control_params.c +++ b/src/modules/sc_pos_control/sc_pos_control_params.c @@ -283,3 +283,16 @@ PARAM_DEFINE_FLOAT(SPC_MAN_Y_MAX, 150.f); * @group Spacecraft Position Control */ PARAM_DEFINE_FLOAT(SPC_MAN_Y_TAU, 0.08f); + +/** + * Numerical velocity derivative low pass cutoff frequency + * + * @unit Hz + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VELD_LP, 5.0f); + From b8482b0a98876dc4e20c0487a6707773d596020c Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 29 Apr 2024 17:23:16 +0200 Subject: [PATCH 49/77] feat: updates to attidude control --- msg/VehicleStatus.msg | 2 +- src/modules/commander/ModeUtil/control_mode.cpp | 2 +- src/modules/sc_att_control/sc_att_control.hpp | 1 + src/modules/sc_att_control/sc_att_control_main.cpp | 3 +-- src/modules/sc_att_control/sc_att_control_params.c | 14 +++++++++++++- 5 files changed, 17 insertions(+), 5 deletions(-) diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 939bb6ed8b3d..31f709b69f97 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -82,7 +82,7 @@ uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 uint8 VEHICLE_TYPE_AIRSHIP = 4 -uint8 VEHICLE_TYPE_SPACECRAFT = 4 +uint8 VEHICLE_TYPE_SPACECRAFT = 5 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 0a07c2989d95..d26c131f3ead 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -39,7 +39,7 @@ namespace mode_util static bool stabilization_required(uint8_t vehicle_type) { - return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, diff --git a/src/modules/sc_att_control/sc_att_control.hpp b/src/modules/sc_att_control/sc_att_control.hpp index 3f12198ffd75..f3adab882c40 100644 --- a/src/modules/sc_att_control/sc_att_control.hpp +++ b/src/modules/sc_att_control/sc_att_control.hpp @@ -147,6 +147,7 @@ class SpacecraftAttitudeControl : public ModuleBase, (ParamFloat) _param_sc_rollrate_max, (ParamFloat) _param_sc_pitchrate_max, (ParamFloat) _param_sc_yawrate_max, + (ParamFloat) _param_sc_man_tilt_max, /* Stabilized mode params */ (ParamFloat) _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */ diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp index 9feb8e8f4ecb..0ee05b9a9b80 100644 --- a/src/modules/sc_att_control/sc_att_control_main.cpp +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -86,6 +86,7 @@ SpacecraftAttitudeControl::parameters_updated() using math::radians; _attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()), radians(_param_sc_yawrate_max.get()))); + _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); } void @@ -122,11 +123,9 @@ SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, _man_roll_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); _man_pitch_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); - // we want to fly towards the direction of (roll, pitch) Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); float v_norm = v.norm(); // the norm of v defines the tilt angle - if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle v *= _man_tilt_max / v_norm; } diff --git a/src/modules/sc_att_control/sc_att_control_params.c b/src/modules/sc_att_control/sc_att_control_params.c index a8b276833264..440edc3a16dd 100644 --- a/src/modules/sc_att_control/sc_att_control_params.c +++ b/src/modules/sc_att_control/sc_att_control_params.c @@ -170,4 +170,16 @@ PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f); * @increment 10 * @group Spacecraft Position Control */ -PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f); \ No newline at end of file +PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f); + +/** + * Maximal tilt angle in Stabilized or Manual mode + * + * @unit deg + * @min 0 + * @max 90 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_TILT_MAX, 90.f); \ No newline at end of file From 94c05b6f88c821c1bfedda78f95e6e6018c4919b Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 2 May 2024 09:53:40 +0200 Subject: [PATCH 50/77] fix: remove FW_ARSP_MODE --- .../init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft | 2 +- .../init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft | 2 +- ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 5a5a1b487f60..2ba40c284573 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -15,7 +15,7 @@ param set-default MAV_TYPE 24 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 -param set-default FW_ARSP_MODE 1 +# param set-default FW_ARSP_MODE 1 # Auto to be provided by Custom Airframe param set-default CA_METHOD 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 9bf69f648df0..a5dc9c8f4880 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -15,7 +15,7 @@ param set-default MAV_TYPE 25 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 -param set-default FW_ARSP_MODE 1 +# param set-default FW_ARSP_MODE 1 # Auto to be provided by Custom Airframe param set-default CA_METHOD 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index 93453f6ce0ab..b62aa9fe71df 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -16,7 +16,7 @@ param set-default MAV_TYPE 24 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 -param set-default FW_ARSP_MODE 1 +# param set-default FW_ARSP_MODE 1 # Auto to be provided by Custom Airframe # param set-default CA_METHOD 0 From 86a99705ce1a8b7860b531319bff25632aab247a Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 2 May 2024 09:59:00 +0200 Subject: [PATCH 51/77] fix: doubled topcis --- src/modules/uxrce_dds_client/dds_topics.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index a60bd84ce35f..a481610f6c26 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -56,7 +56,7 @@ publications: - topic: /fmu/out/vehicle_torque_setpoint type: px4_msgs::msg::VehicleTorqueSetpoint - - topic: /fmu/out/vehicle_torque_setpoint + - topic: /fmu/out/vehicle_thrust_setpoint type: px4_msgs::msg::VehicleThrustSetpoint - topic: /fmu/out/log_message From aca2f09dea73db8e53c1273af1e320663df15cb1 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 2 May 2024 11:20:01 +0200 Subject: [PATCH 52/77] add: manual subscription --- .../SpacecraftPositionControl.cpp | 57 +++++++++++++++++++ .../SpacecraftPositionControl.hpp | 4 ++ 2 files changed, 61 insertions(+) diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index ec0dd218ecea..6271014d7709 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -290,6 +290,8 @@ void SpacecraftPositionControl::Run() PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; + poll_manual_setpoint(); + if (_vehicle_control_mode.flag_control_position_enabled) { // set failsafe setpoint if there hasn't been a new // trajectory setpoint since position control started @@ -330,6 +332,61 @@ void SpacecraftPositionControl::Run() perf_end(_cycle_perf); } +void SpacecraftPositionControl::poll_manual_setpoint() +{ + _setpoint.timestamp = hrt_absolute_time(); + if (_vehicle_control_mode.flag_control_manual_enabled) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); + + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && + !_vehicle_control_mode.flag_control_offboard_enabled) { + + if (_control_mode.flag_control_attitude_enabled) { + // STABILIZED mode generate the attitude setpoint from manual user inputs + _att_sp.roll_body = 0.0; + _att_sp.pitch_body = 0.0; + + /* reset yaw setpoint to current position if needed */ + if (_reset_yaw_sp) { + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + _reset_yaw_sp = false; + + } else { + const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.roll * yaw_rate; + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); + } + + _att_sp.yaw_body = _manual_yaw_sp; + _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; + + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.timestamp = hrt_absolute_time(); + + + _attitude_sp_pub.publish(_att_sp); + + } else { + // Set heading from the manual roll input channel + _yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; + // Set throttle from the manual throttle channel + _throttle_control = _manual_control_setpoint.throttle; + _reset_yaw_sp = true; + } + + } else { + _reset_yaw_sp = true; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } + } +} + trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn) { diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index bca05daefedc..ea09df5931e6 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -92,6 +93,7 @@ class SpacecraftPositionControl : public ModuleBase, uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; @@ -99,9 +101,11 @@ class SpacecraftPositionControl : public ModuleBase, hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ hrt_abstime _time_position_control_enabled{0}; + hrt_abstime _manual_setpoint_last_called{0}; trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint}; vehicle_control_mode_s _vehicle_control_mode{}; + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ DEFINE_PARAMETERS( // Position Control From 56f5056094645575970afc486d643b74e60a5ca5 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 2 May 2024 17:46:30 +0200 Subject: [PATCH 53/77] add: debug attitude setpoint in dds, more updates on control stack --- .../SpacecraftPositionControl.cpp | 65 +++++++++++-------- .../SpacecraftPositionControl.hpp | 12 ++++ src/modules/uxrce_dds_client/dds_topics.yaml | 3 + 3 files changed, 54 insertions(+), 26 deletions(-) diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index 6271014d7709..d9f064262bbb 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -290,14 +290,16 @@ void SpacecraftPositionControl::Run() PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; - poll_manual_setpoint(); + poll_manual_setpoint(dt, vehicle_local_position, v_att); if (_vehicle_control_mode.flag_control_position_enabled) { // set failsafe setpoint if there hasn't been a new // trajectory setpoint since position control started if ((_setpoint.timestamp < _time_position_control_enabled) && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { - + PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f", + (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, + (double)_time_position_control_enabled); _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); } } @@ -332,20 +334,39 @@ void SpacecraftPositionControl::Run() perf_end(_cycle_perf); } -void SpacecraftPositionControl::poll_manual_setpoint() +void SpacecraftPositionControl::poll_manual_setpoint(const float dt, + const vehicle_local_position_s &vehicle_local_position, + const vehicle_attitude_s &_vehicle_att) { - _setpoint.timestamp = hrt_absolute_time(); if (_vehicle_control_mode.flag_control_manual_enabled) { if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { - float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_vehicle_control_mode.flag_control_offboard_enabled) { - if (_control_mode.flag_control_attitude_enabled) { - // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = 0.0; - _att_sp.pitch_body = 0.0; + if (_vehicle_control_mode.flag_control_attitude_enabled) { + // We are in Stabilized mode + // Generate position setpoints + Vector3f target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, + vehicle_local_position.z); + + // Update velocity setpoint + Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); + // TODO(@Pedro-Roque): probably need to move velocity to inertial frame + target_pos_sp = target_pos_sp + target_vel_sp * dt; + + // Update _setpoint + _setpoint.position[0] = target_pos_sp(0); + _setpoint.position[1] = target_pos_sp(1); + _setpoint.position[2] = target_pos_sp(2); + + _setpoint.velocity[0] = target_vel_sp(0); + _setpoint.velocity[1] = target_vel_sp(1); + _setpoint.velocity[2] = target_vel_sp(2); + + // Generate attitude setpoints + const float roll_body = 0.0; + const float pitch_body = 0.0; /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { @@ -354,28 +375,20 @@ void SpacecraftPositionControl::poll_manual_setpoint() _reset_yaw_sp = false; } else { - const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.roll * yaw_rate; - _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); + const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); + const float yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); } - _att_sp.yaw_body = _manual_yaw_sp; - _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; - - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); - q.copyTo(_att_sp.q_d); - - _att_sp.timestamp = hrt_absolute_time(); - + Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp)); + q_sp.copyTo(_setpoint.attitude); - _attitude_sp_pub.publish(_att_sp); + _setpoint.timestamp = hrt_absolute_time(); + PX4_INFO("Setpoint created: %f, %f, %f", (double)_setpoint.position[0], (double)_setpoint.position[1], (double)_setpoint.position[2]); } else { - // Set heading from the manual roll input channel - _yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; - // Set throttle from the manual throttle channel - _throttle_control = _manual_control_setpoint.throttle; - _reset_yaw_sp = true; + // We are in Manual mode + PX4_WARN("Attitude ctl disabled - bypassing position control."); } } else { diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index ea09df5931e6..393451fc1030 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -143,6 +143,12 @@ class SpacecraftPositionControl : public ModuleBase, uint8_t _xy_reset_counter{0}; uint8_t _z_reset_counter{0}; uint8_t _heading_reset_counter{0}; + + // Manual setpoints on yaw and reset + bool _reset_yaw_sp{true}; + float _manual_yaw_sp{0.f}; + float _throttle_control{0.f}; + float _yaw_control{0.f}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; @@ -158,6 +164,12 @@ class SpacecraftPositionControl : public ModuleBase, */ PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &att); + /** + * Check for manual setpoints. + */ + void poll_manual_setpoint(const float dt, const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); + /** * Generate setpoint to bridge no executable setpoint being available. * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index a481610f6c26..762d3d7340da 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -58,6 +58,9 @@ publications: - topic: /fmu/out/vehicle_thrust_setpoint type: px4_msgs::msg::VehicleThrustSetpoint + + - topic: /fmu/out/vehicle_attitude_setpoint + type: px4_msgs::msg::VehicleAttitudeSetpoint - topic: /fmu/out/log_message type: px4_msgs::msg::LogMessage From 73bce2bd39b1b401e01e8231ad243d6b5905e8e0 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 3 May 2024 11:29:21 +0200 Subject: [PATCH 54/77] fix: aiming for static position and attitude keeping --- .../sc_pos_control/SpacecraftPositionControl.cpp | 15 ++++++++++++--- .../sc_pos_control/SpacecraftPositionControl.hpp | 5 +++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index d9f064262bbb..c14e69356aa0 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -34,8 +34,6 @@ #include "SpacecraftPositionControl.hpp" #include -#include -#include #include #include "PositionControl/ControlMath.hpp" @@ -347,14 +345,23 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, if (_vehicle_control_mode.flag_control_attitude_enabled) { // We are in Stabilized mode // Generate position setpoints - Vector3f target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, + if(!stabilized_pos_sp_initialized) { + // Initialize position setpoint + target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, vehicle_local_position.z); + stabilized_pos_sp_initialized = true; + //TODO: Same thing for attitude setpoint and understand why thrust is 0.0 + } + PX4_INFO("Current position: %f, %f, %f", (double)vehicle_local_position.x, (double)vehicle_local_position.y, (double)vehicle_local_position.z); // Update velocity setpoint Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); // TODO(@Pedro-Roque): probably need to move velocity to inertial frame target_pos_sp = target_pos_sp + target_vel_sp * dt; + PX4_INFO("Target velocity: %f, %f, %f", (double)target_vel_sp(0), (double)target_vel_sp(1), (double)target_vel_sp(2)); + PX4_INFO("Target position: %f, %f, %f", (double)target_pos_sp(0), (double)target_pos_sp(1), (double)target_pos_sp(2)); + // Update _setpoint _setpoint.position[0] = target_pos_sp(0); _setpoint.position[1] = target_pos_sp(1); @@ -389,10 +396,12 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, } else { // We are in Manual mode PX4_WARN("Attitude ctl disabled - bypassing position control."); + stabilized_pos_sp_initialized = false; } } else { _reset_yaw_sp = true; + stabilized_pos_sp_initialized = false; } _manual_setpoint_last_called = hrt_absolute_time(); diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index 393451fc1030..d763139b8a89 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include #include @@ -131,6 +133,9 @@ class SpacecraftPositionControl : public ModuleBase, control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + matrix::Vector3f target_pos_sp; + bool stabilized_pos_sp_initialized{false}; + PositionControl _control; /**< class for core PID position control */ hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ From 3961d278d9440c629d50cdda64d2fc6ddbc46f12 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 3 May 2024 18:57:22 +0200 Subject: [PATCH 55/77] fix: attitude setpoint updates only when needed --- .../SpacecraftPositionControl.cpp | 35 +++++++------------ .../SpacecraftPositionControl.hpp | 1 + 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index c14e69356aa0..63b7f8853ac4 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -139,6 +139,8 @@ void SpacecraftPositionControl::parameters_update(bool force) events::send(events::ID("sc_pos_ctrl_man_vel_set"), events::Log::Warning, "Manual speed has been constrained by maximum speed", _param_mpc_vel_max.get()); } + + yaw_rate = math::radians(_param_mpc_man_y_max.get()); } } @@ -336,7 +338,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, const vehicle_local_position_s &vehicle_local_position, const vehicle_attitude_s &_vehicle_att) { - if (_vehicle_control_mode.flag_control_manual_enabled) { + if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) { if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { if (!_vehicle_control_mode.flag_control_climb_rate_enabled && @@ -349,19 +351,16 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, // Initialize position setpoint target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, vehicle_local_position.z); + + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; stabilized_pos_sp_initialized = true; - //TODO: Same thing for attitude setpoint and understand why thrust is 0.0 } - PX4_INFO("Current position: %f, %f, %f", (double)vehicle_local_position.x, (double)vehicle_local_position.y, (double)vehicle_local_position.z); - // Update velocity setpoint Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); // TODO(@Pedro-Roque): probably need to move velocity to inertial frame target_pos_sp = target_pos_sp + target_vel_sp * dt; - PX4_INFO("Target velocity: %f, %f, %f", (double)target_vel_sp(0), (double)target_vel_sp(1), (double)target_vel_sp(2)); - PX4_INFO("Target position: %f, %f, %f", (double)target_pos_sp(0), (double)target_pos_sp(1), (double)target_pos_sp(2)); - // Update _setpoint _setpoint.position[0] = target_pos_sp(0); _setpoint.position[1] = target_pos_sp(1); @@ -372,27 +371,20 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, _setpoint.velocity[2] = target_vel_sp(2); // Generate attitude setpoints + float yaw_sp_move_rate = 0.0; + if (_manual_control_setpoint.throttle > -0.9f){ + yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; + } + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); const float roll_body = 0.0; const float pitch_body = 0.0; - /* reset yaw setpoint to current position if needed */ - if (_reset_yaw_sp) { - const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); - _manual_yaw_sp = vehicle_yaw; - _reset_yaw_sp = false; - - } else { - const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); - const float yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; - _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); - } - Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp)); q_sp.copyTo(_setpoint.attitude); _setpoint.timestamp = hrt_absolute_time(); - PX4_INFO("Setpoint created: %f, %f, %f", (double)_setpoint.position[0], (double)_setpoint.position[1], (double)_setpoint.position[2]); - + PX4_INFO("Position Setpoint created: %f, %f, %f", (double)_setpoint.position[0], (double)_setpoint.position[1], (double)_setpoint.position[2]); + PX4_INFO("Attitude Setpoint created: %f, %f, %f, %f", (double)_setpoint.attitude[0], (double)_setpoint.attitude[1], (double)_setpoint.attitude[2], (double)_setpoint.attitude[3]); } else { // We are in Manual mode PX4_WARN("Attitude ctl disabled - bypassing position control."); @@ -400,7 +392,6 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, } } else { - _reset_yaw_sp = true; stabilized_pos_sp_initialized = false; } diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index d763139b8a89..eea7c1ebe36d 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -134,6 +134,7 @@ class SpacecraftPositionControl : public ModuleBase, control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ matrix::Vector3f target_pos_sp; + float yaw_rate; bool stabilized_pos_sp_initialized{false}; PositionControl _control; /**< class for core PID position control */ From cbfbae16d6a4b1761547359cb6b498f904299ba0 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 3 May 2024 19:08:25 +0200 Subject: [PATCH 56/77] dbg: behavior seems consistent, need to ensure it works on position mode --- .../sc_pos_control/PositionControl/PositionControl.cpp | 4 +++- src/modules/sc_pos_control/SpacecraftPositionControl.cpp | 2 -- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index 906abaf43f2b..3db376cd6a91 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include using namespace matrix; @@ -82,6 +83,7 @@ void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) bool PositionControl::update(const float dt) { bool valid = _inputValid(); + PX4_INFO("valid: %d", valid); if (valid) { _positionControl(); @@ -118,7 +120,7 @@ void PositionControl::_velocityControl(const float dt) // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); - + PX4_INFO("acc_sp: %f, %f, %f", (double)_acc_sp(0), (double)_acc_sp(1), (double)_acc_sp(2)); // Accelaration to Thrust _thr_sp = _acc_sp; _thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max); diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index 63b7f8853ac4..cabb9cb53829 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -383,8 +383,6 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, q_sp.copyTo(_setpoint.attitude); _setpoint.timestamp = hrt_absolute_time(); - PX4_INFO("Position Setpoint created: %f, %f, %f", (double)_setpoint.position[0], (double)_setpoint.position[1], (double)_setpoint.position[2]); - PX4_INFO("Attitude Setpoint created: %f, %f, %f, %f", (double)_setpoint.attitude[0], (double)_setpoint.attitude[1], (double)_setpoint.attitude[2], (double)_setpoint.attitude[3]); } else { // We are in Manual mode PX4_WARN("Attitude ctl disabled - bypassing position control."); From 3ffc29d0ccb48099f6e6fb82a66493a1c5afa69e Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 3 May 2024 22:06:59 +0200 Subject: [PATCH 57/77] feat: position control needs to be tested - arming pos ctl not working --- .../commander/ModeUtil/control_mode.cpp | 21 ++++++++++++------- .../SpacecraftPositionControl.cpp | 1 - 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index d26c131f3ead..e7bb3c1a6118 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -42,6 +42,11 @@ static bool stabilization_required(uint8_t vehicle_type) return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } +static bool if_not_spacecraft(uint8_t vehicle_type) +{ + return vehicle_type != vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; +} + void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, const offboard_control_mode_s &offboard_control_mode, vehicle_control_mode_s &vehicle_control_mode) @@ -65,16 +70,16 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_position_enabled = true; vehicle_control_mode.flag_control_velocity_enabled = true; break; @@ -118,16 +123,16 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, if (offboard_control_mode.position) { vehicle_control_mode.flag_control_position_enabled = true; vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_acceleration_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; } else if (offboard_control_mode.velocity) { vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_acceleration_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index cabb9cb53829..a79f3065ff99 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -385,7 +385,6 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, _setpoint.timestamp = hrt_absolute_time(); } else { // We are in Manual mode - PX4_WARN("Attitude ctl disabled - bypassing position control."); stabilized_pos_sp_initialized = false; } From 3476d7536f38d0ac947de883687149abd07d33ff Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 6 May 2024 14:09:53 +0200 Subject: [PATCH 58/77] feat: adding parameters to replicate hardware pwm for spacecrafts --- .../9000_gazebo-classic_2d_spacecraft | 19 ++-- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 2 +- src/modules/simulation/pwm_out_sim/PWMSim.cpp | 7 +- src/modules/simulation/pwm_out_sim/PWMSim.hpp | 17 +++- .../pwm_out_sim/pwm_out_sim_params.c | 91 +++++++++++++++++++ 5 files changed, 122 insertions(+), 14 deletions(-) create mode 100644 src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 2ba40c284573..157c148eca30 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -88,14 +88,17 @@ param set-default CA_THRUSTER7_AX 0.0 param set-default CA_THRUSTER7_AY -1.0 param set-default CA_THRUSTER7_AZ 0.0 -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 -param set-default PWM_MAIN_FUNC5 105 -param set-default PWM_MAIN_FUNC6 106 -param set-default PWM_MAIN_FUNC7 107 -param set-default PWM_MAIN_FUNC8 108 +param set-default PWM_MAIN_FUNC1 501 +param set-default PWM_MAIN_FUNC2 502 +param set-default PWM_MAIN_FUNC3 503 +param set-default PWM_MAIN_FUNC4 504 +param set-default PWM_MAIN_FUNC5 505 +param set-default PWM_MAIN_FUNC6 506 +param set-default PWM_MAIN_FUNC7 507 +param set-default PWM_MAIN_FUNC8 508 + +param set PWM_SIM_PWM_MAX 10000 +param set PWM_SIM_PWM_MIN 0 # Controller Tunings param set-default SC_ROLLRATE_P 0.14 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index bafb37aeb5c0..4ac956e4c290 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -60,7 +60,7 @@ then fi # -# SR setup +# Spacecraft setup # if [ $VEHICLE_TYPE = sc ] then diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c11a5eb365fe..b83c53d6bd78 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,11 +44,16 @@ PWMSim::PWMSim(bool hil_mode_enabled) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { + // initialize parameters - dynamic instead of static + PWM_SIM_PWM_MAX_MAGIC = _param_pwm_sim_max_magic.get(); + PWM_SIM_PWM_MIN_MAGIC = _param_pwm_sim_min_magic.get(); + PWM_SIM_FAILSAFE_MAGIC = _param_pwm_sim_failsafe_magic.get(); + PWM_SIM_DISARMED_MAGIC = _param_pwm_sim_disarmed_magic.get(); + _mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); _mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC); _mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC); _mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC); - _mixing_output.setIgnoreLockdown(hil_mode_enabled); } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e75..4f210254c4dc 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -38,7 +38,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -76,10 +78,10 @@ class PWMSim : public ModuleBase, public OutputModuleInterface private: void Run() override; - static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900; - static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600; - static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000; - static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000; + uint16_t PWM_SIM_DISARMED_MAGIC; + uint16_t PWM_SIM_FAILSAFE_MAGIC; + uint16_t PWM_SIM_PWM_MIN_MAGIC; + uint16_t PWM_SIM_PWM_MAX_MAGIC; MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -88,4 +90,11 @@ class PWMSim : public ModuleBase, public OutputModuleInterface perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_pwm_sim_max_magic, + (ParamInt) _param_pwm_sim_min_magic, + (ParamInt) _param_pwm_sim_failsafe_magic, + (ParamInt) _param_pwm_sim_disarmed_magic + ) }; diff --git a/src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c b/src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c new file mode 100644 index 000000000000..e6789ff11446 --- /dev/null +++ b/src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pwm_out_sim_params.c + * Parameters for PWM output simulator. + * + * @author Pedro Roque, + */ + +/** + * Maximum PWM Value + * + * Highest PWM value that can be output by the simulator. + * + * @min 0 + * @max 10000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_PWM_MAX, 2000); + +/** + * Minimum PWM Value + * + * Lowest PWM value that can be output by the simulator. + * + * @min -10000 + * @max 2000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_PWM_MIN, 1000); + +/** + * Failsafe PWM Value + * + * Output at Failsafe level. + * + * @min 0 + * @max 10000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_FAILSAFE, 600); + +/** + * Disarm PWM Value + * + * Output at Disarmed state. + * + * @min 0 + * @max 10000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_DISARM, 900); \ No newline at end of file From c890c7c5a3d093a7c970418d0034d47ec4acbc2a Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 6 May 2024 16:21:00 +0200 Subject: [PATCH 59/77] fix: added fixes for vision/mocap data. --- .../airframes/9000_gazebo-classic_2d_spacecraft | 1 + ROMFS/px4fmu_common/init.d/rc.sc_defaults | 7 +++++++ src/modules/uxrce_dds_client/dds_topics.yaml | 4 ++-- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 157c148eca30..a5206c5f599a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -97,6 +97,7 @@ param set-default PWM_MAIN_FUNC6 506 param set-default PWM_MAIN_FUNC7 507 param set-default PWM_MAIN_FUNC8 508 +# PWM Simulation param set PWM_SIM_PWM_MAX 10000 param set PWM_SIM_PWM_MIN 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 7d5b1d5b56a6..6a9812b13731 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -26,3 +26,10 @@ param set-default COM_ARM_HFLT_CHK 0 #Missing params param set-default CP_DIST -1.0 + +# Default to MoCap fusion +param set-default ATT_EXT_HDG_M 2 +param set-default EKF2_EV_CTRL 15 +param set-default EKF2_EV_DELAY 5 +param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 3 \ No newline at end of file diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 762d3d7340da..06274ffd223f 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -94,8 +94,8 @@ subscriptions: - topic: /fmu/in/vehicle_attitude_setpoint type: px4_msgs::msg::VehicleAttitudeSetpoint - - topic: /fmu/in/vehicle_mocap_odometry - type: px4_msgs::msg::VehicleOdometry + # - topic: /fmu/in/vehicle_mocap_odometry + # type: px4_msgs::msg::VehicleOdometry - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint From 11937230ffddc88b97310f10329c4561bb8be3d2 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 6 May 2024 16:23:46 +0200 Subject: [PATCH 60/77] Update submodule --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index e27669f7dd8b..ac47b1255dc8 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit e27669f7dd8bf7d6ca9f993398c4ec04bd58ccbc +Subproject commit ac47b1255dc8079625b9e6e100a4ca7551d6814f From a670085b580212a469c7abb26ba5de21c2c57695 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 6 May 2024 16:31:49 +0200 Subject: [PATCH 61/77] rft: removing undesireable prints --- src/modules/sc_pos_control/PositionControl/PositionControl.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index 3db376cd6a91..3f8b46fc5358 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -83,7 +83,6 @@ void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) bool PositionControl::update(const float dt) { bool valid = _inputValid(); - PX4_INFO("valid: %d", valid); if (valid) { _positionControl(); @@ -120,7 +119,7 @@ void PositionControl::_velocityControl(const float dt) // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); - PX4_INFO("acc_sp: %f, %f, %f", (double)_acc_sp(0), (double)_acc_sp(1), (double)_acc_sp(2)); + // Accelaration to Thrust _thr_sp = _acc_sp; _thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max); From 4f880a116ea435b78f28900a8bbe8026ccb4f5be Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 9 May 2024 20:13:57 +0200 Subject: [PATCH 62/77] feat: updating thruster plugin on gazebo-classic sitl --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index ac47b1255dc8..a02948399015 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit ac47b1255dc8079625b9e6e100a4ca7551d6814f +Subproject commit a029483990152685d0c366cf35befea195f403f4 From 697bcde57516a7ea728726c3dcd5968fd46e2e92 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 13:49:45 +0200 Subject: [PATCH 63/77] fix: format checks --- src/lib/mixer_module/actuator_test.cpp | 1 + .../functions/FunctionThrusters.hpp | 5 +- src/lib/mixer_module/mixer_module.cpp | 2 +- src/modules/commander/Commander.cpp | 1 + .../commander/ModeUtil/control_mode.cpp | 3 +- src/modules/commander/commander_helper.cpp | 3 +- src/modules/commander/commander_helper.h | 2 +- .../ActuatorEffectivenessSpacecraft.cpp | 2 +- .../ActuatorEffectivenessThrusters.cpp | 29 +- .../ActuatorEffectivenessThrusters.hpp | 7 +- .../control_allocator/ControlAllocator.cpp | 12 +- .../sc_att_control/sc_att_control_main.cpp | 15 +- .../PositionControl/PositionControl.cpp | 6 +- .../SpacecraftPositionControl.cpp | 20 +- .../SpacecraftPositionControl.hpp | 2 +- .../sc_rate_control/SpacecraftRateControl.cpp | 475 +++++++++--------- .../sc_thruster_controller.cpp | 298 +++++------ .../sc_thruster_controller.hpp | 63 +-- 18 files changed, 498 insertions(+), 448 deletions(-) diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index 4d98fef3a1f6..5347be6b1129 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -93,6 +93,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve) value += trim.trim[idx]; } } + _current_outputs[i] = value; } else { diff --git a/src/lib/mixer_module/functions/FunctionThrusters.hpp b/src/lib/mixer_module/functions/FunctionThrusters.hpp index de7e7871cac8..61449c021fb5 100644 --- a/src/lib/mixer_module/functions/FunctionThrusters.hpp +++ b/src/lib/mixer_module/functions/FunctionThrusters.hpp @@ -44,7 +44,8 @@ class FunctionThrusters : public FunctionProviderBase static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1, "Unexpected num thrusters"); - static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, "Unexpected thruster idx"); + static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, + "Unexpected thruster idx"); FunctionThrusters(const Context &context) : _topic(&context.work_item, ORB_ID(actuator_motors)), @@ -76,7 +77,7 @@ class FunctionThrusters : public FunctionProviderBase { // TODO(@Pedro-Roque): for now bypass this /*if (thrust_factor > 0.f && thrust_factor <= 1.f) { - + // thrust factor // rel_thrust = factor * x^2 + (1-factor) * x, const float a = thrust_factor; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index bc9fa590d8a1..0a3b58ad821c 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -239,7 +239,7 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &function) == 0) { if ((function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) || - (function >= (int32_t)OutputFunction::Thruster1 && function <= (int32_t)OutputFunction::ThrusterMax)) { + (function >= (int32_t)OutputFunction::Thruster1 && function <= (int32_t)OutputFunction::ThrusterMax)) { switch_requested = true; } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 323b710a5475..7a20ea0a0911 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1718,6 +1718,7 @@ void Commander::updateParameters() } else if (is_ground) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + } else if (is_space) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index f53f807484b2..83c698d2bd4b 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -39,7 +39,8 @@ namespace mode_util static bool stabilization_required(uint8_t vehicle_type) { - return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; + return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } static bool if_not_spacecraft(uint8_t vehicle_type) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 988505927e4c..559cd4a76c18 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -126,7 +126,8 @@ bool is_ground_vehicle(const vehicle_status_s ¤t_status) bool is_spacecraft(const vehicle_status_s ¤t_status) { - return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT_2D || current_status.system_type == VEHICLE_TYPE_SPACECRAFT_3D); + return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT_2D + || current_status.system_type == VEHICLE_TYPE_SPACECRAFT_3D); } // End time for currently blinking LED message, 0 if no blink message diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index daa8abdc0056..631025817f88 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -56,7 +56,7 @@ bool is_vtol(const vehicle_status_s ¤t_status); bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); bool is_fixed_wing(const vehicle_status_s ¤t_status); bool is_ground_vehicle(const vehicle_status_s ¤t_status); -bool is_spacecraft(const vehicle_status_s& current_status); +bool is_spacecraft(const vehicle_status_s ¤t_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp index 526fd23e53f6..ff440dadea4a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp @@ -61,5 +61,5 @@ void ActuatorEffectivenessSpacecraft::updateSetpoint(const matrix::VectorallocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); } + _control_allocation[i]->clipActuatorSetpoint(); } } @@ -690,15 +692,20 @@ ControlAllocator::publish_actuator_controls() // Actuator setpoints are shared between Motors or Thrusters. For now, we assume we have only either of them int actuator_type = 0; + if (_num_actuators[(int)ActuatorType::THRUSTERS] > 0) { actuator_type = (int)ActuatorType::THRUSTERS; } + // motors int motors_idx; - for (motors_idx = 0; motors_idx < _num_actuators[actuator_type] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + + for (motors_idx = 0; motors_idx < _num_actuators[actuator_type] + && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + if (stopped_motors & (1u << motors_idx)) { actuator_motors.control[motors_idx] = NAN; } @@ -717,7 +724,8 @@ ControlAllocator::publish_actuator_controls() if (_num_actuators[(int)ActuatorType::SERVOS] > 0) { int servos_idx; - for (servos_idx = 0; servos_idx < _num_actuators[(int)ActuatorType::SERVOS] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + for (servos_idx = 0; servos_idx < _num_actuators[(int)ActuatorType::SERVOS] + && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp index 0ee05b9a9b80..7256657b8832 100644 --- a/src/modules/sc_att_control/sc_att_control_main.cpp +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -35,7 +35,7 @@ * @file sc_att_control_main.cpp * Spacecraft attitude controller. * Based off the multicopter attitude controller module. - * + * * @author Pedro Roque, * */ @@ -86,7 +86,7 @@ SpacecraftAttitudeControl::parameters_updated() using math::radians; _attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()), radians(_param_sc_yawrate_max.get()))); - _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); + _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); } void @@ -126,6 +126,7 @@ SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); float v_norm = v.norm(); // the norm of v defines the tilt angle + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle v *= _man_tilt_max / v_norm; } @@ -197,8 +198,8 @@ SpacecraftAttitudeControl::Run() if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { /* Removing print - PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], - (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], + PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], + (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], (double)vehicle_attitude_setpoint.q_d[3]);*/ _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d)); @@ -244,6 +245,7 @@ SpacecraftAttitudeControl::Run() } bool attitude_setpoint_generated = false; + if (_vehicle_control_mode.flag_control_attitude_enabled) { // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode @@ -259,11 +261,12 @@ SpacecraftAttitudeControl::Run() _man_roll_input_filter.reset(0.f); _man_pitch_input_filter.reset(0.f); } + Vector3f rates_sp = _attitude_control.update(q); - + // publish rate setpoint vehicle_rates_setpoint_s rates_setpoint{}; - rates_setpoint.roll = rates_sp(0); + rates_setpoint.roll = rates_sp(0); rates_setpoint.pitch = rates_sp(1); rates_setpoint.yaw = rates_sp(2); _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body); diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp index 4c848e940766..3bee760fd2fc 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -161,7 +161,8 @@ bool ScPositionControl::_inputValid() return valid; } -void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const +void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, + vehicle_attitude_s &v_att) const { // Set thrust setpoint attitude_setpoint.thrust_body[0] = _thr_sp(0); @@ -169,11 +170,12 @@ void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitud attitude_setpoint.thrust_body[2] = _thr_sp(2); // Bypass attitude control by giving same attitude setpoint to att control - if(PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3)) ) { + if (PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3))) { attitude_setpoint.q_d[0] = _quat_sp(0); attitude_setpoint.q_d[1] = _quat_sp(1); attitude_setpoint.q_d[2] = _quat_sp(2); attitude_setpoint.q_d[3] = _quat_sp(3); + } else { attitude_setpoint.q_d[0] = v_att.q[0]; attitude_setpoint.q_d[1] = v_att.q[1]; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp index 03801ba32ba5..8b7a9ff16ed6 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -93,9 +93,11 @@ void SpacecraftPositionControl::parameters_update(bool force) if (responsiveness > 0.6f) { num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + } else { num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); } + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); } @@ -194,8 +196,10 @@ PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicl _vel_z_deriv.reset(); } - if(PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) && PX4_ISFINITE(vehicle_attitude.q[3])) { + if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) + && PX4_ISFINITE(vehicle_attitude.q[3])) { states.quaternion = Quatf(vehicle_attitude.q); + } else { states.quaternion = Quatf(); } @@ -298,8 +302,8 @@ void SpacecraftPositionControl::Run() if ((_setpoint.timestamp < _time_position_control_enabled) && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f", - (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, - (double)_time_position_control_enabled); + (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, + (double)_time_position_control_enabled); _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); } } @@ -347,15 +351,16 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, if (_vehicle_control_mode.flag_control_attitude_enabled) { // We are in Stabilized mode // Generate position setpoints - if(!stabilized_pos_sp_initialized) { + if (!stabilized_pos_sp_initialized) { // Initialize position setpoint target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, - vehicle_local_position.z); + vehicle_local_position.z); const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); _manual_yaw_sp = vehicle_yaw; stabilized_pos_sp_initialized = true; } + // Update velocity setpoint Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); // TODO(@Pedro-Roque): probably need to move velocity to inertial frame @@ -372,9 +377,11 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, // Generate attitude setpoints float yaw_sp_move_rate = 0.0; - if (_manual_control_setpoint.throttle > -0.9f){ + + if (_manual_control_setpoint.throttle > -0.9f) { yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; } + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); const float roll_body = 0.0; const float pitch_body = 0.0; @@ -383,6 +390,7 @@ void SpacecraftPositionControl::poll_manual_setpoint(const float dt, q_sp.copyTo(_setpoint.attitude); _setpoint.timestamp = hrt_absolute_time(); + } else { // We are in Manual mode stabilized_pos_sp_initialized = false; diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index b77c4573bf4c..685d0240634b 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -174,7 +174,7 @@ class SpacecraftPositionControl : public ModuleBase, * Check for manual setpoints. */ void poll_manual_setpoint(const float dt, const vehicle_local_position_s - &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); + &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); /** * Generate setpoint to bridge no executable setpoint being available. diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.cpp b/src/modules/sc_rate_control/SpacecraftRateControl.cpp index d0691f8940e5..54ffc4daeaaa 100644 --- a/src/modules/sc_rate_control/SpacecraftRateControl.cpp +++ b/src/modules/sc_rate_control/SpacecraftRateControl.cpp @@ -44,271 +44,278 @@ using namespace time_literals; using math::radians; SpacecraftRateControl::SpacecraftRateControl() - : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _vehicle_torque_setpoint_pub(ORB_ID(vehicle_torque_setpoint)), - _vehicle_thrust_setpoint_pub(ORB_ID(vehicle_thrust_setpoint)), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) { - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; - - parameters_updated(); - _controller_status_pub.advertise(); + : ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _vehicle_torque_setpoint_pub(ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(ORB_ID(vehicle_thrust_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; + + parameters_updated(); + _controller_status_pub.advertise(); } SpacecraftRateControl::~SpacecraftRateControl() { perf_free(_loop_perf); } -bool SpacecraftRateControl::init() { - if (!_vehicle_angular_velocity_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } +bool SpacecraftRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } - return true; + return true; } -void SpacecraftRateControl::parameters_updated() { - // rate control parameters - // The controller gain K is used to convert the parallel (P + I/s + sD) form - // to the ideal (K * [1 + 1/sTi + sTd]) form - const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); +void SpacecraftRateControl::parameters_updated() +{ + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); - _rate_control.setPidGains( - rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), - rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), - rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); - _rate_control.setIntegratorLimit( - Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); + _rate_control.setIntegratorLimit( + Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); - _rate_control.setFeedForwardGain( - Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); + _rate_control.setFeedForwardGain( + Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); - // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), - radians(_param_sc_acro_y_max.get())); + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), + radians(_param_sc_acro_y_max.get())); } -void SpacecraftRateControl::Run() { - if (should_exit()) { - _vehicle_angular_velocity_sub.unregisterCallback(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - parameters_updated(); - } - - /* run controller on gyro changes */ - vehicle_angular_velocity_s angular_velocity; - - if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - const hrt_abstime now = angular_velocity.timestamp_sample; - - // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); - _last_run = now; - - const Vector3f rates{angular_velocity.xyz}; - const Vector3f angular_accel{angular_velocity.xyz_derivative}; - - /* check for updates in other topics */ - _vehicle_control_mode_sub.update(&_vehicle_control_mode); - - if (_vehicle_land_detected_sub.updated()) { - vehicle_land_detected_s vehicle_land_detected; - - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; - _maybe_landed = vehicle_land_detected.maybe_landed; - } - } - - _vehicle_status_sub.update(&_vehicle_status); - - // use rates setpoint topic - vehicle_rates_setpoint_s vehicle_rates_setpoint{}; - - if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { - // generate the rate setpoint from sticks - manual_control_setpoint_s manual_control_setpoint; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - // manual rates control - ACRO mode - const Vector3f man_rate_sp{ - math::superexpo(manual_control_setpoint.roll, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), - math::superexpo(-manual_control_setpoint.pitch, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), - math::superexpo(manual_control_setpoint.yaw, _param_sc_acro_expo_y.get(), _param_sc_acro_supexpoy.get())}; - - _rates_setpoint = man_rate_sp.emult(_acro_rate_max); - _thrust_setpoint(2) = -manual_control_setpoint.throttle; - _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; - - // publish rate setpoint - vehicle_rates_setpoint.roll = _rates_setpoint(0); - vehicle_rates_setpoint.pitch = _rates_setpoint(1); - vehicle_rates_setpoint.yaw = _rates_setpoint(2); - _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); - vehicle_rates_setpoint.timestamp = hrt_absolute_time(); - - _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); - } - - } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { - if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { - _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); - _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); - _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); - _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); - } - } - - // run the rate controller - if (_vehicle_control_mode.flag_control_rates_enabled) { - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || - _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rate_control.resetIntegral(); - } - - // update saturation status from control allocation feedback - control_allocator_status_s control_allocator_status; - - if (_control_allocator_status_sub.update(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; - - if (!control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; - - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } - } - } - - // TODO: send the unallocated value directly for better anti-windup - _rate_control.setSaturationStatus(saturation_positive, saturation_negative); - } - - // run rate controller - const Vector3f att_control = - _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); - - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - _rate_control.getRateControlStatus(rate_ctrl_status); - rate_ctrl_status.timestamp = hrt_absolute_time(); - _controller_status_pub.publish(rate_ctrl_status); - - // publish thrust and torque setpoints - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - - _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); - vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; - vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; - vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; - - // scale setpoints by battery status if enabled - if (_param_sc_bat_scale_en.get()) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status; - - if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { - _battery_status_scale = battery_status.scale; - } - } - - if (_battery_status_scale > 0.f) { - for (int i = 0; i < 3; i++) { - vehicle_thrust_setpoint.xyz[i] = - math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); - vehicle_torque_setpoint.xyz[i] = - math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); - } - } - } - - vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; - vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); - - vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; - vehicle_torque_setpoint.timestamp = hrt_absolute_time(); - _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); - - updateActuatorControlsStatus(vehicle_torque_setpoint, dt); - } - } - - perf_end(_loop_perf); +void SpacecraftRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.roll, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_sc_acro_expo_y.get(), _param_sc_acro_supexpoy.get())}; + + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _thrust_setpoint(2) = -manual_control_setpoint.throttle; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || + _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + const Vector3f att_control = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_sc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = + math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = + math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } + + perf_end(_loop_perf); } -void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s& vehicle_torque_setpoint, - float dt) { - for (int i = 0; i < 3; i++) { - _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; - } +void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } - _energy_integration_time += dt; + _energy_integration_time += dt; - if (_energy_integration_time > 500e-3f) { - actuator_controls_status_s status; - status.timestamp = vehicle_torque_setpoint.timestamp; + if (_energy_integration_time > 500e-3f) { + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; - for (int i = 0; i < 3; i++) { - status.control_power[i] = _control_energy[i] / _energy_integration_time; - _control_energy[i] = 0.f; - } + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } - _actuator_controls_status_pub.publish(status); - _energy_integration_time = 0.f; - } + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } } -int SpacecraftRateControl::task_spawn(int argc, char* argv[]) { +int SpacecraftRateControl::task_spawn(int argc, char *argv[]) +{ - SpacecraftRateControl* instance = new SpacecraftRateControl(); + SpacecraftRateControl *instance = new SpacecraftRateControl(); - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - if (instance->init()) { - return PX4_OK; - } + if (instance->init()) { + return PX4_OK; + } - } else { - PX4_ERR("alloc failed"); - } + } else { + PX4_ERR("alloc failed"); + } - delete instance; - _object.store(nullptr); - _task_id = -1; + delete instance; + _object.store(nullptr); + _task_id = -1; - return PX4_ERROR; + return PX4_ERROR; } -int SpacecraftRateControl::custom_command(int argc, char* argv[]) { return print_usage("unknown command"); } +int SpacecraftRateControl::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int SpacecraftRateControl::print_usage(const char* reason) { - if (reason) { - PX4_WARN("%s\n", reason); - } +int SpacecraftRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( ### Description This implements the spacecraft rate controller. It takes rate setpoints (in acro mode via `manual_control_setpoint` topic) as inputs and outputs torque and thrust setpoints. diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp index 9660b9a47986..e032310ce1b4 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp @@ -40,166 +40,180 @@ #include #include -int ScThrusterController::print_status() { - PX4_INFO("Running"); - // TODO: print additional runtime information about the state of the module +int ScThrusterController::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module - return 0; + return 0; } -int ScThrusterController::custom_command(int argc, char* argv[]) { - if (!is_running()) { - print_usage("not running"); - return 1; - } - /* - // additional custom commands can be handled like this: - if (!strcmp(argv[0], "do-something")) { - get_instance()->do_something(); - return 0; - } - */ - - return print_usage("unknown command"); +int ScThrusterController::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("not running"); + return 1; + } + + /* + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); } -int ScThrusterController::task_spawn(int argc, char* argv[]) { - _task_id = px4_task_spawn_cmd("module", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, (px4_main_t)&run_trampoline, - (char* const*)argv); +int ScThrusterController::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("module", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, (px4_main_t)&run_trampoline, + (char *const *)argv); - if (_task_id < 0) { - _task_id = -1; - return -errno; - } + if (_task_id < 0) { + _task_id = -1; + return -errno; + } - return 0; + return 0; } -ScThrusterController* ScThrusterController::instantiate(int argc, char* argv[]) { - int myoptind = 1; - int ch; - bool error_flag = false; - const char *myoptarg = nullptr; - - // parse CLI arguments - _debug_thruster_print = false; - while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'd': - _debug_thruster_print = true; - PX4_INFO("Debugging enabled"); - break; - case '?': - error_flag = true; - break; - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return nullptr; - } - - ScThrusterController* instance = new ScThrusterController(); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - } - - return instance; +ScThrusterController *ScThrusterController::instantiate(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + bool error_flag = false; + const char *myoptarg = nullptr; + + // parse CLI arguments + _debug_thruster_print = false; + + while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _debug_thruster_print = true; + PX4_INFO("Debugging enabled"); + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + + ScThrusterController *instance = new ScThrusterController(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; } ScThrusterController::ScThrusterController() : ModuleParams(nullptr) {} -void ScThrusterController::run() { - // Subscribe to thruster command - int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); - px4_pollfd_struct_t fds[] = { - {.fd = thrustercmd_sub, .events = POLLIN}, - }; - - // Create publisher for PWM values - struct actuator_motors_s actuator_motors_msg; - memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); - orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); - - // For publishing debug messages - struct my_message_s my_msg; - memset(&my_msg, 0, sizeof(my_msg)); - orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); - - // Thruster command structure - struct thruster_command_s thruster_cmd; - - // initialize parameters - parameters_update(true); - bool debugPrint = ScThrusterController::_debug_thruster_print; - PX4_INFO("Debugging enabled: %d", debugPrint); - - while (!should_exit()) { - // wait for up to 1000ms for data - int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); - - if (pret == 0) { - // Timeout: let the loop run anyway, don't do `continue` here - if (debugPrint) PX4_WARN("Timeout..."); - } else if (pret < 0) { - // this is undesirable but not much we can do - PX4_ERR("poll error %d, %d", pret, errno); - px4_usleep(50000); - continue; - - } else if (fds[0].revents & POLLIN) { - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); - if (debugPrint) - PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, - (double)thruster_cmd.y1, (double)thruster_cmd.y2); - - actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; - actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; - actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; - actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; - actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; - actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; - actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; - actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; - - orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); - - if (debugPrint) { - strcpy(my_msg.text, "Thruster command received!"); - orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); - } - } - parameters_update(); - } - - orb_unsubscribe(thrustercmd_sub); +void ScThrusterController::run() +{ + // Subscribe to thruster command + int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); + px4_pollfd_struct_t fds[] = { + {.fd = thrustercmd_sub, .events = POLLIN}, + }; + + // Create publisher for PWM values + struct actuator_motors_s actuator_motors_msg; + memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); + orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); + + // For publishing debug messages + struct my_message_s my_msg; + memset(&my_msg, 0, sizeof(my_msg)); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); + + // Thruster command structure + struct thruster_command_s thruster_cmd; + + // initialize parameters + parameters_update(true); + bool debugPrint = ScThrusterController::_debug_thruster_print; + PX4_INFO("Debugging enabled: %d", debugPrint); + + while (!should_exit()) { + // wait for up to 1000ms for data + int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + // Timeout: let the loop run anyway, don't do `continue` here + if (debugPrint) { PX4_WARN("Timeout..."); } + + } else if (pret < 0) { + // this is undesirable but not much we can do + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(50000); + continue; + + } else if (fds[0].revents & POLLIN) { + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); + + if (debugPrint) + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, + (double)thruster_cmd.y1, (double)thruster_cmd.y2); + + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; + + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + + if (debugPrint) { + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + } + } + + parameters_update(); + } + + orb_unsubscribe(thrustercmd_sub); } -void ScThrusterController::parameters_update(bool force) { - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s update; - _parameter_update_sub.copy(&update); - - // update parameters from storage - updateParams(); - } +void ScThrusterController::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } } -int ScThrusterController::print_usage(const char* reason) { - if (reason) { - PX4_WARN("%s\n", reason); - } +int ScThrusterController::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( ### Description Section that describes the provided module functionality. diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp index 7fc036f82813..db94f6b78800 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp @@ -58,49 +58,50 @@ using namespace time_literals; -extern "C" __EXPORT int sc_thruster_controller_main(int argc, char* argv[]); +extern "C" __EXPORT int sc_thruster_controller_main(int argc, char *argv[]); -class ScThrusterController : public ModuleBase, public ModuleParams { - public: - ScThrusterController(); +class ScThrusterController : public ModuleBase, public ModuleParams +{ +public: + ScThrusterController(); - virtual ~ScThrusterController() = default; + virtual ~ScThrusterController() = default; - /** @see ModuleBase */ - static int task_spawn(int argc, char* argv[]); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static ScThrusterController* instantiate(int argc, char* argv[]); + /** @see ModuleBase */ + static ScThrusterController *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ - static int custom_command(int argc, char* argv[]); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - /** @see ModuleBase */ - static int print_usage(const char* reason = nullptr); + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; + /** @see ModuleBase::run() */ + void run() override; - /** @see ModuleBase::print_status() */ - int print_status() override; + /** @see ModuleBase::print_status() */ + int print_status() override; - private: - /** - * Check for parameter changes and update them if needed. - * @param parameter_update_sub uorb subscription to parameter_update - * @param force for a parameter update - */ - void parameters_update(bool force = false); +private: + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); - DEFINE_PARAMETERS((ParamInt)_param_sys_autostart, /**< example parameter */ - (ParamInt)_param_sys_autoconfig /**< another parameter */ - ) + DEFINE_PARAMETERS((ParamInt)_param_sys_autostart, /**< example parameter */ + (ParamInt)_param_sys_autoconfig /**< another parameter */ + ) - // Flags - static bool _debug_thruster_print; + // Flags + static bool _debug_thruster_print; - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; }; bool ScThrusterController::_debug_thruster_print = true; From 206de0e968d706feef4537b0960efca55e0584d5 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 14:33:32 +0200 Subject: [PATCH 64/77] fix: disabled unit tests for spacecraft - they need to be designed --- .../AttitudeControl/CMakeLists.txt | 5 ++-- ...Test.cpp => ScAttitudeControlMathTest.cpp} | 0 ...trolTest.cpp => ScAttitudeControlTest.cpp} | 26 +++++++++---------- .../PositionControl/CMakeLists.txt | 5 ++-- ...trolMathTest.cpp => ScControlMathTest.cpp} | 0 ...trolTest.cpp => ScPositionControlTest.cpp} | 18 ++----------- 6 files changed, 21 insertions(+), 33 deletions(-) rename src/modules/sc_att_control/AttitudeControl/{AttitudeControlMathTest.cpp => ScAttitudeControlMathTest.cpp} (100%) rename src/modules/sc_att_control/AttitudeControl/{AttitudeControlTest.cpp => ScAttitudeControlTest.cpp} (88%) rename src/modules/sc_pos_control/PositionControl/{ControlMathTest.cpp => ScControlMathTest.cpp} (100%) rename src/modules/sc_pos_control/PositionControl/{PositionControlTest.cpp => ScPositionControlTest.cpp} (94%) diff --git a/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt index ca8c2c84c185..ab68cd5a74e4 100644 --- a/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt +++ b/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_library(SpacecraftAttitudeControl target_compile_options(SpacecraftAttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_include_directories(SpacecraftAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS SpacecraftAttitudeControl) -px4_add_unit_gtest(SRC AttitudeControlMathTest.cpp LINKLIBS SpacecraftAttitudeControl) +# TODO: Add unit tests +# px4_add_unit_gtest(SRC ScAttitudeControlTest.cpp LINKLIBS SpacecraftAttitudeControl) +# px4_add_unit_gtest(SRC ScAttitudeControlMathTest.cpp LINKLIBS SpacecraftAttitudeControl) diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControlMathTest.cpp b/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlMathTest.cpp similarity index 100% rename from src/modules/sc_att_control/AttitudeControl/AttitudeControlMathTest.cpp rename to src/modules/sc_att_control/AttitudeControl/ScAttitudeControlMathTest.cpp diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlTest.cpp similarity index 88% rename from src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp rename to src/modules/sc_att_control/AttitudeControl/ScAttitudeControlTest.cpp index 2803686c12b1..c7c20c5a62e0 100644 --- a/src/modules/sc_att_control/AttitudeControl/AttitudeControlTest.cpp +++ b/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlTest.cpp @@ -37,19 +37,19 @@ using namespace matrix; -TEST(AttitudeControlTest, AllZeroCase) +TEST(ScAttitudeControlTest, AllZeroCase) { - AttitudeControl attitude_control; + ScAttitudeControl attitude_control; Vector3f rate_setpoint = attitude_control.update(Quatf()); EXPECT_EQ(rate_setpoint, Vector3f()); } -class AttitudeControlConvergenceTest : public ::testing::Test +class ScAttitudeControlConvergenceTest : public ::testing::Test { public: - AttitudeControlConvergenceTest() + ScAttitudeControlConvergenceTest() { - _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f), .4f); + _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f)); _attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f)); } @@ -58,7 +58,7 @@ class AttitudeControlConvergenceTest : public ::testing::Test int i; // need function scope to check how many steps Vector3f rate_setpoint(1000.f, 1000.f, 1000.f); - _attitude_control.setAttitudeSetpoint(_quat_goal, 0.f); + _attitude_control.setAttitudeSetpoint(_quat_goal); for (i = 100; i > 0; i--) { // run attitude control to get rate setpoints @@ -80,12 +80,12 @@ class AttitudeControlConvergenceTest : public ::testing::Test EXPECT_GT(i, 0); } - AttitudeControl _attitude_control; + ScAttitudeControl _attitude_control; Quatf _quat_state; Quatf _quat_goal; }; -TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence) +TEST_F(ScAttitudeControlConvergenceTest, AttitudeControlConvergence) { const int inputs = 8; @@ -112,16 +112,16 @@ TEST_F(AttitudeControlConvergenceTest, AttitudeControlConvergence) } } -TEST(AttitudeControlTest, YawWeightScaling) +TEST(ScAttitudeControlTest, YawWeightScaling) { // GIVEN: default tuning and pure yaw turn command - AttitudeControl attitude_control; + ScAttitudeControl attitude_control; const float yaw_gain = 2.8f; const float yaw_sp = .1f; Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)); - attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), .4f); + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain)); attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f)); - attitude_control.setAttitudeSetpoint(pure_yaw_attitude, 0.f); + attitude_control.setAttitudeSetpoint(pure_yaw_attitude); // WHEN: we run one iteration of the controller Vector3f rate_setpoint = attitude_control.update(Quatf()); @@ -132,7 +132,7 @@ TEST(AttitudeControlTest, YawWeightScaling) EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f); // GIVEN: additional corner case of zero yaw weight - attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain), 0.f); + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain)); // WHEN: we run one iteration of the controller rate_setpoint = attitude_control.update(Quatf()); // THEN: no actuation (also no NAN) diff --git a/src/modules/sc_pos_control/PositionControl/CMakeLists.txt b/src/modules/sc_pos_control/PositionControl/CMakeLists.txt index f2b19319b48a..9b197ae5b185 100644 --- a/src/modules/sc_pos_control/PositionControl/CMakeLists.txt +++ b/src/modules/sc_pos_control/PositionControl/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_library(SpacecraftPositionControl ) target_include_directories(SpacecraftPositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS SpacecraftPositionControl) -px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS SpacecraftPositionControl) +# TODO: add unit tests +# px4_add_unit_gtest(SRC ScControlMathTest.cpp LINKLIBS SpacecraftPositionControl) +# px4_add_unit_gtest(SRC ScPositionControlTest.cpp LINKLIBS SpacecraftPositionControl) diff --git a/src/modules/sc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/sc_pos_control/PositionControl/ScControlMathTest.cpp similarity index 100% rename from src/modules/sc_pos_control/PositionControl/ControlMathTest.cpp rename to src/modules/sc_pos_control/PositionControl/ScControlMathTest.cpp diff --git a/src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/sc_pos_control/PositionControl/ScPositionControlTest.cpp similarity index 94% rename from src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp rename to src/modules/sc_pos_control/PositionControl/ScPositionControlTest.cpp index 0e5a60619a44..a9ce422ed580 100644 --- a/src/modules/sc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/sc_pos_control/PositionControl/ScPositionControlTest.cpp @@ -39,20 +39,7 @@ using namespace matrix; TEST(PositionControlTest, EmptySetpoint) { - PositionControl position_control; - - vehicle_local_position_setpoint_s output_setpoint{}; - position_control.getLocalPositionSetpoint(output_setpoint); - EXPECT_FLOAT_EQ(output_setpoint.x, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.y, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.z, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.yaw, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.yawspeed, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.vx, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.vy, 0.f); - EXPECT_FLOAT_EQ(output_setpoint.vz, 0.f); - EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0)); + ScPositionControl position_control; vehicle_attitude_setpoint_s attitude{}; position_control.getAttitudeSetpoint(attitude); @@ -84,12 +71,11 @@ class PositionControlBasicTest : public ::testing::Test { _position_control.setInputSetpoint(_input_setpoint); const bool ret = _position_control.update(.1f); - _position_control.getLocalPositionSetpoint(_output_setpoint); _position_control.getAttitudeSetpoint(_attitude); return ret; } - PositionControl _position_control; + ScPositionControl _position_control; trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; vehicle_local_position_setpoint_s _output_setpoint{}; vehicle_attitude_setpoint_s _attitude{}; From 0eefd715f56d0a3997c511b18a54db3438b81142 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 15:16:50 +0200 Subject: [PATCH 65/77] add: empy, might fix fails --- .github/workflows/python_checks.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index 1d9cda083e66..7e29d24e38ae 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -18,7 +18,7 @@ jobs: - name: Install Python3 run: sudo apt-get install python3 python3-setuptools python3-pip -y - name: Install tools - run: pip3 install --user mypy types-requests flake8 + run: pip3 install --user mypy types-requests flake8 empy - name: Check MAVSDK test scripts with mypy run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py - name: Check MAVSDK test scripts with flake8 From e4ea746b8c398d644e059ed56e89f82058872e8d Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 15:21:19 +0200 Subject: [PATCH 66/77] fix: reverting changes to workflow --- .github/workflows/python_checks.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index 7e29d24e38ae..1d9cda083e66 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -18,7 +18,7 @@ jobs: - name: Install Python3 run: sudo apt-get install python3 python3-setuptools python3-pip -y - name: Install tools - run: pip3 install --user mypy types-requests flake8 empy + run: pip3 install --user mypy types-requests flake8 - name: Check MAVSDK test scripts with mypy run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py - name: Check MAVSDK test scripts with flake8 From 96de3d92857eb15fa9188e5ecef40c81207896d7 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 15:42:55 +0200 Subject: [PATCH 67/77] feat: updating Gazebo Classic submodule commit --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index a02948399015..f754540e7146 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit a029483990152685d0c366cf35befea195f403f4 +Subproject commit f754540e714642fea897445e69a675245bc6306a From b6169a88eaa8f33b9e4304a3475f85ebbf821adc Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 13 May 2024 16:07:17 +0200 Subject: [PATCH 68/77] fix: submodules --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 8 ++++++++ Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 34b255e2555f..9d11d1e98c4b 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -68,6 +68,14 @@ if(CONFIG_MODULES_MC_RATE_CONTROL) ) endif() +if(CONFIG_MODULES_SC_RATE_CONTROL) + px4_add_romfs_files( + rc.heli_defaults + rc.sc_apps + rc.sc_defaults + ) +endif() + if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( rc.rover_apps diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index f754540e7146..5c15c2efc377 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit f754540e714642fea897445e69a675245bc6306a +Subproject commit 5c15c2efc37773c4d5fedb7dca5afe34fc725b19 From 6a13703abc0a4d4f645a34baca7298c1a3c511ee Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 14 May 2024 09:28:02 +0200 Subject: [PATCH 69/77] fix: fixed styles and configuration files for spacecrafts based on feedback --- CMakeLists.txt | 3 ++- .../airframes/10019_gazebo-classic_omnicopter | 2 +- ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt | 10 +++++++--- ROMFS/px4fmu_common/init.d/rc.sc_apps | 3 ++- ROMFS/px4fmu_common/init.d/rc.sc_defaults | 3 ++- msg/MyMessage.msg | 3 --- 6 files changed, 14 insertions(+), 10 deletions(-) delete mode 100644 msg/MyMessage.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 141c5591508e..2fd9ae728d80 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -516,4 +516,5 @@ if(RES_LEN GREATER 0) PATTERN ".svn" EXCLUDE) else() message(STATUS "No PX4 build files to install.") -endif() \ No newline at end of file +endif() + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter index 37321aae96d3..1a7118e2ccfc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter @@ -7,7 +7,7 @@ # @maintainer Jaeyoung Lim # -. ${R}etc/init.d/rc.sc_apps +. ${R}etc/init.d/rc.mc_apps param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index ef5943e5da1f..5f0980deb3b0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -39,9 +39,6 @@ px4_add_romfs_files( # [22000, 22999] Reserve for custom models - # [70000, 79999] Spacecraft-type platforms - 70000_kth_space_robot - ) if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) @@ -161,3 +158,10 @@ if(CONFIG_MODULES_UUV_ATT_CONTROL) 60002_uuv_bluerov2_heavy ) endif() + +if(CONFIG_MODULES_SC_RATE_CONTROL) + px4_add_romfs_files( + # [70000, 79999] Spacecraft-type platforms + 70000_kth_space_robot + ) +endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index dcbaf87b0b3b..3961a613dbab 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -33,4 +33,5 @@ sc_att_control start sc_pos_control start # Start Space Robot Thruster Controller -sc_thruster_controller start \ No newline at end of file +sc_thruster_controller start + diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 6a9812b13731..a2963f689a40 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -32,4 +32,5 @@ param set-default ATT_EXT_HDG_M 2 param set-default EKF2_EV_CTRL 15 param set-default EKF2_EV_DELAY 5 param set-default EKF2_GPS_CTRL 0 -param set-default EKF2_HGT_REF 3 \ No newline at end of file +param set-default EKF2_HGT_REF 3 + diff --git a/msg/MyMessage.msg b/msg/MyMessage.msg deleted file mode 100644 index 67e087290622..000000000000 --- a/msg/MyMessage.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -char[127] text From db0ed9b6f54cb40bed319b44d190560218549667 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 14 May 2024 09:28:47 +0200 Subject: [PATCH 70/77] fix: init.d cmakelists --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 9d11d1e98c4b..969f4d4b9af5 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -70,7 +70,6 @@ endif() if(CONFIG_MODULES_SC_RATE_CONTROL) px4_add_romfs_files( - rc.heli_defaults rc.sc_apps rc.sc_defaults ) From b2438d76bb16fc221484e000c7f103b89ec33c37 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 14 May 2024 09:36:27 +0200 Subject: [PATCH 71/77] fix: removed MyMessage, replaced with MavlinkLog --- msg/CMakeLists.txt | 1 - platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 5 +++-- .../sc_thruster_controller/sc_thruster_controller.cpp | 6 +++--- .../sc_thruster_controller/sc_thruster_controller.hpp | 2 +- src/modules/uxrce_dds_client/dds_topics.yaml | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index e4ec4a58fb52..86324d5c5d8e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -145,7 +145,6 @@ set(msg_files MissionResult.msg MountOrientation.msg ModeCompleted.msg - MyMessage.msg NavigatorMissionItem.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index b10b869c2414..a8430b5cfb13 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -86,9 +86,10 @@ static int io_timer_handler7(int irq, void *context, void *arg); * We also allow for overrides here but all timer register usage need to be * taken into account */ -#if !defined(BOARD_PWM_FREQ) -// #define BOARD_PWM_FREQ 1000000 +#if !defined(BOARD_PWM_FREQ) && !defined(CONFIG_MODULES_SC_RATE_CONTROL) +#define BOARD_PWM_FREQ 1000000 /* Downscaling by 10 */ +#elif !defined(BOARD_PWM_FREQ) && defined(CONFIG_MODULES_SC_RATE_CONTROL) #define BOARD_PWM_FREQ 100000 #endif diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp index e032310ce1b4..b3aa3ac0506c 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp @@ -136,9 +136,9 @@ void ScThrusterController::run() orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); // For publishing debug messages - struct my_message_s my_msg; + struct mavlink_log_s my_msg; memset(&my_msg, 0, sizeof(my_msg)); - orb_advert_t _my_message_pub = orb_advertise(ORB_ID(my_message), &my_msg); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(mavlink_log), &my_msg); // Thruster command structure struct thruster_command_s thruster_cmd; @@ -183,7 +183,7 @@ void ScThrusterController::run() if (debugPrint) { strcpy(my_msg.text, "Thruster command received!"); - orb_publish(ORB_ID(my_message), _my_message_pub, &my_msg); + orb_publish(ORB_ID(mavlink_log), _my_message_pub, &my_msg); } } diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp index db94f6b78800..49b255f0a4ae 100644 --- a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include using namespace time_literals; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index bd77fa876bd6..39695980bc32 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -90,7 +90,7 @@ publications: type: px4_msgs::msg::LogMessage - topic: /fmu/out/my_message - type: px4_msgs::msg::MyMessage + type: px4_msgs::msg::MavlinkLog - topic: /fmu/out/rc_channels type: px4_msgs::msg::RcChannels From 9ee35c7252fd70269e1266fffb36a37f8cf22764 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 14 May 2024 17:16:08 +0200 Subject: [PATCH 72/77] feat: 6dof spacecraft updates --- .../9001_gazebo-classic_3d_spacecraft | 188 ++++++++++++------ .../gazebo-classic/sitl_gazebo-classic | 2 +- .../sitl_targets_gazebo-classic.cmake | 1 + 3 files changed, 124 insertions(+), 67 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 7443b4c3d13f..ea6488302453 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -12,7 +12,7 @@ param set-default CA_AIRFRAME 14 param set-default MAV_TYPE 25 -param set-default CA_ROTOR_COUNT 8 +param set-default CA_THRUSTER_CNT 12 param set-default CA_R_REV 255 # param set-default FW_ARSP_MODE 1 @@ -24,70 +24,126 @@ param set-default CA_METHOD 0 param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 -param set-default CA_ROTOR0_PX 0.14435 -param set-default CA_ROTOR0_PY -0.14435 -param set-default CA_ROTOR0_KM 0.01697 -param set-default CA_ROTOR0_AX -0.788675 -param set-default CA_ROTOR0_AY -0.211325 -param set-default CA_ROTOR0_AZ -0.57735 - -param set-default CA_ROTOR1_PX -0.14435 -param set-default CA_ROTOR1_PY -0.14435 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR1_AX 0.211325 -param set-default CA_ROTOR1_AY -0.788675 -param set-default CA_ROTOR1_AZ 0.57735 - -param set-default CA_ROTOR2_PX 0.14435 -param set-default CA_ROTOR2_PY 0.14435 -param set-default CA_ROTOR2_KM 0.05 -param set-default CA_ROTOR2_AX -0.211325 -param set-default CA_ROTOR2_AY 0.788675 -param set-default CA_ROTOR2_AZ 0.57735 - -param set-default CA_ROTOR3_PX -0.14435 -param set-default CA_ROTOR3_PY 0.14435 -param set-default CA_ROTOR3_KM 0.05 -param set-default CA_ROTOR3_AX 0.788675 -param set-default CA_ROTOR3_AY 0.211325 -param set-default CA_ROTOR3_AZ -0.57735 - -param set-default CA_ROTOR4_PX 0.14435 -param set-default CA_ROTOR4_PY -0.14435 -param set-default CA_ROTOR4_KM 0.05 -param set-default CA_ROTOR4_AX 0.788675 -param set-default CA_ROTOR4_AY 0.211325 -param set-default CA_ROTOR4_AZ -0.57735 - -param set-default CA_ROTOR5_PX -0.14435 -param set-default CA_ROTOR5_PY -0.14435 -param set-default CA_ROTOR5_KM 0.05 -param set-default CA_ROTOR5_AX -0.211325 -param set-default CA_ROTOR5_AY 0.788675 -param set-default CA_ROTOR5_AZ 0.57735 - -param set-default CA_ROTOR6_PX 0.14435 -param set-default CA_ROTOR6_PY 0.14435 -param set-default CA_ROTOR6_KM 0.05 -param set-default CA_ROTOR6_AX 0.211325 -param set-default CA_ROTOR6_AY -0.788675 -param set-default CA_ROTOR6_AZ 0.57735 - -param set-default CA_ROTOR7_PX -0.14435 -param set-default CA_ROTOR7_PY 0.14435 -param set-default CA_ROTOR7_KM 0.05 -param set-default CA_ROTOR7_AX -0.788675 -param set-default CA_ROTOR7_AY -0.211325 -param set-default CA_ROTOR7_AZ -0.57735 - -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 -param set-default PWM_MAIN_FUNC5 105 -param set-default PWM_MAIN_FUNC6 106 -param set-default PWM_MAIN_FUNC7 107 -param set-default PWM_MAIN_FUNC8 108 +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 + +# Set thrusters +param set-default CA_THRUSTER0_PX -0.50 +param set-default CA_THRUSTER0_PY 0.50 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 0.4 +param set-default CA_THRUSTER0_AX 0.0 +param set-default CA_THRUSTER0_AY -1.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.50 +param set-default CA_THRUSTER1_PY 0.50 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 0.4 +param set-default CA_THRUSTER1_AX 0.0 +param set-default CA_THRUSTER1_AY -1.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX 0.50 +param set-default CA_THRUSTER2_PY -0.50 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 0.4 +param set-default CA_THRUSTER2_AX 0.0 +param set-default CA_THRUSTER2_AY 1.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX -0.50 +param set-default CA_THRUSTER3_PY -0.50 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 0.4 +param set-default CA_THRUSTER3_AX 0.0 +param set-default CA_THRUSTER3_AY 1.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX -0.50 +param set-default CA_THRUSTER4_PY 0.0 +param set-default CA_THRUSTER4_PZ -0.50 +param set-default CA_THRUSTER4_CT 0.4 +param set-default CA_THRUSTER4_AX 1.0 +param set-default CA_THRUSTER4_AY 0.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.50 +param set-default CA_THRUSTER5_PY 0.0 +param set-default CA_THRUSTER5_PZ -0.50 +param set-default CA_THRUSTER5_CT 0.4 +param set-default CA_THRUSTER5_AX -1.0 +param set-default CA_THRUSTER5_AY 0.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX 0.50 +param set-default CA_THRUSTER6_PY 0.0 +param set-default CA_THRUSTER6_PZ 0.50 +param set-default CA_THRUSTER6_CT 0.4 +param set-default CA_THRUSTER6_AX -1.0 +param set-default CA_THRUSTER6_AY 0.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.50 +param set-default CA_THRUSTER7_PY 0.0 +param set-default CA_THRUSTER7_PZ 0.50 +param set-default CA_THRUSTER7_CT 0.4 +param set-default CA_THRUSTER7_AX 1.0 +param set-default CA_THRUSTER7_AY 0.0 +param set-default CA_THRUSTER7_AZ 0.0 + +param set-default CA_THRUSTER8_PX 0.0 +param set-default CA_THRUSTER8_PY -0.50 +param set-default CA_THRUSTER8_PZ -0.50 +param set-default CA_THRUSTER8_CT 0.4 +param set-default CA_THRUSTER8_AX 0.0 +param set-default CA_THRUSTER8_AY 0.0 +param set-default CA_THRUSTER8_AZ 1.0 + +param set-default CA_THRUSTER9_PX 0.0 +param set-default CA_THRUSTER9_PY 0.50 +param set-default CA_THRUSTER9_PZ -0.50 +param set-default CA_THRUSTER9_CT 0.4 +param set-default CA_THRUSTER9_AX 0.0 +param set-default CA_THRUSTER9_AY 0.0 +param set-default CA_THRUSTER9_AZ 1.0 + +param set-default CA_THRUSTER10_PX 0.0 +param set-default CA_THRUSTER10_PY 0.50 +param set-default CA_THRUSTER10_PZ 0.50 +param set-default CA_THRUSTER10_CT 0.4 +param set-default CA_THRUSTER10_AX 0.0 +param set-default CA_THRUSTER10_AY 0.0 +param set-default CA_THRUSTER10_AZ -1.0 + +param set-default CA_THRUSTER11_PX 0.0 +param set-default CA_THRUSTER11_PY -0.50 +param set-default CA_THRUSTER11_PZ 0.50 +param set-default CA_THRUSTER11_CT 0.4 +param set-default CA_THRUSTER11_AX 0.0 +param set-default CA_THRUSTER11_AY 0.0 +param set-default CA_THRUSTER11_AZ -1.0 + +param set-default PWM_MAIN_FUNC1 501 +param set-default PWM_MAIN_FUNC2 502 +param set-default PWM_MAIN_FUNC3 503 +param set-default PWM_MAIN_FUNC4 504 +param set-default PWM_MAIN_FUNC5 505 +param set-default PWM_MAIN_FUNC6 506 +param set-default PWM_MAIN_FUNC7 507 +param set-default PWM_MAIN_FUNC8 508 +param set-default PWM_MAIN_FUNC9 509 +param set-default PWM_MAIN_FUNC10 510 +param set-default PWM_MAIN_FUNC11 511 +param set-default PWM_MAIN_FUNC12 512 + +# PWM Simulation +param set PWM_SIM_PWM_MAX 10000 +param set PWM_SIM_PWM_MIN 0 # Controller Tunings param set-default SC_ROLLRATE_P 0.14 @@ -95,4 +151,4 @@ param set-default SC_PITCHRATE_P 0.14 param set-default SC_ROLLRATE_I 0.3 param set-default SC_PITCHRATE_I 0.3 param set-default SC_ROLLRATE_D 0.004 -param set-default SC_PITCHRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 \ No newline at end of file diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 5c15c2efc377..11693db656c3 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 5c15c2efc37773c4d5fedb7dca5afe34fc725b19 +Subproject commit 11693db656c32891d21a893b8798574354524d8e diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 1e363ab0d144..0805547f6c40 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -120,6 +120,7 @@ if(gazebo_FOUND) windy yosemite space + zero_g ) # find corresponding airframes From 37418439148f1ae83e88414eefbb7958e51e774f Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 15 May 2024 13:09:51 +0200 Subject: [PATCH 73/77] feat: updated thrust of DART spacecraft --- .../9001_gazebo-classic_3d_spacecraft | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index ea6488302453..44e14f930d41 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -35,7 +35,7 @@ param set-default NAV_RCL_ACT 1 param set-default CA_THRUSTER0_PX -0.50 param set-default CA_THRUSTER0_PY 0.50 param set-default CA_THRUSTER0_PZ 0.0 -param set-default CA_THRUSTER0_CT 0.4 +param set-default CA_THRUSTER0_CT 0.237 param set-default CA_THRUSTER0_AX 0.0 param set-default CA_THRUSTER0_AY -1.0 param set-default CA_THRUSTER0_AZ 0.0 @@ -43,7 +43,7 @@ param set-default CA_THRUSTER0_AZ 0.0 param set-default CA_THRUSTER1_PX 0.50 param set-default CA_THRUSTER1_PY 0.50 param set-default CA_THRUSTER1_PZ 0.0 -param set-default CA_THRUSTER1_CT 0.4 +param set-default CA_THRUSTER1_CT 0.237 param set-default CA_THRUSTER1_AX 0.0 param set-default CA_THRUSTER1_AY -1.0 param set-default CA_THRUSTER1_AZ 0.0 @@ -51,7 +51,7 @@ param set-default CA_THRUSTER1_AZ 0.0 param set-default CA_THRUSTER2_PX 0.50 param set-default CA_THRUSTER2_PY -0.50 param set-default CA_THRUSTER2_PZ 0.0 -param set-default CA_THRUSTER2_CT 0.4 +param set-default CA_THRUSTER2_CT 0.237 param set-default CA_THRUSTER2_AX 0.0 param set-default CA_THRUSTER2_AY 1.0 param set-default CA_THRUSTER2_AZ 0.0 @@ -59,7 +59,7 @@ param set-default CA_THRUSTER2_AZ 0.0 param set-default CA_THRUSTER3_PX -0.50 param set-default CA_THRUSTER3_PY -0.50 param set-default CA_THRUSTER3_PZ 0.0 -param set-default CA_THRUSTER3_CT 0.4 +param set-default CA_THRUSTER3_CT 0.237 param set-default CA_THRUSTER3_AX 0.0 param set-default CA_THRUSTER3_AY 1.0 param set-default CA_THRUSTER3_AZ 0.0 @@ -67,7 +67,7 @@ param set-default CA_THRUSTER3_AZ 0.0 param set-default CA_THRUSTER4_PX -0.50 param set-default CA_THRUSTER4_PY 0.0 param set-default CA_THRUSTER4_PZ -0.50 -param set-default CA_THRUSTER4_CT 0.4 +param set-default CA_THRUSTER4_CT 0.237 param set-default CA_THRUSTER4_AX 1.0 param set-default CA_THRUSTER4_AY 0.0 param set-default CA_THRUSTER4_AZ 0.0 @@ -75,7 +75,7 @@ param set-default CA_THRUSTER4_AZ 0.0 param set-default CA_THRUSTER5_PX 0.50 param set-default CA_THRUSTER5_PY 0.0 param set-default CA_THRUSTER5_PZ -0.50 -param set-default CA_THRUSTER5_CT 0.4 +param set-default CA_THRUSTER5_CT 0.237 param set-default CA_THRUSTER5_AX -1.0 param set-default CA_THRUSTER5_AY 0.0 param set-default CA_THRUSTER5_AZ 0.0 @@ -83,7 +83,7 @@ param set-default CA_THRUSTER5_AZ 0.0 param set-default CA_THRUSTER6_PX 0.50 param set-default CA_THRUSTER6_PY 0.0 param set-default CA_THRUSTER6_PZ 0.50 -param set-default CA_THRUSTER6_CT 0.4 +param set-default CA_THRUSTER6_CT 0.237 param set-default CA_THRUSTER6_AX -1.0 param set-default CA_THRUSTER6_AY 0.0 param set-default CA_THRUSTER6_AZ 0.0 @@ -91,7 +91,7 @@ param set-default CA_THRUSTER6_AZ 0.0 param set-default CA_THRUSTER7_PX -0.50 param set-default CA_THRUSTER7_PY 0.0 param set-default CA_THRUSTER7_PZ 0.50 -param set-default CA_THRUSTER7_CT 0.4 +param set-default CA_THRUSTER7_CT 0.237 param set-default CA_THRUSTER7_AX 1.0 param set-default CA_THRUSTER7_AY 0.0 param set-default CA_THRUSTER7_AZ 0.0 @@ -99,7 +99,7 @@ param set-default CA_THRUSTER7_AZ 0.0 param set-default CA_THRUSTER8_PX 0.0 param set-default CA_THRUSTER8_PY -0.50 param set-default CA_THRUSTER8_PZ -0.50 -param set-default CA_THRUSTER8_CT 0.4 +param set-default CA_THRUSTER8_CT 0.237 param set-default CA_THRUSTER8_AX 0.0 param set-default CA_THRUSTER8_AY 0.0 param set-default CA_THRUSTER8_AZ 1.0 @@ -107,7 +107,7 @@ param set-default CA_THRUSTER8_AZ 1.0 param set-default CA_THRUSTER9_PX 0.0 param set-default CA_THRUSTER9_PY 0.50 param set-default CA_THRUSTER9_PZ -0.50 -param set-default CA_THRUSTER9_CT 0.4 +param set-default CA_THRUSTER9_CT 0.237 param set-default CA_THRUSTER9_AX 0.0 param set-default CA_THRUSTER9_AY 0.0 param set-default CA_THRUSTER9_AZ 1.0 @@ -115,7 +115,7 @@ param set-default CA_THRUSTER9_AZ 1.0 param set-default CA_THRUSTER10_PX 0.0 param set-default CA_THRUSTER10_PY 0.50 param set-default CA_THRUSTER10_PZ 0.50 -param set-default CA_THRUSTER10_CT 0.4 +param set-default CA_THRUSTER10_CT 0.237 param set-default CA_THRUSTER10_AX 0.0 param set-default CA_THRUSTER10_AY 0.0 param set-default CA_THRUSTER10_AZ -1.0 @@ -123,7 +123,7 @@ param set-default CA_THRUSTER10_AZ -1.0 param set-default CA_THRUSTER11_PX 0.0 param set-default CA_THRUSTER11_PY -0.50 param set-default CA_THRUSTER11_PZ 0.50 -param set-default CA_THRUSTER11_CT 0.4 +param set-default CA_THRUSTER11_CT 0.237 param set-default CA_THRUSTER11_AX 0.0 param set-default CA_THRUSTER11_AY 0.0 param set-default CA_THRUSTER11_AZ -1.0 From 284f4a0256a58c322b7337d36249ea09bdf15575 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 15 May 2024 14:31:36 +0200 Subject: [PATCH 74/77] feat: added frictionless world --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- .../simulator_mavlink/sitl_targets_gazebo-classic.cmake | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 11693db656c3..eee299cc2e5c 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 11693db656c32891d21a893b8798574354524d8e +Subproject commit eee299cc2e5cba55dc1beeedc76489d60268c764 diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 0805547f6c40..0d5ec6ac8d9a 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -115,6 +115,7 @@ if(gazebo_FOUND) ksql_airport mcmillan_airfield ramped_up_wind + frictionless sonoma_raceway warehouse windy From 2db4428dbeeb5da0122096cfcc461b494dfbad60 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 20 May 2024 13:16:40 +0200 Subject: [PATCH 75/77] fix: compilation issues with fmuv6x --- .../gazebo-classic/sitl_gazebo-classic | 2 +- boards/px4/fmu-v6x/spacecraft.px4board | 19 +++++++++++++++++++ .../SpacecraftPositionControl.hpp | 2 +- .../sc_pos_control/sc_pos_control_params.c | 1 + src/modules/uxrce_dds_client/dds_topics.yaml | 2 +- 5 files changed, 23 insertions(+), 3 deletions(-) create mode 100644 boards/px4/fmu-v6x/spacecraft.px4board diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index eee299cc2e5c..09f233501607 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit eee299cc2e5cba55dc1beeedc76489d60268c764 +Subproject commit 09f2335016074e4706724ce7c44b15c11bf91ce5 diff --git a/boards/px4/fmu-v6x/spacecraft.px4board b/boards/px4/fmu-v6x/spacecraft.px4board new file mode 100644 index 000000000000..c4a125c80496 --- /dev/null +++ b/boards/px4/fmu-v6x/spacecraft.px4board @@ -0,0 +1,19 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_DIFFERENTIAL_DRIVE=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y +CONFIG_MODULES_SC_RATE_CONTROL=y +CONFIG_MODULES_SC_ATT_CONTROL=y +CONFIG_MODULES_SC_POS_CONTROL=y diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp index 685d0240634b..4d15b83bc79f 100644 --- a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -119,7 +119,7 @@ class SpacecraftPositionControl : public ModuleBase, (ParamFloat) _param_mpc_vel_max, (ParamFloat) _param_mpc_vel_cruise, (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_sys_vehicle_resp, (ParamFloat) _param_mpc_acc, (ParamFloat) _param_mpc_acc_max, (ParamFloat) _param_mpc_man_y_max, diff --git a/src/modules/sc_pos_control/sc_pos_control_params.c b/src/modules/sc_pos_control/sc_pos_control_params.c index c01e14310c4f..9196b67306f5 100644 --- a/src/modules/sc_pos_control/sc_pos_control_params.c +++ b/src/modules/sc_pos_control/sc_pos_control_params.c @@ -193,6 +193,7 @@ PARAM_DEFINE_FLOAT(SPC_ACC_MAX, 5.f); */ PARAM_DEFINE_FLOAT(SPC_JERK_AUTO, 4.f); +PARAM_DEFINE_FLOAT(SPC_VEHICLE_RESP, 0.5f); /** * Maximum jerk in Position/Altitude mode * diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 39695980bc32..c42f8059c86d 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -89,7 +89,7 @@ publications: - topic: /fmu/out/log_message type: px4_msgs::msg::LogMessage - - topic: /fmu/out/my_message + - topic: /fmu/out/mavlink_log type: px4_msgs::msg::MavlinkLog - topic: /fmu/out/rc_channels From c5278ca18ddc82cb1add9e8341436280e20dc604 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Tue, 21 May 2024 11:26:59 +0200 Subject: [PATCH 76/77] fix: thruster output range to match hardware --- .../init.d/airframes/70000_kth_space_robot | 2 +- src/lib/mixer_module/mixer_module.cpp | 12 +++++++++--- src/modules/ekf2/module.yaml | 10 ++++++++++ 3 files changed, 20 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index 12357a3d777e..273c01696138 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -19,7 +19,7 @@ param set-default CA_R_REV 255 # param set-default FW_ARSP_MODE 1 # Auto to be provided by Custom Airframe -# param set-default CA_METHOD 0 +param set-default CA_METHOD 0 # disable attitude failure detection param set-default FD_FAIL_P 0 diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 0a3b58ad821c..3a471770ea42 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -533,10 +533,16 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const value = -1.f * value; } - const float output = math::interpolate(value, -1.f, 1.f, + // Thruster is a non-revertible output, so we need to map the value to the correct range + if(_function_assignment[i] >= OutputFunction::Thruster1 && _function_assignment[i] <= OutputFunction::ThrusterMax) { + const float output = math::interpolate(value, 0.f, 1.f, static_cast(_min_value[i]), static_cast(_max_value[i])); - - return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + } else { + const float output = math::interpolate(value, -1.f, 1.f, + static_cast(_min_value[i]), static_cast(_max_value[i])); + return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + } } void diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 9f2f83519637..48af836d8f0b 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -1350,3 +1350,13 @@ parameters: min: 1.0 unit: SD decimal: 1 + EKF2_GRAVITY: + description: + short: Gravity constant + long: Sets the gravity acceleration to be used in the EKF modules. + type: float + default: 9.80665 + min: 0.0 + max: 20.0 + unit: m/s^2 + decimal: 5 \ No newline at end of file From 33a10b4d43b76ab3cf799dbd16924c9207673cbc Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 22 May 2024 15:05:02 +0200 Subject: [PATCH 77/77] fix: failsafe on invalid posctl now is stabilized mode --- .../airframes/9000_gazebo-classic_2d_spacecraft | 8 ++++++++ .../airframes/9001_gazebo-classic_3d_spacecraft | 1 + .../init.d/airframes/70000_kth_space_robot | 10 ++++++++-- src/modules/commander/commander_params.c | 3 ++- src/modules/commander/failsafe/failsafe.cpp | 11 +++++++++++ src/modules/commander/failsafe/failsafe.h | 1 + 6 files changed, 31 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index 57129c2c5fb2..5d17a2239996 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -20,6 +20,14 @@ param set-default CA_R_REV 255 # Auto to be provided by Custom Airframe param set-default CA_METHOD 0 +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + # disable attitude failure detection param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 44e14f930d41..a0ef1317ccb1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -30,6 +30,7 @@ param set-default COM_LOW_BAT_ACT 0 param set-default NAV_DLL_ACT 0 param set-default GF_ACTION 1 param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 # Set thrusters param set-default CA_THRUSTER0_PX -0.50 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index 273c01696138..595b4370521d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -16,11 +16,17 @@ param set-default MAV_TYPE 24 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 255 -# param set-default FW_ARSP_MODE 1 - # Auto to be provided by Custom Airframe param set-default CA_METHOD 0 +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + # disable attitude failure detection param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8f2c3d51ced0..41c7bcb6ba66 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -504,7 +504,8 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * * @value 0 Altitude/Manual * @value 1 Land/Descend - * + * @value 2 Manual + * * @group Commander */ PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 9a9b1e7cad2c..ee12d156e430 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -609,6 +609,17 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ } } + break; + + case position_control_navigation_loss_response::Manual: // Land/Terminate + + // PosCtrl -> Stabilized + if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL + && !modeCanRun(status_flags, user_intended_mode)) { + action = Action::FallbackStab; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB; + } + break; } diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index bb81dd02baeb..8ef224d8388f 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -81,6 +81,7 @@ class Failsafe : public FailsafeBase enum class position_control_navigation_loss_response : int32_t { Altitude_Manual = 0, Land_Descend = 1, + Manual = 2, }; enum class actuator_failure_failsafe_mode : int32_t {