notes.txt

Introduction
============

Betalink is a modified Betaflight firmware that adds one serial command to the
MSP protocol that controls the motor velocity and retrieve the state of the
FC in only one transaction. It adds a motor velocity PID on the FC that uses 
bidirectional DSHOT telemetry as feedback. A command-line program called "betalink"
is provided to test the firmware. A Simulink block is also provided within
the project "RPIt" to interface the FC within Simulink thanks to a dedicated betalink
block.

jacques.gangloff@unistra.fr


Install Betaflight source code
=================================

Into the folder where you installed "betalink", download Betaflight current release source code:
git clone --depth 1 --branch 4.5.1 https://github.com/betaflight/betaflight.git
(Replace "4.5.1" with the current stable Betaflight release number)
Go into the "betaflight" folder
Run "make arm_sdk_install"

Modify Betaflight firmware
==========================

=== Add betalink opcode ===

In src/main/msp/msp.c add at line 158:
extern FAST_DATA_ZERO_INIT uint16_t		blkMotorSpeedRef[MAX_SUPPORTED_MOTORS];
extern FAST_DATA_ZERO_INIT uint16_t		blkMotorFilteredSpeed[MAX_SUPPORTED_MOTORS];
extern FAST_DATA_ZERO_INIT uint8_t		blkPidActiveFlag;

In src/main/msp/msp.c add a new case in function mspFcProcessOutCommandWithArg
near line 2202, in function mspFcProcessOutCommandWithArg, just after "switch (cmdMSP) {":

		case MSP_BETALINK:
		
		// Activate motor velocity control
		blkPidActiveFlag = 1;
		
		// Set motor reference velocity
		for (unsigned i = 0; i < getMotorCount(); i++) {
			if (sbufBytesRemaining(src) >= (int)sizeof(uint16_t)) {
				blkMotorSpeedRef[i] = sbufReadU16(src);
			}
		}
				
		// Send timestamp in us
	    sbufWriteU32(dst, micros());
	      
		// Send battery data
		sbufWriteU16(dst, getBatteryVoltage()); // in 0.01V steps
		sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
		sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
	
		// Send IMU raw data
		{
			#if defined(USE_ACC)
			// Hack scale due to choice of units for sensor data in multiwii

			uint8_t scale;
			if (acc.dev.acc_1G > 512 * 4) {
					scale = 8;
			} else if (acc.dev.acc_1G > 512 * 2) {
					scale = 4;
			} else if (acc.dev.acc_1G >= 512) {
					scale = 2;
			} else {
					scale = 1;
			}
			#endif

			for (int i = 0; i < 3; i++) {
				#if defined(USE_ACC)
				sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
				#else
				sbufWriteU16(dst, 0);
				#endif
			}
			for (int i = 0; i < 3; i++) {
				sbufWriteU16(dst, gyroRateDps(i));
			}
			for (int i = 0; i < 3; i++) {
				#if defined(USE_MAG)
				sbufWriteU16(dst, lrintf(mag.magADC[i]));
				#else
				sbufWriteU16(dst, 0);
				#endif
			}
		}
				
		// Send Euler angles
		sbufWriteU16(dst, attitude.values.roll);
		sbufWriteU16(dst, attitude.values.pitch);
		sbufWriteU16(dst, attitude.values.yaw);
		
		// Send ESC telemetry
		for (unsigned i = 0; i < getMotorCount(); i++) {
			int 		rpm = 0;
			uint16_t 	invalidPct = 10000; // 100.00%

			#ifdef USE_DSHOT_TELEMETRY
			if (motorConfig()->dev.useDshotTelemetry) {
				rpm = (int)blkMotorFilteredSpeed[i];
				#ifdef USE_DSHOT_TELEMETRY_STATS
				if (isDshotMotorTelemetryActive(i)) {
					invalidPct = getDshotTelemetryMotorInvalidPercent(i);
				}
				#endif
			}
			#endif
			sbufWriteU32(dst, rpm);
			sbufWriteU16(dst, invalidPct);
		}
		break;

