### Thrust Mapping for Pavo Pico
#### Thrust Test Stand setup

Screw size M3 20mm, and 16mm.

<div style="text-align: center;">
<img src="https://hackmd.io/_uploads/ryyon5D9A.jpg" width="300">
<img src="https://hackmd.io/_uploads/ByDZjDG9A.jpg" width="300">
</div>

#### Pipeline
0. Mount everything on a vertical plane. After mounting, take raw measurements with only the drone/motor. That is our zero Thrust value. Add a little bit of mass like 5g on top of the drone and measure again. Repeat this process for weights upto max thrust that the drone can generate. (Max Thrust = Drone Weight*TWR)
1. Power up the raspberry Pi with 5V adapter, connect laptop to robotlab wifi, and SSH into the raspberry Pi connected to the force sensor.

    SSH address: 10.0.0.50
    Username: ubuntu
    Password: **r0b0t**
    example: ssh ubuntu@10.0.0.50

**N.B**: If you are not sure about the IP address, check `/etc/netplan` by connecting the pi to a display and the check the file `netcfg-usb-ethernet.yaml` for the ehternet IP address and the file `netcfg-wifi.yaml` for wifi IP address. After changing the IP address, you need to run `sudo netplan apply`
2. Connect the Flight controller to the laptop using USB C. Launch Betaflight configurator. Go to the `motors` tab and set the master slider to different values to take thrust measurements at different throttles.

3. Connect the power supply output to the drone's power connector. Set the power supply to the desired voltage. For a single cell LiPo, the maximum voltage = 4.2 V, Nominal voltage = 3.7 V, Minimum voltage = 3.2 V. Therefore, for a 2S battery, 8.4V represents 100% battery, 7.8V represents 73% battery, 7.4V represents 55% battery, and 6.6V represents 18% battery.
4. On the pi side, cd to `~/thrust_test_stand/logging` and run `cargo build`. This will build the rust code for the thrust_stand. Everytime you make a change in the `~/thrust_test_stand/logging/src/main.rs`, you must re-build.
5. To run a measurement, while you set the master motor slider to a specific throttle on Betaflight, on the pi side, you need to run the `pi-tts` executable from `./~/thrust_test_stand/logging/target/debug/pi-tts`. Usage:

    Usage: ./logging/target/debug/pi-tts <filename.csv>

This will save a csv in the current directory with 100 raw measurements and print the mean of those measurements on the terminal. Feel free the change the [main.rs](https://gitlab.developers.cam.ac.uk/cst/prorok-lab/drones/thrust_test_stand/-/blob/main/logging/src/main.rs?ref_type=heads) code if you wish to change the sample size or anything else. (N.B: you might need to run `sudo chmod 777 dev/gpiomem` and `sudo chmod 777 /dev/gpiochip0` before running pi-tts)
7. Note down the mean value printed on the terminal for the corresponding throttle.

#### Results
<div style="text-align: center;">
<img src="https://hackmd.io/_uploads/S1Wqo4Mc0.png" width="400">
</div>

Raw Measurements to Thrust in Newtons mapping:
$y = 394.5 x - 10.34$

- thrust_to_throttle Quadratic for vbat = 8.4V:
$-14.07 x^{2} + 253.9 x + 1136$
- thrust_to_throttle Quadratic for vbat = 7.8V:
$-26.92 x^{2} + 296.3 x + 1162$
- thrust_to_throttle Quadratic for vbat = 7.4V:
$-32.55 x^{2} + 321.5 x + 1166$
- thrust_to_throttle Quadratic for vbat = 6.6V:
$-83.23 x^{2} + 437.2 x + 1176$

#### Data
https://drive.google.com/drive/folders/1hVy3odrlWDYoCE12Rik2i4Z5xniBwE1U?usp=drive_link

### Thrust Mapping for Raven 2207 with Black Props
#### Pipeline
1. Step 0 and Step 1 same as Pavo Pico.
2. Connect the configured Flight controller to your laptop using a USB-C cable and check the /dev port. The flight controller needs to be configured to turn off battery compensation, stabilization, etc. Ask Ajay for more details.
3. On your laptop, open a terminal and cd to `~/projects/raven_thrust_ws` and source ROS2. Run `start_mavros.sh`. Use the correct /dev port for the flight controller and check if the content looks like this:

     ros2 run mavros mavros_node --ros-args --params-file ~/projects/ros2_sanity_ws/src/Freyja/freyja_configfiles/> -p fcu_url:=/dev/ttyACM0:921600 -p tgt_system:=7

Launch `./start_mavros.sh` in this terminal. Check if the correct FC has been recognized.

4. On your laptop, open a terminal and cd to `~/projects/raven_thrust_ws` and source ROS2. Source `setup_ws.sh`. The content of the file looks like the following:

    #!/bin/bash

    source /opt/ros/humble/setup.bash
    echo "running setup.bash!"

    function arm
    {
      ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 400, param1: 1}"
    }
    function disarm
    {
      ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 400, param1: 0}"
    }
    function guided
    {
      ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 176, param1: 1, param2: 20}"
    }
    function stabilized
    {
      ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 176, param1: 80}"
    }
    function throttle
    {
      ros2 topic pub -1 /mavros/setpoint_raw/attitude mavros_msgs/msg/AttitudeTarget "{thrust: $1}"
    }

