beaglebone AI PRU2 unipolar stepper

am5729-beagleboneai-custom.dts

#include "am5729-beagleboneai.dts"  
// make it easy to determine which dtb you're currently running on  
// (via /proc/device-tree/chosen/)  
/ {  
 chosen {  
  base_dtb = "am5729-beagleboneai-custom.dts";  
  base_dtb_timestamp = __TIMESTAMP__;  
 };  
};  
// eventually these should be available in a header  
#define P9_14  (0x3400 + 4 * 107)  
#define P9_16  (0x3400 + 4 * 108)  
#define P9_19a (0x3400 + 4 * 16)  
#define P9_19b (0x3400 + 4 * 95)  
#define P9_20a (0x3400 + 4 * 17)  
#define P9_20b (0x3400 + 4 * 94)  
// enable i2c-3 on P9.19 (scl) + P9.20 (sda)  
&i2c4 {  
 status = "okay";  
 clock-frequency = <400000>;  
 pinctrl-names = "default";  
 pinctrl-0 = <&i2c4_pins>;  
};  
&dra7_pmx_core {  
 i2c4_pins: i2c4 {  
  pinctrl-single,pins = <  
   DRA7XX_CORE_IOPAD( P9_19a, PIN_INPUT_PULLUP | MUX_MODE7  )  // scl  
   DRA7XX_CORE_IOPAD( P9_19b, PIN_INPUT_PULLUP | MUX_MODE14 )  // (shared pin)  
   DRA7XX_CORE_IOPAD( P9_20a, PIN_INPUT_PULLUP | MUX_MODE7  )  // sda  
   DRA7XX_CORE_IOPAD( P9_20b, PIN_INPUT_PULLUP | MUX_MODE14 )  // (shared pin)  
  >;  
 };  
};  
// enable pwm-2 on P9.14 (out-A) + P9.16 (out-B)  
&epwmss2 {  
 status = "okay";  
};  

//enable PRU 2_0
&pruss_soc_bus1 {
 status = "okay";
};
&pruss2 {
 status = "okay";
};
&pru2_0 {
 status = "okay";
};

&ehrpwm2 {  
 status = "okay";  
 pinctrl-names = "default";  
 pinctrl-0 = <&ehrpwm2_pins>;  
};  
&dra7_pmx_core {  
 ehrpwm2_pins: ehrpwm2 {  
  pinctrl-single,pins = <  
   DRA7XX_CORE_IOPAD( P9_14, PIN_OUTPUT_PULLDOWN | MUX_MODE10 )  // out A  
   DRA7XX_CORE_IOPAD( P9_16, PIN_OUTPUT_PULLDOWN | MUX_MODE10 )  // out B  
  >;  
 };  
};  
  
