Lab 8 - Preliminary Sea Trials
1 Objectives
2 Updating software
2.1 Merging the branch 2.s01 into your own branch
2.2 Updating software on the vehicle
3 Calibrating the AHRS
3.1 Calibration
3.2 Heading verification (instructor checkoff)
4 Understanding indicator light patterns
5 Ballasting
5.1 Vacuum check (self checkoff)
5.2 In-water ballasting (instructor checkoff)
6 Pre-launch system checks
6.1 Verifying sensors and actuators
6.1.1 Test depth sensor (self checkoff)
6.1.2 Test AHRS (self checkoff)
6.1.3 Test GPS (self checkoff)
6.1.4 Test control surfaces (self checkoff)
6.1.5 Test thrusters (self checkoff)
6.2 Verify vacuum (self checkoff)
6.3 Verify the vehicle axes system (instructor checkoff)
6.3.1 Configuring the cruise
6.3.2 Running the mission and inspecting the vehicle axes system
6.3.3 Adjusting PID gains
7 First in-water deployment
7.1 Simulating the mission (self checkoff)} label{sec_sim
7.1.1 Software-in-the-loop simulations
7.1.2 Hardware-in-the-loop simulations
7.2 Runtime launch (instructor checkoff)
7.2.1 Recovering the vehicle
7.2.2 Killing the mission
7.3 Post-mission data analysis
8 Second in-water deployment
9 Tuning the low-level PID control system
1 Objectives
- Learning how to calibrate a micro-electro mechanical systems (MEMS) inertial measurement unit (IMU).
- Learning how to ballast an AUV.
- Learning how to conduct pre-launch systems checks.
- Conducting in-water deployment.
- Learning how to tune the PID gains of an AUV.
2 Updating software
Before in-water experiments, it is always important to ensure that the vehicle is running up-to-date software.
2.1 Merging the branch 2.s01 into your own branch [top]
At this point, you have created a git branch in missions-seabeaver for your own vehicle. However, the class software updates are typically made to the 2.s01 branch. Therefore, you will need to merge the 2.s01 branch into your own branch using your topside computer. Then you will push the updated branch to the github remote server, so that the latest version of your branch (e.g., avi branch in my case) can also be pulled on to your Raspberry Pi and PocketBeagle.
Before we merge the 2.s01 branch into your own branch, it is good practice to check the status of your branch and make sure there are no uncommitted local changes on your topside laptop:
$ git status On branch avi Your branch is up to date with 'origin/avi'. nothing to commit, working tree clean
In my case, the avi branch does not have any modified files that are not committed, or untracked files. So I am ready to merge. However, if you have untracked files or code modifications that are not committed, you may choose to either destroy these changes using $ git reset --hard command or commit these changes (see the Underwater Navigation lab for more information).
Before we merge, let's ensure that we have downloaded the updates to 2.s01 branch on to your topside computer. To do that, let's switch to that branch using $ git checkout 2.s01 command:
$ git checkout 2.s01 Switched to branch '2.s01' Your branch is up to date with 'origin/2.s01'.
Pull the latest version of 2.s01 branch from the git remote server on to your topside laptop using $ git pull origin 2.s01 command:
$ git pull origin 2.s01 Warning: the ECDSA host key for 'github.com' differs from the key for the IP address '140.82.112.4' Offending key for IP in /home/sb-topside-15/.ssh/known_hosts:4 Matching host key in /home/sb-topside-15/.ssh/known_hosts:60 Are you sure you want to continue connecting (yes/no)? yes From github.com:supun-randeni/missions-seabeaver * branch 2.s01 -> FETCH_HEAD Already up to date.
Let's switch back to your own branch (in my case, the branch named avi) using $ git checkout avi command.
$ git checkout avi Switched to branch 'avi' Your branch is up to date with 'origin/avi'.
Finally we can merge the 2.s01 branch into your own branch using $ git merge 2.s01 command:
$ git merge 2.s01
A terminal window might pop up with nano text editor, asking you to the enter a comment about this merge. You may enter a comment and exit by pressing ctrl+x key. If you and the instructors both have edited the same file (which should not be the case here, but it will likely happen in the future), you will get a warning about a merge conflict. In such situations both parties need to discuss and resolve the merge conflict. In case if you see a merge conflict, please contact the instructors.
By merging 2.s01 branch into your own branch, you have made some changes to your own branch in the topside computer. We call them local changes. Now you need to push them to the git remote server, so the latest version of your branch (e.g., avi branch in my case) can be downloaded on to your Raspberry Pi and PocketBeagle:
$ git push origin avi Warning: the ECDSA host key for 'github.com' differs from the key for the IP address '140.82.113.4' Offending key for IP in /home/sb-topside-15/.ssh/known_hosts:9 Matching host key in /home/sb-topside-15/.ssh/known_hosts:60 Are you sure you want to continue connecting (yes/no)? yes Enumerating objects: 4, done. Counting objects: 100% (4/4), done. Delta compression using up to 16 threads Compressing objects: 100% (2/2), done. Writing objects: 100% (2/2), 282 bytes | 282.00 KiB/s, done. Total 2 (delta 1), reused 0 (delta 0) remote: Resolving deltas: 100% (1/1), completed with 1 local object. To github.com:supun-randeni/missions-seabeaver.git 15e9fd6..fed544d avi -> avi
2.2 Updating software on the vehicle [top]
Now you can update MITFrontseat, MITFrontseat-drivers and missions-seabeaver on your vehicle embedded computers. You can use the build_mitfs.sh script on the Raspberry Pi to update and rebuild software on both Raspberry Pi and PocketBeagle at once.
$ cd ~/missions-seabeaver/build_scripts $ ./build_mitfs.sh
Let it update and build.. Once building is complete, please scroll up and inspect the output to ensure that there are no build errors.
3 Calibrating the AHRS
The magnetometers of the attitude and heading reference system (AHRS) of the AUV can be interfered by magnetic fields of other components in the vehicle. In order to obtain an accurate heading estimate, the impact of such magnetic disturbances must be mitigated. Internal magnetic disturbances that are non-time varying can be accounted for using a hard & soft iron (HSI) calibration. Therefore, once the AUV is fully assembled, we need to re-calibrate its AHRS.
3.1 Calibration [top]
For best performance, the calibration should be conducted in a place where magnetic disturbances are minimum. You can use calibrate_xsens_mti3 program on the PocketBeagle to conduct the calibration.
$ calibrate_xsens_mti3
You will be asked a several questions before and after the calibration. For example:
Press Y if you would like to log the data from your calibration dance . Ctrl-c to exit:
You don't necessarily need to log the calibration data to a file; hence you may enter N.
Once the calibration process starts, you need to carry the AUV and spin in circles (i.e. changing the heading angle) while also changing its pitch and roll at least up to around 30 degrees. Calibration data will be recorded for 60 seconds. Ensure that the calibration is successful. Then you will be asked to enter the latitude and longitude of the test site for magnetic declination correction. Latitude and longitude of the MIT Sailing Pavilion are as follows:
- Latitude: 42.3584
- Longitude: -71.0874
3.2 Heading verification (instructor checkoff) [top]
Once the calibration is complete, you can run the test_xsens_mti3 program to verify the heading measurement from the sensor. This program will output the current roll, pitch and heading angles. When you point the vehicle's nose towards north, the sensor should read around 0 degree.
4 Understanding indicator light patterns
The indicator LEDs on the mast of the AUV will blink in different patterns, indicating various states of the vehicle. It is useful to know what these patterns mean, and what the vehicle is trying to tell you.
If your vehicle does not flash mast LEDs, we might need to enable the daemon service (i.e. the service that automatically runs mitfs_safety_daemon program upon boot as a background process); please talk to an instructor.
- Main pressure hull has no vacuum:
- Red: solid with a quick 100 millisecond blink, once every 3 seconds
- Green: off
- White: off
- Main pressure hull vacuum is leaking:
- Red: 1-second blink; i.e. 1 second on, 1 second off
- Green: off
- White: off
- Main pressure hull vacuum is good. A mission is not launched. Vehicle is idle:
- Red: 200-millisecond blink; i.e. 200 millisecond on, 200 millisecond off
- Green: 200-millisecond blink; i.e. 200 millisecond on, 200 millisecond off
- White: 200-millisecond blink; i.e. 200 millisecond on, 200 millisecond off
- Mission has been launched, time to put the vehicle in the water:
- Red: off
- Green: off
- White: 1-second blink; i.e. 1 second on, 1 second off
- Mission has been launched, time to put the vehicle in the water. It is 10 seconds before the propeller starts spinning:
- Red: off
- Green: off
- White: starts to blink faster
- Mission is active:
- Red: 1-second blink; i.e. 1 second on, 1 second off
- Green: 1-second blink; i.e. 1 second on, 1 second off
- White: solid
- Mission is paused. Do not approach the AUV as it may resume the mission (or approach with caution):
- Red: solid
- Green: 1-second blink; i.e. 1 second on, 1 second off
- White: 1-second blink; i.e. 1 second on, 1 second off
- Mission is complete. You can recover the vehicle:
- Red: 1-second blink; i.e. 1 second on, 1 second off
- Green: solid
- White: 1-second blink; i.e. 1 second on, 1 second off
5 Ballasting
Once the vehicle is fully assembled, and the tether is attached, you need to (re)ballast the vehicle to ensure that the hydrostatic condition of the AUV is appropriate.
5.1 Vacuum check (self checkoff) [top]
First, let's double check that the vacuum is holding. Since mitfs_safety_daemon is now automatically being launched upon boot of the PocketBeagle, you should not re-launch it to check vacuum. Instead, you can run the following program on the PocketBeagle to check vacuum:
$ check_vacuum
5.2 In-water ballasting (instructor checkoff) [top]
If the vacuum is holding, ballast the vehicle to be slightly positively buoyant, with level trim and list angles. Please make sure that the rope is tied to the dock (or your partner is holding it) while ballasting, so that you will not accidentally sink the vehicle, or push it under the dock.
6 Pre-launch system checks
Following pre-launch system checks should be conducted before deploying the vehicle:
6.1 Verifying sensors and actuators [top]
6.1.1 Test depth sensor (self checkoff) [top]
Test if the depth sensor is working properly:
$ test_br_depth
6.1.2 Test AHRS (self checkoff) [top]
Check if the AHRS is working properly, and providing you with sensible roll, pitch and heading angle measurements:
$ test_xsens_mti3
6.1.3 Test GPS (self checkoff) [top]
Test if the GPS is working. You can unit test the GPS by using the linux minicom tool (https://linux.die.net/man/1/minicom). Minicom will display the NMEA strings that the GPS outputs.
$ minicom -D /dev/ttyO0 -b 9600 -w
To exit from minicom, press ctrl+A and then x.
6.1.4 Test control surfaces (self checkoff) [top]
Test the control surfaces; and visually inspect if they are in good working order:
$ test_control_surfaces
6.1.5 Test thrusters (self checkoff) [top]
Test the main thruster:
$ test_br_thruster
6.2 Verify vacuum (self checkoff) [top]
Since mitfs_safety_daemon is now automatically being launched upon boot of the PocketBeagle, you should not re-launch it to check vacuum. Instead, you can run the following program on the PocketBeagle to check vacuum:
$ check_vacuum
6.3 Verify the vehicle axes system (instructor checkoff) [top]
When a new SeaBeaver AUV is built, it is important to verify whether the servos that drive the control surfaces are configured properly. For example, if the control surfaces are turning in the correct direction.
This is achieved by running a bench test mission called bench_test. First, let's inspect what this mission/cruise does. Every mission that is configured by the configure_cruise.sh script has two files associated with it. They are located in missions-seabeaver/cruise directory. For example, bench_test cruise has the following two files associated with it:
- bench_test.def
- bench_test_pMITFS_MissionScript.plug
You may open these files using your favorite text editor and inspect them. The most important parameters in the bench_test.def file are as follows:
// Mission related configurations ----------------------------------------------------------------- #define MISSION_START_TIME 10 #define MISSION_END_TIME 300
They define the mission's start and end times in seconds. The most important parameters in the bench_test_pMITFS_MissionScript.plug file are as follows:
// Configuring the mission profile add_mission: DEPLOY_MODE=CONSTANT_COURSE, mission_start_second=10, heading=150, speed=0.0, depth=1.5, duration_minutes=10 add_mission: deploy_mode=constant_course, mission_start_minute=1, heading=150,speed=0.0, depth=1.5, duration_minutes=10 add_mission: deploy_mode=constant_course, mission_start_minute=2, heading=150,speed=0.0, depth=1.5, duration_minutes=10
Each add_mission: line in the file defines a portion of the overall mission; for example, parameters such as the mission type, mission start time, etc. In the bench_test mission, we have three constant_course runs. Constant course missions simply maintain a constant heading, speed and depth for a given duration (or until the next mission starts). In this mission, all three runs have the same heading, speed and depth values. Therefore the vehicle will try to maintain a 150 degree heading, 1.5m depth and a zero speed until the mission end time (i.e. 300 seconds), which is ideal to check if the servos are in the correct axes system.
6.3.1 Configuring the cruise [top]
Let's configure the architecture, vehicle and cruise. On the Raspberry Pi:
$ cd missions-seabeaver/launch_scripts $ ./configure_architecture.sh seabeaver_ii $ ./configure_vehicle.sh avi $ ./configure_cruise.sh bench_test
Please note that you need to configure the vehicle (i.e. configure_vehicle.sh) with the name of your AUV.
6.3.2 Running the mission and inspecting the vehicle axes system [top]
Now you are ready to launch the bench_test mission. You can launch it with the launch_runtime.sh script:
$ launch_runtime.sh
The mission should start in 10 seconds. The AUV is trying to dive, and go towards a heading angle of 150 degrees. Turn the vehicle around and check if the variation of control surface angles makes sense (i.e. are they trying to correct the course?). If they do not make sense, you might need to change the axis configuration of the control surfaces in your vehicle definition file:
// Actuators axis convention #define TOP_RUD_POSITIVE_INCREASE_HEADING true #define STBD_ELV_POSITIVE_NOSE_UP false #define PORT_ELV_POSITIVE_NOSE_UP true
6.3.3 Adjusting PID gains [top]
If axes of the control surfaces make sense, but their response when you turn the AUV seems unreasonable, you might decide to adjust the PID gains. It is recommended to start with simple PID gain settings; for example:
// ************************************************************************************************ // PID control related // ************************************************************************************************ #define ENABLE_TRI_FIN_HEADING_CONTROL true #define ELEVATOR_HEADING_CONTROL_PERCENT 50 #define PROP_MODE_VEHICLE_PITCH_LIMIT 20 #define PROP_MODE_ROLL_KP 0 #define PROP_MODE_ROLL_KI 0 #define PROP_MODE_ROLL_KD 0 #define PROP_MODE_ROLL_MAX_INTEGRAL 10 #define PROP_MODE_PITCH_KP 0.5 #define PROP_MODE_PITCH_KI 0 #define PROP_MODE_PITCH_KD 0 #define PROP_MODE_PITCH_MAX_INTEGRAL 5 #define PROP_MODE_DEPTH_KP 20 #define PROP_MODE_DEPTH_KI 0.2 #define PROP_MODE_DEPTH_KD 0 #define PROP_MODE_DEPTH_MAX_INTEGRAL 6 #define PROP_MODE_HEADING_KP 0.3 #define PROP_MODE_HEADING_KI 0 #define PROP_MODE_HEADING_KD 0 #define PROP_MODE_HEADING_MAX_INTEGRAL 10 #define PROP_MODE_SPEED_KP 20 #define PROP_MODE_SPEED_KI 0.5 #define PROP_MODE_SPEED_KD 0 #define PROP_MODE_SPEED_MAX_INTEGRAL 50 #define PROP_MODE_SPEED_CURVE 0.9:30 | 0.7:25 | 0.3:10 | 1.3:40
Please note that if you decide to change the PID gains, you should change them on your topside laptop, commit and push the changes to the github remote server, and pull them to the Raspberry Pi and PocketBeagle.
7 First in-water deployment
The first sea-trial of the vehicle is a critical component in the AUV development process. It is typical that you will have to repeat the initial mission a several times to debug, and fix any issues that the vehicle may have. To be safe, we typically run a very short and simple mission as the initial in-water deployment, named a_confidence. Similar to any other mission file (e.g. bench_test mission), a_confidence also has two cruise definition files associated with it:
- a_confidence.def
- a_confidence_pMITFS_MissionScript.plug
You can use your favorite text editor to explore these files. Specifically, take a look at the start and end times defined in the a_confidence.def file:
// Mission related configurations ----------------------------------------------------------------- #define MISSION_START_TIME 60 #define MISSION_END_TIME 120
As you see, the propeller will start to spin 60 seconds after you launched the mission. It will end (i.e. mission completion) at 120 seconds (i.e. one minute of flight time). The mission parameters are defined in the a_confidence_pMITFS_MissionScript.plug file:
// Configuring the mission profile add_mission: DEPLOY_MODE=CONSTANT_COURSE, mission_start_second=60, heading=150, speed=0.8, depth=1.0, duration_minutes=2
As you see, there is only one constant course mission in the mission profile, which will command the vehicle to go at a heading of 150 degree, depth of 1 m and a speed of 0.8 m/s.
7.1 Simulating the mission (self checkoff)} label{sec_sim [top]
It is always a good idea to simulate the mission before running it in the water; both in software-in-the-loop and hardware-in-the-loop methods. At this point you should know how to run software and hardware in the loop simulations.
7.1.1 Software-in-the-loop simulations [top]
On the topside computer, you can use the following commands to configure and simulate the mission:
$ cd missions-seabeaver/launch_scripts $ ./configure_architecture.sh seabeaver_ii $ ./configure_vehicle.sh avi $ ./configure_cruise.sh a_confidence $ ./launch_simulation.sh
Please note that you need to configure the vehicle (i.e. configure_vehicle.sh) with the name of your AUV.
7.1.2 Hardware-in-the-loop simulations [top]
As you know at this stage, running a mission in hardware-in-the-loop simulations is a two step process:
- Configuring and running the topside community.
- Configuring and running the vehicle communities.
Following commands can be used on the topside computer to configure and run the topside community:
$ cd missions-seabeaver/launch_scripts $ ./configure_architecture.sh seabeaver_ii $ ./configure_vehicle.sh avi $ ./configure_cruise.sh a_confidence $ launch_topside.sh
Following commands can be used on the Raspberry to configure and run the vehicle backseat and frontseat communities:
$ cd missions-seabeaver/launch_scripts $ ./configure_architecture.sh seabeaver_ii $ ./configure_vehicle.sh avi $ ./configure_cruise.sh a_confidence $ launch_hitl.sh
When you are running hardware-in-the-loop simulations on the actual AUV, please do not run the thruster for more than 30 seconds continuously. Prolonged operation in air can lead to overheating of the thruster.
7.2 Runtime launch (instructor checkoff) [top]
When you are happy with the mission and vehicle, and you know what the vehicle is supposed to do, you can coordinate with the instructors to do the first in-water experiment.
Configure the vehicle using following commands on the Raspberry Pi:
$ cd missions-seabeaver/launch_scripts $ ./configure_architecture.sh seabeaver_ii $ ./configure_vehicle.sh avi $ ./configure_cruise.sh a_confidence
Please note that you need to configure the vehicle (i.e. configure_vehicle.sh) with the name of your AUV.
When you are ready to deploy the vehicle, and the river is clear of boat traffic, launch the mission using the following command:
$ launch_runtime.sh
Now you have 60 seconds to place the vehicle in the water. The white LED indicator will start to blink faster 10 seconds before the propeller start to spin.
Please consider that the thruster is live at all times, and do NOT poke fingers inside the thruster!
7.2.1 Recovering the vehicle [top]
When the vehicle completed the mission (i.e. 60 seconds after the propeller started spinning in this case), the AUV will surface, and you may recover it.
7.2.2 Killing the mission [top]
Once you brought the AUV back to the lab (i.e. close to the router), the AUV will automatically reconnect to the SeaBeaverNetwork wifi network. Then the terminal window on your topside laptop will resume, and you can enter ctrl+c to kill the mission.
7.3 Post-mission data analysis [top]
Now we can download the vehicle log files from the Raspberry Pi to your topside laptop (using scp command), and visualize/analyze data using the alogview tool. Let's create a directory on your topside laptop where you want to store your logs. For instance, I have created a folder named avi_logs, and within it, a sub-folder with today's date:
$ cd ~ $ mkdir avi_logs $ cd avi_logs $ mkdir 20240430 $ cd 20240507
Now you can copy the logs to this folder using secure-copy:
$ scp -r seabeaver-raspi@192.168.0.50:/home/seabeaver-raspi/missions-seabeaver/logs/LOG* .
Now you can visualize these logs using alogview, as you have done in previous labs. One interesting measurement that you might want to observe is the variation of water temperature at different depths and locations in the river. This is logged under EXTERNAL_TEMPERATURE MOOS variable.
8 Second in-water deployment
When you are confident with your vehicle, you can run a more courageous mission named b_zigzag. This is a 7.5-minute long mission, where the vehicle will conduct several different constant course runs. You can inspect the b_zigzag.def and b_zigzag_pMITFS_MissionScript.plug files and observe the mission profile that you are about to run.
It is recommended to simulate the mission in software-in-the-loop as well as hardware-in-the-loop before launching it in-water. You can follow the steps outlined in Section ??? to do that. Once you are happy with the mission, and you know what to expect, you can coordinate with the instructors to carryout the b_zigzag mission. Configure the vehicle using following commands on the Raspberry Pi:
$ cd missions-seabeaver/launch_scripts $ ./configure_architecture.sh seabeaver_ii $ ./configure_vehicle.sh avi $ ./configure_cruise.sh b_zigzag
When you are ready to deploy the vehicle, and the river is clear of boat traffic, launch the mission using the following command:
$ launch_runtime.sh
Once you recovered the vehicle, kill the mission, download the logs, and analyze the data.
9 Tuning the low-level PID control system
At this stage, you may decide to further tune the PID gains, depending on the control performance. You should change them on your topside laptop, commit and push the changes to the github remote server, and pull them to the Raspberry Pi and PocketBeagle.
Document Maintained by: supun@mit.edu
Page built from LaTeX source using texwiki, developed at MIT. Errata to issues@moos-ivp.org. Get PDF