Add a TODO

builder/assets/hardware_setup.sh

@@ -79,4 +79,14 @@ if ! grep -q "^bcm2835-v4l2" /etc/modules;
 then printf "bcm2835-v4l2\n" >> /etc/modules
 fi
 
-echo_stamp "#8 End of configure hardware interfaces"
+echo_stamp "#8 Check if Compute Module 4"
+if grep -q "Compute Module 4" "/proc/device-tree/model"; then
+  echo_stamp "Enable USB on Compute Module 4"
+  echo "dtoverlay=dwc2,dr_mode=host" >> /boot/config.txt
+
+  echo_stamp "Enable OV7251 camera overlay"
+  echo "#dtoverlay=ov7251 # OV7251 camera on CAM1" >> /boot/config.txt
+  echo "dtoverlay=ov7251cam0 # OV7251 camera on CAM0" >> /boot/config.txt
+fi
+
+echo_stamp "#9 End of configure hardware interfaces"

builder/assets/ov7251cam0-overlay.dts

@@ -0,0 +1,111 @@
+// SPDX-License-Identifier: GPL-2.0-only
+// Definitions for OV7251 camera module on VC I2C bus
+/dts-v1/;
+/plugin/;
+
+//#include <dt-bindings/gpio/gpio.h>
+
+/{
+	compatible = "brcm,bcm2835";
+
+	fragment@0 {
+		target = <&i2c_vc>;
+		__overlay__ {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "okay";
+
+			ov7251: ov7251@60 {
+				compatible = "ovti,ov7251";
+				reg = <0x60>;
+				status = "okay";
+
+				clocks = <&ov7251_clk>;
+				clock-names = "xclk";
+				clock-frequency = <24000000>;
+
+				vdddo-supply = <&ov7251_dovdd>;
+				vdda-supply = <&ov7251_avdd>;
+				vddd-supply = <&ov7251_dvdd>;
+
+				enable-gpios = <&gpio 41 0>;
+
+				port {
+					ov7251_0: endpoint {
+						remote-endpoint = <&csi0_ep>;
+						clock-lanes = <0>;
+						data-lanes = <1>;
+						clock-noncontinuous;
+						link-frequencies =
+							/bits/ 64 <456000000>;
+					};
+				};
+			};
+		};
+	};
+
+	fragment@1 {
+		target = <&csi0>;
+		__overlay__ {
+			status = "okay";
+
+			port {
+				csi0_ep: endpoint {
+					remote-endpoint = <&ov7251_0>;
+					data-lanes = <1>;
+				};
+			};
+		};
+	};
+
+	fragment@2 {
+		target = <&i2c0if>;
+		__overlay__ {
+			status = "okay";
+		};
+	};
+
+	fragment@3 {
+		target-path="/";
+		__overlay__ {
+			ov7251_avdd: fixedregulator@0 {
+				compatible = "regulator-fixed";
+				regulator-name = "ov7251_avdd";
+				regulator-min-microvolt = <2800000>;
+				regulator-max-microvolt = <2800000>;
+			};
+			ov7251_dovdd: fixedregulator@1 {
+				compatible = "regulator-fixed";
+				regulator-name = "ov7251_dovdd";
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+			};
+			ov7251_dvdd: fixedregulator@2 {
+				compatible = "regulator-fixed";
+				regulator-name = "ov7251_dvdd";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1200000>;
+			};
+			ov7251_clk: ov7251-clk {
+				compatible = "fixed-clock";
+				#clock-cells = <0>;
+				clock-frequency = <24000000>;
+			};
+		};
+	};
+
+	fragment@4 {
+		target = <&i2c0mux>;
+		__overlay__ {
+			status = "okay";
+		};
+	};
+
+	fragment@5 {
+		target-path="/__overrides__";
+		__overlay__ {
+			cam0-pwdn-ctrl = <&ov7251>,"enable-gpios:0";
+			cam0-pwdn      = <&ov7251>,"enable-gpios:4";
+		};
+	};
+};