In src/main/msp/msp_protocol.h add a new opcode at the end of the file:
#define MSP_BETALINK             207    //in & out message    Betalink transaction opcode

=== Add watchdog code ===

In src/main/msp/msp_serial.c line 516, insert following code before
"mspPostProcessFnPtr mspPostProcessFn = NULL;" in function mspSerialProcess:

				// Watchdog: stop motor if too long serial inactivity
				if ( 	millis()-mspPort->lastActivityMs > 
							( 1000 * BLK_SERIAL_WATCHDOG / serialConfig()->serial_update_rate_hz ) )	{
					for (unsigned i = 0; i < getMotorCount(); i++) {
						motor_disarmed[i] = motorConvertFromExternal(PWM_RANGE_MIN);
					}
					blkPidInitFlag = 0;
					blkPidActiveFlag = 0;
				}
				
Line 44:
#include "flight/mixer.h"

#include "pg/motor.h"

#include "rx/rx.h"

Line 51:
extern FAST_DATA_ZERO_INIT uint8_t	blkPidInitFlag;
extern FAST_DATA_ZERO_INIT uint8_t	blkPidActiveFlag;

In src/main/msp/msp_serial.h line 31, insert following code:
#define BLK_SERIAL_WATCHDOG 100			// Number of serial task polling periods

=== Add motor speed regulation on FC ===

In src/main/fc/core.c line 1279, before function "taskMainPidLoop" insert:
// Betalink velocity regulation
#include "../../../../betalink/taskMotorPidLoop.c"

In src/main/fc/core.c, near line 1297, in function "taskMainPidLoop", insert
taskMotorPidLoop();
after
subTaskRcCommand(currentTimeUs);

Compile betalink firmware
===========================

After having made all the source modifications listed above, you can compile the 
modified Betaflight firmware.

In the "betaflight" folder, run "make TARGET=STM32F745"
(Replace "STM32F745" with the name of your target)
(ex: for "IFRC/IFLIGHT_F405_AIO(STM32F405)", TARGET=STM32F405)
Flash the hex file located in obj dir (board_name is stored in config)

Test betalink
=============

"betalink" executable is a CLI program to test basic functionality of the modified 
Betaflight firmware

* Open betalink.c 

* Modify the flags and defines below according to your setup :
// Flags
#define BLK_STANDALONE 										// main is added for testing
#define BLK_THREADED_UPDATE								// activate threaded update
//#define BLK_TWO_FC                        // Test with rwo FCs
//#define BLK_SINGLE_THREAD										// Only one thread per device at a time
//#define BLK_DEBUG													// Display additional debug infos

// Defines

#define BLK_DEV_SERIALNB  3399628743		  // Serial number of the test FC
#define BLK_DEV2_SERIALNB 707372631       // Serial number of the second test FC

If you want to test two betalink modified FC simultaneously, uncomment BLK_TWO_FC
Modify BLK_DEV_SERIALNB according to your FC serial number.
The serial number is displayed by running betalink:
[...]
  dfsfuse63: serial number=3259486894
  nsmb0: serial number=2830586041
  ttys000: serial number=1162087058
  cu.usbmodem0x80000001: serial number=136178235
  tty.usbmodem0x80000001: serial number=2810339146
Error initializing serial port.

In the example above, the FC is connected to tty.usbmodem0x80000001, so the serial
number is 2810339146 and the define becomes:
#define BLK_DEV_SERIALNB  2810339146		  // Serial number of the test FC

* Compile with:
gcc -o betalink -Wall -g -pthread betalink.c

* Run betalink CLI program:
./betalink