// Here's the obnoxious part: since u-boot doesn't have same pin defaults yet, all pins not  
// explicitly setup above should be overridden here.  This will eventually no longer be needed.  
&cape_pins_default {  
 pinctrl-single,pins = <  
  DRA7XX_CORE_IOPAD( 0x372C, PIN_INPUT_PULLDOWN | MUX_MODE15 ) // P9.11a (no gpio)  
  DRA7XX_CORE_IOPAD( 0x3620, PIN_INPUT          | MUX_MODE14 ) // P9.11b  
  DRA7XX_CORE_IOPAD( 0x36AC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.12  
  DRA7XX_CORE_IOPAD( 0x3730, PIN_INPUT_PULLDOWN | MUX_MODE15 ) // P9.13  (no gpio)  
 // DRA7XX_CORE_IOPAD( 0x35AC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.14  
  DRA7XX_CORE_IOPAD( 0x3514, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.15  
 // DRA7XX_CORE_IOPAD( 0x35B0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.16  
  DRA7XX_CORE_IOPAD( 0x37CC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.17a  
  DRA7XX_CORE_IOPAD( 0x36B8, PIN_INPUT          | MUX_MODE14 ) // P9.17b  
  DRA7XX_CORE_IOPAD( 0x37C8, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.18a  
  DRA7XX_CORE_IOPAD( 0x36B4, PIN_INPUT          | MUX_MODE14 ) // P9.18b  
 // DRA7XX_CORE_IOPAD( 0x3440, PIN_INPUT_PULLUP   | MUX_MODE14 ) // P9.19a  
 // DRA7XX_CORE_IOPAD( 0x357C, PIN_INPUT          | MUX_MODE14 ) // P9.19b  
 // DRA7XX_CORE_IOPAD( 0x3444, PIN_INPUT_PULLUP   | MUX_MODE14 ) // P9.20a  
 // DRA7XX_CORE_IOPAD( 0x3578, PIN_INPUT          | MUX_MODE14 ) // P9.20b  
  DRA7XX_CORE_IOPAD( 0x34F0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.21a  
  DRA7XX_CORE_IOPAD( 0x37C4, PIN_INPUT          | MUX_MODE14 ) // P9.21b  
  DRA7XX_CORE_IOPAD( 0x369C, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.22a  
  DRA7XX_CORE_IOPAD( 0x37C0, PIN_INPUT          | MUX_MODE14 ) // P9.22b  
  DRA7XX_CORE_IOPAD( 0x37B4, PIN_INPUT_PULLUP   | MUX_MODE14 ) // P9.23  
  DRA7XX_CORE_IOPAD( 0x368C, PIN_INPUT_PULLUP   | MUX_MODE14 ) // P9.24  
  DRA7XX_CORE_IOPAD( 0x3694, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.25  
  DRA7XX_CORE_IOPAD( 0x3688, PIN_INPUT_PULLUP   | MUX_MODE14 ) // P9.26a  
  DRA7XX_CORE_IOPAD( 0x3544, PIN_INPUT          | MUX_MODE14 ) // P9.26b  
  DRA7XX_CORE_IOPAD( 0x35A0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.27a  
  DRA7XX_CORE_IOPAD( 0x36B0, PIN_INPUT          | MUX_MODE14 ) // P9.27b  
  DRA7XX_CORE_IOPAD( 0x36E0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.28  
  DRA7XX_CORE_IOPAD( 0x36D8, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.29a  
  DRA7XX_CORE_IOPAD( 0x36A8, PIN_INPUT          | MUX_MODE14 ) // P9.29b  
  DRA7XX_CORE_IOPAD( 0x36DC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.30  
  DRA7XX_CORE_IOPAD( 0x36D4, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.31a  
  DRA7XX_CORE_IOPAD( 0x36A4, PIN_INPUT          | MUX_MODE14 ) // P9.31b  
  DRA7XX_CORE_IOPAD( 0x36A0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.41a  
  DRA7XX_CORE_IOPAD( 0x3580, PIN_INPUT          | MUX_MODE14 ) // P9.41b  
  DRA7XX_CORE_IOPAD( 0x36E4, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P9.42a  
  DRA7XX_CORE_IOPAD( 0x359C, PIN_INPUT          | MUX_MODE14 ) // P9.42b  
  DRA7XX_CORE_IOPAD( 0x379C, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.3  
  DRA7XX_CORE_IOPAD( 0x37A0, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.4  
  DRA7XX_CORE_IOPAD( 0x378C, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.5  
  DRA7XX_CORE_IOPAD( 0x3790, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.6  
  DRA7XX_CORE_IOPAD( 0x36EC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.7  
  DRA7XX_CORE_IOPAD( 0x36F0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.8  
  DRA7XX_CORE_IOPAD( 0x3698, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.9  
  DRA7XX_CORE_IOPAD( 0x36E8, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.10  
  DRA7XX_CORE_IOPAD( 0x3510, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.11  
  DRA7XX_CORE_IOPAD( 0x350C, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.12  
  DRA7XX_CORE_IOPAD( 0x3590, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.13  
  DRA7XX_CORE_IOPAD( 0x3598, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.14  
  DRA7XX_CORE_IOPAD( 0x3570, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.15a  
  DRA7XX_CORE_IOPAD( 0x35B4, PIN_INPUT          | MUX_MODE14 ) // P8.15b  
  DRA7XX_CORE_IOPAD( 0x35BC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.16  
  DRA7XX_CORE_IOPAD( 0x3624, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.17  
  DRA7XX_CORE_IOPAD( 0x3588, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.18  
  DRA7XX_CORE_IOPAD( 0x358C, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.19  
  DRA7XX_CORE_IOPAD( 0x3780, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.20  
  DRA7XX_CORE_IOPAD( 0x377C, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.21  
  DRA7XX_CORE_IOPAD( 0x3798, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.22  
  DRA7XX_CORE_IOPAD( 0x3794, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.23  
  DRA7XX_CORE_IOPAD( 0x3788, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.24  
  DRA7XX_CORE_IOPAD( 0x3784, PIN_INPUT_PULLUP   | MUX_MODE4 ) // P8.25  
  DRA7XX_CORE_IOPAD( 0x35B8, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.26  
  DRA7XX_CORE_IOPAD( 0x35D8, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.27a  
  DRA7XX_CORE_IOPAD( 0x3628, PIN_INPUT          | MUX_MODE14 ) // P8.27b  
  DRA7XX_CORE_IOPAD( 0x35C8, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.28a  
  DRA7XX_CORE_IOPAD( 0x362C, PIN_INPUT          | MUX_MODE14 ) // P8.28b  
  DRA7XX_CORE_IOPAD( 0x35D4, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.29a  
  DRA7XX_CORE_IOPAD( 0x3630, PIN_INPUT          | MUX_MODE14 ) // P8.29b  
  DRA7XX_CORE_IOPAD( 0x35CC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.30a  
  DRA7XX_CORE_IOPAD( 0x3634, PIN_INPUT          | MUX_MODE14 ) // P8.30b  
  DRA7XX_CORE_IOPAD( 0x3614, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.31a  
  DRA7XX_CORE_IOPAD( 0x373C, PIN_INPUT_PULLDOWN | MUX_MODE15 ) // P8.31b (no gpio)  
  DRA7XX_CORE_IOPAD( 0x3618, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.32a  
  DRA7XX_CORE_IOPAD( 0x3740, PIN_INPUT_PULLDOWN | MUX_MODE15 ) // P8.32b (no gpio)  
  DRA7XX_CORE_IOPAD( 0x3610, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.33a  
  DRA7XX_CORE_IOPAD( 0x34E8, PIN_INPUT          | MUX_MODE14 ) // P8.33b  
  DRA7XX_CORE_IOPAD( 0x3608, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.34a  
  DRA7XX_CORE_IOPAD( 0x3564, PIN_INPUT          | MUX_MODE14 ) // P8.34b  
  DRA7XX_CORE_IOPAD( 0x360C, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.35a  
  DRA7XX_CORE_IOPAD( 0x34E4, PIN_INPUT          | MUX_MODE14 ) // P8.35b  
  DRA7XX_CORE_IOPAD( 0x3604, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.36a  
  DRA7XX_CORE_IOPAD( 0x3568, PIN_INPUT          | MUX_MODE14 ) // P8.36b  
  DRA7XX_CORE_IOPAD( 0x35FC, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.37a  
  DRA7XX_CORE_IOPAD( 0x3738, PIN_INPUT_PULLDOWN | MUX_MODE15 ) // P8.37b (no gpio)  
  DRA7XX_CORE_IOPAD( 0x3600, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.38a  
  DRA7XX_CORE_IOPAD( 0x3734, PIN_INPUT_PULLDOWN | MUX_MODE15 ) // P8.38b (no gpio)  
  DRA7XX_CORE_IOPAD( 0x35F4, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.39  
  DRA7XX_CORE_IOPAD( 0x35F8, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.40  
  DRA7XX_CORE_IOPAD( 0x35EC, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.41  
  DRA7XX_CORE_IOPAD( 0x35F0, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.42  
  DRA7XX_CORE_IOPAD( 0x35E4, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.43  
  DRA7XX_CORE_IOPAD( 0x35E8, PIN_INPUT_PULLDOWN | MUX_MODE13 ) // P8.44  
  DRA7XX_CORE_IOPAD( 0x35DC, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.45a  
  DRA7XX_CORE_IOPAD( 0x361C, PIN_INPUT          | MUX_MODE14 ) // P8.45b  
  DRA7XX_CORE_IOPAD( 0x35E0, PIN_INPUT_PULLDOWN | MUX_MODE14 ) // P8.46a  
  DRA7XX_CORE_IOPAD( 0x3638, PIN_INPUT          | MUX_MODE14 ) // P8.46b  
 >;  
};  

nr_stepper.pru2_0.c

/* Controlling unipolar stepper motors with the PRU.
We get counter running over 32bits (by setting CMP0 to UINT_MAX and reset on
CMP0), to avoid dealing with 64bit numbers.
We let the counter wrap around freely.

When we want to trigger a step, we use another comparator and set it in the
future like an alarm clock. We only use uint32_t numbers so that we get nice
modular arithmetics even if the counter has wrapped around.
*/

#include <stdint.h>
#include <limits.h>
#include <pru_cfg.h>
#include <pru_intc.h>
#include <pru_iep.h>
#include <pru_rpmsg.h>
#include <rsc_types.h>
#include "resource_table_0.h"
#include "prugpio.h"

volatile register uint32_t __R30;
volatile register uint32_t __R31;

// Host-0 Interrupt sets bit 30 in register R31
#define HOST_INT            ((uint32_t) 1 << 30)

/* The PRU-ICSS system events used for RPMsg are defined in the Linux device tree
 * PRU0 uses system event 16 (To ARM) and 17 (From ARM)
 * PRU1 uses system event 18 (To ARM) and 19 (From ARM)*/
#define TO_ARM_HOST         16
#define FROM_ARM_HOST       17

/* Using the name 'rpmsg-pru' will probe the rpmsg_pru driver found
 * at linux-x.y.z/drivers/rpmsg/rpmsg_pru.c*/
#define CHAN_NAME           "rpmsg-pru"
#define CHAN_DESC           "Channel 32"
#define CHAN_PORT           32

/* Used to make sure the Linux drivers are ready for RPMsg communication
 * Found at linux-x.y.z/include/uapi/linux/virtio_config.h*/
#define VIRTIO_CONFIG_S_DRIVER_OK   4

char payload[RPMSG_BUF_SIZE];

uint32_t minMotorPeriod = 1100 * 1000; //150Hz per wire at 200MHz
uint32_t maxMotorPeriod = 5 * 1000 * 1000; //2.5Hz per wire at 200MHz
uint16_t maxSpeedHid = 350;
//computed
uint32_t rangeMotorPeriod;
uint32_t speedRatio;
uint32_t squareMaxSpeedHid;

typedef struct {
	uint32_t period;
	// we are going to multiply the period by a power of 2, because some motors have a different gearbox
	uint8_t periodShift;
	uint8_t enabled;
	uint8_t stepIndex;
	uint8_t stepDir;
	// CMP_STS_bit.CMP_HIT and CMP_CFG_bit.CMP_EN bit mask
	uint16_t cmpMask;
	// pointer to the CT_IEP.CMP0_i register
	volatile uint32_t *comparator;
	// how far left the motors pins are in the GPIO register
	uint8_t r30Shift;
} motor_t;

uint32_t computeCmp(uint16_t speedHid) {
	return maxMotorPeriod - (speedRatio * speedHid);
}

void main() {
	rangeMotorPeriod = maxMotorPeriod - minMotorPeriod;
	speedRatio = rangeMotorPeriod / maxSpeedHid;
	squareMaxSpeedHid = maxSpeedHid * maxSpeedHid;

	struct pru_rpmsg_transport transport;
    uint16_t src, dst, len;
    volatile uint8_t *status;

    CT_INTC.SICR_bit.STATUS_CLR_INDEX = FROM_ARM_HOST;

    // Make sure the Linux drivers are ready for RPMsg communication
    status = &resourceTable.rpmsg_vdev.status;
    while (!(*status & VIRTIO_CONFIG_S_DRIVER_OK));
    pru_rpmsg_init(&transport, &resourceTable.rpmsg_vring0, &resourceTable.rpmsg_vring1, TO_ARM_HOST, FROM_ARM_HOST);
    while (pru_rpmsg_channel(RPMSG_NS_CREATE, &transport, CHAN_NAME, CHAN_DESC, CHAN_PORT) != PRU_RPMSG_SUCCESS);

	uint32_t *gpio3 = (uint32_t *)GPIO3;
	uint32_t *gpio5 = (uint32_t *)GPIO5;
	CT_CFG.SYSCFG_bit.STANDBY_INIT = 0;
	CT_IEP.GLB_CFG_bit.CNT_ENABLE = 0;
	CT_IEP.COMPEN_bit.COMPEN_CNT = 0;
	// TESTING TESTING Wrap around, set to 0
	CT_IEP.LOW_COUNTER = UINT_MAX - 100000;
	// by keeping all the high word and hig reset word at the
	CT_IEP.HIGH_COUNTER = 0;
	CT_IEP.HIGH_CNT_RST = 0;
	CT_IEP.GLB_STS_bit.CNT_OVF = 1;
	CT_IEP.CMP_STS_bit.CMP_HIT = 0xFF;

	// using CMP0 to cap the global counter at UINT_MAX, effectively keeping it at 32bits to avoid doing 64bits operations.
	CT_IEP.CMP0_0 = UINT_MAX;
	CT_IEP.CMP1_0 = 0;
	CT_IEP.CMP_CFG_bit.CMP0_RST_CNT_EN = 1;
	CT_IEP.CMP_CFG_bit.CMP_EN = 0b001;
	// let the counter running free
	CT_IEP.GLB_CFG_bit.CNT_ENABLE = 1;

	gpio5[GPIO_SETDATAOUT] = 0 ;
	gpio5[GPIO_CLEARDATAOUT] = 0 | USR1;
	gpio3[GPIO_SETDATAOUT] = 0 ;
	gpio3[GPIO_CLEARDATAOUT] = 0 | USR3 | USR4 | USR0 | USR2;
#define STEP_TRAIN_LEN 4
    // it's like -1, but in modular algebra, the PRU doesn't have signed ops.
	uint8_t backwardsStep = STEP_TRAIN_LEN - 1;
	uint8_t forwardsStep = 1;
	uint8_t stepTrain[STEP_TRAIN_LEN] = {
		0b1001,
		0b0011,
		0b0110,
		0b1100,
	};

#define MOTOR_COUNT 3
	motor_t motors[MOTOR_COUNT] = {{
			.period = 0,
			.periodShift = 0,
			.enabled = 0,
			.stepDir = forwardsStep,
			.stepIndex = 0,
			.cmpMask = 0b00100,
			.comparator = &CT_IEP.CMP0_2,
			.r30Shift = 0
		}, {
			.period = 0,
			.periodShift = 0,
			.enabled = 0,
			.stepDir = forwardsStep,
			.stepIndex = 0,
			.cmpMask = 0b01000,
			.comparator = &CT_IEP.CMP0_3,
			.r30Shift = 4
		}, {
			.period = 0,
			.periodShift = 0,
			.enabled = 0,
			.stepDir = forwardsStep,
			.stepIndex = 0,
			.cmpMask = 0b10000,
			.comparator = &CT_IEP.CMP0_4,
			.r30Shift = 8
		}
	};

	// USE CMP1 as a communicaiton watchdog
	uint32_t commCMP = 500 * 1000 * 1000; // 2Hz
	uint32_t commCmpMask = 0b00010;
	volatile uint32_t *commComparator = &CT_IEP.CMP0_1;

	__R30 = 0;
	while(1) {
		if (__R31 & HOST_INT) {
			uint32_t currentTime = CT_IEP.LOW_COUNTER;
			while (pru_rpmsg_receive(&transport, &src, &dst, payload, &len) == PRU_RPMSG_SUCCESS) {
				// reset comm watchdog
				*commComparator = commCMP + currentTime;
				CT_IEP.CMP_CFG_bit.CMP_EN |= commCmpMask;
				CT_IEP.CMP_STS_bit.CMP_HIT = commCmpMask;
				int16_t *vals = (int16_t* )payload;
				for (uint16_t i = 0; i < MOTOR_COUNT; i++) {
					int16_t val = vals[i];
					uint8_t speedDir = val > 0 ? 1 : 0;
					uint16_t speedHid = abs(val);
					speedHid = speedHid > maxSpeedHid ? maxSpeedHid : speedHid;
					if (speedHid == 0) {
						motors[i].enabled = 0;
						__R30 &= ~(0b1111 << motors[i].r30Shift);
						CT_IEP.CMP_CFG_bit.CMP_EN &= ~motors[i].cmpMask;
					} else {
						motors[i].stepDir = speedDir ? backwardsStep : forwardsStep;
						motors[i].period = computeCmp(speedHid) << motors[i].periodShift;
						// we don't change the current step if moto is running:
						// at low speed USB data
						// comes at a faster rate than the steps and we
						// would be continuously pushing back the comparator before
						// it triggers, never running the wheel
						if (!motors[i].enabled) {
							motors[i].enabled = 1;
							*motors[i].comparator = motors[i].period + currentTime;
						}
						CT_IEP.CMP_CFG_bit.CMP_EN |= motors[i].cmpMask;
					}
				}
			}
		}
		uint32_t hits = CT_IEP.CMP_STS_bit.CMP_HIT;

		if ((hits & commCmpMask) != 0) {
			CT_IEP.CMP_STS_bit.CMP_HIT = commCmpMask;
			// no communcation received for a certain time, cutting all the motors
			for (uint16_t i= 0; i < MOTOR_COUNT; i++) {
				motors[i].enabled = 0;
				CT_IEP.CMP_CFG_bit.CMP_EN &= ~motors[i].cmpMask;
				CT_IEP.CMP_STS_bit.CMP_HIT = motors[i].cmpMask;
				uint32_t r30Mask= 0b1111 << motors[i].r30Shift;
				__R30 &= ~r30Mask;
			}
		}
		for (uint16_t i= 0; i < MOTOR_COUNT; i++) {
			if ((hits & motors[i].cmpMask) != 0) {
				uint32_t currentTime = *motors[i].comparator;
				CT_IEP.CMP_STS_bit.CMP_HIT = motors[i].cmpMask;
				motors[i].stepIndex = (motors[i].stepIndex + motors[i].stepDir) % STEP_TRAIN_LEN;
				uint32_t currentStep = (stepTrain[motors[i].stepIndex]) << motors[i].r30Shift;
				*motors[i].comparator += motors[i].period;
				uint32_t r30Mask= 0b1111 << motors[i].r30Shift;
				__R30 = (__R30 & ~r30Mask) | (r30Mask & (motors[i].enabled ? currentStep : 0));
			}
		}
	}
}

// Turns off LED triggers
#pragma DATA_SECTION(init_pins, ".init_pins")
#pragma RETAIN(init_pins)
const char init_pins[] =
	"/sys/class/leds/beaglebone:green:usr0/trigger\0none\0" \
	"/sys/class/leds/beaglebone:green:usr1/trigger\0none\0" \
	"/sys/class/leds/beaglebone:green:usr2/trigger\0none\0" \
	"/sys/class/leds/beaglebone:green:usr3/trigger\0none\0" \
	"/sys/class/leds/beaglebone:green:usr4/trigger\0none\0" \
	"\0\0";

nr_test_ramp.py

import struct
import time

def main():
	led = True
	rx, ry, rz = 0, 0, 0
	tx, ty, tz = 0, 0, 0
	with open("/dev/rpmsg_pru32", "wb", buffering=False) as fo:
		for d in range(-350, 350, 2):
			fo.write(struct.pack('=hhh', d, d, d));
			time.sleep(0.2)
		fo.write(struct.pack('=hhh', 0, 0, 0));

if __name__ == "__main__":
	main()

nr_test_usb.py

import struct
import math
import numpy as np
import operator
import itertools

# read the 3DConnexion Spacemouse and send the data to the PRU by rpmsg
# the data has to be sent in [-350, 350] in int16_T LE

# pressing the buttons toggles the LED as a demo

RADIUS = 87 # mm
WHEEL_RADIUS = 29 # mm

JOYSTICK_2_WHEELS= (
	[-math.sqrt(3)/2, 0.5, RADIUS],
	[0, -1, RADIUS],
	[math.sqrt(3)/2, 0.5, RADIUS])


def dot(x, y):
     assert len(x) == len(y)
     return sum(itertools.starmap(operator.mul, zip(x, y)))

def matmult(m, v):
     return [int(dot(row, v)) for row in m]

def main():
	led = True
	rx, ry, rz = 0, 0, 0
	tx, ty, tz = 0, 0, 0
	button = 0
	with open("/dev/rpmsg_pru32", "wb", buffering=False) as fo:
		with open('/dev/hidraw0', 'rb+') as hid:
			while True:
				if led:
					hid.write(b'\x04\x01')
				else:
					hid.write(b'\x04\x00')
				data = hid.read(1)[0]
				if data == 3:
					button, end = hid.read(2)
					if button > 0:
						led = not led
				if data == 2 or data == 1:
					val = struct.unpack('=hhh', hid.read(6))
				if data == 2 :
					rx, ry, rz = val
				if data == 1:
					tx, ty, tz = val
				res = matmult(JOYSTICK_2_WHEELS, (ty, tx, 0))
				print("\033[?res: {}".format(res))
				print("Rx: {: 4d}, Ry: {: 4d}, Rz: {: 4d}".format(rx, ry, rz))
				print("Tx: {: 4d}, Ty: {: 4d}, Tz: {: 4d}".format(tx, ty, tz))
				print("\033[4F\033[?25h")
				fo.write(struct.pack('=hhh', *res));

if __name__ == "__main__":
	main()

nr_web.html

<!DOCTYPE html>
<html>
<head>
    <meta charset="utf-8">
    <meta name="viewport" content="initial-scale=1.0, user-scalable=no">
    <meta name="apple-mobile-web-app-capable" content="yes">
    <meta name="apple-mobile-web-app-status-bar-style" content="black">
    <title>BeagleBone Black Demo</title>

<style type="text/css">
    td {
        width: 5em;
        text-align: right;
    }
</style>


</head>
<body>
<h1>Holobot</h1>
Move the input device to get it detected.
<div style="width: 100%; float: left;">
    <div style="width: 40%;float:left">
        <h2>PRU</h2>
        <iframe width="100" height="40" src='/pru'></iframe>
        <div style='display:inline-block'>
            <form method='POST' action="/pru" style=''>
                <input type="hidden" name="action" value="start"/>
                <input type="submit" value="Start PRU"/>
            </form>
            <form method='POST' action="/pru" style=''>
                <input type="hidden" name="action" value="stop"/>
                <input type="submit" value="Stop PRU"/>
            </form>
        </div>
    </div>
    <div style="width: 40%;float:right">
        <h2>Camera</h2>
        <div style='display:inline-block'>
            <form method='POST' action="/camera" style=''>
                <input type="hidden" name="action" value="start"/>
                <input type="submit" value="Start Camera"/>
            </form>
        </div>
    </div>
</div>
<div style="width: 100%; float: left;">
    <div style='width: 40%;float:left'>
        <h2>Input</h2>
        <div>
            <table>
                <tr><td width='5em'>Tx</td><td width='5'>Ty</td><td width='5'>Rz</td></tr>
                <tr><td id="tx">&nbsp;</td><td id="ty"></td><td id="rz"></td></tr>
            </table>

        </div>
    </div>
    <div style='width: 40%;float:right'>
        <h2>Motors</h2>
        <div>
            <table>
                <tr><td width='5em'>A</td><td width='5'>B</td><td width='5'>C</td></tr>
                <tr><td id="ta">&nbsp;</td><td id="tb"></td><td id="tc"></td></tr>
            </table>

        </div>
    </div>
</div>
<h2>WebSocket</h2>
<dl>
    <dt>Buffered Amount:</dt>
    <dd id="bufferedAmount"></dd>
    <dt>Round Trip Time:</dt>
    <dd id="roundTrip"></dd>
    <dt>Last Error:</dt>
    <dd id="error"></dd>
</dl>
</dl>
    <script>
/* global performance */
/* global navigator */
const RADIUS = 87 // mm
const WHEEL_RADIUS = 29 // mm
const JOYSTICK_2_WHEELS = [
    [-Math.sqrt(3) / 2, 0.5, RADIUS / WHEEL_RADIUS / 3],
    [0, -1, RADIUS / WHEEL_RADIUS / 3],
    [Math.sqrt(3) / 2, 0.5, RADIUS / WHEEL_RADIUS / 3]]

var socket = new WebSocket(`ws://${window.location.hostname}:8082`);

socket.onopen = function(event) {
    console.log("WebSocket is open now.");
};
socket.addEventListener('message', function (event) {
    let t1 = performance.now();
    const diff = t1 - t0;
    roundTripTime.textContent = diff.toFixed(3) + 'ms'
    errorElement.textContent = event.data
});


let gamePad = null
let interval = null
let lastTimeStamp = null
let t0 = 0

function sumArray(a) {
    return a.reduce((accumulator, value) => accumulator + value, 0)
}

const motors = ['ta', 'tb', 'tc'].map(id => document.getElementById(id))
console.log('motors', motors)
const inputs = ['tx', 'ty', 'rz'].map(id => document.getElementById(id))
const bufferedAmountElement = document.getElementById('bufferedAmount')
const roundTripTime = document.getElementById('roundTrip')
const errorElement = document.getElementById('error')

function saturate(value) {
    const maxMagnitude = 320
    const arr = [-maxMagnitude, value, maxMagnitude]
    arr.sort((a, b) => a-b)
    console.log(arr)
    return arr[1]
}

window.addEventListener("gamepadconnected", function(e) {
    gamePad = e.gamepad;
    console.log("Gamepad connected at index %d: %s. %d buttons, %d axes.",
        gamePad.index, gamePad.id,
        gamePad.buttons.length, gamePad.axes.length);
    console.log(gamePad)

    interval = setInterval(() => {
        gamePad = navigator.getGamepads()[gamePad.index]
        const timeStamp = gamePad.timestamp
        if (timeStamp != lastTimeStamp) {
            console.log('button', JSON.stringify(gamePad.buttons[0].pressed))
            lastTimeStamp = timeStamp
            // Tx, Ty, Rz
            let vector = [gamePad.axes[1],  gamePad.axes[0],  -gamePad.axes[5]]
            if (gamePad.buttons[0].pressed) {
                vector = [vector[0], 0 , 0]
            }
            const transformedAxes = JOYSTICK_2_WHEELS.map(row => saturate(Math.floor(233 * sumArray(row.map((v, index) => v * vector[index])))))
            const buffer = new  ArrayBuffer(transformedAxes.length*2)
            const view = new DataView(buffer)
            transformedAxes.forEach((a, i) => {
                view.setInt16(i*2, a, true)
            })
            transformedAxes.forEach((a, index) => motors[index].textContent = a)
            vector.forEach((v, index) => {inputs[index].textContent = v.toFixed(3)})
            socket.send(buffer)
            t0 = performance.now();
        }
        bufferedAmountElement.textContent = socket.bufferedAmount
    }, 50);
});
    </script>
</body>
</html>

nr_web.js

const app = require('http').createServer(handler);
const WebSocket = require('ws');
const fs =  require('fs')
const fsp = fs.promises
const b = require('bonescript');
const querystring = require('querystring');
// DOC: https://github.com/fivdi/i2c-bus#api
const i2c = require('i2c-bus');

const CAMERA_I2C_BUS = 3
const CAMERA_I2C_ADDR = 0x3C
const REGISTER = {
  SCCB_ID: 0x3100
}




const port = 8081
app.listen(port);

const wss = new WebSocket.Server({
  port: 8082,
  perMessageDeflate: {
    zlibDeflateOptions: {
      // See zlib defaults.
      chunkSize: 1024,
      memLevel: 7,
      level: 3
    },
    zlibInflateOptions: {
      chunkSize: 10 * 1024
    },
    // Other options settable:
    clientNoContextTakeover: true, // Defaults to negotiated value.
    serverNoContextTakeover: true, // Defaults to negotiated value.
    serverMaxWindowBits: 10, // Defaults to negotiated value.
    // Below options specified as default values.
    concurrencyLimit: 10, // Limits zlib concurrency for perf.
    threshold: 1024 // Size (in bytes) below which messages
    // should not be compressed.
  }
});


console.log('Server running on: http://' + getIPAddress() + ':' + port);

function readBody(stream) {
  return new Promise((resolve, reject) => {
    var bufs = [];
    const data = (d) => {bufs.push(d);}
    const end = () => {
      cleanup();
      resolve(Buffer.concat(bufs))
    }
    const error = (e) => {
      cleanup();
      reject(e)
    }
    const cleanup = () => {
      stream.removeListener('data', data)
      stream.removeListener('end', end)
      stream.removeListener('error', error)
    }
    stream.on('data', data);
    stream.on('end', end);
    stream.on('error', error)
  })
}

async function writeFile(file, data, ignoredErrors = []) {
  const error = new Error()
  try {
    await fsp.writeFile(file, data, { flag:'a' });
  } catch(e) {
    e.stack = error.stack
    e.message = file+' '+e.message
    if(!ignoredErrors.includes(e.code))
      throw e
  }
}

async function sleep(waitTimeInMs) {
  return new Promise(resolve => setTimeout(resolve, waitTimeInMs));
}

async function writeReg(bus, register, value) {
  const buff = Buffer.from([register>>8, register & 0xff, value])
  await bus.i2cWrite(CAMERA_I2C_ADDR, buff.length, buff)
}

async function readReg(bus, register) {
  const buff = Buffer.from([register>>8, register & 0xff])
  await bus.i2cWrite(CAMERA_I2C_ADDR, buff.length, buff)
  const rbuf = Buffer.alloc(1);
  const result = await bus.i2cRead(CAMERA_I2C_ADDR, rbuf.length, rbuf)
  return rbuf[0]
}

async function handler (req, res) {
  console.log('REQ', typeof req.url, `'${req.url}'`, req.method)
  try {
    if (req.url === '/') {
      console.log('in slash')
      const data = await fsp.readFile(__dirname + '/nr_web.html')
      res.writeHead(200);
      res.end(data);
    } else if (req.url === '/pru') {
      console.log('in /pru')
      if (req.method === 'POST') {
        const body = (await readBody(req)).toString('utf-8')
        const parsedPairs = querystring.parse(body)
        await fsp.writeFile('/dev/remoteproc/pruss2-core0/state', parsedPairs['action'],  { flag:'a' })
        console.log('redirecting...')
        res.writeHead(303, {Location: '/'})
        res.end();
      }
      if (req.method === 'GET') {
        res.writeHead(200)
        res.end(await fsp.readFile('/dev/remoteproc/pruss2-core0/state'));
      }
    } else if (req.url === '/camera' && req.method === 'POST') {
      console.log('in /camera')

      await writeFile('/sys/class/pwm/pwmchip0/export', '0', ['EBUSY']);
      await sleep(1000)
      await writeFile('/sys/class/pwm/pwmchip0/pwm-0:0/period', '160');
      await writeFile('/sys/class/pwm/pwmchip0/pwm-0:0/duty_cycle', '80');
      await writeFile('/sys/class/pwm/pwmchip0/pwm-0:0/enable', '1');
      const bus = await i2c.openPromisified(CAMERA_I2C_BUS)
      const result = await readReg(bus, 0x3100)
      console.log('FROM BUS', result)
      for (const [register, value] of ov5640Init) {
        await writeReg(bus, register, value)
      }

      res.writeHead(303, {Location: '/'})
      res.end();
    } else {
      res.writeHead(404);
      res.end();
    }
  } catch (err) {
    console.log('error', err)
    res.writeHead(500);
    res.end(err.toString() );
  }
}

// Get server IP address on LAN
function getIPAddress() {
  var interfaces = require('os').networkInterfaces();
  for (var devName in interfaces) {
    var iface = interfaces[devName];
    for (var i = 0; i < iface.length; i++) {
      var alias = iface[i];
      if (alias.family === 'IPv4' && alias.address !== '127.0.0.1' && !alias.internal)
        return alias.address;
    }
  }
  return '0.0.0.0';
}

wss.on('connection', function connection(ws) {
  ws.on('message', function incoming(message) {
    console.log('message', message)
    fs.writeFile('/dev/rpmsg_pru32', message,  { flag:'a' }, err => {
      ws.send(JSON.stringify(err));
      if (err) console.log('wrote', err)})
  });
});


// https://github.com/ArduCAM/STM32/blob/acef20beeb50f92f1c5dccbce24314acefe4508d/HardWare/ov5640_regs.h#L8
const ov5640Init = [
  [ 0x4740, 0x20 ],

		[ 0x4050, 0x6e ],
    [ 0x4051, 0x8f ],

		[ 0x3008, 0x42 ],
		[ 0x3103, 0x03 ],
		[ 0x3017, 0x7f ],
		[ 0x3018, 0xff ],
		[ 0x302c, 0x02 ],
		[ 0x3108, 0x01 ],
		[ 0x3630, 0x2e ],//2e
		[ 0x3632, 0xe2 ],
		[ 0x3633, 0x23 ],//23
		[ 0x3621, 0xe0 ],
		[ 0x3704, 0xa0 ],
		[ 0x3703, 0x5a ],
		[ 0x3715, 0x78 ],
		[ 0x3717, 0x01 ],
		[ 0x370b, 0x60 ],
		[ 0x3705, 0x1a ],
		[ 0x3905, 0x02 ],
		[ 0x3906, 0x10 ],
		[ 0x3901, 0x0a ],
		[ 0x3731, 0x12 ],
		[ 0x3600, 0x08 ],
		[ 0x3601, 0x33 ],
		[ 0x302d, 0x60 ],
		[ 0x3620, 0x52 ],
		[ 0x371b, 0x20 ],
		[ 0x471c, 0x50 ],

		[ 0x3a18, 0x00 ],
		[ 0x3a19, 0xf8 ],

		[ 0x3635, 0x1c ],//1c
		[ 0x3634, 0x40 ],
		[ 0x3622, 0x01 ],

		[ 0x3c04, 0x28 ],
		[ 0x3c05, 0x98 ],
		[ 0x3c06, 0x00 ],
		[ 0x3c07, 0x08 ],
		[ 0x3c08, 0x00 ],
		[ 0x3c09, 0x1c ],
		[ 0x3c0a, 0x9c ],
		[ 0x3c0b, 0x40 ],

		[ 0x3820, 0x41 ],
		[ 0x3821, 0x01 ], //07

		//windows setup
		[ 0x3800, 0x00 ],
		[ 0x3801, 0x00 ],
		[ 0x3802, 0x00 ],
		[ 0x3803, 0x04 ],
		[ 0x3804, 0x0a ],
		[ 0x3805, 0x3f ],
		[ 0x3806, 0x07 ],
		[ 0x3807, 0x9b ],
		[ 0x3808, 0x05 ],
		[ 0x3809, 0x00 ],
		[ 0x380a, 0x03 ],
		[ 0x380b, 0xc0 ],
		[ 0x3810, 0x00 ],
		[ 0x3811, 0x10 ],
		[ 0x3812, 0x00 ],
		[ 0x3813, 0x06 ],
		[ 0x3814, 0x31 ],
		[ 0x3815, 0x31 ],

		[ 0x3034, 0x1a ],
		[ 0x3035, 0x21 ], //15fps
		[ 0x3036, 0x46 ],
		[ 0x3037, 0x13 ],
		[ 0x3038, 0x00 ],
		[ 0x3039, 0x00 ],

		[ 0x380c, 0x07 ],
		[ 0x380d, 0x68 ],
		[ 0x380e, 0x03 ], //03
		[ 0x380f, 0xd8 ], //d8

		[ 0x3c01, 0xb4 ],
		[ 0x3c00, 0x04 ],
		[ 0x3a08, 0x00 ],
		[ 0x3a09, 0x93 ],
		[ 0x3a0e, 0x06 ],
		[ 0x3a0a, 0x00 ],
		[ 0x3a0b, 0x7b ],
		[ 0x3a0d, 0x08 ],

		[ 0x3a00, 0x3c ], //15fps-10fps
		[ 0x3a02, 0x05 ],
		[ 0x3a03, 0xc4 ],
		[ 0x3a14, 0x05 ],
		[ 0x3a15, 0xc4 ],

		[ 0x3618, 0x00 ],
		[ 0x3612, 0x29 ],
		[ 0x3708, 0x64 ],
		[ 0x3709, 0x52 ],
		[ 0x370c, 0x03 ],

		[ 0x4001, 0x02 ],
		[ 0x4004, 0x02 ],
		[ 0x3000, 0x00 ],
		[ 0x3002, 0x1c ],
		[ 0x3004, 0xff ],
		[ 0x3006, 0xc3 ],
		[ 0x300e, 0x58 ],
		[ 0x302e, 0x00 ],
		[ 0x4300, 0x30 ],
		[ 0x501f, 0x00 ],
		[ 0x4713, 0x03 ],
		[ 0x4407, 0x04 ],
		[ 0x460b, 0x35 ],
		[ 0x460c, 0x22 ],//add by bright
	  [ 0x3824, 0x01 ],//add by bright
		[ 0x5001, 0xa3 ],

		[ 0x3406, 0x01 ],//awbinit
		[ 0x3400, 0x06 ],
		[ 0x3401, 0x80 ],
		[ 0x3402, 0x04 ],
		[ 0x3403, 0x00 ],
		[ 0x3404, 0x06 ],
		[ 0x3405, 0x00 ],
	  //awb
		[ 0x5180, 0xff ],
		[ 0x5181, 0xf2 ],
		[ 0x5182, 0x00 ],
		[ 0x5183, 0x14 ],
		[ 0x5184, 0x25 ],
		[ 0x5185, 0x24 ],
		[ 0x5186, 0x16 ],
		[ 0x5187, 0x16 ],
		[ 0x5188, 0x16 ],
		[ 0x5189, 0x62 ],
		[ 0x518a, 0x62 ],
		[ 0x518b, 0xf0 ],
		[ 0x518c, 0xb2 ],
		[ 0x518d, 0x50 ],
		[ 0x518e, 0x30 ],
		[ 0x518f, 0x30 ],
		[ 0x5190, 0x50 ],
		[ 0x5191, 0xf8 ],
		[ 0x5192, 0x04 ],
		[ 0x5193, 0x70 ],
		[ 0x5194, 0xf0 ],
		[ 0x5195, 0xf0 ],
		[ 0x5196, 0x03 ],
		[ 0x5197, 0x01 ],
		[ 0x5198, 0x04 ],
		[ 0x5199, 0x12 ],
		[ 0x519a, 0x04 ],
		[ 0x519b, 0x00 ],
		[ 0x519c, 0x06 ],
		[ 0x519d, 0x82 ],
		[ 0x519e, 0x38 ],
		//color matrix
		[ 0x5381, 0x1e ],
		[ 0x5382, 0x5b ],
		[ 0x5383, 0x14 ],
		[ 0x5384, 0x06 ],
		[ 0x5385, 0x82 ],
		[ 0x5386, 0x88 ],
		[ 0x5387, 0x7c ],
		[ 0x5388, 0x60 ],
		[ 0x5389, 0x1c ],
		[ 0x538a, 0x01 ],
		[ 0x538b, 0x98 ],
		//sharp&noise
		[ 0x5300, 0x08 ],
		[ 0x5301, 0x30 ],
		[ 0x5302, 0x3f ],
		[ 0x5303, 0x10 ],
		[ 0x5304, 0x08 ],
		[ 0x5305, 0x30 ],
		[ 0x5306, 0x18 ],
		[ 0x5307, 0x28 ],
		[ 0x5309, 0x08 ],
		[ 0x530a, 0x30 ],
		[ 0x530b, 0x04 ],
		[ 0x530c, 0x06 ],
		//gamma
		[ 0x5480, 0x01 ],
		[ 0x5481, 0x06 ],
		[ 0x5482, 0x12 ],
		[ 0x5483, 0x24 ],
		[ 0x5484, 0x4a ],
		[ 0x5485, 0x58 ],
		[ 0x5486, 0x65 ],
		[ 0x5487, 0x72 ],
		[ 0x5488, 0x7d ],
		[ 0x5489, 0x88 ],
		[ 0x548a, 0x92 ],
		[ 0x548b, 0xa3 ],
		[ 0x548c, 0xb2 ],
		[ 0x548d, 0xc8 ],
		[ 0x548e, 0xdd ],
		[ 0x548f, 0xf0 ],
		[ 0x5490, 0x15 ],
		//UV adjust
		[ 0x5580, 0x06 ],
		[ 0x5583, 0x40 ],
		[ 0x5584, 0x20 ],
		[ 0x5589, 0x10 ],
		[ 0x558a, 0x00 ],
		[ 0x558b, 0xf8 ],
		//lens shading
		[ 0x5000, 0xa7 ],
		[ 0x5800, 0x20 ],
		[ 0x5801, 0x19 ],
		[ 0x5802, 0x17 ],
		[ 0x5803, 0x16 ],
		[ 0x5804, 0x18 ],
		[ 0x5805, 0x21 ],
		[ 0x5806, 0x0F ],
		[ 0x5807, 0x0A ],
		[ 0x5808, 0x07 ],
		[ 0x5809, 0x07 ],
		[ 0x580a, 0x0A ],
		[ 0x580b, 0x0C ],
		[ 0x580c, 0x0A ],
		[ 0x580d, 0x03 ],
		[ 0x580e, 0x01 ],
		[ 0x580f, 0x01 ],
		[ 0x5810, 0x03 ],
		[ 0x5811, 0x09 ],
		[ 0x5812, 0x0A ],
		[ 0x5813, 0x03 ],
		[ 0x5814, 0x01 ],
		[ 0x5815, 0x01 ],
		[ 0x5816, 0x03 ],
		[ 0x5817, 0x08 ],
		[ 0x5818, 0x10 ],
		[ 0x5819, 0x0A ],
		[ 0x581a, 0x06 ],
		[ 0x581b, 0x06 ],
		[ 0x581c, 0x08 ],
		[ 0x581d, 0x0E ],
		[ 0x581e, 0x22 ],
		[ 0x581f, 0x18 ],
		[ 0x5820, 0x13 ],
		[ 0x5821, 0x12 ],
		[ 0x5822, 0x16 ],
		[ 0x5823, 0x1E ],
		[ 0x5824, 0x64 ],
		[ 0x5825, 0x2A ],
		[ 0x5826, 0x2C ],
		[ 0x5827, 0x2A ],
		[ 0x5828, 0x46 ],
		[ 0x5829, 0x2A ],
		[ 0x582a, 0x26 ],
		[ 0x582b, 0x24 ],
		[ 0x582c, 0x26 ],
		[ 0x582d, 0x2A ],
		[ 0x582e, 0x28 ],
		[ 0x582f, 0x42 ],
		[ 0x5830, 0x40 ],
		[ 0x5831, 0x42 ],
		[ 0x5832, 0x08 ],
		[ 0x5833, 0x28 ],
		[ 0x5834, 0x26 ],
		[ 0x5835, 0x24 ],
		[ 0x5836, 0x26 ],
		[ 0x5837, 0x2A ],
		[ 0x5838, 0x44 ],
		[ 0x5839, 0x4A ],
		[ 0x583a, 0x2C ],
		[ 0x583b, 0x2a ],
		[ 0x583c, 0x46 ],
		[ 0x583d, 0xCE ],

		[ 0x5688, 0x22 ],
		[ 0x5689, 0x22 ],
		[ 0x568a, 0x42 ],
		[ 0x568b, 0x24 ],
		[ 0x568c, 0x42 ],
		[ 0x568d, 0x24 ],
		[ 0x568e, 0x22 ],
		[ 0x568f, 0x22 ],

		[ 0x5025, 0x00 ],

		[ 0x3a0f, 0x30 ],
		[ 0x3a10, 0x28 ],
		[ 0x3a1b, 0x30 ],
		[ 0x3a1e, 0x28 ],
		[ 0x3a11, 0x61 ],
		[ 0x3a1f, 0x10 ],

		[ 0x4005, 0x1a ],
		[ 0x3406, 0x00 ],//awbinit
    [ 0x3503, 0x00 ],//awbinit
		[ 0x3008, 0x02 ],
		[ 0xffff, 0xff ],
  ]