builder/image-build.sh

@@ -15,7 +15,8 @@
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-01-12/2021-01-11-raspios-buster-armhf-lite.zip"
+# https://www.raspberrypi.org/software/operating-systems/#raspberry-pi-os-32-bit
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-03-25/2021-03-04-raspios-buster-armhf-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -102,6 +103,8 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/rsysrot
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.socket' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
+# build hardware drivers
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-hardware.sh'
 # software install
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
 # examples

builder/image-hardware.sh

@@ -0,0 +1,9 @@
+#! /usr/bin/env bash
+
+set -ex
+
+echo "Build overlay for OV7251 camera on CAM0"
+
+# TODO: check Raspberry Pi OS version /boot/issue.txt
+
+dtc -I dts -O dtb -o /boot/overlays/ov7251cam0.dtbo /home/pi/catkin_ws/src/clover/builder/assets/ov7251cam0-overlay.dts

builder/image-network.sh

@@ -60,4 +60,24 @@ domain-needed
 quiet-dhcp6
 EOF
 
-echo_stamp "#4 End of network installation"
+echo_stamp "#4 Build the RTL8814AU Wi-Fi adapter driver"
+cd /home/pi
+git clone https://github.com/aircrack-ng/rtl8812au.git --depth=1
+cd rtl8812au
+echo kernel version: $(uname -r)
+echo kernel version from procfs: $(cat /proc/version)
+echo version: $(git describe --tags --always)
+sed -i 's/CONFIG_PLATFORM_I386_PC = y/CONFIG_PLATFORM_I386_PC = n/g' Makefile # https://github.com/aircrack-ng/rtl8812au#for-raspberry-rpi
+sed -i 's/CONFIG_PLATFORM_ARM_RPI = n/CONFIG_PLATFORM_ARM_RPI = y/g' Makefile
+# sed -i 's/CONFIG_PLATFORM_ARM64_RPI = n/CONFIG_PLATFORM_ARM64_RPI = y/g' Makefile
+apt-cache policy raspberrypi-kernel-headers
+apt-get install -y raspberrypi-kernel-headers dkms
+ls /lib/modules
+KERNEL_VERSION=$(cd /lib/modules && echo *-v7l+) # can't use `uname` as it gives incorrect value in emulated environment
+echo make
+make KERNEL_VER=$KERNEL_VERSION KVER=$KERNEL_VERSION
+echo make install
+make install KERNEL_VER=$KERNEL_VERSION KVER=$KERNEL_VERSION
+# TODO: rm -rf /home/pi/rtl8812au
+
+echo_stamp "#5 End of network installation"

clover/camera_info/ov7251.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+distortion_model: rational_polynomial
+camera_name: main_camera_optical
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [ 279.33, 0, 304.288, 0, 280.417, 268.697, 0, 0, 1, ]
+distortion_coefficients:
+  rows: 1
+  cols: 14
+  data: [ 0.155023, -0.0432218, 0.00152622, 6.48563e-05, 0.00390709, 0.495321, -0.0748121, 0.00553976, 0, 0, 0, 0, 0, 0, ]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [ 1, 0, 0, 0, 1, 0, 0, 0, 1, ]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [ 279.33, 0, 304.288, 0, 0, 280.417, 268.697, 0, 0, 0, 1, 0, ]

clover/launch/clover.launch

@@ -11,7 +11,7 @@
     <arg name="rangefinder_vl53l1x" default="true"/>
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
-    <arg name="rc" default="false"/>
+    <arg name="rc" default="true"/>
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 

clover/launch/mavros.launch

@@ -7,8 +7,7 @@
     <arg name="respawn" default="true"/>
     <arg name="distance_sensor_remap" default="rangefinder/range"/>
     <arg name="usb_device" default="/dev/px4fmu"/>