This bash script sets up functions for arming, disarming, switching to guided mode, and publishing a specific thrust command. Example:
`guided` (throttle values should be printing on the terminal where mavros is running) > `throttle 0.XX` > `arm` > `throttle 0.0` > `disarm`
Repeat this for throttles = [0, 0.02, 0.04, 0.06, 0.08, 0.1, 0.13, 0.15, 0.17, 0.2, 0.25, 0.3, 0.35, 0.4]
5. Connect the power supply output to the drone's power connector. Set the power supply to the desired voltage. For a single cell LiPo, the maximum voltage = 4.2 V, Nominal voltage = 3.7 V, Minimum voltage = 3.2 V. Test points = (4.2,3.9,3.6,3.3). Therefore, for a 4S battery, 16.8V represents 100% battery, 15.6V represents 70% battery, 14.4V represents 40% battery, and 13.2V represents 10% battery.
6. To run a measurement, while you set the throttle to desired value on step 4. And on the pi side, you need to run the `pi-tts` executable from `./~/thrust_test_stand/logging/target/debug/pi-tts`. This will save a csv in the current directory with 100 raw measurements and print the mean of those measurements on the terminal. Feel free the change the [main.rs](https://gitlab.developers.cam.ac.uk/cst/prorok-lab/drones/thrust_test_stand/-/blob/main/logging/src/main.rs?ref_type=heads) code if you wish to change the sample size or anything else.
6. Note down the mean value printed on the terminal for the corresponding throttle.



<div style="text-align: center;">
<img src="https://hackmd.io/_uploads/rJmDO_sj0.jpg" width="400">
<img src="https://hackmd.io/_uploads/r1Hw__sjC.jpg" width="400">
</div>

#### Results
<div style="text-align: center;">
<img src="https://hackmd.io/_uploads/ryoF-vLn0.png" width="400">
<img src="https://hackmd.io/_uploads/ByoY-PU2C.png" width="400">
<img src="https://hackmd.io/_uploads/S1iSzD8hR.png" width="400">
</div>

Raw Measurements to Thrust in Newtons mapping:
$y = -400.7 x + 5.16$

def thrust_to_throttle(thrust: float, coeffs: list):
    value = thrust
    a, b, c = coeffs
    det = b**2 - 4*a*(c - value)
    
    return (-b + np.sqrt(det)) / (2 * a)
    
---------------------------------------

throttle_to_thrust coeff for vbat = 16.8V: 
[ 0.55540583 10.80879523 -0.15239388]
throttle_to_thrust coeff for vbat = 15.6V: 
[-0.14760495 10.43810104 -0.13434255]
throttle_to_thrust coeff for vbat = 14.4V: 
[-1.91057637 10.78123793 -0.05896835]
throttle_to_thrust coeff for vbat = 13.2V: 
[-3.74411458 11.55497434 -0.10923331]

#### Data
https://drive.google.com/drive/folders/1nBHkRRdTQQB7GaVfv3lyz64TGsDI-H9c?usp=sharing