Typical output:
[...]
BLK: betalink firmware detected!
> Motor count:              4
> Battery cell count:       0
> Battery capacity [mAh]:   0
> Battery voltage [V]:      0.04
> Battery mAh drawn [mAh]:  0
> Battery current [A]:      0.00
> Accelerometer [m/s/s]:    x:-1.89	y:0.02	z:8.91
> Gyrometer [deg/s]:        x:-0.24	y:-0.30	z:0.06
> Magnetometer [uT]:        x:0.00	y:0.00	z:0.00
> Roll [deg]:               0.00
> Pitch [deg]:              11.90
> Yaw [deg]:                346.60
> Motor #0:
>  Velocity [rpm]:          0
>  Invalid DSHOT [%]:       100.00
>  Temperature [degC]:      0
>  Voltage [V]:             0.00
>  Current [A]:             0.00
>  Consumption [mAh]:       0
> Motor #1:
>  Velocity [rpm]:          0
[...]
#:228490002	[84	us]	ax:-97	ay:1	az:456	gx:-4	gy:-5	gz:1	r:0	p:11	y:346	rpm:0
#:228490002	[36	us]	ax:-97	ay:1	az:456	gx:-4	gy:-5	gz:1	r:0	p:11	y:346	rpm:0
#:228490002	[29	us]	ax:-97	ay:1	az:456	gx:-4	gy:-5	gz:1	r:0	p:11	y:346	rpm:0
#:228490002	[28	us]	ax:-97	ay:1	az:456	gx:-4	gy:-5	gz:1	r:0	p:11	y:346	rpm:0
[...]
# min: 10 us	 max: 231 us	 avg: 34 us

If you get the "BLK: betalink firmware detected!" message, you are running an
operational betalink firmware.

If motors, ESC and battery are connected, the motor velocity reference of the
betalink velocity PID loop switches alternatively from BLK_MIN_VEL to
BLK_MIN_VEL+BLK_STEP_VEL. So you should see the motors spinning.
Don't forget to enable bidirectional DSHOT with betaflight configurator,
since the PID velocity loop relies on this measurement to work.

In the following output:
#:2844768057	[16009	us]	ax:-93	ay:31	az:457	gx:1	gy:-10	gz:1	r:3	p:11	y:65	rpm:0

16009	us indicates the time the FC takes to respond to a command in microseconds
When increasing the "serial_update_rate_hz" on the FC (see section below) to 1000Hz:
- The average response time on a STM32F4 is 10 milliseconds
- The average response time on a STM32F7 is 3 milliseconds

ax:-93	ay:31	az:457 gives the current accelerations
gx:1	gy:-10	gz:1 gives the current gyro measurements
r:3	p:11	y:65 gives the current roll pitch yaw angles
rpm:0 give the current motor rpm

If you get "BLK: Packet with unexpected size received." error messages, it means that
the "betalink" program sends commands too quickly and the FC has not enough time to
process them before a new command arrives. To solve this issue, either tune the FC serial
update rate (see below), increase "BLK_PERIOD" or switch to a non-threaded version of
the transaction by commenting out "BLK_THREADED_UPDATE"

Advanced CLI tuning
===================

To change serial task frequency, in Betaflight configurator CLI:
set serial_update_rate_hz=1000
save

To tune the RPM low pass filter cut frequency to avoid aliasing in CLI:
set rpm_notch_lpf=150
save

Customize Jetson
================

* Customize boot args:
In /boot/extlinux/extlinux.conf, add "usbcore.autosuspend=-1" at the end of the "APPEND" line

* Add rc.local:
cat /etc/rc.local 
#!/bin/bash

# Some instructions

exit 0

sudo nano /etc/systemd/system/rc-local.service
[Unit]
 Description=/etc/rc.local Compatibility
 ConditionPathExists=/etc/rc.local

[Service]
 Type=idle
 ExecStart=/etc/rc.local start
 TimeoutSec=0
 StandardOutput=tty

[Install]
 WantedBy=multi-user.target
 
sudo chmod +x /etc/rc.local

sudo systemctl enable rc-local
sudo systemctl start rc-local.service
sudo systemctl status rc-local.service

* Exit DFU mode on all STM32 connected devices:
sudo apt-get install python-usb python3-usb
Add this code in /etc/rc.local:
# Exit DFU mode on DFU USB devices
sleep 5
until ! pydfu -x 2>/dev/null
do
	sleep 10
done