-    <arg name="prefix" default="" unless="$(eval fcu_conn == 'usb')"/>
+    <arg name="prefix" default="bash -c 'while [ ! -e $(arg usb_device) ]; do sleep 1; done; $0 $@'" if="$(eval fcu_conn == 'usb')"/>
-    <arg name="prefix" default="rosrun clover waitfile $(arg usb_device)" if="$(eval fcu_conn == 'usb')"/>
 
     <node pkg="mavros" type="mavros_node" name="mavros" launch-prefix="$(arg prefix)" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
         <!-- UART connection -->

clover/src/optical_flow.cpp

@@ -206,7 +206,7 @@ private:
 				} catch (const tf2::TransformException& e) {
 					// Invalidate previous frame
 					prev_.release();
-					goto publish_debug;
+					return;
 				}
 			}
 
@@ -218,10 +218,6 @@ private:
 			flow_.quality = (uint8_t)(response * 255);
 			flow_pub_.publish(flow_);
 
-			prev_ = curr_.clone();
-			prev_stamp_ = msg->header.stamp;
-
-publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -238,9 +234,12 @@ publish_debug:
 			static geometry_msgs::TwistStamped velo;
 			velo.header.stamp = msg->header.stamp;
 			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
-			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
+			velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
 			velo_pub_.publish(velo);
+
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
 		}
 	}
 

clover/src/waitfile

@@ -1,9 +0,0 @@
-#!/usr/bin/env bash
-
-# $ ./waitfile <file> <command> <args...> 
-# wait until <file> appears and then invoke <command> with <args>
-
-echo "wait for file $1"
-while [ ! -e "$1" ]; do sleep 1; done;
-echo "file $1 appeared"
-"${@:2}"

clover/www/clover.err

@@ -1 +0,0 @@
-/tmp/clover.err

clover/www/index.html

@@ -1,25 +1,17 @@
-<style>
-	body { font-family: sans-serif; }
-	main { max-width: 600px; margin: 50px auto; }
-</style>
-
 <title>Clover Drone Kit Tools</title>
 
-<main>
+<h1>Clover Drone Kit Tools</h1>
-	<h1>Clover Drone Kit Tools</h1>
 
-	<ul>
+<ul>
-		<li><a href="docs">View documentation</a> (snapshot of <a href="https://clover.coex.tech">clover.coex.tech</a>)</li>
+	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clover.coex.tech">clover.coex.tech</a>)</li>
-		<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
+	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
-		<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
+	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
-		<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
+	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
-		<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
+	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
-		<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
+	<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
-		<li><a href="clover.err">Clover console</a> (<code>/tmp/clover.err</code>)</li>
+</ul>
-	</ul>
 
-	<div class="version"></div>
+<div class="version"></div>
-</main>
 
 <script type="text/javascript">
 	document.querySelector("#wvs").href = location.protocol + '//' + location.hostname + ':8080';
@@ -29,7 +21,7 @@
 	fetch('clover_version').then(function(response) {
 		if (response.status !== 200) return;
 		response.text().then(function(text) {
-			document.querySelector('.version').innerHTML = 'Version: <code>' + text + '</code>';
+			document.querySelector('.version').innerHTML = 'Version: ' + text;
 		});
 	});
 </script>

clover_blocks/src/clover_blocks

@@ -11,8 +11,7 @@
 from __future__ import print_function
 
 import rospy
-import os, sys
+import os
-import traceback
 import threading
 import re
 import uuid
@@ -117,12 +116,7 @@ def run(req):
                 rospy.loginfo('Program forced to stop')
             except Exception as e:
                 rospy.logerr(str(e))
-                traceback.print_exc()
+                error_pub.publish(str(e))
-                etype, value, tb = sys.exc_info()
-                fmt = traceback.format_exception(etype, value, tb)
-                fmt.pop(1) # remove 'clover_blocks' file frame
-                exc_info = ''.join(fmt)
-                error_pub.publish(str(e) + '\n\n' + exc_info)
 
             rospy.loginfo('Program terminated')
             running_lock.release()