Merge "Update phoenix to alpha"
diff --git a/WORKSPACE b/WORKSPACE
index a824e0c..49586b6 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -251,11 +251,11 @@
load("@com_grail_bazel_toolchain//toolchain:rules.bzl", "llvm", "llvm_toolchain")
-llvm_version = "13.0.0"
+llvm_version = "16.0.3"
llvm(
name = "llvm_k8",
- distribution = "clang+llvm-%s-x86_64-linux-gnu-ubuntu-16.04.tar.xz" % llvm_version,
+ distribution = "clang+llvm-%s-x86_64-linux-gnu-ubuntu-22.04.tar.xz" % llvm_version,
llvm_version = llvm_version,
)
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index 1dbcd44..18d2ce1 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -51,8 +51,6 @@
return 0;
}
- uint64_t message_count = 0;
-
aos::monotonic_clock::time_point next_send_time =
aos::monotonic_clock::min_time;
@@ -74,7 +72,6 @@
cli_info.event_loop->MakeRawFetcher(channel);
if (fetcher->Fetch()) {
printer.PrintMessage(channel, fetcher->context());
- ++message_count;
}
}
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 1ed4337..827dd46 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -113,15 +113,15 @@
public:
using SharedSpan = std::shared_ptr<const absl::Span<const uint8_t>>;
- enum class [[nodiscard]] Error{
- // Represents success and no error
- kOk,
+ enum class [[nodiscard]] Error {
+ // Represents success and no error
+ kOk,
- // Error for messages on channels being sent faster than their
- // frequency and channel storage duration allow
- kMessagesSentTooFast,
- // Access to Redzone was attempted in Sender Queue
- kInvalidRedzone,
+ // Error for messages on channels being sent faster than their
+ // frequency and channel storage duration allow
+ kMessagesSentTooFast,
+ // Access to Redzone was attempted in Sender Queue
+ kInvalidRedzone,
};
RawSender(EventLoop *event_loop, const Channel *channel);
diff --git a/aos/events/logging/multinode_logger_test_lib.cc b/aos/events/logging/multinode_logger_test_lib.cc
index 64dd1cd..9b3e00c 100644
--- a/aos/events/logging/multinode_logger_test_lib.cc
+++ b/aos/events/logging/multinode_logger_test_lib.cc
@@ -544,14 +544,12 @@
}
std::vector<std::tuple<std::string, std::string, int>> result;
- int channel = 0;
for (size_t i = 0; i < counts.size(); ++i) {
if (counts[i] != 0) {
const Channel *channel = config->channels()->Get(i);
result.push_back(std::make_tuple(channel->name()->str(),
channel->type()->str(), counts[i]));
}
- ++channel;
}
return result;
diff --git a/aos/logging/logging.h b/aos/logging/logging.h
index c71e5c4..8d83037 100644
--- a/aos/logging/logging.h
+++ b/aos/logging/logging.h
@@ -51,7 +51,7 @@
// It's currently using __PRETTY_FUNCTION__ because both GCC and Clang support
// that and it gives nicer results in C++ than the standard __func__ (which
// would also work).
-//#define LOG_CURRENT_FUNCTION __PRETTY_FUNCTION__
+// #define LOG_CURRENT_FUNCTION __PRETTY_FUNCTION__
#define LOG_CURRENT_FUNCTION __func__
#define LOG_SOURCENAME __FILE__
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index 19f3420..34f578e 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -62,7 +62,6 @@
const Node *my_node,
const Node *other_node) {
std::vector<bool> stream_reply_with_timestamp;
- int channel_index = 0;
for (const Channel *channel : *config->channels()) {
if (configuration::ChannelIsSendableOnNode(channel, other_node)) {
const Connection *connection =
@@ -78,7 +77,6 @@
configuration::ChannelMessageIsLoggedOnNode(channel, my_node));
}
}
- ++channel_index;
}
return stream_reply_with_timestamp;
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 55b6b02..b4ebef4 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -1820,7 +1820,6 @@
}
if (filter_fps_.size() != 0 && !destructor) {
- size_t node_a_index = 0;
for (const auto &filters : filters_per_node_) {
for (const auto &filter : filters) {
while (true) {
@@ -1832,7 +1831,6 @@
WriteFilter(filter.filter, *sample);
}
}
- ++node_a_index;
}
}
diff --git a/aos/network/rawrtc.h b/aos/network/rawrtc.h
index f43cc5e..7908124 100644
--- a/aos/network/rawrtc.h
+++ b/aos/network/rawrtc.h
@@ -170,9 +170,7 @@
void Open();
// Returns the connection if Open has been called.
- struct rawrtc_peer_connection *connection() {
- return connection_;
- }
+ struct rawrtc_peer_connection *connection() { return connection_; }
private:
// Trampolines from C -> C++.
diff --git a/aos/network/sctp_client.h b/aos/network/sctp_client.h
index 06f6b15..9b9265e 100644
--- a/aos/network/sctp_client.h
+++ b/aos/network/sctp_client.h
@@ -44,9 +44,7 @@
void SetPriorityScheduler(sctp_assoc_t assoc_id);
// Remote to send to.
- struct sockaddr_storage sockaddr_remote() const {
- return sockaddr_remote_;
- }
+ struct sockaddr_storage sockaddr_remote() const { return sockaddr_remote_; }
void LogSctpStatus(sctp_assoc_t assoc_id);
diff --git a/compilers/yocto_orin_rootfs.BUILD b/compilers/yocto_orin_rootfs.BUILD
new file mode 100644
index 0000000..425b63a
--- /dev/null
+++ b/compilers/yocto_orin_rootfs.BUILD
@@ -0,0 +1,41 @@
+filegroup(
+ name = "sysroot_files",
+ srcs = glob(
+ include = [
+ "include/**",
+ "lib/**",
+ "lib64/**",
+ "usr/include/**",
+ "usr/lib/**",
+ "usr/lib64/**",
+ ],
+ exclude = [
+ "usr/share/**",
+ ],
+ ),
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "argus",
+ srcs = [
+ "usr/lib/libnvargus_socketclient.so",
+ ],
+ hdrs = glob(
+ include = ["usr/include/Argus/**"],
+ ),
+ includes = ["usr/include/Argus/utils/"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "eglstream",
+ srcs = [
+ #"usr/lib/libnvargus_socketclient.so",
+ ],
+ hdrs = glob(
+ include = ["usr/include/EGLStream/**"],
+ ),
+ includes = ["usr/include/EGLStream/"],
+ visibility = ["//visibility:public"],
+)
diff --git a/debian/BUILD.zlib.bazel b/debian/BUILD.zlib.bazel
index c0f81fa..c859eda 100644
--- a/debian/BUILD.zlib.bazel
+++ b/debian/BUILD.zlib.bazel
@@ -8,6 +8,8 @@
copts = [
"-w",
"-Dverbose=-1",
+ "-Wno-unused-but-set-variable",
+ "-Wno-implicit-function-declaration",
],
includes = [
".",
diff --git a/debian/boringssl.patch b/debian/boringssl.patch
index d12a7db..b7ac2ae 100644
--- a/debian/boringssl.patch
+++ b/debian/boringssl.patch
@@ -49,7 +49,7 @@
# Modern build environments should be able to set this to use atomic
# operations for reference counting rather than locks. However, it's
-@@ -86,17 +103,26 @@ posix_copts = [
+@@ -86,17 +103,29 @@ posix_copts = [
boringssl_copts = select({
":linux_x86_64": posix_copts,
":linux_ppc64le": posix_copts,
@@ -62,7 +62,10 @@
],
"//conditions:default": ["-DOPENSSL_NO_ASM"],
+}) + compiler_select({
-+ "clang": [],
++ "clang": [
++ "-Wno-unused-but-set-variable",
++ "-Wno-array-parameter",
++ ],
+ "gcc": [
+ "-Wno-array-parameter",
+ ],
diff --git a/debian/clapack.BUILD b/debian/clapack.BUILD
index 9ed7f82..c85e2df 100644
--- a/debian/clapack.BUILD
+++ b/debian/clapack.BUILD
@@ -295,7 +295,9 @@
"-Wno-unused-but-set-variable",
] + compiler_select({
"clang": [
+ "-Wno-deprecated-non-prototype",
"-Wno-self-assign",
+ "-Wno-unused-but-set-parameter",
],
"gcc": [
"-Wno-implicit-fallthrough",
diff --git a/debian/curl.BUILD b/debian/curl.BUILD
index c6c2d2c..a4f6526 100644
--- a/debian/curl.BUILD
+++ b/debian/curl.BUILD
@@ -426,6 +426,7 @@
"-Wno-cast-qual",
"-Wno-format-nonliteral",
"-Wno-tautological-type-limit-compare",
+ "-Wno-unused-but-set-variable",
],
}) + select({
":macos": [
diff --git a/debian/slycot.BUILD b/debian/slycot.BUILD
index 21c0df7..fb3f376 100644
--- a/debian/slycot.BUILD
+++ b/debian/slycot.BUILD
@@ -15,6 +15,7 @@
] + compiler_select({
"clang": [
"-Wno-unused-but-set-parameter",
+ "-Wno-deprecated-non-prototype",
],
"gcc": [
"-Wno-discarded-qualifiers",
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index e30ca98..b2cecc0 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -92,8 +92,8 @@
}
const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>
- &velocity_drivetrain() const {
+ HybridKalman<2, 2, 2>> &
+ velocity_drivetrain() const {
return *velocity_drivetrain_;
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d8dbe72..79e7da7 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -120,8 +120,8 @@
return coefficients().U_max;
}
Scalar U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<Scalar, number_of_inputs, number_of_states>
- &U_limit_coefficient() const {
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &
+ U_limit_coefficient() const {
return coefficients().U_limit_coefficient;
}
Scalar U_limit_coefficient(int i, int j) const {
@@ -145,14 +145,14 @@
Scalar &mutable_Y(int i) { return mutable_Y()(i); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -398,14 +398,14 @@
int index() const { return index_; }
const HybridKalmanCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
- &coefficients(int index) const {
+ number_of_outputs> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const HybridKalmanCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
- &coefficients() const {
+ number_of_outputs> &
+ coefficients() const {
return *coefficients_[index_];
}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 1fa8ac2..2a15201 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -61,8 +61,9 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs>
- &controller() const {
+ const StateFeedbackLoop<number_of_states, number_of_inputs,
+ number_of_outputs> &
+ controller() const {
return *loop_;
}
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 295beca..c5d05c1 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -144,8 +144,8 @@
return coefficients().U_max;
}
Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
- const Eigen::Matrix<Scalar, number_of_inputs, number_of_states>
- &U_limit_coefficient() const {
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &
+ U_limit_coefficient() const {
return coefficients().U_limit_coefficient;
}
Scalar U_limit_coefficient(int i, int j) const {
@@ -173,14 +173,14 @@
size_t coefficients_size() const { return coefficients_.size(); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -326,14 +326,14 @@
int index() const { return index_; }
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -450,14 +450,14 @@
int index() const { return index_; }
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -550,8 +550,8 @@
PlantType *mutable_plant() { return &plant_; }
const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &controller() const {
+ number_of_outputs, Scalar> &
+ controller() const {
return controller_;
}
diff --git a/frc971/orin/build_rootfs.py b/frc971/orin/build_rootfs.py
new file mode 100755
index 0000000..f82bed7
--- /dev/null
+++ b/frc971/orin/build_rootfs.py
@@ -0,0 +1,412 @@
+#!/usr/bin/python3
+
+import contextlib
+import pathlib
+import collections
+import subprocess
+import shlex
+import os
+
+IMAGE = "arm64_bookworm_debian_yocto.img"
+YOCTO = "/home/austin/local/jetpack/robot-yocto/build"
+
+REQUIRED_DEPS = ["debootstrap", "u-boot-tools"]
+
+
+@contextlib.contextmanager
+def scoped_loopback(image):
+ """Mounts an image as a loop back device."""
+ result = subprocess.run(["sudo", "losetup", "--show", "-f", image],
+ check=True,
+ stdout=subprocess.PIPE)
+ device = result.stdout.decode('utf-8').strip()
+ print("Mounted", image, "to", repr(device))
+ try:
+ yield device
+ finally:
+ subprocess.run(["sudo", "losetup", "-d", device], check=True)
+
+
+@contextlib.contextmanager
+def scoped_mount(image):
+ """Mounts an image as a partition."""
+ partition = f"{image}.partition"
+ try:
+ os.mkdir(partition)
+ except FileExistsError:
+ pass
+
+ result = subprocess.run(["sudo", "mount", "-o", "loop", image, partition],
+ check=True)
+
+ try:
+ yield partition
+ finally:
+ subprocess.run(
+ ["sudo", "rm", f"{partition}/usr/bin/qemu-aarch64-static"])
+ subprocess.run(["sudo", "umount", partition], check=True)
+
+
+def check_required_deps(deps):
+ """Checks if the provided list of dependencies is installed."""
+ missing_deps = []
+ for dep in deps:
+ result = subprocess.run(["dpkg-query", "-W", "-f='${Status}'", dep],
+ check=True,
+ stdout=subprocess.PIPE)
+
+ if "install ok installed" not in result.stdout.decode('utf-8'):
+ missing_deps.append(dep)
+
+ if len(missing_deps) > 0:
+ print("Missing dependencies, please install:")
+ print("sudo apt-get install", " ".join(missing_deps))
+
+
+def make_image(image):
+ """Makes an image and creates an xfs filesystem on it."""
+ result = subprocess.run([
+ "dd", "if=/dev/zero", f"of={image}", "bs=1", "count=0",
+ "seek=8589934592"
+ ],
+ check=True)
+
+ with scoped_loopback(image) as loopback:
+ subprocess.run([
+ "sudo", "mkfs.xfs", "-d", "su=128k", "-d", "sw=1", "-L", "rootfs",
+ loopback
+ ],
+ check=True)
+
+
+def target_unescaped(cmd):
+ """Runs a command as root with bash -c cmd, ie without escaping."""
+ subprocess.run([
+ "sudo", "chroot", "--userspec=0:0", f"{PARTITION}",
+ "qemu-aarch64-static", "/bin/bash", "-c", cmd
+ ],
+ check=True)
+
+
+def target(cmd):
+ """Runs a command as root with escaping."""
+ target_unescaped(shlex.join([shlex.quote(c) for c in cmd]))
+
+
+def pi_target_unescaped(cmd):
+ """Runs a command as pi with bash -c cmd, ie without escaping."""
+ subprocess.run([
+ "sudo", "chroot", "--userspec=pi:pi", "--groups=pi", f"{PARTITION}",
+ "qemu-aarch64-static", "/bin/bash", "-c", cmd
+ ],
+ check=True)
+
+
+def pi_target(cmd):
+ """Runs a command as pi with escaping."""
+ pi_target_unescaped(shlex.join([shlex.quote(c) for c in cmd]))
+
+
+def copyfile(owner, permissions, file):
+ """Copies a file from contents/{file} with the provided owner and permissions."""
+ print("copyfile", owner, permissions, file)
+ subprocess.run(["sudo", "cp", f"contents/{file}", f"{PARTITION}/{file}"],
+ check=True)
+ subprocess.run(["sudo", "chmod", permissions, f"{PARTITION}/{file}"],
+ check=True)
+ target(["chown", owner, f"/{file}"])
+
+
+def target_mkdir(owner_group, permissions, folder):
+ """Creates a directory recursively with the provided permissions and ownership."""
+ print("target_mkdir", owner_group, permissions, folder)
+ owner, group = owner_group.split('.')
+ target(
+ ["install", "-d", "-m", permissions, "-o", owner, "-g", group, folder])
+
+
+def list_packages():
+ """Lists all installed packages.
+
+ Returns:
+ A dictionary with keys as packages, and values as versions.
+ """
+ result = subprocess.run([
+ "sudo", "chroot", "--userspec=0:0", f"{PARTITION}",
+ "qemu-aarch64-static", "/bin/bash", "-c",
+ "dpkg-query -W -f='${Package} ${Version}\n'"
+ ],
+ check=True,
+ stdout=subprocess.PIPE)
+
+ device = result.stdout.decode('utf-8').strip()
+
+ r = {}
+ for line in result.stdout.decode('utf-8').strip().split('\n'):
+ package, version = line.split(' ')
+ r[package] = version
+
+ return r
+
+
+def list_yocto_packages():
+ """Lists all packages in the Yocto folder.
+
+ Returns:
+ list of Package classes.
+ """
+ Package = collections.namedtuple(
+ 'Package', ['path', 'name', 'version', 'architecture'])
+ result = []
+ pathlist = pathlib.Path(f"{YOCTO}/tmp/deploy/deb").glob('**/*.deb')
+ for path in pathlist:
+ # Strip off the path, .deb, and split on _ to parse the package info.
+ s = os.path.basename(str(path))[:-4].split('_')
+ result.append(Package(str(path), s[0], s[1], s[2]))
+
+ return result
+
+
+def install_packages(new_packages, existing_packages):
+ """Installs the provided yocto packages, if they are new."""
+ # To install the yocto packages, first copy them into a folder in /tmp, then install them, then clean the folder up.
+ target(["mkdir", "-p", "/tmp/yocto_packages"])
+ try:
+ to_install = []
+ for package in new_packages:
+ if package.name in existing_packages and existing_packages[
+ package.name] == package.version:
+ print('Skipping', package)
+ continue
+
+ subprocess.run([
+ "sudo", "cp", package.path,
+ f"{PARTITION}/tmp/yocto_packages/{os.path.basename(package.path)}"
+ ],
+ check=True)
+ to_install.append(package)
+
+ if len(to_install) > 0:
+ target(["dpkg", "-i"] + [
+ f"/tmp/yocto_packages/{os.path.basename(package.path)}"
+ for package in to_install
+ ])
+
+ finally:
+ target(["rm", "-rf", "/tmp/yocto_packages"])
+
+
+def install_virtual_packages(virtual_packages):
+ """Builds and installs the provided virtual packages."""
+ try:
+ target(["mkdir", "-p", "/tmp/yocto_packages"])
+ for virtual_package in virtual_packages:
+ subprocess.run(
+ ["dpkg-deb", "--build", f"virtual_packages/{virtual_package}"],
+ check=True)
+ subprocess.run([
+ "sudo", "cp", f"virtual_packages/{virtual_package}.deb",
+ f"{PARTITION}/tmp/yocto_packages/{virtual_package}.deb"
+ ],
+ check=True)
+
+ target(["dpkg", "-i"] + [
+ f"/tmp/yocto_packages/{package}.deb"
+ for package in virtual_packages
+ ])
+
+ finally:
+ target(["rm", "-rf", "/tmp/yocto_packages"])
+
+
+def main():
+ check_required_deps(REQUIRED_DEPS)
+
+ new_image = not os.path.exists(IMAGE)
+ if new_image:
+ make_image(IMAGE)
+
+ with scoped_mount(IMAGE) as partition:
+ if new_image:
+ subprocess.run([
+ "sudo", "debootstrap", "--arch=arm64", "--no-check-gpg",
+ "--foreign", "bookworm", partition,
+ "http://deb.debian.org/debian/"
+ ],
+ check=True)
+
+ subprocess.run([
+ "sudo", "cp", "/usr/bin/qemu-aarch64-static",
+ f"{partition}/usr/bin/"
+ ],
+ check=True)
+
+ global PARTITION
+ PARTITION = partition
+
+ if new_image:
+ target(["/debootstrap/debootstrap", "--second-stage"])
+
+ target([
+ "useradd", "-m", "-p",
+ '$y$j9T$85lzhdky63CTj.two7Zj20$pVY53UR0VebErMlm8peyrEjmxeiRw/rfXfx..9.xet1',
+ '-s', '/bin/bash', 'pi'
+ ])
+ target(["addgroup", "debug"])
+ target(["addgroup", "crypto"])
+ target(["addgroup", "trusty"])
+
+ if not os.path.exists(
+ f"{partition}/etc/apt/sources.list.d/bullseye-backports.list"):
+ copyfile("root.root", "644",
+ "etc/apt/sources.list.d/bullseye-backports.list")
+ target(["apt-get", "update"])
+
+ target([
+ "apt-get", "-y", "install", "gnupg", "wget", "systemd",
+ "systemd-resolved", "locales"
+ ])
+
+ target(["localedef", "-i", "en_US", "-f", "UTF-8", "en_US.UTF-8"])
+
+ target_mkdir("root.root", "755", "run/systemd")
+ target_mkdir("systemd-resolve.systemd-resolve", "755",
+ "run/systemd/resolve")
+ copyfile("systemd-resolve.systemd-resolve", "644",
+ "run/systemd/resolve/stub-resolv.conf")
+ target(["systemctl", "enable", "systemd-resolved"])
+
+ target([
+ "apt-get", "-y", "install", "bpfcc-tools", "sudo",
+ "openssh-server", "python3", "bash-completion", "git", "v4l-utils",
+ "cpufrequtils", "pmount", "rsync", "vim-nox", "chrony",
+ "libopencv-calib3d406", "libopencv-contrib406",
+ "libopencv-core406", "libopencv-features2d406",
+ "libopencv-flann406", "libopencv-highgui406",
+ "libopencv-imgcodecs406", "libopencv-imgproc406",
+ "libopencv-ml406", "libopencv-objdetect406", "libopencv-photo406",
+ "libopencv-shape406", "libopencv-stitching406",
+ "libopencv-superres406", "libopencv-video406",
+ "libopencv-videoio406", "libopencv-videostab406",
+ "libopencv-viz406", "libnice10", "pmount", "libnice-dev", "feh",
+ "libgstreamer1.0-0", "libgstreamer-plugins-base1.0-0",
+ "libgstreamer-plugins-bad1.0-0", "gstreamer1.0-plugins-base",
+ "gstreamer1.0-plugins-good", "gstreamer1.0-plugins-bad",
+ "gstreamer1.0-plugins-ugly", "gstreamer1.0-nice", "usbutils",
+ "locales", "trace-cmd", "clinfo", "jq", "strace", "sysstat",
+ "lm-sensors", "can-utils", "xfsprogs", "gstreamer1.0-tools",
+ "bridge-utils", "net-tools", "apt-file", "parted", "xxd"
+ ])
+ target(["apt-get", "clean"])
+
+ target(["usermod", "-a", "-G", "sudo", "pi"])
+ target(["usermod", "-a", "-G", "video", "pi"])
+ target(["usermod", "-a", "-G", "systemd-journal", "pi"])
+ target(["usermod", "-a", "-G", "dialout", "pi"])
+
+ virtual_packages = [
+ 'libglib-2.0-0', 'libglvnd', 'libgtk-3-0', 'libxcb-glx', 'wayland'
+ ]
+
+ install_virtual_packages(virtual_packages)
+
+ yocto_package_names = [
+ 'tegra-argus-daemon', 'tegra-firmware', 'tegra-firmware-tegra234',
+ 'tegra-firmware-vic', 'tegra-firmware-xusb',
+ 'tegra-libraries-argus-daemon-base', 'tegra-libraries-camera',
+ 'tegra-libraries-core', 'tegra-libraries-cuda',
+ 'tegra-libraries-eglcore', 'tegra-libraries-glescore',
+ 'tegra-libraries-glxcore', 'tegra-libraries-multimedia',
+ 'tegra-libraries-multimedia-utils',
+ 'tegra-libraries-multimedia-v4l', 'tegra-libraries-nvsci',
+ 'tegra-libraries-vulkan', 'tegra-nvphs', 'tegra-nvphs-base',
+ 'libnvidia-egl-wayland1'
+ ]
+ yocto_packages = list_yocto_packages()
+ packages = list_packages()
+
+ install_packages([
+ package for package in yocto_packages
+ if package.name in yocto_package_names
+ ], packages)
+
+ # Now, install the kernel and modules after all the normal packages are in.
+ yocto_packages_to_install = [
+ package for package in yocto_packages
+ if (package.name.startswith('kernel-module-') or package.name.
+ startswith('kernel-5.10') or package.name == 'kernel-modules')
+ ]
+
+ packages_to_remove = []
+
+ # Remove kernel-module-* packages + kernel- package.
+ for key in packages:
+ if key.startswith('kernel-module') or key.startswith(
+ 'kernel-5.10'):
+ already_installed = False
+ for index, yocto_package in enumerate(
+ yocto_packages_to_install):
+ if key == yocto_package.name and packages[
+ key] == yocto_package.version:
+ print('Found already installed package', key,
+ yocto_package)
+ already_installed = True
+ del yocto_packages_to_install[index]
+ break
+ if not already_installed:
+ packages_to_remove.append(key)
+
+ print("Removing", packages_to_remove)
+ if len(packages_to_remove) > 0:
+ target(['dpkg', '--purge'] + packages_to_remove)
+ print("Installing",
+ [package.name for package in yocto_packages_to_install])
+
+ install_packages(yocto_packages_to_install, packages)
+
+ target(["systemctl", "enable", "nvargus-daemon.service"])
+
+ copyfile("root.root", "644", "etc/sysctl.d/sctp.conf")
+ copyfile("root.root", "644", "etc/systemd/logind.conf")
+ copyfile("root.root", "555",
+ "etc/bash_completion.d/aos_dump_autocomplete")
+ copyfile("root.root", "644", "etc/security/limits.d/rt.conf")
+ copyfile("root.root", "644", "etc/systemd/system/usb-mount@.service")
+ copyfile("root.root", "644", "etc/chrony/chrony.conf")
+ target_mkdir("root.root", "700", "root/bin")
+ target_mkdir("pi.pi", "755", "home/pi/.ssh")
+ copyfile("pi.pi", "600", "home/pi/.ssh/authorized_keys")
+ target_mkdir("root.root", "700", "root/bin")
+ copyfile("root.root", "644", "etc/systemd/system/grow-rootfs.service")
+ copyfile("root.root", "500", "root/bin/change_hostname.sh")
+ copyfile("root.root", "700", "root/trace.sh")
+ copyfile("root.root", "440", "etc/sudoers")
+ copyfile("root.root", "644", "etc/fstab")
+ copyfile("root.root", "644",
+ "var/nvidia/nvcam/settings/camera_overrides.isp")
+
+ target_mkdir("root.root", "755", "etc/systemd/network")
+ copyfile("root.root", "644", "etc/systemd/network/eth0.network")
+ copyfile("root.root", "644", "etc/systemd/network/80-can.network")
+ target(["/root/bin/change_hostname.sh", "pi-971-1"])
+
+ target(["systemctl", "enable", "systemd-networkd"])
+ target(["systemctl", "enable", "grow-rootfs"])
+ target(["apt-file", "update"])
+
+ target(["ldconfig"])
+
+ if not os.path.exists(f"{partition}/home/pi/.dotfiles"):
+ pi_target_unescaped(
+ "cd /home/pi/ && git clone --separate-git-dir=/home/pi/.dotfiles https://github.com/AustinSchuh/.dotfiles.git tmpdotfiles && rsync --recursive --verbose --exclude .git tmpdotfiles/ /home/pi/ && rm -r tmpdotfiles && git --git-dir=/home/pi/.dotfiles/ --work-tree=/home/pi/ config --local status.showUntrackedFiles no"
+ )
+ pi_target(["vim", "-c", "\":qa!\""])
+
+ target_unescaped(
+ "cd /root/ && git clone --separate-git-dir=/root/.dotfiles https://github.com/AustinSchuh/.dotfiles.git tmpdotfiles && rsync --recursive --verbose --exclude .git tmpdotfiles/ /root/ && rm -r tmpdotfiles && git --git-dir=/root/.dotfiles/ --work-tree=/root/ config --local status.showUntrackedFiles no"
+ )
+ target(["vim", "-c", "\":qa!\""])
+
+
+if __name__ == '__main__':
+ main()
diff --git a/frc971/orin/contents/etc/apt/sources.list.d/bullseye-backports.list b/frc971/orin/contents/etc/apt/sources.list.d/bullseye-backports.list
new file mode 100644
index 0000000..6c98bcb
--- /dev/null
+++ b/frc971/orin/contents/etc/apt/sources.list.d/bullseye-backports.list
@@ -0,0 +1,2 @@
+deb http://deb.debian.org/debian bullseye-backports main
+deb http://deb.debian.org/debian bullseye main
diff --git a/frc971/orin/contents/etc/bash_completion.d/aos_dump_autocomplete b/frc971/orin/contents/etc/bash_completion.d/aos_dump_autocomplete
new file mode 100644
index 0000000..1b82c46
--- /dev/null
+++ b/frc971/orin/contents/etc/bash_completion.d/aos_dump_autocomplete
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+if [[ -e "/home/pi/bin/aos_dump_autocomplete.sh" ]]; then
+ . /home/pi/bin/aos_dump_autocomplete.sh
+fi
diff --git a/frc971/orin/contents/etc/chrony/chrony.conf b/frc971/orin/contents/etc/chrony/chrony.conf
new file mode 100644
index 0000000..6e03143
--- /dev/null
+++ b/frc971/orin/contents/etc/chrony/chrony.conf
@@ -0,0 +1,59 @@
+# Welcome to the chrony configuration file. See chrony.conf(5) for more
+# information about usable directives.
+
+# Include configuration files found in /etc/chrony/conf.d.
+confdir /etc/chrony/conf.d
+
+# Use Debian vendor zone.
+pool 2.debian.pool.ntp.org iburst
+
+# Use time sources from DHCP.
+sourcedir /run/chrony-dhcp
+
+# Use NTP sources found in /etc/chrony/sources.d.
+sourcedir /etc/chrony/sources.d
+
+# This directive specify the location of the file containing ID/key pairs for
+# NTP authentication.
+keyfile /etc/chrony/chrony.keys
+
+# This directive specify the file into which chronyd will store the rate
+# information.
+driftfile /var/lib/chrony/chrony.drift
+
+# Save NTS keys and cookies.
+ntsdumpdir /var/lib/chrony
+
+# Uncomment the following line to turn logging on.
+#log tracking measurements statistics
+
+# Log files location.
+logdir /var/log/chrony
+
+# Stop bad estimates upsetting machine clock.
+maxupdateskew 100.0
+
+# This directive enables kernel synchronisation (every 11 minutes) of the
+# real-time clock. Note that it can’t be used along with the 'rtcfile' directive.
+rtcsync
+
+# Always step the time if it's more than 4ms off.
+makestep 0.004 -1
+
+# The next 2 settings are used to control how much slewing of the clock chrony
+# is allowed to do. The total will be the sum of both and we have a hard limit
+# at 500ppm coming from AOS.
+
+# The maximum slewing allowed to correct an offset with the reference clock.
+# Note that this value doesn't include corrections due to frequency errors
+# (drift).
+maxslewrate 200
+
+# Maximum frequency error (drift) of the clock. Chrony will try to compensate
+# for drift and this setting limits how large that correction can be.
+maxdrift 100
+
+# Get TAI-UTC offset and leap seconds from the system tz database.
+# This directive must be commented out when using time sources serving
+# leap-smeared time.
+leapsectz right/UTC
diff --git a/frc971/orin/contents/etc/fstab b/frc971/orin/contents/etc/fstab
new file mode 100644
index 0000000..fd54025
--- /dev/null
+++ b/frc971/orin/contents/etc/fstab
@@ -0,0 +1,8 @@
+# /etc/fstab: static file system information.
+#
+# Use 'blkid' to print the universally unique identifier for a
+# device; this may be used with UUID= as a more robust way to name devices
+# that works even if disks are added and removed. See fstab(5).
+#
+# <file system> <mount point> <type> <options> <dump> <pass>
+/dev/nvme0n1p1 / xfs rw,noatime,discard 0 0
diff --git a/frc971/orin/contents/etc/security/limits.d/rt.conf b/frc971/orin/contents/etc/security/limits.d/rt.conf
new file mode 100644
index 0000000..ad2c08b
--- /dev/null
+++ b/frc971/orin/contents/etc/security/limits.d/rt.conf
@@ -0,0 +1,3 @@
+pi - nice -20
+pi - rtprio 95
+pi - memlock unlimited
diff --git a/frc971/orin/contents/etc/sudoers b/frc971/orin/contents/etc/sudoers
new file mode 100644
index 0000000..4ce0b30
--- /dev/null
+++ b/frc971/orin/contents/etc/sudoers
@@ -0,0 +1,27 @@
+#
+# This file MUST be edited with the 'visudo' command as root.
+#
+# Please consider adding local content in /etc/sudoers.d/ instead of
+# directly modifying this file.
+#
+# See the man page for details on how to write a sudoers file.
+#
+Defaults env_reset
+Defaults mail_badpass
+Defaults secure_path="/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"
+
+# Host alias specification
+
+# User alias specification
+
+# Cmnd alias specification
+
+# User privilege specification
+root ALL=(ALL:ALL) ALL
+
+# Allow members of group sudo to execute any command
+%sudo ALL=NOPASSWD: ALL
+
+# See sudoers(5) for more information on "@include" directives:
+
+@includedir /etc/sudoers.d
diff --git a/frc971/orin/contents/etc/sysctl.d/sctp.conf b/frc971/orin/contents/etc/sysctl.d/sctp.conf
new file mode 100644
index 0000000..1fcf2ca
--- /dev/null
+++ b/frc971/orin/contents/etc/sysctl.d/sctp.conf
@@ -0,0 +1,4 @@
+# https://wwwx.cs.unc.edu/~sparkst/howto/network_tuning.php
+# Make the buffers big enough for an image
+net.core.wmem_max=8388608
+net.core.rmem_max=8388608
diff --git a/frc971/orin/contents/etc/systemd/logind.conf b/frc971/orin/contents/etc/systemd/logind.conf
new file mode 100644
index 0000000..17ced30
--- /dev/null
+++ b/frc971/orin/contents/etc/systemd/logind.conf
@@ -0,0 +1,36 @@
+# This file is part of systemd.
+#
+# systemd is free software; you can redistribute it and/or modify it
+# under the terms of the GNU Lesser General Public License as published by
+# the Free Software Foundation; either version 2.1 of the License, or
+# (at your option) any later version.
+#
+# See logind.conf(5) for details.
+
+[Login]
+#NAutoVTs=6
+#ReserveVT=6
+#KillUserProcesses=no
+#KillOnlyUsers=
+#KillExcludeUsers=root
+#InhibitDelayMaxSec=5
+#HandlePowerKey=poweroff
+#HandleSuspendKey=suspend
+#HandleHibernateKey=hibernate
+#HandleLidSwitch=suspend
+#HandleLidSwitchExternalPower=suspend
+#HandleLidSwitchDocked=ignore
+#PowerKeyIgnoreInhibited=no
+#SuspendKeyIgnoreInhibited=no
+#HibernateKeyIgnoreInhibited=no
+#LidSwitchIgnoreInhibited=yes
+#HoldoffTimeoutSec=30s
+#IdleAction=ignore
+#IdleActionSec=30min
+#RuntimeDirectorySize=10%
+
+# We don't want systemd removing IPC. This makes it so nothing new can talk to
+# existing channels.
+RemoveIPC=no
+#InhibitorsMax=8192
+#SessionsMax=8192
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-can.network
new file mode 100644
index 0000000..e75db33
--- /dev/null
+++ b/frc971/orin/contents/etc/systemd/network/80-can.network
@@ -0,0 +1,9 @@
+[Match]
+Name=can0
+
+[CAN]
+BitRate=1M
+DataBitRate=8M
+RestartSec=1000ms
+BusErrorReporting=yes
+FDMode=yes
diff --git a/frc971/orin/contents/etc/systemd/network/eth0.network b/frc971/orin/contents/etc/systemd/network/eth0.network
new file mode 100644
index 0000000..8bfaf61
--- /dev/null
+++ b/frc971/orin/contents/etc/systemd/network/eth0.network
@@ -0,0 +1,11 @@
+[Match]
+Name=eth0
+
+[Network]
+Address=10.9.71.20/24
+Gateway=10.9.71.13
+DNS=8.8.8.8
+
+# ipv6 adds an extra 10 seconds to boot, and we don't use it...
+# Disable all the route discovery and stuff
+LinkLocalAddressing=no
diff --git a/frc971/orin/contents/etc/systemd/system/grow-rootfs.service b/frc971/orin/contents/etc/systemd/system/grow-rootfs.service
new file mode 100644
index 0000000..5ab3803
--- /dev/null
+++ b/frc971/orin/contents/etc/systemd/system/grow-rootfs.service
@@ -0,0 +1,19 @@
+[Unit]
+Description=Grow partition and filesystem to fit disk
+DefaultDependencies=no
+Before=local-fs.target
+Before=shutdown.target
+After=-.mount
+BindsTo=-.mount
+
+# Backport of <https://github.com/systemd/systemd/pull/14618>.
+After=systemd-remount-fs.service
+
+[Service]
+Type=oneshot
+ExecStart=/bin/bash -c "/sbin/xfs_growfs $(systemctl show --property What --value -- -.mount)"
+TimeoutSec=0
+RemainAfterExit=yes
+
+[Install]
+WantedBy=multi-user.target
diff --git a/frc971/orin/contents/etc/systemd/system/usb-mount@.service b/frc971/orin/contents/etc/systemd/system/usb-mount@.service
new file mode 100644
index 0000000..934d456
--- /dev/null
+++ b/frc971/orin/contents/etc/systemd/system/usb-mount@.service
@@ -0,0 +1,7 @@
+[Unit]
+Description=Mount USB Drive on %i
+[Service]
+Type=oneshot
+RemainAfterExit=true
+ExecStart=/usr/bin/pmount --umask 000 /dev/%i /media/%i
+ExecStop=/usr/bin/pumount /dev/%i
diff --git a/frc971/orin/contents/home/pi/.ssh/authorized_keys b/frc971/orin/contents/home/pi/.ssh/authorized_keys
new file mode 100644
index 0000000..58b82cc
--- /dev/null
+++ b/frc971/orin/contents/home/pi/.ssh/authorized_keys
@@ -0,0 +1,21 @@
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDI002gCm4aRVrIcg2G4/qF4D1oNY74HFFAHjNUIgvrmqSEn+Oy+pxigpJFiZZaJMJpaw4kpd1IEpZxhooZm4DC4/bVV3wAFTw/OJI7D75WgrRrBRHd95TMBwYyNUhoDOcPAoZ69+IL9P0rhmNjgCv6Y+3PG+Rl6IqRPuf3dXX/PT3E/h8B18PRkEnas/3WTW8goov6x10kVAa5I+iQansiyAbPQF7E+Q5mpsnl26V2vpHo1UAk7y+TD7jqifEn13TmLeTkDXmaIOflQeOBMAdErftuqrClPa00VbejP18v02RI/jOIAQ250g0hN3zvKi2eNHUPdAzlMB4cSvZspRrB /home/austin/.ssh/id_rsa
+
+ssh-rsa-cert-v01@openssh.com AAAAHHNzaC1yc2EtY2VydC12MDFAb3BlbnNzaC5jb20AAAAgzaqXNuB589EgR6/ljdYhp5Ca+B8eimCTmmC23oQvNyIAAAADAQABAAABAQDI002gCm4aRVrIcg2G4/qF4D1oNY74HFFAHjNUIgvrmqSEn+Oy+pxigpJFiZZaJMJpaw4kpd1IEpZxhooZm4DC4/bVV3wAFTw/OJI7D75WgrRrBRHd95TMBwYyNUhoDOcPAoZ69+IL9P0rhmNjgCv6Y+3PG+Rl6IqRPuf3dXX/PT3E/h8B18PRkEnas/3WTW8goov6x10kVAa5I+iQansiyAbPQF7E+Q5mpsnl26V2vpHo1UAk7y+TD7jqifEn13TmLeTkDXmaIOflQeOBMAdErftuqrClPa00VbejP18v02RI/jOIAQ250g0hN3zvKi2eNHUPdAzlMB4cSvZspRrBAAAAAAAAAAAAAAABAAAAHmF1c3Rpbi5zY2h1aEBibHVlcml2ZXJ0ZWNoLmNvbQAAAA8AAAADYnJ0AAAABHJvb3QAAAAAWGi3PAAAAABjhSpmAAAAAAAAAIIAAAAVcGVybWl0LVgxMS1mb3J3YXJkaW5nAAAAAAAAABdwZXJtaXQtYWdlbnQtZm9yd2FyZGluZwAAAAAAAAAWcGVybWl0LXBvcnQtZm9yd2FyZGluZwAAAAAAAAAKcGVybWl0LXB0eQAAAAAAAAAOcGVybWl0LXVzZXItcmMAAAAAAAAAAAAAAKwAAAATZWNkc2Etc2hhMi1uaXN0cDUyMQAAAAhuaXN0cDUyMQAAAIUEANvRmN8fXmKOO6xQPsllgbHxX+hvP4sU8/ayxw1K9C2MlGT3OKPgnjWEmvEPgpPR+/YQ6asQnP+jucdgCM8q7+c4ATwFnMO7yl2LCU1UkCKShzoumXflKC9rWNVT6MY4PTbpQXui5XE0gIZrjKrkcfCGjvRouUasM/C5Zro/aGQFkL6XAAAApwAAABNlY2RzYS1zaGEyLW5pc3RwNTIxAAAAjAAAAEIAswnueuP8iT7Qbzr1yBx6tLbNY9jewA6NEkLnFJtu11VzBFFaLWxeXwKPy3ajT0DCzt6EX6YKBHfYngnzdyjP9KkAAABCAWsxaA9D59ToYmbEKT//85dczH397v6As8WeQMAMzKfJYVSJBceHiwt6EbRKd6m+xUsd/Sr4Bj/Eu2VvwplqCpOq /home/austin/.ssh/id_rsa
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDLyVTxr8s3kjz+PEjEymgwC8o64IZgBRJbtq9HZ16ufEHqxCD6WK21v8XsbAyTo3/vIfiae+SxhZTC9PMA1AQXuXCBTcvH1avHlHNPgnfxOfzNpU5LSZx/hqrx9tJ+ELV6m34XUbAhIhXJSyiPE2Mst8et6XUvXLgQ8hr0vwXZ3jitI0WzdoZE2svQhn/Cw+NnFiIyhVm4VTnw0bo5XVvvCawvZdTWsyXIvYx9P7rJ5Kvr1eJTZB+tDynzEFxJZeC+lnE6kV8NudC/7hLwwn1Uvqon17Z4P8ukxDsaD2Y4a2v0zqqN0FkEAKjhcqRWdyHM2JOeygRJa1sABNzt4gJB austin@ASchuh-T480s
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQDDs88Uby+A7k69WSUXp6wmDrsIIFib5bsJ/0oHdjRWSZc4gHVGVF4cFCIJsjvuiQ3LlQ1vVb4ot7kXDNhPWEENRiuMVN3bovTr0fIjSi+YzhidIUZV44LhIkf2XorjpBjfdKE8YyZgYU0wway6myLijoEy6UnLaYerFjUh0k0p+R/axNtD48Glge82pvihosNt4J4592PGbfoTg7hgPizz4Yp39MtYN0OAqHDSrthU/ceA97prMo9tugozHthDasNAb1u/KiOr86dswLiGhwfM0aWAStIu+jie8fKzFtPFFvCyeEaGTYJ/nKiTq2qX2VNLk2zoqXoP6OPHTztejMtRyuRZxx3+mwhDT1lwUQx/ZsFqMTuIOjGQjpzQg3/Q2Y7rnSeQmgc5LglzaH5SRQ7i3nXJvDm1akdHRFFjarBw9Pb2p8DsDaTmJ6gpoEFqZZa1RM5ZCab8bL9z0pHBdpqhIXcPflDQE7Qi8win+LlWBwFyjhu5PvNnAKFEv6uQC1M= austin@aschuh-3950x
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQDONITvwkh8whp/ac9mhqIk+H4JV7gHIO6OYiOeJfd8mJGAKbox6JNPsnZ2Yewse9oc+MDKZ+dyLqBsfIVG0MUh96hJZHwIYW6wObcZt7Zj6c+JzbAQNavWltsu0IIA7wVrdz2qOXLuH3MhM5URP29IsmF9EUp2H1/iRUMlwz3+1GckYWmiJl+JKNct1DofJb4RdbY2iG34QgS6FvLEKZBxGYPboPD0DLylymdAZAq2TjCcngiq+5BWmy79BLoA2xwJjiz1WPbSILIygfX6e85FGf9ZskylfgHcltAP4xrg3ci7ygJZSw7rd37pV6PfxOz4vGmA7ROT+ocbkXtzkmGk6Qb8HPqT9uRl9Y2Wm4YQMIrREHvjXtGHePCS6R8rrK6og4U4l6Fn4ghicemoRcQXTeX8RJFN+jCpBmks+P6UbYW8k2424kVhNMHz2sHT+BU5Klz9HQvcqlCqupqiPT6HF/gzMZgMHAf2Dp17CpoDbhkhLmCE1TmFOKqNqHP31fs= milindupadhyay@mdu-brt
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDNUJEb8hoIboZ+FWUp9OX37xRs6s63pXfkF0Hirka+ovIziorNLxR6BixqD7xkVBwXgyMo+YRRy80eHLX2NoItoRLoMfT4uqhu/izdidLcUrpYicAxMIi1pCi1SS5BcKCHbyEqS5FrHzea+0dhIeemb1EP2NXxTDlFUINTni/Aw0z8bB3UI0tY0+K8zypuDKohL9tWOOln6e0VirhPhSCpfs89959jhmeI7Al1DuyZw1vfcaVgQ/7W59rHqP8RNcziMrwwt5hCMQQRxEu4Dd11jBiLklS+wLBI8exP/aEVXGwwc6D/zhg2zma4wQ7HuivZF+sMrP4T1RlLMK3ngbMX james@hp-debian
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQCaElJXdu6YMo7n32jAYXVPW9HKdp2vbE1BcKz+hqZDiIpMtIwmyu+KQD7XHOfkDrDLlsKRRnEvAKAhE+Q5apHdVwnR70h79khBjvRZqXBt+lip3GyGBuymE6+2bAL6zMnZ6oKBvKcPRdIUEbv1jvJV4SfCERGp0/yQxlf/h+yy2pGTEV2iGZerfYBDIyggDzrhjEeiFUL7In4kR2dQId/BPlzZx9AVIdF6CIY7MnfsPBZUkrk9XQrCJOxzN2gGPLdan3DMCp1o2K93dvojl4dQguphanwOdw8wm+wlMiucY/buHgqqbS0EFOz/taQDBFfQ2v7ckht5CFvCY56YpGFaVW7ztf7OYbU6t8LS3dsoN8+rAesBe9fJhJWR/0zFIAM7WW6j95Sppikqh0ZcaQK2/eMKFqn8R7K9zJ20BOI+QYGNnLvIVXlFzRt0/Qy/GczhejpJddwK9zbYxVKJyMNpVz3ZUlr+nRR2UtlcM+lWe7Kbu3HxTPBNk6vSCXPE+Pc= ravago@rjmacAir
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDSB1Nji8HEfnQX6E2FIfyvolrlihiBKukyLMA5isJhtvdaOXVOrSilpd91wB+ljLOfDy+4XWK4+8p84tvggdfvJdsLRtSBhExJNqe2ViyfUqh0O61lZg4Yvw6DGFEdS+DrqIkZh1v3WQ1Py1Mpt5V18dSu42DttHCigXFnYlpZfx9n4QT6GphkWYE3hLWHQH3uSkujzgkr86WreUi4idRDmq/r21H/MBx4q3uwWuftLS6oX79y7aukpieopeoWN8WUNscwUpUpmAn6rdFe/IHl0u8Zw4wmXoYSIQErFGsOK/rA3nKuK0uvVvUQkVKBZpjMOAAuaLugu9NYsOgqIZf/ filip@Filips-MacBook-Pro.local
+
+ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAILZrP0on5xPZHadvlMN2/+iZzEbIeGpS5MT5hXfb+kP0 uid@pop-os
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQC+t3HrUbZb9P/zWw9+qH9i8HBcCBRO6BS0rYZMz3UuXFSxhLendcqmAJQ+cu1lI0sb2HAyIhv4d5CQrjZ2en/ZyN93DThWNRWA+iH+ybkJhpUiRVSa/e1rz06o//kL+PVoZEHRUpyy7iJgEgcRxnVZd90baMkkm51bF0NSVn+iGCIao76VS+PBGwtQFghJyAvTz+w/hLYmFtXnY2DRfPvbw8H3cehvDhUBY9V+MUwucMU38Yfucr/BguPyw/sRFzRJtpIzP+JFQZzy/Bz4DI06vIWBps6NZqGo+ETXsrlEmNRKXgp3YWMZg/VcT6IIi9NijtDt6gHNDLfxX1Q41cYErBIaUpDKWPpGJ9z+/FxtxxBHwhIQPzSvdxqgsoGKyy7rFfWOPqWHzASsAcdx8V0EPIL4VmJQOd461WRfZxAhDdo2hQe37joRjcHW2i1nJcpVSepMf7EJH/AoYcifp+b+JUpMZx79HmKtE1cua2JcfZ6b20w7rQzLIP3M2WvrDak= deepachainani@Yashs-MBP
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDG+8q7VDZv1bI2s/LDLZvjX4HInQobPF9QfBi8sNJdu98drWzSIlTIRMfOFuN8Vbq2E/wr6eKXOUwq/Brxu4sZ6+po6+sT6IqyE5cd+FffNJXZUc+QxXC6maD20i7k8gNJuo+BRa6VbSR25WarRoGT+pYl03jbhR304Wn8wIYIS5cAfaP2wzwNPOvCZzzwJrba2jeoeVWRUTuqLRAw4NNq5eZskdpv7w/3qw901z7RKzuTfYiAnvsRm8o6E0uRncyujzvXoXD2jyWdKC/boIv/l1Is/XwsSJahq1NIK3y95jZHoUXpDQsq49U0wOByBpKEyQYcI54nhfwYagVsYRY9 jim.o@bluerivert.com
diff --git a/frc971/orin/contents/root/bin/change_hostname.sh b/frc971/orin/contents/root/bin/change_hostname.sh
new file mode 100755
index 0000000..785903e
--- /dev/null
+++ b/frc971/orin/contents/root/bin/change_hostname.sh
@@ -0,0 +1,55 @@
+#!/bin/bash
+
+set -xeuo pipefail
+
+HOSTNAME="$1"
+
+# TODO<Jim>: Should probably add handling for imu hostname, too
+if [[ ! "${HOSTNAME}" =~ ^pi-[0-9]*-[0-9]$ ]]; then
+ echo "Invalid hostname ${HOSTNAME}, needs to be pi-[team#]-[pi#]"
+ exit 1
+fi
+
+TEAM_NUMBER="$(echo ${HOSTNAME} | sed 's/pi-\(.*\)-.*/\1/')"
+PI_NUMBER="$(echo ${HOSTNAME} | sed 's/pi-.*-\(.*\)/\1/')"
+IP_BASE="$(echo ${TEAM_NUMBER} | sed 's/\(.*\)\(..\)/10.\1.\2/')"
+IP="${IP_BASE}.$(( 100 + ${PI_NUMBER}))"
+
+echo "Changing to team number ${TEAM_NUMBER}, IP ${IP}"
+
+sed -i "s/^Address=.*$/Address=${IP}\/24/" /etc/systemd/network/eth0.network
+sed -i "s/^Gateway=.*$/Gateway=${IP_BASE}.13/" /etc/systemd/network/eth0.network
+
+echo "${HOSTNAME}" > /etc/hostname
+
+# Make sure a 127.0.* entry exists to make things looking up localhost happy.
+if grep '^127.0.1.1' /etc/hosts > /dev/null;
+then
+ sed -i "s/\(127\.0\.1\.1\t\).*$/\1${HOSTNAME}/" /etc/hosts
+else
+ echo -e "127.0.1.1\t${HOSTNAME}" >> /etc/hosts
+fi
+
+# Put corret team number in pi's IP addresses, or add them if needed
+if grep '^10\.[0-9]*\.[0-9]*\.[0-9]*\s*pi-[0-9]*-[0-9] pi[0-9]$' /etc/hosts >/dev/null ;
+then
+ sed -i "s/^10\.[0-9]*\.[0-9]*\(\.[0-9]*\s*pi-\)[0-9]*\(-[0-9] pi[0-9]\)\(.*\)$/${IP_BASE}\1${TEAM_NUMBER}\2\3/" /etc/hosts
+else
+ for i in {1..6}; do
+ imu=""
+ # Add imu name to pi6. Put space in this string, since extra
+ # spaces otherwise will make the above grep fail
+ if [[ ${i} == 6 ]]; then
+ imu=" imu"
+ fi
+ echo -e "${IP_BASE}.$(( i + 100 ))\tpi-${TEAM_NUMBER}-${i} pi${i}${imu}" >> /etc/hosts
+ done
+fi
+
+# Put correct team number in roborio's address, or add it if missing
+if grep '^10\.[0-9]*\.[0-9]*\.2\s*roborio$' /etc/hosts >/dev/null;
+then
+ sed -i "s/^10\.[0-9]*\.[0-9]*\(\.2\s*roborio\)$/${IP_BASE}\1/" /etc/hosts
+else
+ echo -e "${IP_BASE}.2\troborio" >> /etc/hosts
+fi
diff --git a/frc971/orin/contents/root/trace.sh b/frc971/orin/contents/root/trace.sh
new file mode 100644
index 0000000..4b69b4e
--- /dev/null
+++ b/frc971/orin/contents/root/trace.sh
@@ -0,0 +1,19 @@
+#!/bin/sh
+
+echo 1 > /sys/kernel/debug/tracing/tracing_on
+echo 30720 > /sys/kernel/debug/tracing/buffer_size_kb
+echo 1 > /sys/kernel/debug/tracing/events/tegra_rtcpu/enable
+echo 1 > /sys/kernel/debug/tracing/events/freertos/enable
+echo 2 > /sys/kernel/debug/camrtc/log-level
+echo 1 > /sys/kernel/debug/tracing/events/camera_common/enable
+echo > /sys/kernel/debug/tracing/trace
+
+echo file vi2_fops.c +p > /sys/kernel/debug/dynamic_debug/control
+echo file csi2_fops.c +p > /sys/kernel/debug/dynamic_debug/control
+
+echo file vi4_fops.c +p > /sys/kernel/debug/dynamic_debug/control
+echo file csi.c +p > /sys/kernel/debug/dynamic_debug/control
+echo file csi4_fops.c +p > /sys/kernel/debug/dynamic_debug/control
+echo file nvcsi.c +p > /sys/kernel/debug/dynamic_debug/control
+
+cat /sys/kernel/debug/tracing/trace /sys/kernel/debug/tracing/trace_pipe
diff --git a/frc971/orin/contents/run/systemd/resolve/stub-resolv.conf b/frc971/orin/contents/run/systemd/resolve/stub-resolv.conf
new file mode 100644
index 0000000..fa4bebb
--- /dev/null
+++ b/frc971/orin/contents/run/systemd/resolve/stub-resolv.conf
@@ -0,0 +1,3 @@
+domain lan
+search lan
+nameserver 127.0.0.53
diff --git a/frc971/orin/contents/var/nvidia/nvcam/settings/camera_overrides.isp b/frc971/orin/contents/var/nvidia/nvcam/settings/camera_overrides.isp
new file mode 100644
index 0000000..82363b2
--- /dev/null
+++ b/frc971/orin/contents/var/nvidia/nvcam/settings/camera_overrides.isp
@@ -0,0 +1,1761 @@
+# Char-lite Calibration
+# command: nv_wrapper 0 optical_black_file ..\imx477\outputs\ob.cfg lsc ..\imx477\outputs\lsc.cfg awb ..\imx477\outputs\awb.cfg ccm ..\imx477\outputs\ccm.cfg public_params public_params.cfg out_name ..\imx477\outputs\camera_overrides.isp
+# version: 1.8.0
+# created on: 16-Jul-2020 17:10
+
+#-**************************************************************
+#-* Do not modify
+#-**************************************************************
+#* =============================================================
+#* ISP Configuration Override File
+#* Copyright (c) 2012-2017, NVIDIA CORPORATION. All rights reserved.
+#* Model:model
+#* Integrator:
+#* Part#:
+#* Sensor:sensor
+#* Size:
+#* Version: version
+#* FileName & Location:/data/nvcam/settings/camera_overrides.isp
+#* Updated:Thu Jul 20 19:15:20 2017 PST
+#* =============================================================
+ap15Function.lensShading = TRUE;
+ae.MeanAlg.HigherTarget = 120;
+ae.MeanAlg.LowerTarget = 120;
+ae.MeanAlg.HigherBrightness = 10000;
+ae.MeanAlg.LowerBrightness = 600;
+ae.MeanAlg.SlopFactor = 0.3;
+ae.MeanAlg.MinTailMass = 0.001;
+ae.MeanAlg.CriticalMass = 0.015;
+ae.MeanAlg.ConvergeSpeed = 0.5;
+ae.MaxFstopDeltaPos = 0.4;
+ae.MaxFstopDeltaNeg = 0.5;
+defaults.autoFramerateRange = {7.5, 120.0};
+ae.ApertureStepToFNumberLUT = {2.4};
+ae.ExposureTuningTable.Preview[0] = {2.4, 0.03333, 1.0, 1.0};
+ae.ExposureTuningTable.Preview[1] = {2.4, 0.03333, 2.0, 1.0};
+ae.ExposureTuningTable.Preview[2] = {2.4, 0.03333, 4.0, 1.0};
+ae.ExposureTuningTable.Preview[3] = {2.4, 0.03333, 8.0, 1.0};
+ae.ExposureTuningTable.Preview[4] = {2.4, 0.03333, 8.0, 1.0};
+ae.ExposureTuningTable.Preview[5] = {2.4, 0.06666, 8.0, 1.0};
+ae.ExposureTuningTable.Preview[6] = {2.4, 0.06666, 16.0, 1.0};
+ae.ExposureTuningTable.Preview[6] = {2.4, 0.06666, 22.0, 1.0};
+flicker.ConfidenceThreshold = 32;
+flicker.SuccessFrameCount = 8;
+flicker.FailureFrameCount = 3;
+flicker.CorrectionFreqListEntries = 2;
+flicker.CorrectionFreqList = {100, 120};
+defaults.saturation = 1.0;
+ae.saturation.Preview[0] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Preview[1] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Preview[2] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Preview[3] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Still[0] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Still[1] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Still[2] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Still[3] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Video[0] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Video[1] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Video[2] = {100, 100, 100, 100, 100, 100, 100};
+ae.saturation.Video[3] = {100, 100, 100, 100, 100, 100, 100};
+sharpness.v2.Preview[0] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Preview[1] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Preview[2] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Preview[3] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Still[0] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Still[1] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Still[2] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Still[3] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Video[0] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Video[1] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Video[2] = {3, 3, 5, 5, 5, 5, 5};
+sharpness.v2.Video[3] = {3, 3, 5, 5, 5, 5, 5};
+tc.Enable=true;
+tc.Version=v2;
+tc.v2.user.presets[0].Gamma.Value=2.4000;
+tc.v2.user.presets[0].UserCurve.Enable=FALSE;
+tc.v2.user.presets[0].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[0].CurveControl.Enable=TRUE;
+tc.v2.user.presets[0].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[0].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[0].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[0].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[0].Brightness.Enable=FALSE;
+tc.v2.user.presets[0].Brightness.Value=1.0000;
+tc.v2.user.presets[1].Gamma.Value=2.4000;
+tc.v2.user.presets[1].UserCurve.Enable=FALSE;
+tc.v2.user.presets[1].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[1].CurveControl.Enable=TRUE;
+tc.v2.user.presets[1].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[1].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[1].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[1].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[1].Brightness.Enable=FALSE;
+tc.v2.user.presets[1].Brightness.Value=1.0000;
+tc.v2.user.presets[2].Gamma.Value=2.4000;
+tc.v2.user.presets[2].UserCurve.Enable=FALSE;
+tc.v2.user.presets[2].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[2].CurveControl.Enable=TRUE;
+tc.v2.user.presets[2].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[2].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[2].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[2].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[2].Brightness.Enable=FALSE;
+tc.v2.user.presets[2].Brightness.Value=1.0000;
+tc.v2.user.presets[3].Gamma.Value=2.4000;
+tc.v2.user.presets[3].UserCurve.Enable=FALSE;
+tc.v2.user.presets[3].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[3].CurveControl.Enable=TRUE;
+tc.v2.user.presets[3].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[3].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[3].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[3].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[3].Brightness.Enable=FALSE;
+tc.v2.user.presets[3].Brightness.Value=1.0000;
+tc.v2.user.presets[4].Gamma.Value=2.4000;
+tc.v2.user.presets[4].UserCurve.Enable=FALSE;
+tc.v2.user.presets[4].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[4].CurveControl.Enable=TRUE;
+tc.v2.user.presets[4].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[4].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[4].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[4].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[4].Brightness.Enable=FALSE;
+tc.v2.user.presets[4].Brightness.Value=1.0000;
+tc.v2.user.presets[5].Gamma.Value=2.4000;
+tc.v2.user.presets[5].UserCurve.Enable=FALSE;
+tc.v2.user.presets[5].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[5].CurveControl.Enable=TRUE;
+tc.v2.user.presets[5].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[5].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[5].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[5].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[5].Brightness.Enable=FALSE;
+tc.v2.user.presets[5].Brightness.Value=1.0000;
+tc.v2.user.presets[6].Gamma.Value=2.4000;
+tc.v2.user.presets[6].UserCurve.Enable=FALSE;
+tc.v2.user.presets[6].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[6].CurveControl.Enable=TRUE;
+tc.v2.user.presets[6].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[6].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[6].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[6].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[6].Brightness.Enable=FALSE;
+tc.v2.user.presets[6].Brightness.Value=1.0000;
+tc.v2.user.presets[7].Gamma.Value=2.4000;
+tc.v2.user.presets[7].UserCurve.Enable=FALSE;
+tc.v2.user.presets[7].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[7].CurveControl.Enable=TRUE;
+tc.v2.user.presets[7].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[7].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[7].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[7].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[7].Brightness.Enable=FALSE;
+tc.v2.user.presets[7].Brightness.Value=1.0000;
+tc.v2.user.presets[8].Gamma.Value=2.4000;
+tc.v2.user.presets[8].UserCurve.Enable=FALSE;
+tc.v2.user.presets[8].UserCurve.Points={ 0.0000, 0.0000, 0.2500, 0.2500, 0.5000, 0.5000, 0.7500, 0.7500, 1.0000, 1.0000 };
+tc.v2.user.presets[8].CurveControl.Enable=TRUE;
+tc.v2.user.presets[8].CurveControl.AdjustHighlights=0.7500;
+tc.v2.user.presets[8].CurveControl.MidtoneBrightness=0.5000;
+tc.v2.user.presets[8].CurveControl.MidtoneContrast=0.5000;
+tc.v2.user.presets[8].CurveControl.AdjustShadows=0.2500;
+tc.v2.user.presets[8].Brightness.Enable=FALSE;
+tc.v2.user.presets[8].Brightness.Value=1.0000;
+tc.v2.user.UseIndices={ 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+tc.v2.user.FlashUseIndices= { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+tc.v2.UseAxisX={ 1068.0000, 2136.0000, 9970.0000, 14242.0000 };
+tc.v2.UseAxisY={ 1.0000, 1.0000, 1.0000, 1.0000 };
+tc.v2.FlashUseAxisX={ 1068.0000, 2136.0000, 9970.0000, 14242.0000 };
+tc.v2.FlashUseAxisY={ 1.0000, 1.0000, 1.0000, 1.0000 };
+tc.v2.initStats.brightness=3811;
+tc.v2.initStats.tcdre=1.0;
+flash.IntensityCalibrateEnable = false;
+flash.Led1.FlashLevel[1] = 180;
+flash.Led1.TorchLevel[1] = 100;
+af.module_cal_enable = 0;
+af.settle_time = 30;
+af.inf = 220;
+af.macro = 510;
+af.inf_offset = 0;
+af.macro_offset = 0;
+af.macro_max = 100;
+noiseReduction.v2.MaxValue = 7;
+noiseReduction.v2.Preview[0] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Preview[1] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Preview[2] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Preview[3] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Still[0] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Still[1] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Still[2] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Still[3] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Video[0] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Video[1] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Video[2] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v2.Video[3] = { 1, 2, 3, 5, 6, 7, 7 };
+noiseReduction.v6.Chroma.Enable = TRUE;
+noiseReduction.v6.Luma.Enable = TRUE;
+noiseReduction.v6.Chroma.GainThreshold = 1.0;
+noiseReduction.v6.Luma.GainThreshold = 2.0;
+noiseReduction.v6.NumSignalConditioningPoints = 7;
+noiseReduction.v6.SignalConditioning[0].Gain = 2;
+noiseReduction.v6.SignalConditioning[0].NoiseBlend = 0.95;
+noiseReduction.v6.SignalConditioning[0].Shaping = 1.0;
+noiseReduction.v6.SignalConditioning[1].Gain = 4;
+noiseReduction.v6.SignalConditioning[1].NoiseBlend = 0.90;
+noiseReduction.v6.SignalConditioning[1].Shaping = 1.0;
+noiseReduction.v6.SignalConditioning[2].Gain = 6;
+noiseReduction.v6.SignalConditioning[2].NoiseBlend = 0.86;
+noiseReduction.v6.SignalConditioning[2].Shaping = 1.0;
+noiseReduction.v6.SignalConditioning[3].Gain = 8;
+noiseReduction.v6.SignalConditioning[3].NoiseBlend = 0.82;
+noiseReduction.v6.SignalConditioning[3].Shaping = 1.0;
+noiseReduction.v6.SignalConditioning[4].Gain = 10;
+noiseReduction.v6.SignalConditioning[4].NoiseBlend = 0.80;
+noiseReduction.v6.SignalConditioning[4].Shaping = 1.0;
+noiseReduction.v6.SignalConditioning[5].Gain = 12;
+noiseReduction.v6.SignalConditioning[5].NoiseBlend = 0.79;
+noiseReduction.v6.SignalConditioning[5].Shaping = 1.0;
+noiseReduction.v6.SignalConditioning[6].Gain = 16;
+noiseReduction.v6.SignalConditioning[6].NoiseBlend = 0.40;
+noiseReduction.v6.SignalConditioning[6].Shaping = 1.0;
+noiseReduction.v6.NumChromaPoints = 7;
+noiseReduction.v6.Chroma[0].Gain = 1;
+noiseReduction.v6.Chroma[0].FilterStrength = 0.03;
+noiseReduction.v6.Chroma[0].Levels = 2;
+noiseReduction.v6.Chroma[0].Scaling = 1.0;
+noiseReduction.v6.Chroma[0].SignalBoost = 1.0;
+noiseReduction.v6.Chroma[1].Gain = 1.1;
+noiseReduction.v6.Chroma[1].FilterStrength = 0.04;
+noiseReduction.v6.Chroma[1].Levels = 2;
+noiseReduction.v6.Chroma[1].Scaling = 1.0;
+noiseReduction.v6.Chroma[1].SignalBoost = 1.0;
+noiseReduction.v6.Chroma[2].Gain = 2;
+noiseReduction.v6.Chroma[2].FilterStrength = 0.06;
+noiseReduction.v6.Chroma[2].Levels = 3;
+noiseReduction.v6.Chroma[2].Scaling = 1.0;
+noiseReduction.v6.Chroma[2].SignalBoost = 1.0;
+noiseReduction.v6.Chroma[3].Gain = 4;
+noiseReduction.v6.Chroma[3].FilterStrength = 0.07;
+noiseReduction.v6.Chroma[3].Levels = 3;
+noiseReduction.v6.Chroma[3].Scaling = 1.0;
+noiseReduction.v6.Chroma[3].SignalBoost = 1.0;
+noiseReduction.v6.Chroma[4].Gain = 6;
+noiseReduction.v6.Chroma[4].FilterStrength = 0.08;
+noiseReduction.v6.Chroma[4].Levels = 3;
+noiseReduction.v6.Chroma[4].Scaling = 1.0;
+noiseReduction.v6.Chroma[4].SignalBoost = 1.0;
+noiseReduction.v6.Chroma[5].Gain = 8;
+noiseReduction.v6.Chroma[5].FilterStrength = 0.09;
+noiseReduction.v6.Chroma[5].Levels = 3;
+noiseReduction.v6.Chroma[5].Scaling = 1.0;
+noiseReduction.v6.Chroma[5].SignalBoost = 1.0;
+noiseReduction.v6.Chroma[6].Gain = 10;
+noiseReduction.v6.Chroma[6].FilterStrength = 0.10;
+noiseReduction.v6.Chroma[6].Levels = 3;
+noiseReduction.v6.Chroma[6].Scaling = 1.0;
+noiseReduction.v6.Chroma[6].SignalBoost = 1.0;
+noiseReduction.v6.NumLumaPoints = 7;
+noiseReduction.v6.Luma[0].Gain = 2;
+noiseReduction.v6.Luma[0].FilterStrength = 0.5;
+noiseReduction.v6.Luma[0].Levels = 1;
+noiseReduction.v6.Luma[0].Scaling = 1.0;
+noiseReduction.v6.Luma[0].SignalBoost = 1.0;
+noiseReduction.v6.Luma[1].Gain = 4;
+noiseReduction.v6.Luma[1].FilterStrength = 0.8;
+noiseReduction.v6.Luma[1].Levels = 1;
+noiseReduction.v6.Luma[1].Scaling = 1.0;
+noiseReduction.v6.Luma[1].SignalBoost = 1.0;
+noiseReduction.v6.Luma[2].Gain = 6;
+noiseReduction.v6.Luma[2].FilterStrength = 1.0;
+noiseReduction.v6.Luma[2].Levels = 1;
+noiseReduction.v6.Luma[2].Scaling = 1.0;
+noiseReduction.v6.Luma[2].SignalBoost = 1.0;
+noiseReduction.v6.Luma[3].Gain = 8;
+noiseReduction.v6.Luma[3].FilterStrength = 1.3;
+noiseReduction.v6.Luma[3].Levels = 1;
+noiseReduction.v6.Luma[3].Scaling = 1.0;
+noiseReduction.v6.Luma[3].SignalBoost = 1.0;
+noiseReduction.v6.Luma[4].Gain = 10;
+noiseReduction.v6.Luma[4].FilterStrength = 1.5;
+noiseReduction.v6.Luma[4].Levels = 1;
+noiseReduction.v6.Luma[4].Scaling = 1.0;
+noiseReduction.v6.Luma[4].SignalBoost = 1.0;
+noiseReduction.v6.Luma[5].Gain = 12;
+noiseReduction.v6.Luma[5].FilterStrength = 1.6;
+noiseReduction.v6.Luma[5].Levels = 1;
+noiseReduction.v6.Luma[5].Scaling = 1.0;
+noiseReduction.v6.Luma[5].SignalBoost = 1.0;
+noiseReduction.v6.Luma[6].Gain = 16;
+noiseReduction.v6.Luma[6].FilterStrength = 0.20;
+noiseReduction.v6.Luma[6].Levels = 1;
+noiseReduction.v6.Luma[6].Scaling = 1.0;
+noiseReduction.v6.Luma[6].SignalBoost = 1.0;
+colorEffects.sepia.param1 = -0.1500;
+colorEffects.sepia.param2 = 0.2000;
+colorEffects.aqua.param1 = 0.3000;
+colorEffects.aqua.param2 = -0.3500;
+colorEffects.solarize.numPoints = 4;
+colorEffects.solarize.point[0] = {0, 0};
+colorEffects.solarize.point[1] = {128, 160};
+colorEffects.solarize.point[2] = {196, 160};
+colorEffects.solarize.point[3] = {256, 0};
+colorEffects.solarize.point[4] = {0, 0};
+colorEffects.solarize.point[5] = {0, 0};
+colorEffects.solarize.point[6] = {0, 0};
+colorEffects.solarize.point[7] = {0, 0};
+colorEffects.solarize.point[8] = {0, 0};
+colorEffects.solarize.point[9] = {0, 0};
+colorEffects.posterize.numPoints = 8;
+colorEffects.posterize.point[0] = {0, 0};
+colorEffects.posterize.point[1] = {54, 0};
+colorEffects.posterize.point[2] = {69, 85};
+colorEffects.posterize.point[3] = {123, 85};
+colorEffects.posterize.point[4] = {138, 170};
+colorEffects.posterize.point[5] = {192, 170};
+colorEffects.posterize.point[6] = {207, 256};
+colorEffects.posterize.point[7] = {256, 256};
+colorEffects.posterize.point[8] = {0, 0};
+colorEffects.posterize.point[9] = {0, 0};
+mwbCCTTint.incandescent = {2950, 0.0};
+mwbCCTTint.fluorescent = {4300, 0.0};
+mwbCCTTint.warmfluorescent = {2940, 0.0};
+mwbCCTTint.daylight = {6100, 0.0};
+mwbCCTTint.cloudy = {7000, 0.0};
+mwbCCTTint.twilight = {10000, 0.0};
+mwbCCTTint.shade = {6550, 0.0};
+lensShading.module_cal_enable = 0;
+awb.module_cal_enable = 0;
+lensShading.falloff_PointsCount = 6;
+lensShading.falloff.Preview[0] = {1.0, 20.0};
+lensShading.falloff.Preview[1] = {2.0, 20.0};
+lensShading.falloff.Preview[2] = {4.0, 20.0};
+lensShading.falloff.Preview[3] = {8.0, 20.0};
+lensShading.falloff.Preview[4] = {16.0, 30.0};
+lensShading.falloff.Preview[5] = {32.0, 30.0};
+lensShading.falloff.Still[0] = {1.0, 20.0};
+lensShading.falloff.Still[1] = {2.0, 20.0};
+lensShading.falloff.Still[2] = {4.0, 20.0};
+lensShading.falloff.Still[3] = {8.0, 20.0};
+lensShading.falloff.Still[4] = {16.0, 30.0};
+lensShading.falloff.Still[5] = {32.0, 30.0};
+lensShading.falloff.Video[0] = {1.0, 20.0};
+lensShading.falloff.Video[1] = {2.0, 20.0};
+lensShading.falloff.Video[2] = {4.0, 20.0};
+lensShading.falloff.Video[3] = {8.0, 20.0};
+lensShading.falloff.Video[4] = {16.0, 30.0};
+lensShading.falloff.Video[5] = {32.0, 30.0};
+#-----------------------------------------------------
+# AWB Parameters
+#-----------------------------------------------------
+
+# AWB
+awb.NumGrayLineSoftClampPoints = 2;
+awb.GrayLineSoftClamp[0] = {0.9268, 0.9268};
+awb.GrayLineSoftClamp[1] = {1.6582, 1.6582};
+awb.GrayLineSoftClamp[2] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[3] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[4] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[5] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[6] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[7] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[8] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[9] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[10] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[11] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[12] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[13] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[14] = {0.0000, 0.0000};
+awb.GrayLineSoftClamp[15] = {0.0000, 0.0000};
+awb.GrayLineSoftClampSlopeBeforeFirstPoint = 0.0000;
+awb.GrayLineSoftClampSlopeAfterLastPoint = 0.0000;
+awb.GrayLineThickness = 0.0088;
+awb.HighU = 1.6582;
+awb.LowU = 0.9268;
+awb.v4.NumGrayLineSoftClampPoints = 3;
+awb.v4.GrayLineSoftClamp[0] = {0.9268, 0.9268};
+awb.v4.GrayLineSoftClamp[1] = {0.9268, 0.9268};
+awb.v4.GrayLineSoftClamp[2] = {1.6582, 1.6582};
+awb.v4.GrayLineSoftClamp[3] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[4] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[5] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[6] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[7] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[8] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[9] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[10] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[11] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[12] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[13] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[14] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClamp[15] = {0.0000, 0.0000};
+awb.v4.GrayLineSoftClampSlopeBeforeFirstPoint = 0.0000;
+awb.v4.GrayLineSoftClampSlopeAfterLastPoint = 0.0000;
+awb.v4.GrayLineThickness = 0.0442;
+awb.v4.HighU = 1.6582;
+awb.v4.LowU = 0.9268;
+
+#Lens shading falloff factor
+lensShading.falloff_factor = 20;
+#-**************************************************************************
+#-* Below parameters are calculated from calibration process. Do not edit!
+#-**************************************************************************
+
+# AWB
+awb.GrayLineSlope = -0.9946;
+awb.GrayLineIntercept = 2.4544;
+awb.UtoMIRED = {-330.909004284264, 700.071557556732};
+awb.MIREDtoU = {-0.003021978813, 2.115601414567};
+awb.UtoCCT = {5351.936855019085,-2504.031973940120};
+awb.CCTtoU = {0.000186848243,0.467873975679};
+awb.NumDiscreteLights = 0;
+awb.v4.GrayLineSlope = -0.9946;
+awb.v4.GrayLineIntercept = 2.4544;
+awb.v4.UtoMIRED = {-330.909004284264, 700.071557556732};
+awb.v4.MIREDtoU = {-0.003021978813, 2.115601414567};
+awb.v4.UtoCCT = {5147.548412945803,-2189.798723064104};
+awb.v4.CCTtoU = {0.000194267236,0.425406144322};
+awb.v4.FusionNumLights = 5;
+awb.v4.FusionLights[0] = {224,384,384,135};
+awb.v4.FusionLights[1] = {224,384,384,135};
+awb.v4.FusionLights[2] = {249,594,594,281};
+awb.v4.FusionLights[3] = {227,555,555,253};
+awb.v4.FusionLights[4] = {214,662,662,442};
+awb.v4.FusionInitLight = 4;
+# CCM
+colorCorrection.srgbMatrix[0] = {1.53743000,-0.19831000, 0.05253000};
+colorCorrection.srgbMatrix[1] = {-0.20559000, 1.69073000,-0.45222000};
+colorCorrection.srgbMatrix[2] = {-0.33184000,-0.49242000, 1.39969000};
+# - sensor gain value corresponding to ISO100
+ae.PerChannelGainAdjustment = {1.0, 1.0, 1.0, 1.0};
+
+# Optical Black
+opticalBlack.manualBiasR = 64;
+opticalBlack.manualBiasGR = 64;
+opticalBlack.manualBiasGB = 64;
+opticalBlack.manualBiasB = 64;
+opticalBlack.float.manualBiasR = 0.06256109;
+opticalBlack.float.manualBiasGR = 0.06256109;
+opticalBlack.float.manualBiasGB = 0.06256109;
+opticalBlack.float.manualBiasB = 0.06256109;
+#-----------------------------------------------------
+# Lens Shading Parameters
+ap15Function.lensShading = TRUE;
+
+#-----------------------------------------------------
+
+# Lens Shading Surfaces
+falloff_srfc.controlPoint[0][0] = 0.000000000000;
+falloff_srfc.controlPoint[0][1] = 0.170708874040;
+falloff_srfc.controlPoint[0][2] = 0.340238317593;
+falloff_srfc.controlPoint[0][3] = 0.452085048909;
+falloff_srfc.controlPoint[0][4] = 0.677164760676;
+falloff_srfc.controlPoint[0][5] = 0.677330142506;
+falloff_srfc.controlPoint[0][6] = 0.452002154012;
+falloff_srfc.controlPoint[0][7] = 0.339406304229;
+falloff_srfc.controlPoint[0][8] = 0.170434181672;
+falloff_srfc.controlPoint[0][9] = 0.000000000000;
+falloff_srfc.controlPoint[1][0] = 0.072132534499;
+falloff_srfc.controlPoint[1][1] = 0.296887547258;
+falloff_srfc.controlPoint[1][2] = 0.466416990811;
+falloff_srfc.controlPoint[1][3] = 0.578263722127;
+falloff_srfc.controlPoint[1][4] = 0.803343433894;
+falloff_srfc.controlPoint[1][5] = 0.803508815724;
+falloff_srfc.controlPoint[1][6] = 0.578180827230;
+falloff_srfc.controlPoint[1][7] = 0.465584977447;
+falloff_srfc.controlPoint[1][8] = 0.296612854890;
+falloff_srfc.controlPoint[1][9] = 0.071377962941;
+falloff_srfc.controlPoint[2][0] = 0.167436650479;
+falloff_srfc.controlPoint[2][1] = 0.392191663238;
+falloff_srfc.controlPoint[2][2] = 0.561721106791;
+falloff_srfc.controlPoint[2][3] = 0.673567838107;
+falloff_srfc.controlPoint[2][4] = 0.898647549874;
+falloff_srfc.controlPoint[2][5] = 0.898812931703;
+falloff_srfc.controlPoint[2][6] = 0.673484943209;
+falloff_srfc.controlPoint[2][7] = 0.560889093426;
+falloff_srfc.controlPoint[2][8] = 0.391916970870;
+falloff_srfc.controlPoint[2][9] = 0.166682078921;
+falloff_srfc.controlPoint[3][0] = 0.230173639911;
+falloff_srfc.controlPoint[3][1] = 0.454928652670;
+falloff_srfc.controlPoint[3][2] = 0.624458096222;
+falloff_srfc.controlPoint[3][3] = 0.736304827538;
+falloff_srfc.controlPoint[3][4] = 0.961384539306;
+falloff_srfc.controlPoint[3][5] = 0.961549921135;
+falloff_srfc.controlPoint[3][6] = 0.736221932641;
+falloff_srfc.controlPoint[3][7] = 0.623626082858;
+falloff_srfc.controlPoint[3][8] = 0.454653960301;
+falloff_srfc.controlPoint[3][9] = 0.229419068352;
+falloff_srfc.controlPoint[4][0] = 0.356522103492;
+falloff_srfc.controlPoint[4][1] = 0.581277116251;
+falloff_srfc.controlPoint[4][2] = 0.750806559804;
+falloff_srfc.controlPoint[4][3] = 0.862653291120;
+falloff_srfc.controlPoint[4][4] = 1.087733002887;
+falloff_srfc.controlPoint[4][5] = 1.087898384717;
+falloff_srfc.controlPoint[4][6] = 0.862570396223;
+falloff_srfc.controlPoint[4][7] = 0.749974546440;
+falloff_srfc.controlPoint[4][8] = 0.581002423883;
+falloff_srfc.controlPoint[4][9] = 0.355767531934;
+falloff_srfc.controlPoint[5][0] = 0.356704584387;
+falloff_srfc.controlPoint[5][1] = 0.581459597146;
+falloff_srfc.controlPoint[5][2] = 0.750989040699;
+falloff_srfc.controlPoint[5][3] = 0.862835772015;
+falloff_srfc.controlPoint[5][4] = 1.087915483783;
+falloff_srfc.controlPoint[5][5] = 1.088080865612;
+falloff_srfc.controlPoint[5][6] = 0.862752877118;
+falloff_srfc.controlPoint[5][7] = 0.750157027335;
+falloff_srfc.controlPoint[5][8] = 0.581184904778;
+falloff_srfc.controlPoint[5][9] = 0.355950012829;
+falloff_srfc.controlPoint[6][0] = 0.230082637765;
+falloff_srfc.controlPoint[6][1] = 0.454837650524;
+falloff_srfc.controlPoint[6][2] = 0.624367094077;
+falloff_srfc.controlPoint[6][3] = 0.736213825393;
+falloff_srfc.controlPoint[6][4] = 0.961293537160;
+falloff_srfc.controlPoint[6][5] = 0.961458918989;
+falloff_srfc.controlPoint[6][6] = 0.736130930495;
+falloff_srfc.controlPoint[6][7] = 0.623535080712;
+falloff_srfc.controlPoint[6][8] = 0.454562958156;
+falloff_srfc.controlPoint[6][9] = 0.229328066207;
+falloff_srfc.controlPoint[7][0] = 0.166849337330;
+falloff_srfc.controlPoint[7][1] = 0.391604350089;
+falloff_srfc.controlPoint[7][2] = 0.561133793642;
+falloff_srfc.controlPoint[7][3] = 0.672980524958;
+falloff_srfc.controlPoint[7][4] = 0.898060236726;
+falloff_srfc.controlPoint[7][5] = 0.898225618555;
+falloff_srfc.controlPoint[7][6] = 0.672897630061;
+falloff_srfc.controlPoint[7][7] = 0.560301780278;
+falloff_srfc.controlPoint[7][8] = 0.391329657721;
+falloff_srfc.controlPoint[7][9] = 0.166094765772;
+falloff_srfc.controlPoint[8][0] = 0.071906715933;
+falloff_srfc.controlPoint[8][1] = 0.296661728692;
+falloff_srfc.controlPoint[8][2] = 0.466191172244;
+falloff_srfc.controlPoint[8][3] = 0.578037903561;
+falloff_srfc.controlPoint[8][4] = 0.803117615328;
+falloff_srfc.controlPoint[8][5] = 0.803282997157;
+falloff_srfc.controlPoint[8][6] = 0.577955008663;
+falloff_srfc.controlPoint[8][7] = 0.465359158880;
+falloff_srfc.controlPoint[8][8] = 0.296387036323;
+falloff_srfc.controlPoint[8][9] = 0.071152144374;
+falloff_srfc.controlPoint[9][0] = 0.000000000000;
+falloff_srfc.controlPoint[9][1] = 0.170146049063;
+falloff_srfc.controlPoint[9][2] = 0.339675492615;
+falloff_srfc.controlPoint[9][3] = 0.451522223932;
+falloff_srfc.controlPoint[9][4] = 0.676601935699;
+falloff_srfc.controlPoint[9][5] = 0.676767317528;
+falloff_srfc.controlPoint[9][6] = 0.451439329034;
+falloff_srfc.controlPoint[9][7] = 0.338843479251;
+falloff_srfc.controlPoint[9][8] = 0.169871356694;
+falloff_srfc.controlPoint[9][9] = 0.000000000000;
+lensShading.correction_type.enableWPC = FALSE;
+lensShading.correction_type.enableLSC = TRUE;
+lensShading.ctrlPointsCount = 3;
+lensShading.imageHeight = 3040;
+lensShading.imageWidth = 4056;
+lensShading.leftPatchFactor = 0.25000;
+lensShading.centerPatchFactor = 0.50000;
+lensShading.topPatchFactor = 0.25000;
+lensShading.middlePatchFactor = 0.50000;
+lensShading.ctrlPoints[0].cct = 2856;
+lensShading.ctrlPoints[0].light_family = 1;
+lensShading.ctrlPoints[0].controlPointR[0][0] = 1.134521484375;
+lensShading.ctrlPoints[0].controlPointR[0][1] = 0.960693359375;
+lensShading.ctrlPoints[0].controlPointR[0][2] = 1.151367187500;
+lensShading.ctrlPoints[0].controlPointR[0][3] = 1.100463867188;
+lensShading.ctrlPoints[0].controlPointR[0][4] = 1.139526367188;
+lensShading.ctrlPoints[0].controlPointR[0][5] = 1.077514648438;
+lensShading.ctrlPoints[0].controlPointR[0][6] = 1.081542968750;
+lensShading.ctrlPoints[0].controlPointR[0][7] = 1.109619140625;
+lensShading.ctrlPoints[0].controlPointR[0][8] = 0.838378906250;
+lensShading.ctrlPoints[0].controlPointR[0][9] = 1.468627929688;
+lensShading.ctrlPoints[0].controlPointR[1][0] = 1.025634765625;
+lensShading.ctrlPoints[0].controlPointR[1][1] = 1.146606445313;
+lensShading.ctrlPoints[0].controlPointR[1][2] = 1.111816406250;
+lensShading.ctrlPoints[0].controlPointR[1][3] = 1.135498046875;
+lensShading.ctrlPoints[0].controlPointR[1][4] = 1.042846679688;
+lensShading.ctrlPoints[0].controlPointR[1][5] = 1.204956054688;
+lensShading.ctrlPoints[0].controlPointR[1][6] = 1.078491210938;
+lensShading.ctrlPoints[0].controlPointR[1][7] = 1.197753906250;
+lensShading.ctrlPoints[0].controlPointR[1][8] = 0.818115234375;
+lensShading.ctrlPoints[0].controlPointR[1][9] = 1.154174804688;
+lensShading.ctrlPoints[0].controlPointR[2][0] = 1.112792968750;
+lensShading.ctrlPoints[0].controlPointR[2][1] = 1.075439453125;
+lensShading.ctrlPoints[0].controlPointR[2][2] = 1.148925781250;
+lensShading.ctrlPoints[0].controlPointR[2][3] = 1.136962890625;
+lensShading.ctrlPoints[0].controlPointR[2][4] = 1.143798828125;
+lensShading.ctrlPoints[0].controlPointR[2][5] = 1.107055664063;
+lensShading.ctrlPoints[0].controlPointR[2][6] = 1.113891601563;
+lensShading.ctrlPoints[0].controlPointR[2][7] = 1.111083984375;
+lensShading.ctrlPoints[0].controlPointR[2][8] = 1.073852539063;
+lensShading.ctrlPoints[0].controlPointR[2][9] = 1.022583007813;
+lensShading.ctrlPoints[0].controlPointR[3][0] = 1.095703125000;
+lensShading.ctrlPoints[0].controlPointR[3][1] = 1.097290039063;
+lensShading.ctrlPoints[0].controlPointR[3][2] = 1.140258789063;
+lensShading.ctrlPoints[0].controlPointR[3][3] = 1.126342773438;
+lensShading.ctrlPoints[0].controlPointR[3][4] = 1.130981445313;
+lensShading.ctrlPoints[0].controlPointR[3][5] = 1.116210937500;
+lensShading.ctrlPoints[0].controlPointR[3][6] = 1.117797851563;
+lensShading.ctrlPoints[0].controlPointR[3][7] = 1.092651367188;
+lensShading.ctrlPoints[0].controlPointR[3][8] = 1.078857421875;
+lensShading.ctrlPoints[0].controlPointR[3][9] = 1.041381835938;
+lensShading.ctrlPoints[0].controlPointR[4][0] = 1.107177734375;
+lensShading.ctrlPoints[0].controlPointR[4][1] = 1.159790039063;
+lensShading.ctrlPoints[0].controlPointR[4][2] = 1.095092773438;
+lensShading.ctrlPoints[0].controlPointR[4][3] = 1.141723632813;
+lensShading.ctrlPoints[0].controlPointR[4][4] = 1.090087890625;
+lensShading.ctrlPoints[0].controlPointR[4][5] = 1.096801757813;
+lensShading.ctrlPoints[0].controlPointR[4][6] = 1.114868164063;
+lensShading.ctrlPoints[0].controlPointR[4][7] = 1.161254882813;
+lensShading.ctrlPoints[0].controlPointR[4][8] = 1.033935546875;
+lensShading.ctrlPoints[0].controlPointR[4][9] = 1.079833984375;
+lensShading.ctrlPoints[0].controlPointR[5][0] = 1.095092773438;
+lensShading.ctrlPoints[0].controlPointR[5][1] = 1.094238281250;
+lensShading.ctrlPoints[0].controlPointR[5][2] = 1.160766601563;
+lensShading.ctrlPoints[0].controlPointR[5][3] = 1.147216796875;
+lensShading.ctrlPoints[0].controlPointR[5][4] = 1.112060546875;
+lensShading.ctrlPoints[0].controlPointR[5][5] = 1.165771484375;
+lensShading.ctrlPoints[0].controlPointR[5][6] = 1.118530273438;
+lensShading.ctrlPoints[0].controlPointR[5][7] = 1.082763671875;
+lensShading.ctrlPoints[0].controlPointR[5][8] = 1.109252929688;
+lensShading.ctrlPoints[0].controlPointR[5][9] = 1.014770507813;
+lensShading.ctrlPoints[0].controlPointR[6][0] = 1.158203125000;
+lensShading.ctrlPoints[0].controlPointR[6][1] = 1.112304687500;
+lensShading.ctrlPoints[0].controlPointR[6][2] = 1.166992187500;
+lensShading.ctrlPoints[0].controlPointR[6][3] = 1.148315429688;
+lensShading.ctrlPoints[0].controlPointR[6][4] = 1.166870117188;
+lensShading.ctrlPoints[0].controlPointR[6][5] = 1.145141601563;
+lensShading.ctrlPoints[0].controlPointR[6][6] = 1.122192382813;
+lensShading.ctrlPoints[0].controlPointR[6][7] = 1.142456054688;
+lensShading.ctrlPoints[0].controlPointR[6][8] = 1.010253906250;
+lensShading.ctrlPoints[0].controlPointR[6][9] = 1.154907226563;
+lensShading.ctrlPoints[0].controlPointR[7][0] = 1.243652343750;
+lensShading.ctrlPoints[0].controlPointR[7][1] = 1.133300781250;
+lensShading.ctrlPoints[0].controlPointR[7][2] = 1.146850585938;
+lensShading.ctrlPoints[0].controlPointR[7][3] = 1.158447265625;
+lensShading.ctrlPoints[0].controlPointR[7][4] = 1.169311523438;
+lensShading.ctrlPoints[0].controlPointR[7][5] = 1.160888671875;
+lensShading.ctrlPoints[0].controlPointR[7][6] = 1.123657226563;
+lensShading.ctrlPoints[0].controlPointR[7][7] = 1.081054687500;
+lensShading.ctrlPoints[0].controlPointR[7][8] = 1.047363281250;
+lensShading.ctrlPoints[0].controlPointR[7][9] = 1.225341796875;
+lensShading.ctrlPoints[0].controlPointR[8][0] = 1.191650390625;
+lensShading.ctrlPoints[0].controlPointR[8][1] = 1.046508789063;
+lensShading.ctrlPoints[0].controlPointR[8][2] = 1.124511718750;
+lensShading.ctrlPoints[0].controlPointR[8][3] = 1.161865234375;
+lensShading.ctrlPoints[0].controlPointR[8][4] = 1.163696289063;
+lensShading.ctrlPoints[0].controlPointR[8][5] = 1.153076171875;
+lensShading.ctrlPoints[0].controlPointR[8][6] = 1.125976562500;
+lensShading.ctrlPoints[0].controlPointR[8][7] = 1.067016601563;
+lensShading.ctrlPoints[0].controlPointR[8][8] = 1.066406250000;
+lensShading.ctrlPoints[0].controlPointR[8][9] = 1.251098632813;
+lensShading.ctrlPoints[0].controlPointR[9][0] = 1.592041015625;
+lensShading.ctrlPoints[0].controlPointR[9][1] = 1.106567382813;
+lensShading.ctrlPoints[0].controlPointR[9][2] = 1.173828125000;
+lensShading.ctrlPoints[0].controlPointR[9][3] = 1.132080078125;
+lensShading.ctrlPoints[0].controlPointR[9][4] = 1.157226562500;
+lensShading.ctrlPoints[0].controlPointR[9][5] = 1.137695312500;
+lensShading.ctrlPoints[0].controlPointR[9][6] = 1.100219726563;
+lensShading.ctrlPoints[0].controlPointR[9][7] = 1.160766601563;
+lensShading.ctrlPoints[0].controlPointR[9][8] = 1.062622070313;
+lensShading.ctrlPoints[0].controlPointR[9][9] = 1.739868164063;
+lensShading.ctrlPoints[0].controlPointGR[0][0] = 1.100097656250;
+lensShading.ctrlPoints[0].controlPointGR[0][1] = 1.002075195313;
+lensShading.ctrlPoints[0].controlPointGR[0][2] = 1.099365234375;
+lensShading.ctrlPoints[0].controlPointGR[0][3] = 1.088500976563;
+lensShading.ctrlPoints[0].controlPointGR[0][4] = 1.087158203125;
+lensShading.ctrlPoints[0].controlPointGR[0][5] = 1.086181640625;
+lensShading.ctrlPoints[0].controlPointGR[0][6] = 1.064208984375;
+lensShading.ctrlPoints[0].controlPointGR[0][7] = 1.101684570313;
+lensShading.ctrlPoints[0].controlPointGR[0][8] = 0.797363281250;
+lensShading.ctrlPoints[0].controlPointGR[0][9] = 1.501220703125;
+lensShading.ctrlPoints[0].controlPointGR[1][0] = 1.005615234375;
+lensShading.ctrlPoints[0].controlPointGR[1][1] = 1.123046875000;
+lensShading.ctrlPoints[0].controlPointGR[1][2] = 1.123046875000;
+lensShading.ctrlPoints[0].controlPointGR[1][3] = 1.106079101563;
+lensShading.ctrlPoints[0].controlPointGR[1][4] = 1.078369140625;
+lensShading.ctrlPoints[0].controlPointGR[1][5] = 1.133789062500;
+lensShading.ctrlPoints[0].controlPointGR[1][6] = 1.052612304688;
+lensShading.ctrlPoints[0].controlPointGR[1][7] = 1.211547851563;
+lensShading.ctrlPoints[0].controlPointGR[1][8] = 0.774658203125;
+lensShading.ctrlPoints[0].controlPointGR[1][9] = 1.173461914063;
+lensShading.ctrlPoints[0].controlPointGR[2][0] = 1.088745117188;
+lensShading.ctrlPoints[0].controlPointGR[2][1] = 1.061279296875;
+lensShading.ctrlPoints[0].controlPointGR[2][2] = 1.074218750000;
+lensShading.ctrlPoints[0].controlPointGR[2][3] = 1.102661132813;
+lensShading.ctrlPoints[0].controlPointGR[2][4] = 1.085937500000;
+lensShading.ctrlPoints[0].controlPointGR[2][5] = 1.096435546875;
+lensShading.ctrlPoints[0].controlPointGR[2][6] = 1.085571289063;
+lensShading.ctrlPoints[0].controlPointGR[2][7] = 1.080810546875;
+lensShading.ctrlPoints[0].controlPointGR[2][8] = 1.092163085938;
+lensShading.ctrlPoints[0].controlPointGR[2][9] = 0.992431640625;
+lensShading.ctrlPoints[0].controlPointGR[3][0] = 1.065063476563;
+lensShading.ctrlPoints[0].controlPointGR[3][1] = 1.099609375000;
+lensShading.ctrlPoints[0].controlPointGR[3][2] = 1.099365234375;
+lensShading.ctrlPoints[0].controlPointGR[3][3] = 1.106689453125;
+lensShading.ctrlPoints[0].controlPointGR[3][4] = 1.090698242188;
+lensShading.ctrlPoints[0].controlPointGR[3][5] = 1.088623046875;
+lensShading.ctrlPoints[0].controlPointGR[3][6] = 1.085571289063;
+lensShading.ctrlPoints[0].controlPointGR[3][7] = 1.080322265625;
+lensShading.ctrlPoints[0].controlPointGR[3][8] = 1.042724609375;
+lensShading.ctrlPoints[0].controlPointGR[3][9] = 1.036987304688;
+lensShading.ctrlPoints[0].controlPointGR[4][0] = 1.100708007813;
+lensShading.ctrlPoints[0].controlPointGR[4][1] = 1.085083007813;
+lensShading.ctrlPoints[0].controlPointGR[4][2] = 1.118652343750;
+lensShading.ctrlPoints[0].controlPointGR[4][3] = 1.099121093750;
+lensShading.ctrlPoints[0].controlPointGR[4][4] = 1.078613281250;
+lensShading.ctrlPoints[0].controlPointGR[4][5] = 1.072509765625;
+lensShading.ctrlPoints[0].controlPointGR[4][6] = 1.091186523438;
+lensShading.ctrlPoints[0].controlPointGR[4][7] = 1.120605468750;
+lensShading.ctrlPoints[0].controlPointGR[4][8] = 1.055175781250;
+lensShading.ctrlPoints[0].controlPointGR[4][9] = 1.049072265625;
+lensShading.ctrlPoints[0].controlPointGR[5][0] = 1.065551757813;
+lensShading.ctrlPoints[0].controlPointGR[5][1] = 1.103759765625;
+lensShading.ctrlPoints[0].controlPointGR[5][2] = 1.095458984375;
+lensShading.ctrlPoints[0].controlPointGR[5][3] = 1.116943359375;
+lensShading.ctrlPoints[0].controlPointGR[5][4] = 1.092529296875;
+lensShading.ctrlPoints[0].controlPointGR[5][5] = 1.109741210938;
+lensShading.ctrlPoints[0].controlPointGR[5][6] = 1.088989257813;
+lensShading.ctrlPoints[0].controlPointGR[5][7] = 1.098632812500;
+lensShading.ctrlPoints[0].controlPointGR[5][8] = 1.044067382813;
+lensShading.ctrlPoints[0].controlPointGR[5][9] = 1.027587890625;
+lensShading.ctrlPoints[0].controlPointGR[6][0] = 1.129882812500;
+lensShading.ctrlPoints[0].controlPointGR[6][1] = 1.072387695313;
+lensShading.ctrlPoints[0].controlPointGR[6][2] = 1.142822265625;
+lensShading.ctrlPoints[0].controlPointGR[6][3] = 1.111816406250;
+lensShading.ctrlPoints[0].controlPointGR[6][4] = 1.127075195313;
+lensShading.ctrlPoints[0].controlPointGR[6][5] = 1.117553710938;
+lensShading.ctrlPoints[0].controlPointGR[6][6] = 1.098876953125;
+lensShading.ctrlPoints[0].controlPointGR[6][7] = 1.090087890625;
+lensShading.ctrlPoints[0].controlPointGR[6][8] = 1.019653320313;
+lensShading.ctrlPoints[0].controlPointGR[6][9] = 1.123779296875;
+lensShading.ctrlPoints[0].controlPointGR[7][0] = 1.216796875000;
+lensShading.ctrlPoints[0].controlPointGR[7][1] = 1.040893554688;
+lensShading.ctrlPoints[0].controlPointGR[7][2] = 1.096801757813;
+lensShading.ctrlPoints[0].controlPointGR[7][3] = 1.152221679688;
+lensShading.ctrlPoints[0].controlPointGR[7][4] = 1.105346679688;
+lensShading.ctrlPoints[0].controlPointGR[7][5] = 1.128295898438;
+lensShading.ctrlPoints[0].controlPointGR[7][6] = 1.091186523438;
+lensShading.ctrlPoints[0].controlPointGR[7][7] = 1.113647460938;
+lensShading.ctrlPoints[0].controlPointGR[7][8] = 0.949462890625;
+lensShading.ctrlPoints[0].controlPointGR[7][9] = 1.240234375000;
+lensShading.ctrlPoints[0].controlPointGR[8][0] = 1.189575195313;
+lensShading.ctrlPoints[0].controlPointGR[8][1] = 1.028442382813;
+lensShading.ctrlPoints[0].controlPointGR[8][2] = 1.112915039063;
+lensShading.ctrlPoints[0].controlPointGR[8][3] = 1.089843750000;
+lensShading.ctrlPoints[0].controlPointGR[8][4] = 1.161132812500;
+lensShading.ctrlPoints[0].controlPointGR[8][5] = 1.097900390625;
+lensShading.ctrlPoints[0].controlPointGR[8][6] = 1.094238281250;
+lensShading.ctrlPoints[0].controlPointGR[8][7] = 1.018676757813;
+lensShading.ctrlPoints[0].controlPointGR[8][8] = 1.049926757813;
+lensShading.ctrlPoints[0].controlPointGR[8][9] = 1.245361328125;
+lensShading.ctrlPoints[0].controlPointGR[9][0] = 1.515502929688;
+lensShading.ctrlPoints[0].controlPointGR[9][1] = 1.076782226563;
+lensShading.ctrlPoints[0].controlPointGR[9][2] = 1.101684570313;
+lensShading.ctrlPoints[0].controlPointGR[9][3] = 1.114746093750;
+lensShading.ctrlPoints[0].controlPointGR[9][4] = 1.101440429688;
+lensShading.ctrlPoints[0].controlPointGR[9][5] = 1.131469726563;
+lensShading.ctrlPoints[0].controlPointGR[9][6] = 1.059570312500;
+lensShading.ctrlPoints[0].controlPointGR[9][7] = 1.145385742188;
+lensShading.ctrlPoints[0].controlPointGR[9][8] = 1.019897460938;
+lensShading.ctrlPoints[0].controlPointGR[9][9] = 1.733398437500;
+lensShading.ctrlPoints[0].controlPointGB[0][0] = 1.117187500000;
+lensShading.ctrlPoints[0].controlPointGB[0][1] = 1.017211914063;
+lensShading.ctrlPoints[0].controlPointGB[0][2] = 1.104248046875;
+lensShading.ctrlPoints[0].controlPointGB[0][3] = 1.093139648438;
+lensShading.ctrlPoints[0].controlPointGB[0][4] = 1.103027343750;
+lensShading.ctrlPoints[0].controlPointGB[0][5] = 1.083740234375;
+lensShading.ctrlPoints[0].controlPointGB[0][6] = 1.072387695313;
+lensShading.ctrlPoints[0].controlPointGB[0][7] = 1.144775390625;
+lensShading.ctrlPoints[0].controlPointGB[0][8] = 0.770751953125;
+lensShading.ctrlPoints[0].controlPointGB[0][9] = 1.514770507813;
+lensShading.ctrlPoints[0].controlPointGB[1][0] = 1.018310546875;
+lensShading.ctrlPoints[0].controlPointGB[1][1] = 1.127197265625;
+lensShading.ctrlPoints[0].controlPointGB[1][2] = 1.090209960938;
+lensShading.ctrlPoints[0].controlPointGB[1][3] = 1.122802734375;
+lensShading.ctrlPoints[0].controlPointGB[1][4] = 1.068603515625;
+lensShading.ctrlPoints[0].controlPointGB[1][5] = 1.144165039063;
+lensShading.ctrlPoints[0].controlPointGB[1][6] = 1.074829101563;
+lensShading.ctrlPoints[0].controlPointGB[1][7] = 1.159301757813;
+lensShading.ctrlPoints[0].controlPointGB[1][8] = 0.852539062500;
+lensShading.ctrlPoints[0].controlPointGB[1][9] = 1.158813476563;
+lensShading.ctrlPoints[0].controlPointGB[2][0] = 1.115112304688;
+lensShading.ctrlPoints[0].controlPointGB[2][1] = 1.026489257813;
+lensShading.ctrlPoints[0].controlPointGB[2][2] = 1.140136718750;
+lensShading.ctrlPoints[0].controlPointGB[2][3] = 1.093750000000;
+lensShading.ctrlPoints[0].controlPointGB[2][4] = 1.116455078125;
+lensShading.ctrlPoints[0].controlPointGB[2][5] = 1.094360351563;
+lensShading.ctrlPoints[0].controlPointGB[2][6] = 1.088623046875;
+lensShading.ctrlPoints[0].controlPointGB[2][7] = 1.106933593750;
+lensShading.ctrlPoints[0].controlPointGB[2][8] = 1.067504882813;
+lensShading.ctrlPoints[0].controlPointGB[2][9] = 1.041259765625;
+lensShading.ctrlPoints[0].controlPointGB[3][0] = 1.063110351563;
+lensShading.ctrlPoints[0].controlPointGB[3][1] = 1.118896484375;
+lensShading.ctrlPoints[0].controlPointGB[3][2] = 1.090820312500;
+lensShading.ctrlPoints[0].controlPointGB[3][3] = 1.110229492188;
+lensShading.ctrlPoints[0].controlPointGB[3][4] = 1.095947265625;
+lensShading.ctrlPoints[0].controlPointGB[3][5] = 1.096557617188;
+lensShading.ctrlPoints[0].controlPointGB[3][6] = 1.090942382813;
+lensShading.ctrlPoints[0].controlPointGB[3][7] = 1.093261718750;
+lensShading.ctrlPoints[0].controlPointGB[3][8] = 1.048706054688;
+lensShading.ctrlPoints[0].controlPointGB[3][9] = 1.037231445313;
+lensShading.ctrlPoints[0].controlPointGB[4][0] = 1.107788085938;
+lensShading.ctrlPoints[0].controlPointGB[4][1] = 1.068359375000;
+lensShading.ctrlPoints[0].controlPointGB[4][2] = 1.127197265625;
+lensShading.ctrlPoints[0].controlPointGB[4][3] = 1.099121093750;
+lensShading.ctrlPoints[0].controlPointGB[4][4] = 1.084106445313;
+lensShading.ctrlPoints[0].controlPointGB[4][5] = 1.066650390625;
+lensShading.ctrlPoints[0].controlPointGB[4][6] = 1.100585937500;
+lensShading.ctrlPoints[0].controlPointGB[4][7] = 1.133056640625;
+lensShading.ctrlPoints[0].controlPointGB[4][8] = 1.051391601563;
+lensShading.ctrlPoints[0].controlPointGB[4][9] = 1.067993164063;
+lensShading.ctrlPoints[0].controlPointGB[5][0] = 1.061889648438;
+lensShading.ctrlPoints[0].controlPointGB[5][1] = 1.123413085938;
+lensShading.ctrlPoints[0].controlPointGB[5][2] = 1.090698242188;
+lensShading.ctrlPoints[0].controlPointGB[5][3] = 1.124145507813;
+lensShading.ctrlPoints[0].controlPointGB[5][4] = 1.074584960938;
+lensShading.ctrlPoints[0].controlPointGB[5][5] = 1.134033203125;
+lensShading.ctrlPoints[0].controlPointGB[5][6] = 1.088500976563;
+lensShading.ctrlPoints[0].controlPointGB[5][7] = 1.110351562500;
+lensShading.ctrlPoints[0].controlPointGB[5][8] = 1.036376953125;
+lensShading.ctrlPoints[0].controlPointGB[5][9] = 1.032226562500;
+lensShading.ctrlPoints[0].controlPointGB[6][0] = 1.125488281250;
+lensShading.ctrlPoints[0].controlPointGB[6][1] = 1.076782226563;
+lensShading.ctrlPoints[0].controlPointGB[6][2] = 1.135009765625;
+lensShading.ctrlPoints[0].controlPointGB[6][3] = 1.111694335938;
+lensShading.ctrlPoints[0].controlPointGB[6][4] = 1.130371093750;
+lensShading.ctrlPoints[0].controlPointGB[6][5] = 1.120483398438;
+lensShading.ctrlPoints[0].controlPointGB[6][6] = 1.097290039063;
+lensShading.ctrlPoints[0].controlPointGB[6][7] = 1.094482421875;
+lensShading.ctrlPoints[0].controlPointGB[6][8] = 1.017700195313;
+lensShading.ctrlPoints[0].controlPointGB[6][9] = 1.139282226563;
+lensShading.ctrlPoints[0].controlPointGB[7][0] = 1.214599609375;
+lensShading.ctrlPoints[0].controlPointGB[7][1] = 1.060180664063;
+lensShading.ctrlPoints[0].controlPointGB[7][2] = 1.109130859375;
+lensShading.ctrlPoints[0].controlPointGB[7][3] = 1.139404296875;
+lensShading.ctrlPoints[0].controlPointGB[7][4] = 1.105468750000;
+lensShading.ctrlPoints[0].controlPointGB[7][5] = 1.122070312500;
+lensShading.ctrlPoints[0].controlPointGB[7][6] = 1.092529296875;
+lensShading.ctrlPoints[0].controlPointGB[7][7] = 1.104370117188;
+lensShading.ctrlPoints[0].controlPointGB[7][8] = 0.979003906250;
+lensShading.ctrlPoints[0].controlPointGB[7][9] = 1.221557617188;
+lensShading.ctrlPoints[0].controlPointGB[8][0] = 1.171875000000;
+lensShading.ctrlPoints[0].controlPointGB[8][1] = 1.030273437500;
+lensShading.ctrlPoints[0].controlPointGB[8][2] = 1.096435546875;
+lensShading.ctrlPoints[0].controlPointGB[8][3] = 1.103759765625;
+lensShading.ctrlPoints[0].controlPointGB[8][4] = 1.139770507813;
+lensShading.ctrlPoints[0].controlPointGB[8][5] = 1.121459960938;
+lensShading.ctrlPoints[0].controlPointGB[8][6] = 1.088989257813;
+lensShading.ctrlPoints[0].controlPointGB[8][7] = 1.006225585938;
+lensShading.ctrlPoints[0].controlPointGB[8][8] = 1.081665039063;
+lensShading.ctrlPoints[0].controlPointGB[8][9] = 1.246459960938;
+lensShading.ctrlPoints[0].controlPointGB[9][0] = 1.529052734375;
+lensShading.ctrlPoints[0].controlPointGB[9][1] = 1.055419921875;
+lensShading.ctrlPoints[0].controlPointGB[9][2] = 1.115356445313;
+lensShading.ctrlPoints[0].controlPointGB[9][3] = 1.094238281250;
+lensShading.ctrlPoints[0].controlPointGB[9][4] = 1.119750976563;
+lensShading.ctrlPoints[0].controlPointGB[9][5] = 1.108276367188;
+lensShading.ctrlPoints[0].controlPointGB[9][6] = 1.066284179688;
+lensShading.ctrlPoints[0].controlPointGB[9][7] = 1.156982421875;
+lensShading.ctrlPoints[0].controlPointGB[9][8] = 0.994873046875;
+lensShading.ctrlPoints[0].controlPointGB[9][9] = 1.735473632813;
+lensShading.ctrlPoints[0].controlPointB[0][0] = 1.137939453125;
+lensShading.ctrlPoints[0].controlPointB[0][1] = 1.042846679688;
+lensShading.ctrlPoints[0].controlPointB[0][2] = 1.180297851563;
+lensShading.ctrlPoints[0].controlPointB[0][3] = 1.141113281250;
+lensShading.ctrlPoints[0].controlPointB[0][4] = 1.173828125000;
+lensShading.ctrlPoints[0].controlPointB[0][5] = 1.112426757813;
+lensShading.ctrlPoints[0].controlPointB[0][6] = 1.125732421875;
+lensShading.ctrlPoints[0].controlPointB[0][7] = 1.078247070313;
+lensShading.ctrlPoints[0].controlPointB[0][8] = 0.961303710938;
+lensShading.ctrlPoints[0].controlPointB[0][9] = 1.374267578125;
+lensShading.ctrlPoints[0].controlPointB[1][0] = 1.055786132813;
+lensShading.ctrlPoints[0].controlPointB[1][1] = 1.235839843750;
+lensShading.ctrlPoints[0].controlPointB[1][2] = 1.083984375000;
+lensShading.ctrlPoints[0].controlPointB[1][3] = 1.199096679688;
+lensShading.ctrlPoints[0].controlPointB[1][4] = 1.049316406250;
+lensShading.ctrlPoints[0].controlPointB[1][5] = 1.266479492188;
+lensShading.ctrlPoints[0].controlPointB[1][6] = 1.101318359375;
+lensShading.ctrlPoints[0].controlPointB[1][7] = 1.285400390625;
+lensShading.ctrlPoints[0].controlPointB[1][8] = 0.756103515625;
+lensShading.ctrlPoints[0].controlPointB[1][9] = 1.257324218750;
+lensShading.ctrlPoints[0].controlPointB[2][0] = 1.153320312500;
+lensShading.ctrlPoints[0].controlPointB[2][1] = 1.080078125000;
+lensShading.ctrlPoints[0].controlPointB[2][2] = 1.217041015625;
+lensShading.ctrlPoints[0].controlPointB[2][3] = 1.160156250000;
+lensShading.ctrlPoints[0].controlPointB[2][4] = 1.203247070313;
+lensShading.ctrlPoints[0].controlPointB[2][5] = 1.126708984375;
+lensShading.ctrlPoints[0].controlPointB[2][6] = 1.142700195313;
+lensShading.ctrlPoints[0].controlPointB[2][7] = 1.089965820313;
+lensShading.ctrlPoints[0].controlPointB[2][8] = 1.209228515625;
+lensShading.ctrlPoints[0].controlPointB[2][9] = 0.934448242188;
+lensShading.ctrlPoints[0].controlPointB[3][0] = 1.117187500000;
+lensShading.ctrlPoints[0].controlPointB[3][1] = 1.179565429688;
+lensShading.ctrlPoints[0].controlPointB[3][2] = 1.145507812500;
+lensShading.ctrlPoints[0].controlPointB[3][3] = 1.174682617188;
+lensShading.ctrlPoints[0].controlPointB[3][4] = 1.156982421875;
+lensShading.ctrlPoints[0].controlPointB[3][5] = 1.140625000000;
+lensShading.ctrlPoints[0].controlPointB[3][6] = 1.150268554688;
+lensShading.ctrlPoints[0].controlPointB[3][7] = 1.119018554688;
+lensShading.ctrlPoints[0].controlPointB[3][8] = 1.095703125000;
+lensShading.ctrlPoints[0].controlPointB[3][9] = 1.050292968750;
+lensShading.ctrlPoints[0].controlPointB[4][0] = 1.148681640625;
+lensShading.ctrlPoints[0].controlPointB[4][1] = 1.168457031250;
+lensShading.ctrlPoints[0].controlPointB[4][2] = 1.172363281250;
+lensShading.ctrlPoints[0].controlPointB[4][3] = 1.175781250000;
+lensShading.ctrlPoints[0].controlPointB[4][4] = 1.140258789063;
+lensShading.ctrlPoints[0].controlPointB[4][5] = 1.153076171875;
+lensShading.ctrlPoints[0].controlPointB[4][6] = 1.117553710938;
+lensShading.ctrlPoints[0].controlPointB[4][7] = 1.195678710938;
+lensShading.ctrlPoints[0].controlPointB[4][8] = 1.082275390625;
+lensShading.ctrlPoints[0].controlPointB[4][9] = 1.064575195313;
+lensShading.ctrlPoints[0].controlPointB[5][0] = 1.143920898438;
+lensShading.ctrlPoints[0].controlPointB[5][1] = 1.122070312500;
+lensShading.ctrlPoints[0].controlPointB[5][2] = 1.201904296875;
+lensShading.ctrlPoints[0].controlPointB[5][3] = 1.177368164063;
+lensShading.ctrlPoints[0].controlPointB[5][4] = 1.154907226563;
+lensShading.ctrlPoints[0].controlPointB[5][5] = 1.172485351563;
+lensShading.ctrlPoints[0].controlPointB[5][6] = 1.172729492188;
+lensShading.ctrlPoints[0].controlPointB[5][7] = 1.101928710938;
+lensShading.ctrlPoints[0].controlPointB[5][8] = 1.129638671875;
+lensShading.ctrlPoints[0].controlPointB[5][9] = 1.034912109375;
+lensShading.ctrlPoints[0].controlPointB[6][0] = 1.174804687500;
+lensShading.ctrlPoints[0].controlPointB[6][1] = 1.164306640625;
+lensShading.ctrlPoints[0].controlPointB[6][2] = 1.193359375000;
+lensShading.ctrlPoints[0].controlPointB[6][3] = 1.195312500000;
+lensShading.ctrlPoints[0].controlPointB[6][4] = 1.186523437500;
+lensShading.ctrlPoints[0].controlPointB[6][5] = 1.188598632813;
+lensShading.ctrlPoints[0].controlPointB[6][6] = 1.139160156250;
+lensShading.ctrlPoints[0].controlPointB[6][7] = 1.148437500000;
+lensShading.ctrlPoints[0].controlPointB[6][8] = 1.061401367188;
+lensShading.ctrlPoints[0].controlPointB[6][9] = 1.144042968750;
+lensShading.ctrlPoints[0].controlPointB[7][0] = 1.367065429688;
+lensShading.ctrlPoints[0].controlPointB[7][1] = 1.026977539063;
+lensShading.ctrlPoints[0].controlPointB[7][2] = 1.276733398438;
+lensShading.ctrlPoints[0].controlPointB[7][3] = 1.187011718750;
+lensShading.ctrlPoints[0].controlPointB[7][4] = 1.210571289063;
+lensShading.ctrlPoints[0].controlPointB[7][5] = 1.171508789063;
+lensShading.ctrlPoints[0].controlPointB[7][6] = 1.181274414063;
+lensShading.ctrlPoints[0].controlPointB[7][7] = 1.132690429688;
+lensShading.ctrlPoints[0].controlPointB[7][8] = 1.095703125000;
+lensShading.ctrlPoints[0].controlPointB[7][9] = 1.181762695313;
+lensShading.ctrlPoints[0].controlPointB[8][0] = 1.191650390625;
+lensShading.ctrlPoints[0].controlPointB[8][1] = 1.103515625000;
+lensShading.ctrlPoints[0].controlPointB[8][2] = 1.111938476563;
+lensShading.ctrlPoints[0].controlPointB[8][3] = 1.196655273438;
+lensShading.ctrlPoints[0].controlPointB[8][4] = 1.145141601563;
+lensShading.ctrlPoints[0].controlPointB[8][5] = 1.185913085938;
+lensShading.ctrlPoints[0].controlPointB[8][6] = 1.109375000000;
+lensShading.ctrlPoints[0].controlPointB[8][7] = 1.027221679688;
+lensShading.ctrlPoints[0].controlPointB[8][8] = 1.090820312500;
+lensShading.ctrlPoints[0].controlPointB[8][9] = 1.261230468750;
+lensShading.ctrlPoints[0].controlPointB[9][0] = 1.585205078125;
+lensShading.ctrlPoints[0].controlPointB[9][1] = 1.170654296875;
+lensShading.ctrlPoints[0].controlPointB[9][2] = 1.156127929688;
+lensShading.ctrlPoints[0].controlPointB[9][3] = 1.155395507813;
+lensShading.ctrlPoints[0].controlPointB[9][4] = 1.184204101563;
+lensShading.ctrlPoints[0].controlPointB[9][5] = 1.171875000000;
+lensShading.ctrlPoints[0].controlPointB[9][6] = 1.123291015625;
+lensShading.ctrlPoints[0].controlPointB[9][7] = 1.159790039063;
+lensShading.ctrlPoints[0].controlPointB[9][8] = 1.119750976563;
+lensShading.ctrlPoints[0].controlPointB[9][9] = 1.673583984375;
+lensShading.ctrlPoints[1].cct = 4000;
+lensShading.ctrlPoints[1].light_family = 2;
+lensShading.ctrlPoints[1].controlPointR[0][0] = 1.083862304688;
+lensShading.ctrlPoints[1].controlPointR[0][1] = 0.958007812500;
+lensShading.ctrlPoints[1].controlPointR[0][2] = 1.130615234375;
+lensShading.ctrlPoints[1].controlPointR[0][3] = 1.063964843750;
+lensShading.ctrlPoints[1].controlPointR[0][4] = 1.088989257813;
+lensShading.ctrlPoints[1].controlPointR[0][5] = 1.084594726563;
+lensShading.ctrlPoints[1].controlPointR[0][6] = 1.072143554688;
+lensShading.ctrlPoints[1].controlPointR[0][7] = 1.107177734375;
+lensShading.ctrlPoints[1].controlPointR[0][8] = 0.815429687500;
+lensShading.ctrlPoints[1].controlPointR[0][9] = 1.580322265625;
+lensShading.ctrlPoints[1].controlPointR[1][0] = 0.999633789063;
+lensShading.ctrlPoints[1].controlPointR[1][1] = 1.102416992188;
+lensShading.ctrlPoints[1].controlPointR[1][2] = 1.067504882813;
+lensShading.ctrlPoints[1].controlPointR[1][3] = 1.113647460938;
+lensShading.ctrlPoints[1].controlPointR[1][4] = 1.112670898438;
+lensShading.ctrlPoints[1].controlPointR[1][5] = 1.102416992188;
+lensShading.ctrlPoints[1].controlPointR[1][6] = 1.081298828125;
+lensShading.ctrlPoints[1].controlPointR[1][7] = 1.187500000000;
+lensShading.ctrlPoints[1].controlPointR[1][8] = 0.771728515625;
+lensShading.ctrlPoints[1].controlPointR[1][9] = 1.229492187500;
+lensShading.ctrlPoints[1].controlPointR[2][0] = 1.086914062500;
+lensShading.ctrlPoints[1].controlPointR[2][1] = 1.023315429688;
+lensShading.ctrlPoints[1].controlPointR[2][2] = 1.174926757813;
+lensShading.ctrlPoints[1].controlPointR[2][3] = 1.089599609375;
+lensShading.ctrlPoints[1].controlPointR[2][4] = 1.111572265625;
+lensShading.ctrlPoints[1].controlPointR[2][5] = 1.117797851563;
+lensShading.ctrlPoints[1].controlPointR[2][6] = 1.111450195313;
+lensShading.ctrlPoints[1].controlPointR[2][7] = 1.166137695313;
+lensShading.ctrlPoints[1].controlPointR[2][8] = 1.044799804688;
+lensShading.ctrlPoints[1].controlPointR[2][9] = 1.093383789063;
+lensShading.ctrlPoints[1].controlPointR[3][0] = 1.056518554688;
+lensShading.ctrlPoints[1].controlPointR[3][1] = 1.092895507813;
+lensShading.ctrlPoints[1].controlPointR[3][2] = 1.084472656250;
+lensShading.ctrlPoints[1].controlPointR[3][3] = 1.109375000000;
+lensShading.ctrlPoints[1].controlPointR[3][4] = 1.092895507813;
+lensShading.ctrlPoints[1].controlPointR[3][5] = 1.099609375000;
+lensShading.ctrlPoints[1].controlPointR[3][6] = 1.109252929688;
+lensShading.ctrlPoints[1].controlPointR[3][7] = 1.090209960938;
+lensShading.ctrlPoints[1].controlPointR[3][8] = 1.074584960938;
+lensShading.ctrlPoints[1].controlPointR[3][9] = 1.081420898438;
+lensShading.ctrlPoints[1].controlPointR[4][0] = 1.095581054688;
+lensShading.ctrlPoints[1].controlPointR[4][1] = 1.059204101563;
+lensShading.ctrlPoints[1].controlPointR[4][2] = 1.116210937500;
+lensShading.ctrlPoints[1].controlPointR[4][3] = 1.092895507813;
+lensShading.ctrlPoints[1].controlPointR[4][4] = 1.082519531250;
+lensShading.ctrlPoints[1].controlPointR[4][5] = 1.094360351563;
+lensShading.ctrlPoints[1].controlPointR[4][6] = 1.110229492188;
+lensShading.ctrlPoints[1].controlPointR[4][7] = 1.224975585938;
+lensShading.ctrlPoints[1].controlPointR[4][8] = 0.971557617188;
+lensShading.ctrlPoints[1].controlPointR[4][9] = 1.152832031250;
+lensShading.ctrlPoints[1].controlPointR[5][0] = 1.061035156250;
+lensShading.ctrlPoints[1].controlPointR[5][1] = 1.090332031250;
+lensShading.ctrlPoints[1].controlPointR[5][2] = 1.116821289063;
+lensShading.ctrlPoints[1].controlPointR[5][3] = 1.118408203125;
+lensShading.ctrlPoints[1].controlPointR[5][4] = 1.095092773438;
+lensShading.ctrlPoints[1].controlPointR[5][5] = 1.105590820313;
+lensShading.ctrlPoints[1].controlPointR[5][6] = 1.118774414063;
+lensShading.ctrlPoints[1].controlPointR[5][7] = 1.063232421875;
+lensShading.ctrlPoints[1].controlPointR[5][8] = 1.124145507813;
+lensShading.ctrlPoints[1].controlPointR[5][9] = 1.058837890625;
+lensShading.ctrlPoints[1].controlPointR[6][0] = 1.113281250000;
+lensShading.ctrlPoints[1].controlPointR[6][1] = 1.043212890625;
+lensShading.ctrlPoints[1].controlPointR[6][2] = 1.131225585938;
+lensShading.ctrlPoints[1].controlPointR[6][3] = 1.104370117188;
+lensShading.ctrlPoints[1].controlPointR[6][4] = 1.133911132813;
+lensShading.ctrlPoints[1].controlPointR[6][5] = 1.117553710938;
+lensShading.ctrlPoints[1].controlPointR[6][6] = 1.117431640625;
+lensShading.ctrlPoints[1].controlPointR[6][7] = 1.149169921875;
+lensShading.ctrlPoints[1].controlPointR[6][8] = 1.019775390625;
+lensShading.ctrlPoints[1].controlPointR[6][9] = 1.210449218750;
+lensShading.ctrlPoints[1].controlPointR[7][0] = 1.201782226563;
+lensShading.ctrlPoints[1].controlPointR[7][1] = 1.068115234375;
+lensShading.ctrlPoints[1].controlPointR[7][2] = 1.099975585938;
+lensShading.ctrlPoints[1].controlPointR[7][3] = 1.140380859375;
+lensShading.ctrlPoints[1].controlPointR[7][4] = 1.080566406250;
+lensShading.ctrlPoints[1].controlPointR[7][5] = 1.162231445313;
+lensShading.ctrlPoints[1].controlPointR[7][6] = 1.121337890625;
+lensShading.ctrlPoints[1].controlPointR[7][7] = 1.080932617188;
+lensShading.ctrlPoints[1].controlPointR[7][8] = 1.054565429688;
+lensShading.ctrlPoints[1].controlPointR[7][9] = 1.302734375000;
+lensShading.ctrlPoints[1].controlPointR[8][0] = 1.157836914063;
+lensShading.ctrlPoints[1].controlPointR[8][1] = 0.961791992188;
+lensShading.ctrlPoints[1].controlPointR[8][2] = 1.110351562500;
+lensShading.ctrlPoints[1].controlPointR[8][3] = 1.092651367188;
+lensShading.ctrlPoints[1].controlPointR[8][4] = 1.166992187500;
+lensShading.ctrlPoints[1].controlPointR[8][5] = 1.119873046875;
+lensShading.ctrlPoints[1].controlPointR[8][6] = 1.091552734375;
+lensShading.ctrlPoints[1].controlPointR[8][7] = 1.112548828125;
+lensShading.ctrlPoints[1].controlPointR[8][8] = 1.015258789063;
+lensShading.ctrlPoints[1].controlPointR[8][9] = 1.356323242188;
+lensShading.ctrlPoints[1].controlPointR[9][0] = 1.489013671875;
+lensShading.ctrlPoints[1].controlPointR[9][1] = 1.096069335938;
+lensShading.ctrlPoints[1].controlPointR[9][2] = 1.102050781250;
+lensShading.ctrlPoints[1].controlPointR[9][3] = 1.106079101563;
+lensShading.ctrlPoints[1].controlPointR[9][4] = 1.133422851563;
+lensShading.ctrlPoints[1].controlPointR[9][5] = 1.097045898438;
+lensShading.ctrlPoints[1].controlPointR[9][6] = 1.115844726563;
+lensShading.ctrlPoints[1].controlPointR[9][7] = 1.141357421875;
+lensShading.ctrlPoints[1].controlPointR[9][8] = 1.062133789063;
+lensShading.ctrlPoints[1].controlPointR[9][9] = 1.936035156250;
+lensShading.ctrlPoints[1].controlPointGR[0][0] = 1.063476562500;
+lensShading.ctrlPoints[1].controlPointGR[0][1] = 1.007202148438;
+lensShading.ctrlPoints[1].controlPointGR[0][2] = 1.059814453125;
+lensShading.ctrlPoints[1].controlPointGR[0][3] = 1.056518554688;
+lensShading.ctrlPoints[1].controlPointGR[0][4] = 1.074951171875;
+lensShading.ctrlPoints[1].controlPointGR[0][5] = 1.055541992188;
+lensShading.ctrlPoints[1].controlPointGR[0][6] = 1.048339843750;
+lensShading.ctrlPoints[1].controlPointGR[0][7] = 1.097412109375;
+lensShading.ctrlPoints[1].controlPointGR[0][8] = 0.760375976563;
+lensShading.ctrlPoints[1].controlPointGR[0][9] = 1.566162109375;
+lensShading.ctrlPoints[1].controlPointGR[1][0] = 0.996337890625;
+lensShading.ctrlPoints[1].controlPointGR[1][1] = 1.042358398438;
+lensShading.ctrlPoints[1].controlPointGR[1][2] = 1.114990234375;
+lensShading.ctrlPoints[1].controlPointGR[1][3] = 1.078369140625;
+lensShading.ctrlPoints[1].controlPointGR[1][4] = 1.059204101563;
+lensShading.ctrlPoints[1].controlPointGR[1][5] = 1.084594726563;
+lensShading.ctrlPoints[1].controlPointGR[1][6] = 1.051025390625;
+lensShading.ctrlPoints[1].controlPointGR[1][7] = 1.207519531250;
+lensShading.ctrlPoints[1].controlPointGR[1][8] = 0.743041992188;
+lensShading.ctrlPoints[1].controlPointGR[1][9] = 1.232666015625;
+lensShading.ctrlPoints[1].controlPointGR[2][0] = 1.065307617188;
+lensShading.ctrlPoints[1].controlPointGR[2][1] = 1.065429687500;
+lensShading.ctrlPoints[1].controlPointGR[2][2] = 1.051269531250;
+lensShading.ctrlPoints[1].controlPointGR[2][3] = 1.074707031250;
+lensShading.ctrlPoints[1].controlPointGR[2][4] = 1.070678710938;
+lensShading.ctrlPoints[1].controlPointGR[2][5] = 1.080444335938;
+lensShading.ctrlPoints[1].controlPointGR[2][6] = 1.066772460938;
+lensShading.ctrlPoints[1].controlPointGR[2][7] = 1.076904296875;
+lensShading.ctrlPoints[1].controlPointGR[2][8] = 1.064819335938;
+lensShading.ctrlPoints[1].controlPointGR[2][9] = 1.026000976563;
+lensShading.ctrlPoints[1].controlPointGR[3][0] = 1.040161132813;
+lensShading.ctrlPoints[1].controlPointGR[3][1] = 1.072875976563;
+lensShading.ctrlPoints[1].controlPointGR[3][2] = 1.065917968750;
+lensShading.ctrlPoints[1].controlPointGR[3][3] = 1.080688476563;
+lensShading.ctrlPoints[1].controlPointGR[3][4] = 1.057250976563;
+lensShading.ctrlPoints[1].controlPointGR[3][5] = 1.068481445313;
+lensShading.ctrlPoints[1].controlPointGR[3][6] = 1.069458007813;
+lensShading.ctrlPoints[1].controlPointGR[3][7] = 1.082641601563;
+lensShading.ctrlPoints[1].controlPointGR[3][8] = 1.013916015625;
+lensShading.ctrlPoints[1].controlPointGR[3][9] = 1.064819335938;
+lensShading.ctrlPoints[1].controlPointGR[4][0] = 1.075927734375;
+lensShading.ctrlPoints[1].controlPointGR[4][1] = 1.062500000000;
+lensShading.ctrlPoints[1].controlPointGR[4][2] = 1.084594726563;
+lensShading.ctrlPoints[1].controlPointGR[4][3] = 1.075561523438;
+lensShading.ctrlPoints[1].controlPointGR[4][4] = 1.058227539063;
+lensShading.ctrlPoints[1].controlPointGR[4][5] = 1.040161132813;
+lensShading.ctrlPoints[1].controlPointGR[4][6] = 1.084228515625;
+lensShading.ctrlPoints[1].controlPointGR[4][7] = 1.129028320313;
+lensShading.ctrlPoints[1].controlPointGR[4][8] = 1.037841796875;
+lensShading.ctrlPoints[1].controlPointGR[4][9] = 1.075561523438;
+lensShading.ctrlPoints[1].controlPointGR[5][0] = 1.031005859375;
+lensShading.ctrlPoints[1].controlPointGR[5][1] = 1.084228515625;
+lensShading.ctrlPoints[1].controlPointGR[5][2] = 1.067993164063;
+lensShading.ctrlPoints[1].controlPointGR[5][3] = 1.088256835938;
+lensShading.ctrlPoints[1].controlPointGR[5][4] = 1.037109375000;
+lensShading.ctrlPoints[1].controlPointGR[5][5] = 1.110839843750;
+lensShading.ctrlPoints[1].controlPointGR[5][6] = 1.072387695313;
+lensShading.ctrlPoints[1].controlPointGR[5][7] = 1.089355468750;
+lensShading.ctrlPoints[1].controlPointGR[5][8] = 1.032592773438;
+lensShading.ctrlPoints[1].controlPointGR[5][9] = 1.048095703125;
+lensShading.ctrlPoints[1].controlPointGR[6][0] = 1.088867187500;
+lensShading.ctrlPoints[1].controlPointGR[6][1] = 1.049560546875;
+lensShading.ctrlPoints[1].controlPointGR[6][2] = 1.101440429688;
+lensShading.ctrlPoints[1].controlPointGR[6][3] = 1.084838867188;
+lensShading.ctrlPoints[1].controlPointGR[6][4] = 1.105346679688;
+lensShading.ctrlPoints[1].controlPointGR[6][5] = 1.082641601563;
+lensShading.ctrlPoints[1].controlPointGR[6][6] = 1.086303710938;
+lensShading.ctrlPoints[1].controlPointGR[6][7] = 1.095214843750;
+lensShading.ctrlPoints[1].controlPointGR[6][8] = 1.002319335938;
+lensShading.ctrlPoints[1].controlPointGR[6][9] = 1.155517578125;
+lensShading.ctrlPoints[1].controlPointGR[7][0] = 1.180175781250;
+lensShading.ctrlPoints[1].controlPointGR[7][1] = 1.003051757813;
+lensShading.ctrlPoints[1].controlPointGR[7][2] = 1.097290039063;
+lensShading.ctrlPoints[1].controlPointGR[7][3] = 1.098632812500;
+lensShading.ctrlPoints[1].controlPointGR[7][4] = 1.064941406250;
+lensShading.ctrlPoints[1].controlPointGR[7][5] = 1.135131835938;
+lensShading.ctrlPoints[1].controlPointGR[7][6] = 1.071289062500;
+lensShading.ctrlPoints[1].controlPointGR[7][7] = 1.106689453125;
+lensShading.ctrlPoints[1].controlPointGR[7][8] = 0.937988281250;
+lensShading.ctrlPoints[1].controlPointGR[7][9] = 1.290161132813;
+lensShading.ctrlPoints[1].controlPointGR[8][0] = 1.148437500000;
+lensShading.ctrlPoints[1].controlPointGR[8][1] = 0.993652343750;
+lensShading.ctrlPoints[1].controlPointGR[8][2] = 1.059570312500;
+lensShading.ctrlPoints[1].controlPointGR[8][3] = 1.084960937500;
+lensShading.ctrlPoints[1].controlPointGR[8][4] = 1.137329101563;
+lensShading.ctrlPoints[1].controlPointGR[8][5] = 1.063110351563;
+lensShading.ctrlPoints[1].controlPointGR[8][6] = 1.081542968750;
+lensShading.ctrlPoints[1].controlPointGR[8][7] = 1.033203125000;
+lensShading.ctrlPoints[1].controlPointGR[8][8] = 1.046020507813;
+lensShading.ctrlPoints[1].controlPointGR[8][9] = 1.294311523438;
+lensShading.ctrlPoints[1].controlPointGR[9][0] = 1.435180664063;
+lensShading.ctrlPoints[1].controlPointGR[9][1] = 1.056640625000;
+lensShading.ctrlPoints[1].controlPointGR[9][2] = 1.080322265625;
+lensShading.ctrlPoints[1].controlPointGR[9][3] = 1.072509765625;
+lensShading.ctrlPoints[1].controlPointGR[9][4] = 1.072143554688;
+lensShading.ctrlPoints[1].controlPointGR[9][5] = 1.103027343750;
+lensShading.ctrlPoints[1].controlPointGR[9][6] = 1.054809570313;
+lensShading.ctrlPoints[1].controlPointGR[9][7] = 1.144165039063;
+lensShading.ctrlPoints[1].controlPointGR[9][8] = 0.990600585938;
+lensShading.ctrlPoints[1].controlPointGR[9][9] = 1.867187500000;
+lensShading.ctrlPoints[1].controlPointGB[0][0] = 1.058959960938;
+lensShading.ctrlPoints[1].controlPointGB[0][1] = 1.006103515625;
+lensShading.ctrlPoints[1].controlPointGB[0][2] = 1.062500000000;
+lensShading.ctrlPoints[1].controlPointGB[0][3] = 1.068115234375;
+lensShading.ctrlPoints[1].controlPointGB[0][4] = 1.078125000000;
+lensShading.ctrlPoints[1].controlPointGB[0][5] = 1.072998046875;
+lensShading.ctrlPoints[1].controlPointGB[0][6] = 1.060913085938;
+lensShading.ctrlPoints[1].controlPointGB[0][7] = 1.130493164063;
+lensShading.ctrlPoints[1].controlPointGB[0][8] = 0.752685546875;
+lensShading.ctrlPoints[1].controlPointGB[0][9] = 1.575805664063;
+lensShading.ctrlPoints[1].controlPointGB[1][0] = 1.013305664063;
+lensShading.ctrlPoints[1].controlPointGB[1][1] = 1.072143554688;
+lensShading.ctrlPoints[1].controlPointGB[1][2] = 1.126464843750;
+lensShading.ctrlPoints[1].controlPointGB[1][3] = 1.084350585938;
+lensShading.ctrlPoints[1].controlPointGB[1][4] = 1.074462890625;
+lensShading.ctrlPoints[1].controlPointGB[1][5] = 1.066650390625;
+lensShading.ctrlPoints[1].controlPointGB[1][6] = 1.065429687500;
+lensShading.ctrlPoints[1].controlPointGB[1][7] = 1.163574218750;
+lensShading.ctrlPoints[1].controlPointGB[1][8] = 0.819091796875;
+lensShading.ctrlPoints[1].controlPointGB[1][9] = 1.206420898438;
+lensShading.ctrlPoints[1].controlPointGB[2][0] = 1.064575195313;
+lensShading.ctrlPoints[1].controlPointGB[2][1] = 1.018066406250;
+lensShading.ctrlPoints[1].controlPointGB[2][2] = 1.081787109375;
+lensShading.ctrlPoints[1].controlPointGB[2][3] = 1.066894531250;
+lensShading.ctrlPoints[1].controlPointGB[2][4] = 1.082031250000;
+lensShading.ctrlPoints[1].controlPointGB[2][5] = 1.097167968750;
+lensShading.ctrlPoints[1].controlPointGB[2][6] = 1.069946289063;
+lensShading.ctrlPoints[1].controlPointGB[2][7] = 1.097045898438;
+lensShading.ctrlPoints[1].controlPointGB[2][8] = 1.030029296875;
+lensShading.ctrlPoints[1].controlPointGB[2][9] = 1.057250976563;
+lensShading.ctrlPoints[1].controlPointGB[3][0] = 1.042358398438;
+lensShading.ctrlPoints[1].controlPointGB[3][1] = 1.088012695313;
+lensShading.ctrlPoints[1].controlPointGB[3][2] = 1.060546875000;
+lensShading.ctrlPoints[1].controlPointGB[3][3] = 1.079589843750;
+lensShading.ctrlPoints[1].controlPointGB[3][4] = 1.075683593750;
+lensShading.ctrlPoints[1].controlPointGB[3][5] = 1.061523437500;
+lensShading.ctrlPoints[1].controlPointGB[3][6] = 1.079833984375;
+lensShading.ctrlPoints[1].controlPointGB[3][7] = 1.086181640625;
+lensShading.ctrlPoints[1].controlPointGB[3][8] = 1.037353515625;
+lensShading.ctrlPoints[1].controlPointGB[3][9] = 1.059570312500;
+lensShading.ctrlPoints[1].controlPointGB[4][0] = 1.065307617188;
+lensShading.ctrlPoints[1].controlPointGB[4][1] = 1.032104492188;
+lensShading.ctrlPoints[1].controlPointGB[4][2] = 1.093261718750;
+lensShading.ctrlPoints[1].controlPointGB[4][3] = 1.076416015625;
+lensShading.ctrlPoints[1].controlPointGB[4][4] = 1.058837890625;
+lensShading.ctrlPoints[1].controlPointGB[4][5] = 1.047119140625;
+lensShading.ctrlPoints[1].controlPointGB[4][6] = 1.084350585938;
+lensShading.ctrlPoints[1].controlPointGB[4][7] = 1.123535156250;
+lensShading.ctrlPoints[1].controlPointGB[4][8] = 1.023071289063;
+lensShading.ctrlPoints[1].controlPointGB[4][9] = 1.088134765625;
+lensShading.ctrlPoints[1].controlPointGB[5][0] = 1.045410156250;
+lensShading.ctrlPoints[1].controlPointGB[5][1] = 1.088745117188;
+lensShading.ctrlPoints[1].controlPointGB[5][2] = 1.071166992188;
+lensShading.ctrlPoints[1].controlPointGB[5][3] = 1.085571289063;
+lensShading.ctrlPoints[1].controlPointGB[5][4] = 1.048706054688;
+lensShading.ctrlPoints[1].controlPointGB[5][5] = 1.100830078125;
+lensShading.ctrlPoints[1].controlPointGB[5][6] = 1.077026367188;
+lensShading.ctrlPoints[1].controlPointGB[5][7] = 1.108764648438;
+lensShading.ctrlPoints[1].controlPointGB[5][8] = 1.026245117188;
+lensShading.ctrlPoints[1].controlPointGB[5][9] = 1.050537109375;
+lensShading.ctrlPoints[1].controlPointGB[6][0] = 1.083007812500;
+lensShading.ctrlPoints[1].controlPointGB[6][1] = 1.046142578125;
+lensShading.ctrlPoints[1].controlPointGB[6][2] = 1.093139648438;
+lensShading.ctrlPoints[1].controlPointGB[6][3] = 1.083374023438;
+lensShading.ctrlPoints[1].controlPointGB[6][4] = 1.103271484375;
+lensShading.ctrlPoints[1].controlPointGB[6][5] = 1.090087890625;
+lensShading.ctrlPoints[1].controlPointGB[6][6] = 1.084350585938;
+lensShading.ctrlPoints[1].controlPointGB[6][7] = 1.096679687500;
+lensShading.ctrlPoints[1].controlPointGB[6][8] = 0.999877929688;
+lensShading.ctrlPoints[1].controlPointGB[6][9] = 1.158569335938;
+lensShading.ctrlPoints[1].controlPointGB[7][0] = 1.194213867188;
+lensShading.ctrlPoints[1].controlPointGB[7][1] = 0.993652343750;
+lensShading.ctrlPoints[1].controlPointGB[7][2] = 1.113159179688;
+lensShading.ctrlPoints[1].controlPointGB[7][3] = 1.089599609375;
+lensShading.ctrlPoints[1].controlPointGB[7][4] = 1.090087890625;
+lensShading.ctrlPoints[1].controlPointGB[7][5] = 1.096191406250;
+lensShading.ctrlPoints[1].controlPointGB[7][6] = 1.089965820313;
+lensShading.ctrlPoints[1].controlPointGB[7][7] = 1.106689453125;
+lensShading.ctrlPoints[1].controlPointGB[7][8] = 0.935791015625;
+lensShading.ctrlPoints[1].controlPointGB[7][9] = 1.284179687500;
+lensShading.ctrlPoints[1].controlPointGB[8][0] = 1.118652343750;
+lensShading.ctrlPoints[1].controlPointGB[8][1] = 0.987182617188;
+lensShading.ctrlPoints[1].controlPointGB[8][2] = 1.050781250000;
+lensShading.ctrlPoints[1].controlPointGB[8][3] = 1.075561523438;
+lensShading.ctrlPoints[1].controlPointGB[8][4] = 1.114379882813;
+lensShading.ctrlPoints[1].controlPointGB[8][5] = 1.108764648438;
+lensShading.ctrlPoints[1].controlPointGB[8][6] = 1.055419921875;
+lensShading.ctrlPoints[1].controlPointGB[8][7] = 1.031738281250;
+lensShading.ctrlPoints[1].controlPointGB[8][8] = 1.052490234375;
+lensShading.ctrlPoints[1].controlPointGB[8][9] = 1.282348632813;
+lensShading.ctrlPoints[1].controlPointGB[9][0] = 1.449096679688;
+lensShading.ctrlPoints[1].controlPointGB[9][1] = 1.060302734375;
+lensShading.ctrlPoints[1].controlPointGB[9][2] = 1.064453125000;
+lensShading.ctrlPoints[1].controlPointGB[9][3] = 1.069213867188;
+lensShading.ctrlPoints[1].controlPointGB[9][4] = 1.088012695313;
+lensShading.ctrlPoints[1].controlPointGB[9][5] = 1.073852539063;
+lensShading.ctrlPoints[1].controlPointGB[9][6] = 1.069213867188;
+lensShading.ctrlPoints[1].controlPointGB[9][7] = 1.146728515625;
+lensShading.ctrlPoints[1].controlPointGB[9][8] = 0.974243164063;
+lensShading.ctrlPoints[1].controlPointGB[9][9] = 1.875122070313;
+lensShading.ctrlPoints[1].controlPointB[0][0] = 1.054565429688;
+lensShading.ctrlPoints[1].controlPointB[0][1] = 1.022338867188;
+lensShading.ctrlPoints[1].controlPointB[0][2] = 1.072387695313;
+lensShading.ctrlPoints[1].controlPointB[0][3] = 1.086425781250;
+lensShading.ctrlPoints[1].controlPointB[0][4] = 1.060546875000;
+lensShading.ctrlPoints[1].controlPointB[0][5] = 1.094848632813;
+lensShading.ctrlPoints[1].controlPointB[0][6] = 1.073486328125;
+lensShading.ctrlPoints[1].controlPointB[0][7] = 1.129028320313;
+lensShading.ctrlPoints[1].controlPointB[0][8] = 0.770996093750;
+lensShading.ctrlPoints[1].controlPointB[0][9] = 1.599243164063;
+lensShading.ctrlPoints[1].controlPointB[1][0] = 1.010498046875;
+lensShading.ctrlPoints[1].controlPointB[1][1] = 1.098999023438;
+lensShading.ctrlPoints[1].controlPointB[1][2] = 1.100830078125;
+lensShading.ctrlPoints[1].controlPointB[1][3] = 1.086425781250;
+lensShading.ctrlPoints[1].controlPointB[1][4] = 1.076171875000;
+lensShading.ctrlPoints[1].controlPointB[1][5] = 1.110717773438;
+lensShading.ctrlPoints[1].controlPointB[1][6] = 1.059204101563;
+lensShading.ctrlPoints[1].controlPointB[1][7] = 1.228149414063;
+lensShading.ctrlPoints[1].controlPointB[1][8] = 0.770141601563;
+lensShading.ctrlPoints[1].controlPointB[1][9] = 1.236816406250;
+lensShading.ctrlPoints[1].controlPointB[2][0] = 1.067260742188;
+lensShading.ctrlPoints[1].controlPointB[2][1] = 1.054321289063;
+lensShading.ctrlPoints[1].controlPointB[2][2] = 1.088867187500;
+lensShading.ctrlPoints[1].controlPointB[2][3] = 1.076904296875;
+lensShading.ctrlPoints[1].controlPointB[2][4] = 1.115844726563;
+lensShading.ctrlPoints[1].controlPointB[2][5] = 1.054931640625;
+lensShading.ctrlPoints[1].controlPointB[2][6] = 1.097167968750;
+lensShading.ctrlPoints[1].controlPointB[2][7] = 1.081054687500;
+lensShading.ctrlPoints[1].controlPointB[2][8] = 1.086547851563;
+lensShading.ctrlPoints[1].controlPointB[2][9] = 1.056274414063;
+lensShading.ctrlPoints[1].controlPointB[3][0] = 1.047119140625;
+lensShading.ctrlPoints[1].controlPointB[3][1] = 1.085083007813;
+lensShading.ctrlPoints[1].controlPointB[3][2] = 1.082519531250;
+lensShading.ctrlPoints[1].controlPointB[3][3] = 1.091430664063;
+lensShading.ctrlPoints[1].controlPointB[3][4] = 1.065185546875;
+lensShading.ctrlPoints[1].controlPointB[3][5] = 1.085693359375;
+lensShading.ctrlPoints[1].controlPointB[3][6] = 1.086425781250;
+lensShading.ctrlPoints[1].controlPointB[3][7] = 1.124511718750;
+lensShading.ctrlPoints[1].controlPointB[3][8] = 1.034057617188;
+lensShading.ctrlPoints[1].controlPointB[3][9] = 1.084716796875;
+lensShading.ctrlPoints[1].controlPointB[4][0] = 1.101562500000;
+lensShading.ctrlPoints[1].controlPointB[4][1] = 1.040283203125;
+lensShading.ctrlPoints[1].controlPointB[4][2] = 1.105224609375;
+lensShading.ctrlPoints[1].controlPointB[4][3] = 1.083862304688;
+lensShading.ctrlPoints[1].controlPointB[4][4] = 1.076904296875;
+lensShading.ctrlPoints[1].controlPointB[4][5] = 1.046752929688;
+lensShading.ctrlPoints[1].controlPointB[4][6] = 1.095214843750;
+lensShading.ctrlPoints[1].controlPointB[4][7] = 1.146118164063;
+lensShading.ctrlPoints[1].controlPointB[4][8] = 1.032592773438;
+lensShading.ctrlPoints[1].controlPointB[4][9] = 1.117553710938;
+lensShading.ctrlPoints[1].controlPointB[5][0] = 1.028930664063;
+lensShading.ctrlPoints[1].controlPointB[5][1] = 1.111450195313;
+lensShading.ctrlPoints[1].controlPointB[5][2] = 1.084960937500;
+lensShading.ctrlPoints[1].controlPointB[5][3] = 1.096069335938;
+lensShading.ctrlPoints[1].controlPointB[5][4] = 1.064819335938;
+lensShading.ctrlPoints[1].controlPointB[5][5] = 1.103881835938;
+lensShading.ctrlPoints[1].controlPointB[5][6] = 1.091796875000;
+lensShading.ctrlPoints[1].controlPointB[5][7] = 1.100708007813;
+lensShading.ctrlPoints[1].controlPointB[5][8] = 1.072753906250;
+lensShading.ctrlPoints[1].controlPointB[5][9] = 1.058105468750;
+lensShading.ctrlPoints[1].controlPointB[6][0] = 1.100830078125;
+lensShading.ctrlPoints[1].controlPointB[6][1] = 1.057128906250;
+lensShading.ctrlPoints[1].controlPointB[6][2] = 1.108032226563;
+lensShading.ctrlPoints[1].controlPointB[6][3] = 1.095703125000;
+lensShading.ctrlPoints[1].controlPointB[6][4] = 1.102416992188;
+lensShading.ctrlPoints[1].controlPointB[6][5] = 1.102416992188;
+lensShading.ctrlPoints[1].controlPointB[6][6] = 1.101318359375;
+lensShading.ctrlPoints[1].controlPointB[6][7] = 1.127319335938;
+lensShading.ctrlPoints[1].controlPointB[6][8] = 1.002929687500;
+lensShading.ctrlPoints[1].controlPointB[6][9] = 1.188842773438;
+lensShading.ctrlPoints[1].controlPointB[7][0] = 1.173461914063;
+lensShading.ctrlPoints[1].controlPointB[7][1] = 1.100463867188;
+lensShading.ctrlPoints[1].controlPointB[7][2] = 1.071166992188;
+lensShading.ctrlPoints[1].controlPointB[7][3] = 1.115844726563;
+lensShading.ctrlPoints[1].controlPointB[7][4] = 1.096313476563;
+lensShading.ctrlPoints[1].controlPointB[7][5] = 1.147949218750;
+lensShading.ctrlPoints[1].controlPointB[7][6] = 1.094238281250;
+lensShading.ctrlPoints[1].controlPointB[7][7] = 1.166992187500;
+lensShading.ctrlPoints[1].controlPointB[7][8] = 0.900268554688;
+lensShading.ctrlPoints[1].controlPointB[7][9] = 1.378295898438;
+lensShading.ctrlPoints[1].controlPointB[8][0] = 1.127197265625;
+lensShading.ctrlPoints[1].controlPointB[8][1] = 0.955200195313;
+lensShading.ctrlPoints[1].controlPointB[8][2] = 1.065795898438;
+lensShading.ctrlPoints[1].controlPointB[8][3] = 1.076049804688;
+lensShading.ctrlPoints[1].controlPointB[8][4] = 1.104248046875;
+lensShading.ctrlPoints[1].controlPointB[8][5] = 1.072021484375;
+lensShading.ctrlPoints[1].controlPointB[8][6] = 1.081420898438;
+lensShading.ctrlPoints[1].controlPointB[8][7] = 1.028564453125;
+lensShading.ctrlPoints[1].controlPointB[8][8] = 1.075195312500;
+lensShading.ctrlPoints[1].controlPointB[8][9] = 1.251831054688;
+lensShading.ctrlPoints[1].controlPointB[9][0] = 1.462280273438;
+lensShading.ctrlPoints[1].controlPointB[9][1] = 1.096191406250;
+lensShading.ctrlPoints[1].controlPointB[9][2] = 1.081420898438;
+lensShading.ctrlPoints[1].controlPointB[9][3] = 1.086791992188;
+lensShading.ctrlPoints[1].controlPointB[9][4] = 1.099975585938;
+lensShading.ctrlPoints[1].controlPointB[9][5] = 1.113769531250;
+lensShading.ctrlPoints[1].controlPointB[9][6] = 1.082397460938;
+lensShading.ctrlPoints[1].controlPointB[9][7] = 1.170532226563;
+lensShading.ctrlPoints[1].controlPointB[9][8] = 0.970336914063;
+lensShading.ctrlPoints[1].controlPointB[9][9] = 1.959106445313;
+lensShading.ctrlPoints[2].cct = 6500;
+lensShading.ctrlPoints[2].light_family = 1;
+lensShading.ctrlPoints[2].controlPointR[0][0] = 1.082397460938;
+lensShading.ctrlPoints[2].controlPointR[0][1] = 0.986572265625;
+lensShading.ctrlPoints[2].controlPointR[0][2] = 1.100585937500;
+lensShading.ctrlPoints[2].controlPointR[0][3] = 1.100708007813;
+lensShading.ctrlPoints[2].controlPointR[0][4] = 1.096191406250;
+lensShading.ctrlPoints[2].controlPointR[0][5] = 1.100341796875;
+lensShading.ctrlPoints[2].controlPointR[0][6] = 1.104492187500;
+lensShading.ctrlPoints[2].controlPointR[0][7] = 1.140136718750;
+lensShading.ctrlPoints[2].controlPointR[0][8] = 0.767333984375;
+lensShading.ctrlPoints[2].controlPointR[0][9] = 1.623535156250;
+lensShading.ctrlPoints[2].controlPointR[1][0] = 0.982910156250;
+lensShading.ctrlPoints[2].controlPointR[1][1] = 1.103637695313;
+lensShading.ctrlPoints[2].controlPointR[1][2] = 1.125610351563;
+lensShading.ctrlPoints[2].controlPointR[1][3] = 1.096313476563;
+lensShading.ctrlPoints[2].controlPointR[1][4] = 1.091796875000;
+lensShading.ctrlPoints[2].controlPointR[1][5] = 1.183593750000;
+lensShading.ctrlPoints[2].controlPointR[1][6] = 1.061645507813;
+lensShading.ctrlPoints[2].controlPointR[1][7] = 1.266723632813;
+lensShading.ctrlPoints[2].controlPointR[1][8] = 0.764282226563;
+lensShading.ctrlPoints[2].controlPointR[1][9] = 1.251586914063;
+lensShading.ctrlPoints[2].controlPointR[2][0] = 1.108032226563;
+lensShading.ctrlPoints[2].controlPointR[2][1] = 1.014282226563;
+lensShading.ctrlPoints[2].controlPointR[2][2] = 1.171508789063;
+lensShading.ctrlPoints[2].controlPointR[2][3] = 1.122192382813;
+lensShading.ctrlPoints[2].controlPointR[2][4] = 1.146850585938;
+lensShading.ctrlPoints[2].controlPointR[2][5] = 1.096069335938;
+lensShading.ctrlPoints[2].controlPointR[2][6] = 1.154785156250;
+lensShading.ctrlPoints[2].controlPointR[2][7] = 1.110229492188;
+lensShading.ctrlPoints[2].controlPointR[2][8] = 1.091308593750;
+lensShading.ctrlPoints[2].controlPointR[2][9] = 1.099487304688;
+lensShading.ctrlPoints[2].controlPointR[3][0] = 1.066894531250;
+lensShading.ctrlPoints[2].controlPointR[3][1] = 1.099243164063;
+lensShading.ctrlPoints[2].controlPointR[3][2] = 1.107910156250;
+lensShading.ctrlPoints[2].controlPointR[3][3] = 1.119506835938;
+lensShading.ctrlPoints[2].controlPointR[3][4] = 1.123657226563;
+lensShading.ctrlPoints[2].controlPointR[3][5] = 1.114990234375;
+lensShading.ctrlPoints[2].controlPointR[3][6] = 1.124267578125;
+lensShading.ctrlPoints[2].controlPointR[3][7] = 1.120849609375;
+lensShading.ctrlPoints[2].controlPointR[3][8] = 1.092651367188;
+lensShading.ctrlPoints[2].controlPointR[3][9] = 1.100463867188;
+lensShading.ctrlPoints[2].controlPointR[4][0] = 1.096679687500;
+lensShading.ctrlPoints[2].controlPointR[4][1] = 1.090454101563;
+lensShading.ctrlPoints[2].controlPointR[4][2] = 1.130981445313;
+lensShading.ctrlPoints[2].controlPointR[4][3] = 1.119750976563;
+lensShading.ctrlPoints[2].controlPointR[4][4] = 1.127807617188;
+lensShading.ctrlPoints[2].controlPointR[4][5] = 1.086181640625;
+lensShading.ctrlPoints[2].controlPointR[4][6] = 1.149902343750;
+lensShading.ctrlPoints[2].controlPointR[4][7] = 1.204956054688;
+lensShading.ctrlPoints[2].controlPointR[4][8] = 1.016113281250;
+lensShading.ctrlPoints[2].controlPointR[4][9] = 1.176025390625;
+lensShading.ctrlPoints[2].controlPointR[5][0] = 1.069335937500;
+lensShading.ctrlPoints[2].controlPointR[5][1] = 1.100952148438;
+lensShading.ctrlPoints[2].controlPointR[5][2] = 1.120117187500;
+lensShading.ctrlPoints[2].controlPointR[5][3] = 1.127807617188;
+lensShading.ctrlPoints[2].controlPointR[5][4] = 1.097656250000;
+lensShading.ctrlPoints[2].controlPointR[5][5] = 1.150268554688;
+lensShading.ctrlPoints[2].controlPointR[5][6] = 1.127929687500;
+lensShading.ctrlPoints[2].controlPointR[5][7] = 1.130249023438;
+lensShading.ctrlPoints[2].controlPointR[5][8] = 1.116088867188;
+lensShading.ctrlPoints[2].controlPointR[5][9] = 1.090454101563;
+lensShading.ctrlPoints[2].controlPointR[6][0] = 1.109130859375;
+lensShading.ctrlPoints[2].controlPointR[6][1] = 1.076049804688;
+lensShading.ctrlPoints[2].controlPointR[6][2] = 1.141113281250;
+lensShading.ctrlPoints[2].controlPointR[6][3] = 1.131347656250;
+lensShading.ctrlPoints[2].controlPointR[6][4] = 1.144287109375;
+lensShading.ctrlPoints[2].controlPointR[6][5] = 1.161743164063;
+lensShading.ctrlPoints[2].controlPointR[6][6] = 1.140014648438;
+lensShading.ctrlPoints[2].controlPointR[6][7] = 1.165771484375;
+lensShading.ctrlPoints[2].controlPointR[6][8] = 1.043457031250;
+lensShading.ctrlPoints[2].controlPointR[6][9] = 1.234252929688;
+lensShading.ctrlPoints[2].controlPointR[7][0] = 1.234008789063;
+lensShading.ctrlPoints[2].controlPointR[7][1] = 1.033447265625;
+lensShading.ctrlPoints[2].controlPointR[7][2] = 1.155761718750;
+lensShading.ctrlPoints[2].controlPointR[7][3] = 1.151245117188;
+lensShading.ctrlPoints[2].controlPointR[7][4] = 1.151123046875;
+lensShading.ctrlPoints[2].controlPointR[7][5] = 1.177246093750;
+lensShading.ctrlPoints[2].controlPointR[7][6] = 1.147583007813;
+lensShading.ctrlPoints[2].controlPointR[7][7] = 1.153808593750;
+lensShading.ctrlPoints[2].controlPointR[7][8] = 0.965332031250;
+lensShading.ctrlPoints[2].controlPointR[7][9] = 1.398925781250;
+lensShading.ctrlPoints[2].controlPointR[8][0] = 1.171752929688;
+lensShading.ctrlPoints[2].controlPointR[8][1] = 0.985839843750;
+lensShading.ctrlPoints[2].controlPointR[8][2] = 1.111938476563;
+lensShading.ctrlPoints[2].controlPointR[8][3] = 1.116210937500;
+lensShading.ctrlPoints[2].controlPointR[8][4] = 1.182128906250;
+lensShading.ctrlPoints[2].controlPointR[8][5] = 1.095703125000;
+lensShading.ctrlPoints[2].controlPointR[8][6] = 1.146606445313;
+lensShading.ctrlPoints[2].controlPointR[8][7] = 1.030761718750;
+lensShading.ctrlPoints[2].controlPointR[8][8] = 1.168945312500;
+lensShading.ctrlPoints[2].controlPointR[8][9] = 1.306396484375;
+lensShading.ctrlPoints[2].controlPointR[9][0] = 1.507080078125;
+lensShading.ctrlPoints[2].controlPointR[9][1] = 1.070190429688;
+lensShading.ctrlPoints[2].controlPointR[9][2] = 1.147705078125;
+lensShading.ctrlPoints[2].controlPointR[9][3] = 1.104614257813;
+lensShading.ctrlPoints[2].controlPointR[9][4] = 1.175659179688;
+lensShading.ctrlPoints[2].controlPointR[9][5] = 1.151245117188;
+lensShading.ctrlPoints[2].controlPointR[9][6] = 1.116577148438;
+lensShading.ctrlPoints[2].controlPointR[9][7] = 1.212402343750;
+lensShading.ctrlPoints[2].controlPointR[9][8] = 1.031616210938;
+lensShading.ctrlPoints[2].controlPointR[9][9] = 1.976562500000;
+lensShading.ctrlPoints[2].controlPointGR[0][0] = 1.052246093750;
+lensShading.ctrlPoints[2].controlPointGR[0][1] = 0.989624023438;
+lensShading.ctrlPoints[2].controlPointGR[0][2] = 1.065917968750;
+lensShading.ctrlPoints[2].controlPointGR[0][3] = 1.062866210938;
+lensShading.ctrlPoints[2].controlPointGR[0][4] = 1.069213867188;
+lensShading.ctrlPoints[2].controlPointGR[0][5] = 1.055175781250;
+lensShading.ctrlPoints[2].controlPointGR[0][6] = 1.047119140625;
+lensShading.ctrlPoints[2].controlPointGR[0][7] = 1.119018554688;
+lensShading.ctrlPoints[2].controlPointGR[0][8] = 0.749389648438;
+lensShading.ctrlPoints[2].controlPointGR[0][9] = 1.583740234375;
+lensShading.ctrlPoints[2].controlPointGR[1][0] = 0.991088867188;
+lensShading.ctrlPoints[2].controlPointGR[1][1] = 1.075317382813;
+lensShading.ctrlPoints[2].controlPointGR[1][2] = 1.092407226563;
+lensShading.ctrlPoints[2].controlPointGR[1][3] = 1.066162109375;
+lensShading.ctrlPoints[2].controlPointGR[1][4] = 1.073974609375;
+lensShading.ctrlPoints[2].controlPointGR[1][5] = 1.107666015625;
+lensShading.ctrlPoints[2].controlPointGR[1][6] = 1.054321289063;
+lensShading.ctrlPoints[2].controlPointGR[1][7] = 1.211547851563;
+lensShading.ctrlPoints[2].controlPointGR[1][8] = 0.758422851563;
+lensShading.ctrlPoints[2].controlPointGR[1][9] = 1.214477539063;
+lensShading.ctrlPoints[2].controlPointGR[2][0] = 1.067626953125;
+lensShading.ctrlPoints[2].controlPointGR[2][1] = 1.038696289063;
+lensShading.ctrlPoints[2].controlPointGR[2][2] = 1.075683593750;
+lensShading.ctrlPoints[2].controlPointGR[2][3] = 1.082519531250;
+lensShading.ctrlPoints[2].controlPointGR[2][4] = 1.061889648438;
+lensShading.ctrlPoints[2].controlPointGR[2][5] = 1.078247070313;
+lensShading.ctrlPoints[2].controlPointGR[2][6] = 1.071777343750;
+lensShading.ctrlPoints[2].controlPointGR[2][7] = 1.100463867188;
+lensShading.ctrlPoints[2].controlPointGR[2][8] = 1.030883789063;
+lensShading.ctrlPoints[2].controlPointGR[2][9] = 1.070190429688;
+lensShading.ctrlPoints[2].controlPointGR[3][0] = 1.039184570313;
+lensShading.ctrlPoints[2].controlPointGR[3][1] = 1.067260742188;
+lensShading.ctrlPoints[2].controlPointGR[3][2] = 1.070190429688;
+lensShading.ctrlPoints[2].controlPointGR[3][3] = 1.076049804688;
+lensShading.ctrlPoints[2].controlPointGR[3][4] = 1.068115234375;
+lensShading.ctrlPoints[2].controlPointGR[3][5] = 1.064941406250;
+lensShading.ctrlPoints[2].controlPointGR[3][6] = 1.078247070313;
+lensShading.ctrlPoints[2].controlPointGR[3][7] = 1.082641601563;
+lensShading.ctrlPoints[2].controlPointGR[3][8] = 1.037719726563;
+lensShading.ctrlPoints[2].controlPointGR[3][9] = 1.064941406250;
+lensShading.ctrlPoints[2].controlPointGR[4][0] = 1.063842773438;
+lensShading.ctrlPoints[2].controlPointGR[4][1] = 1.074340820313;
+lensShading.ctrlPoints[2].controlPointGR[4][2] = 1.084960937500;
+lensShading.ctrlPoints[2].controlPointGR[4][3] = 1.075195312500;
+lensShading.ctrlPoints[2].controlPointGR[4][4] = 1.049194335938;
+lensShading.ctrlPoints[2].controlPointGR[4][5] = 1.057373046875;
+lensShading.ctrlPoints[2].controlPointGR[4][6] = 1.074951171875;
+lensShading.ctrlPoints[2].controlPointGR[4][7] = 1.162475585938;
+lensShading.ctrlPoints[2].controlPointGR[4][8] = 0.997802734375;
+lensShading.ctrlPoints[2].controlPointGR[4][9] = 1.105102539063;
+lensShading.ctrlPoints[2].controlPointGR[5][0] = 1.037841796875;
+lensShading.ctrlPoints[2].controlPointGR[5][1] = 1.067138671875;
+lensShading.ctrlPoints[2].controlPointGR[5][2] = 1.070190429688;
+lensShading.ctrlPoints[2].controlPointGR[5][3] = 1.090820312500;
+lensShading.ctrlPoints[2].controlPointGR[5][4] = 1.054687500000;
+lensShading.ctrlPoints[2].controlPointGR[5][5] = 1.092773437500;
+lensShading.ctrlPoints[2].controlPointGR[5][6] = 1.088623046875;
+lensShading.ctrlPoints[2].controlPointGR[5][7] = 1.078613281250;
+lensShading.ctrlPoints[2].controlPointGR[5][8] = 1.067016601563;
+lensShading.ctrlPoints[2].controlPointGR[5][9] = 1.041992187500;
+lensShading.ctrlPoints[2].controlPointGR[6][0] = 1.086181640625;
+lensShading.ctrlPoints[2].controlPointGR[6][1] = 1.045654296875;
+lensShading.ctrlPoints[2].controlPointGR[6][2] = 1.097900390625;
+lensShading.ctrlPoints[2].controlPointGR[6][3] = 1.087158203125;
+lensShading.ctrlPoints[2].controlPointGR[6][4] = 1.093383789063;
+lensShading.ctrlPoints[2].controlPointGR[6][5] = 1.099243164063;
+lensShading.ctrlPoints[2].controlPointGR[6][6] = 1.084838867188;
+lensShading.ctrlPoints[2].controlPointGR[6][7] = 1.109863281250;
+lensShading.ctrlPoints[2].controlPointGR[6][8] = 0.992065429688;
+lensShading.ctrlPoints[2].controlPointGR[6][9] = 1.174804687500;
+lensShading.ctrlPoints[2].controlPointGR[7][0] = 1.161254882813;
+lensShading.ctrlPoints[2].controlPointGR[7][1] = 1.059204101563;
+lensShading.ctrlPoints[2].controlPointGR[7][2] = 1.054931640625;
+lensShading.ctrlPoints[2].controlPointGR[7][3] = 1.099487304688;
+lensShading.ctrlPoints[2].controlPointGR[7][4] = 1.091918945313;
+lensShading.ctrlPoints[2].controlPointGR[7][5] = 1.106567382813;
+lensShading.ctrlPoints[2].controlPointGR[7][6] = 1.087890625000;
+lensShading.ctrlPoints[2].controlPointGR[7][7] = 1.078857421875;
+lensShading.ctrlPoints[2].controlPointGR[7][8] = 0.982543945313;
+lensShading.ctrlPoints[2].controlPointGR[7][9] = 1.277221679688;
+lensShading.ctrlPoints[2].controlPointGR[8][0] = 1.147827148438;
+lensShading.ctrlPoints[2].controlPointGR[8][1] = 0.933715820313;
+lensShading.ctrlPoints[2].controlPointGR[8][2] = 1.106079101563;
+lensShading.ctrlPoints[2].controlPointGR[8][3] = 1.079223632813;
+lensShading.ctrlPoints[2].controlPointGR[8][4] = 1.116577148438;
+lensShading.ctrlPoints[2].controlPointGR[8][5] = 1.088745117188;
+lensShading.ctrlPoints[2].controlPointGR[8][6] = 1.073852539063;
+lensShading.ctrlPoints[2].controlPointGR[8][7] = 1.063964843750;
+lensShading.ctrlPoints[2].controlPointGR[8][8] = 1.001953125000;
+lensShading.ctrlPoints[2].controlPointGR[8][9] = 1.307861328125;
+lensShading.ctrlPoints[2].controlPointGR[9][0] = 1.432128906250;
+lensShading.ctrlPoints[2].controlPointGR[9][1] = 1.074584960938;
+lensShading.ctrlPoints[2].controlPointGR[9][2] = 1.054809570313;
+lensShading.ctrlPoints[2].controlPointGR[9][3] = 1.069580078125;
+lensShading.ctrlPoints[2].controlPointGR[9][4] = 1.086669921875;
+lensShading.ctrlPoints[2].controlPointGR[9][5] = 1.098266601563;
+lensShading.ctrlPoints[2].controlPointGR[9][6] = 1.064208984375;
+lensShading.ctrlPoints[2].controlPointGR[9][7] = 1.130493164063;
+lensShading.ctrlPoints[2].controlPointGR[9][8] = 1.020874023438;
+lensShading.ctrlPoints[2].controlPointGR[9][9] = 1.854125976563;
+lensShading.ctrlPoints[2].controlPointGB[0][0] = 1.051025390625;
+lensShading.ctrlPoints[2].controlPointGB[0][1] = 1.005004882813;
+lensShading.ctrlPoints[2].controlPointGB[0][2] = 1.065551757813;
+lensShading.ctrlPoints[2].controlPointGB[0][3] = 1.067382812500;
+lensShading.ctrlPoints[2].controlPointGB[0][4] = 1.087280273438;
+lensShading.ctrlPoints[2].controlPointGB[0][5] = 1.060424804688;
+lensShading.ctrlPoints[2].controlPointGB[0][6] = 1.056884765625;
+lensShading.ctrlPoints[2].controlPointGB[0][7] = 1.148681640625;
+lensShading.ctrlPoints[2].controlPointGB[0][8] = 0.717529296875;
+lensShading.ctrlPoints[2].controlPointGB[0][9] = 1.603759765625;
+lensShading.ctrlPoints[2].controlPointGB[1][0] = 0.990478515625;
+lensShading.ctrlPoints[2].controlPointGB[1][1] = 1.097167968750;
+lensShading.ctrlPoints[2].controlPointGB[1][2] = 1.074096679688;
+lensShading.ctrlPoints[2].controlPointGB[1][3] = 1.083862304688;
+lensShading.ctrlPoints[2].controlPointGB[1][4] = 1.040039062500;
+lensShading.ctrlPoints[2].controlPointGB[1][5] = 1.114501953125;
+lensShading.ctrlPoints[2].controlPointGB[1][6] = 1.069580078125;
+lensShading.ctrlPoints[2].controlPointGB[1][7] = 1.151367187500;
+lensShading.ctrlPoints[2].controlPointGB[1][8] = 0.858032226563;
+lensShading.ctrlPoints[2].controlPointGB[1][9] = 1.179931640625;
+lensShading.ctrlPoints[2].controlPointGB[2][0] = 1.070068359375;
+lensShading.ctrlPoints[2].controlPointGB[2][1] = 0.998046875000;
+lensShading.ctrlPoints[2].controlPointGB[2][2] = 1.112426757813;
+lensShading.ctrlPoints[2].controlPointGB[2][3] = 1.072631835938;
+lensShading.ctrlPoints[2].controlPointGB[2][4] = 1.104736328125;
+lensShading.ctrlPoints[2].controlPointGB[2][5] = 1.074462890625;
+lensShading.ctrlPoints[2].controlPointGB[2][6] = 1.067749023438;
+lensShading.ctrlPoints[2].controlPointGB[2][7] = 1.120361328125;
+lensShading.ctrlPoints[2].controlPointGB[2][8] = 1.015991210938;
+lensShading.ctrlPoints[2].controlPointGB[2][9] = 1.084960937500;
+lensShading.ctrlPoints[2].controlPointGB[3][0] = 1.026855468750;
+lensShading.ctrlPoints[2].controlPointGB[3][1] = 1.093994140625;
+lensShading.ctrlPoints[2].controlPointGB[3][2] = 1.059814453125;
+lensShading.ctrlPoints[2].controlPointGB[3][3] = 1.080932617188;
+lensShading.ctrlPoints[2].controlPointGB[3][4] = 1.064941406250;
+lensShading.ctrlPoints[2].controlPointGB[3][5] = 1.072998046875;
+lensShading.ctrlPoints[2].controlPointGB[3][6] = 1.080810546875;
+lensShading.ctrlPoints[2].controlPointGB[3][7] = 1.083618164063;
+lensShading.ctrlPoints[2].controlPointGB[3][8] = 1.053588867188;
+lensShading.ctrlPoints[2].controlPointGB[3][9] = 1.062133789063;
+lensShading.ctrlPoints[2].controlPointGB[4][0] = 1.076782226563;
+lensShading.ctrlPoints[2].controlPointGB[4][1] = 1.031738281250;
+lensShading.ctrlPoints[2].controlPointGB[4][2] = 1.087158203125;
+lensShading.ctrlPoints[2].controlPointGB[4][3] = 1.073608398438;
+lensShading.ctrlPoints[2].controlPointGB[4][4] = 1.062377929688;
+lensShading.ctrlPoints[2].controlPointGB[4][5] = 1.045654296875;
+lensShading.ctrlPoints[2].controlPointGB[4][6] = 1.080932617188;
+lensShading.ctrlPoints[2].controlPointGB[4][7] = 1.153320312500;
+lensShading.ctrlPoints[2].controlPointGB[4][8] = 0.983642578125;
+lensShading.ctrlPoints[2].controlPointGB[4][9] = 1.118408203125;
+lensShading.ctrlPoints[2].controlPointGB[5][0] = 1.018066406250;
+lensShading.ctrlPoints[2].controlPointGB[5][1] = 1.095703125000;
+lensShading.ctrlPoints[2].controlPointGB[5][2] = 1.065429687500;
+lensShading.ctrlPoints[2].controlPointGB[5][3] = 1.087402343750;
+lensShading.ctrlPoints[2].controlPointGB[5][4] = 1.044067382813;
+lensShading.ctrlPoints[2].controlPointGB[5][5] = 1.110595703125;
+lensShading.ctrlPoints[2].controlPointGB[5][6] = 1.083862304688;
+lensShading.ctrlPoints[2].controlPointGB[5][7] = 1.099487304688;
+lensShading.ctrlPoints[2].controlPointGB[5][8] = 1.058837890625;
+lensShading.ctrlPoints[2].controlPointGB[5][9] = 1.044067382813;
+lensShading.ctrlPoints[2].controlPointGB[6][0] = 1.081665039063;
+lensShading.ctrlPoints[2].controlPointGB[6][1] = 1.040649414063;
+lensShading.ctrlPoints[2].controlPointGB[6][2] = 1.098754882813;
+lensShading.ctrlPoints[2].controlPointGB[6][3] = 1.081787109375;
+lensShading.ctrlPoints[2].controlPointGB[6][4] = 1.099243164063;
+lensShading.ctrlPoints[2].controlPointGB[6][5] = 1.097412109375;
+lensShading.ctrlPoints[2].controlPointGB[6][6] = 1.086425781250;
+lensShading.ctrlPoints[2].controlPointGB[6][7] = 1.107788085938;
+lensShading.ctrlPoints[2].controlPointGB[6][8] = 0.997436523438;
+lensShading.ctrlPoints[2].controlPointGB[6][9] = 1.164062500000;
+lensShading.ctrlPoints[2].controlPointGB[7][0] = 1.151733398438;
+lensShading.ctrlPoints[2].controlPointGB[7][1] = 1.036254882813;
+lensShading.ctrlPoints[2].controlPointGB[7][2] = 1.056152343750;
+lensShading.ctrlPoints[2].controlPointGB[7][3] = 1.100463867188;
+lensShading.ctrlPoints[2].controlPointGB[7][4] = 1.096679687500;
+lensShading.ctrlPoints[2].controlPointGB[7][5] = 1.093383789063;
+lensShading.ctrlPoints[2].controlPointGB[7][6] = 1.085205078125;
+lensShading.ctrlPoints[2].controlPointGB[7][7] = 1.075683593750;
+lensShading.ctrlPoints[2].controlPointGB[7][8] = 0.987182617188;
+lensShading.ctrlPoints[2].controlPointGB[7][9] = 1.268310546875;
+lensShading.ctrlPoints[2].controlPointGB[8][0] = 1.137695312500;
+lensShading.ctrlPoints[2].controlPointGB[8][1] = 0.948608398438;
+lensShading.ctrlPoints[2].controlPointGB[8][2] = 1.092163085938;
+lensShading.ctrlPoints[2].controlPointGB[8][3] = 1.074584960938;
+lensShading.ctrlPoints[2].controlPointGB[8][4] = 1.100097656250;
+lensShading.ctrlPoints[2].controlPointGB[8][5] = 1.108154296875;
+lensShading.ctrlPoints[2].controlPointGB[8][6] = 1.072998046875;
+lensShading.ctrlPoints[2].controlPointGB[8][7] = 1.067626953125;
+lensShading.ctrlPoints[2].controlPointGB[8][8] = 0.983398437500;
+lensShading.ctrlPoints[2].controlPointGB[8][9] = 1.322387695313;
+lensShading.ctrlPoints[2].controlPointGB[9][0] = 1.421752929688;
+lensShading.ctrlPoints[2].controlPointGB[9][1] = 1.078857421875;
+lensShading.ctrlPoints[2].controlPointGB[9][2] = 1.049560546875;
+lensShading.ctrlPoints[2].controlPointGB[9][3] = 1.065185546875;
+lensShading.ctrlPoints[2].controlPointGB[9][4] = 1.099609375000;
+lensShading.ctrlPoints[2].controlPointGB[9][5] = 1.079833984375;
+lensShading.ctrlPoints[2].controlPointGB[9][6] = 1.067749023438;
+lensShading.ctrlPoints[2].controlPointGB[9][7] = 1.132812500000;
+lensShading.ctrlPoints[2].controlPointGB[9][8] = 1.021606445313;
+lensShading.ctrlPoints[2].controlPointGB[9][9] = 1.829223632813;
+lensShading.ctrlPoints[2].controlPointB[0][0] = 1.067626953125;
+lensShading.ctrlPoints[2].controlPointB[0][1] = 0.951171875000;
+lensShading.ctrlPoints[2].controlPointB[0][2] = 1.110473632813;
+lensShading.ctrlPoints[2].controlPointB[0][3] = 1.056518554688;
+lensShading.ctrlPoints[2].controlPointB[0][4] = 1.093994140625;
+lensShading.ctrlPoints[2].controlPointB[0][5] = 1.054687500000;
+lensShading.ctrlPoints[2].controlPointB[0][6] = 1.069213867188;
+lensShading.ctrlPoints[2].controlPointB[0][7] = 1.134399414063;
+lensShading.ctrlPoints[2].controlPointB[0][8] = 0.739135742188;
+lensShading.ctrlPoints[2].controlPointB[0][9] = 1.590576171875;
+lensShading.ctrlPoints[2].controlPointB[1][0] = 0.969238281250;
+lensShading.ctrlPoints[2].controlPointB[1][1] = 1.175659179688;
+lensShading.ctrlPoints[2].controlPointB[1][2] = 1.016113281250;
+lensShading.ctrlPoints[2].controlPointB[1][3] = 1.098632812500;
+lensShading.ctrlPoints[2].controlPointB[1][4] = 1.035156250000;
+lensShading.ctrlPoints[2].controlPointB[1][5] = 1.125122070313;
+lensShading.ctrlPoints[2].controlPointB[1][6] = 1.055908203125;
+lensShading.ctrlPoints[2].controlPointB[1][7] = 1.160644531250;
+lensShading.ctrlPoints[2].controlPointB[1][8] = 0.858886718750;
+lensShading.ctrlPoints[2].controlPointB[1][9] = 1.210815429688;
+lensShading.ctrlPoints[2].controlPointB[2][0] = 1.090698242188;
+lensShading.ctrlPoints[2].controlPointB[2][1] = 0.956787109375;
+lensShading.ctrlPoints[2].controlPointB[2][2] = 1.132934570313;
+lensShading.ctrlPoints[2].controlPointB[2][3] = 1.060791015625;
+lensShading.ctrlPoints[2].controlPointB[2][4] = 1.104858398438;
+lensShading.ctrlPoints[2].controlPointB[2][5] = 1.052490234375;
+lensShading.ctrlPoints[2].controlPointB[2][6] = 1.083129882813;
+lensShading.ctrlPoints[2].controlPointB[2][7] = 1.119140625000;
+lensShading.ctrlPoints[2].controlPointB[2][8] = 1.034790039063;
+lensShading.ctrlPoints[2].controlPointB[2][9] = 1.066894531250;
+lensShading.ctrlPoints[2].controlPointB[3][0] = 1.028808593750;
+lensShading.ctrlPoints[2].controlPointB[3][1] = 1.086791992188;
+lensShading.ctrlPoints[2].controlPointB[3][2] = 1.064331054688;
+lensShading.ctrlPoints[2].controlPointB[3][3] = 1.079223632813;
+lensShading.ctrlPoints[2].controlPointB[3][4] = 1.063842773438;
+lensShading.ctrlPoints[2].controlPointB[3][5] = 1.071533203125;
+lensShading.ctrlPoints[2].controlPointB[3][6] = 1.080810546875;
+lensShading.ctrlPoints[2].controlPointB[3][7] = 1.100219726563;
+lensShading.ctrlPoints[2].controlPointB[3][8] = 1.048217773438;
+lensShading.ctrlPoints[2].controlPointB[3][9] = 1.080810546875;
+lensShading.ctrlPoints[2].controlPointB[4][0] = 1.089111328125;
+lensShading.ctrlPoints[2].controlPointB[4][1] = 1.040405273438;
+lensShading.ctrlPoints[2].controlPointB[4][2] = 1.078857421875;
+lensShading.ctrlPoints[2].controlPointB[4][3] = 1.073730468750;
+lensShading.ctrlPoints[2].controlPointB[4][4] = 1.048706054688;
+lensShading.ctrlPoints[2].controlPointB[4][5] = 1.068847656250;
+lensShading.ctrlPoints[2].controlPointB[4][6] = 1.084960937500;
+lensShading.ctrlPoints[2].controlPointB[4][7] = 1.156372070313;
+lensShading.ctrlPoints[2].controlPointB[4][8] = 1.000610351563;
+lensShading.ctrlPoints[2].controlPointB[4][9] = 1.117797851563;
+lensShading.ctrlPoints[2].controlPointB[5][0] = 1.024047851563;
+lensShading.ctrlPoints[2].controlPointB[5][1] = 1.084228515625;
+lensShading.ctrlPoints[2].controlPointB[5][2] = 1.072387695313;
+lensShading.ctrlPoints[2].controlPointB[5][3] = 1.081665039063;
+lensShading.ctrlPoints[2].controlPointB[5][4] = 1.074829101563;
+lensShading.ctrlPoints[2].controlPointB[5][5] = 1.076416015625;
+lensShading.ctrlPoints[2].controlPointB[5][6] = 1.093750000000;
+lensShading.ctrlPoints[2].controlPointB[5][7] = 1.072509765625;
+lensShading.ctrlPoints[2].controlPointB[5][8] = 1.104492187500;
+lensShading.ctrlPoints[2].controlPointB[5][9] = 1.054077148438;
+lensShading.ctrlPoints[2].controlPointB[6][0] = 1.086547851563;
+lensShading.ctrlPoints[2].controlPointB[6][1] = 1.045410156250;
+lensShading.ctrlPoints[2].controlPointB[6][2] = 1.100219726563;
+lensShading.ctrlPoints[2].controlPointB[6][3] = 1.083251953125;
+lensShading.ctrlPoints[2].controlPointB[6][4] = 1.095581054688;
+lensShading.ctrlPoints[2].controlPointB[6][5] = 1.097167968750;
+lensShading.ctrlPoints[2].controlPointB[6][6] = 1.093872070313;
+lensShading.ctrlPoints[2].controlPointB[6][7] = 1.130249023438;
+lensShading.ctrlPoints[2].controlPointB[6][8] = 0.980346679688;
+lensShading.ctrlPoints[2].controlPointB[6][9] = 1.195190429688;
+lensShading.ctrlPoints[2].controlPointB[7][0] = 1.171020507813;
+lensShading.ctrlPoints[2].controlPointB[7][1] = 1.062500000000;
+lensShading.ctrlPoints[2].controlPointB[7][2] = 1.063598632813;
+lensShading.ctrlPoints[2].controlPointB[7][3] = 1.118530273438;
+lensShading.ctrlPoints[2].controlPointB[7][4] = 1.072143554688;
+lensShading.ctrlPoints[2].controlPointB[7][5] = 1.124389648438;
+lensShading.ctrlPoints[2].controlPointB[7][6] = 1.093627929688;
+lensShading.ctrlPoints[2].controlPointB[7][7] = 1.110961914063;
+lensShading.ctrlPoints[2].controlPointB[7][8] = 0.973266601563;
+lensShading.ctrlPoints[2].controlPointB[7][9] = 1.320312500000;
+lensShading.ctrlPoints[2].controlPointB[8][0] = 1.119140625000;
+lensShading.ctrlPoints[2].controlPointB[8][1] = 0.935791015625;
+lensShading.ctrlPoints[2].controlPointB[8][2] = 1.070434570313;
+lensShading.ctrlPoints[2].controlPointB[8][3] = 1.055541992188;
+lensShading.ctrlPoints[2].controlPointB[8][4] = 1.120483398438;
+lensShading.ctrlPoints[2].controlPointB[8][5] = 1.078979492188;
+lensShading.ctrlPoints[2].controlPointB[8][6] = 1.070922851563;
+lensShading.ctrlPoints[2].controlPointB[8][7] = 1.046630859375;
+lensShading.ctrlPoints[2].controlPointB[8][8] = 1.051147460938;
+lensShading.ctrlPoints[2].controlPointB[8][9] = 1.285156250000;
+lensShading.ctrlPoints[2].controlPointB[9][0] = 1.434814453125;
+lensShading.ctrlPoints[2].controlPointB[9][1] = 1.068481445313;
+lensShading.ctrlPoints[2].controlPointB[9][2] = 1.075805664063;
+lensShading.ctrlPoints[2].controlPointB[9][3] = 1.076904296875;
+lensShading.ctrlPoints[2].controlPointB[9][4] = 1.084106445313;
+lensShading.ctrlPoints[2].controlPointB[9][5] = 1.102050781250;
+lensShading.ctrlPoints[2].controlPointB[9][6] = 1.076049804688;
+lensShading.ctrlPoints[2].controlPointB[9][7] = 1.137207031250;
+lensShading.ctrlPoints[2].controlPointB[9][8] = 1.001098632813;
+lensShading.ctrlPoints[2].controlPointB[9][9] = 1.901000976563;
diff --git a/frc971/orin/doflash_frc971.sh b/frc971/orin/doflash_frc971.sh
new file mode 100644
index 0000000..ecfa90c
--- /dev/null
+++ b/frc971/orin/doflash_frc971.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+set -ex
+
+TMPDIR=$(mktemp -d /tmp/yoctoflash.XXXXXXXXXX)
+
+# Cleanup on exit.
+function finish {
+ sudo rm -rf "${TMPDIR}"
+}
+trap finish EXIT
+
+# Assumes that the image has been copied into ./
+tar xf frc971-image-orin-nx-8g.tegraflash.tar.gz -C "${TMPDIR}"
+
+# Replace the rootfs with our new image.
+cp --sparse=always arm64_bookworm_debian_yocto.img "${TMPDIR}/frc971-image.ext4"
+
+cd ${TMPDIR}
+
+sudo ./initrd-flash
diff --git a/frc971/orin/virtual_packages/libglib-2.0-0/DEBIAN/control b/frc971/orin/virtual_packages/libglib-2.0-0/DEBIAN/control
new file mode 100644
index 0000000..1f9e4f0
--- /dev/null
+++ b/frc971/orin/virtual_packages/libglib-2.0-0/DEBIAN/control
@@ -0,0 +1,7 @@
+Package: libglib-2.0-0
+Version: 2.74.6-2
+Architecture: arm64
+Depends: libglib2.0-bin
+Maintainer: Austin Schuh <austin.linux@gmail.com>
+Provides: libglib-2.0-0
+Description: Adapter for libglib2.0-bin
diff --git a/frc971/orin/virtual_packages/libglvnd/DEBIAN/control b/frc971/orin/virtual_packages/libglvnd/DEBIAN/control
new file mode 100644
index 0000000..15ee630
--- /dev/null
+++ b/frc971/orin/virtual_packages/libglvnd/DEBIAN/control
@@ -0,0 +1,7 @@
+Package: libglvnd
+Version: 1.6.0-1
+Architecture: arm64
+Depends: libglvnd0
+Maintainer: Austin Schuh <austin.linux@gmail.com>
+Provides: libglvnd
+Description: Adapter for libglvnd
diff --git a/frc971/orin/virtual_packages/libgtk-3-0/DEBIAN/control b/frc971/orin/virtual_packages/libgtk-3-0/DEBIAN/control
new file mode 100644
index 0000000..8d85196
--- /dev/null
+++ b/frc971/orin/virtual_packages/libgtk-3-0/DEBIAN/control
@@ -0,0 +1,7 @@
+Package: libgtk-3.0
+Version: 3.24.38-2~deb12u1
+Architecture: arm64
+Depends: libgtk-3-0
+Maintainer: Austin Schuh <austin.linux@gmail.com>
+Provides: libgtk-3.0
+Description: Adapter for libgtk-3-0
diff --git a/frc971/orin/virtual_packages/libxcb-glx/DEBIAN/control b/frc971/orin/virtual_packages/libxcb-glx/DEBIAN/control
new file mode 100644
index 0000000..16c3060
--- /dev/null
+++ b/frc971/orin/virtual_packages/libxcb-glx/DEBIAN/control
@@ -0,0 +1,7 @@
+Package: libxcb-glx
+Version: 1.15-1
+Architecture: arm64
+Depends: libxcb-glx0
+Maintainer: Austin Schuh <austin.linux@gmail.com>
+Provides: libxcb-glx
+Description: Adapter for libxcb-glx0
diff --git a/frc971/orin/virtual_packages/wayland/DEBIAN/control b/frc971/orin/virtual_packages/wayland/DEBIAN/control
new file mode 100644
index 0000000..04ac8ca
--- /dev/null
+++ b/frc971/orin/virtual_packages/wayland/DEBIAN/control
@@ -0,0 +1,7 @@
+Package: wayland
+Version: 1.21.0-1
+Architecture: arm64
+Depends: libwayland-client0, libwayland-cursor0, libwayland-egl1, libwayland-server0
+Maintainer: Austin Schuh <austin.linux@gmail.com>
+Provides: wayland
+Description: Adapter for wayland
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 82cc6b3..b8edcae 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -25,8 +25,10 @@
DEFINE_uint32(
min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
-DEFINE_uint32(min_id, 12, "Minimum valid charuco id");
-DEFINE_uint32(max_id, 15, "Minimum valid charuco id");
+DEFINE_uint32(min_id, 0, "Minimum valid charuco id");
+DEFINE_uint32(max_diamonds, 0,
+ "Maximum number of diamonds to see. Set to 0 for no limit");
+DEFINE_uint32(max_id, 15, "Maximum valid charuco id");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
DEFINE_bool(
draw_axes, false,
@@ -280,9 +282,21 @@
}
CharucoExtractor::CharucoExtractor(
+ const calibration::CameraCalibration *calibration,
+ const TargetType target_type)
+ : event_loop_(nullptr),
+ target_type_(target_type),
+ calibration_(CHECK_NOTNULL(calibration)) {
+ VLOG(2) << "Configuring CharucoExtractor without event_loop";
+ SetupTargetData();
+ VLOG(2) << "Camera matrix:\n" << calibration_.CameraIntrinsics();
+ VLOG(2) << "Distortion Coefficients:\n" << calibration_.CameraDistCoeffs();
+}
+
+CharucoExtractor::CharucoExtractor(
aos::EventLoop *event_loop,
- const calibration::CameraCalibration *calibration, TargetType target_type,
- std::string_view image_channel,
+ const calibration::CameraCalibration *calibration,
+ const TargetType target_type, std::string_view image_channel,
std::function<void(cv::Mat, monotonic_clock::time_point,
std::vector<cv::Vec4i>,
std::vector<std::vector<cv::Point2f>>, bool,
@@ -303,9 +317,34 @@
void CharucoExtractor::HandleImage(cv::Mat rgb_image,
const monotonic_clock::time_point eof) {
+ // Set up the variables we'll use in the callback function
+ bool valid = false;
+ // ids and corners for final, refined board / marker detections
+ // Using Vec4i type since it supports Charuco Diamonds
+ // And overloading it using 1st int in Vec4i for others target types
+ std::vector<cv::Vec4i> result_ids;
+ std::vector<std::vector<cv::Point2f>> result_corners;
+
+ // Return a list of poses; for Charuco Board there will be just one
+ std::vector<Eigen::Vector3d> rvecs_eigen;
+ std::vector<Eigen::Vector3d> tvecs_eigen;
+ ProcessImage(rgb_image, eof, event_loop_->monotonic_now(), result_ids,
+ result_corners, valid, rvecs_eigen, tvecs_eigen);
+
+ handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+}
+
+void CharucoExtractor::ProcessImage(
+ cv::Mat rgb_image, const monotonic_clock::time_point eof,
+ const monotonic_clock::time_point current_time,
+ std::vector<cv::Vec4i> &result_ids,
+ std::vector<std::vector<cv::Point2f>> &result_corners, bool &valid,
+ std::vector<Eigen::Vector3d> &rvecs_eigen,
+ std::vector<Eigen::Vector3d> &tvecs_eigen) {
const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(
- event_loop_->monotonic_now() - eof)
+ std::chrono::duration_cast<std::chrono::duration<double>>(current_time -
+ eof)
.count();
// Have found this useful if there is blurry / noisy images
@@ -318,22 +357,10 @@
cv::cvtColor(thresh, rgb_image, cv::COLOR_GRAY2RGB);
}
- // Set up the variables we'll use in the callback function
- bool valid = false;
- // Return a list of poses; for Charuco Board there will be just one
- std::vector<Eigen::Vector3d> rvecs_eigen;
- std::vector<Eigen::Vector3d> tvecs_eigen;
-
// ids and corners for initial aruco marker detections
std::vector<int> marker_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
- // ids and corners for final, refined board / marker detections
- // Using Vec4i type since it supports Charuco Diamonds
- // And overloading it using 1st int in Vec4i for others target types
- std::vector<cv::Vec4i> result_ids;
- std::vector<std::vector<cv::Point2f>> result_corners;
-
// Do initial marker detection; this is the same for all target types
cv::aruco::detectMarkers(rgb_image, dictionary_, marker_corners, marker_ids);
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
@@ -429,11 +456,14 @@
square_length_ / marker_length_,
diamond_corners, diamond_ids);
- // Check that we have exactly one charuco diamond. For calibration, we
- // can constrain things so that this is the case
- if (diamond_ids.size() == 1) {
- // TODO<Jim>: Could probably make this check more general than requiring
- // range of ids
+ // Check that we have an acceptable number of diamonds detected.
+ // Should be at least one, and no more than FLAGS_max_diamonds.
+ // Different calibration routines will require different values for this
+ if (diamond_ids.size() > 0 &&
+ (FLAGS_max_diamonds == 0 ||
+ diamond_ids.size() <= FLAGS_max_diamonds)) {
+ // TODO<Jim>: Could probably make this check more general than
+ // requiring range of ids
bool all_valid_ids = true;
for (uint i = 0; i < 4; i++) {
uint id = diamond_ids[0][i];
@@ -446,14 +476,15 @@
cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
diamond_ids);
- // estimate pose for diamonds doesn't return valid, so marking true
+ // estimate pose for diamonds doesn't return valid, so marking
+ // true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(
diamond_corners, square_length_, calibration_.CameraIntrinsics(),
calibration_.CameraDistCoeffs(), rvecs, tvecs);
- DrawTargetPoses(rgb_image, rvecs, tvecs);
+ DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
result_ids = diamond_ids;
result_corners = diamond_corners;
@@ -463,12 +494,12 @@
} else {
if (diamond_ids.size() == 0) {
// OK to not see any markers sometimes
- VLOG(2)
- << "Found aruco markers, but no valid charuco diamond targets";
+ VLOG(2) << "Found aruco markers, but no valid charuco diamond "
+ "targets";
} else {
- // But should never detect multiple
- LOG(FATAL) << "Found multiple charuco diamond markers. Should only "
- "be one";
+ VLOG(2) << "Found too many number of diamond markers, which likely "
+ "means false positives were detected: "
+ << diamond_ids.size() << " > " << FLAGS_max_diamonds;
}
}
} else {
@@ -476,9 +507,6 @@
<< static_cast<uint8_t>(target_type_);
}
}
-
- handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
- rvecs_eigen, tvecs_eigen);
}
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 8af7b8c..2f274bb 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -93,6 +93,11 @@
// extracted from it.
class CharucoExtractor {
public:
+ // Setting up a constructor that doesn't require an event_loop, so we can call
+ // and get results back from ProcessImage directly
+ CharucoExtractor(const calibration::CameraCalibration *calibration,
+ const TargetType target_type);
+
// The callback takes the following arguments:
// cv::Mat -> image with overlays drawn on it.
// monotonic_clock::time_point -> Time on this node when this image was
@@ -117,7 +122,16 @@
std::vector<Eigen::Vector3d>)> &&handle_charuco_fn);
// Handles the image by detecting the charuco board in it.
- void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
+ void HandleImage(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof);
+
+ void ProcessImage(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof,
+ const aos::monotonic_clock::time_point current_time,
+ std::vector<cv::Vec4i> &result_ids,
+ std::vector<std::vector<cv::Point2f>> &result_corners,
+ bool &valid, std::vector<Eigen::Vector3d> &rvecs_eigen,
+ std::vector<Eigen::Vector3d> &tvecs_eigen);
// Returns the aruco dictionary in use.
cv::Ptr<cv::aruco::Dictionary> dictionary() const { return dictionary_; }
@@ -140,8 +154,8 @@
void DrawTargetPoses(cv::Mat rgb_image, std::vector<cv::Vec3d> rvecs,
std::vector<cv::Vec3d> tvecs);
- // Helper function to convert rotation (rvecs) and translation (tvecs) vectors
- // into Eigen vectors and store in corresponding vectors
+ // Helper function to convert rotation (rvecs) and translation (tvecs)
+ // vectors into Eigen vectors and store in corresponding vectors
void PackPoseResults(std::vector<cv::Vec3d> &rvecs,
std::vector<cv::Vec3d> &tvecs,
std::vector<Eigen::Vector3d> *rvecs_eigen,
@@ -184,7 +198,8 @@
const foxglove::PointsAnnotationType line_type =
foxglove::PointsAnnotationType::POINTS);
-// Creates a PointsAnnotation to build up ImageAnnotations with different types
+// Creates a PointsAnnotation to build up ImageAnnotations with different
+// types
flatbuffers::Offset<foxglove::PointsAnnotation> BuildPointsAnnotation(
flatbuffers::FlatBufferBuilder *fbb,
const aos::monotonic_clock::time_point monotonic_now,
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index c4af163..8077f16 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -303,6 +303,9 @@
ceres::LocalParameterization *quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
+ int min_constraint_id = std::numeric_limits<int>::max();
+ int max_constraint_id = std::numeric_limits<int>::min();
+
for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {
@@ -325,6 +328,15 @@
<< "Should have counted constraints for " << id_pair.first << "->"
<< id_pair.second;
+ VLOG(1) << "Adding constraint pair: " << id_pair.first << " and "
+ << id_pair.second;
+ // Store min & max id's; assumes first id < second id
+ if (id_pair.first < min_constraint_id) {
+ min_constraint_id = id_pair.first;
+ }
+ if (id_pair.second > max_constraint_id) {
+ max_constraint_id = id_pair.second;
+ }
// Normalize constraint cost by occurances
size_t constraint_count = constraint_counts_[id_pair];
// Scale all costs so the total cost comes out to more reasonable numbers
@@ -358,6 +370,12 @@
// setting one of the poses as constant so the optimizer cannot change it.
CHECK_NE(poses->count(FLAGS_frozen_target_id), 0ul)
<< "Got no poses for frozen target id " << FLAGS_frozen_target_id;
+ CHECK_GE(FLAGS_frozen_target_id, min_constraint_id)
+ << "target to freeze index " << FLAGS_frozen_target_id
+ << " must be in range of constraints, > " << min_constraint_id;
+ CHECK_LE(FLAGS_frozen_target_id, max_constraint_id)
+ << "target to freeze index " << FLAGS_frozen_target_id
+ << " must be in range of constraints, < " << max_constraint_id;
ceres::examples::MapOfPoses::iterator pose_start_iter =
poses->find(FLAGS_frozen_target_id);
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
@@ -479,8 +497,8 @@
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_start)).inverse() *
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_end));
auto constraint = PoseUtils::Affine3dToPose3d(H_start_end);
- LOG(INFO) << id_start << "->" << id_end << ": " << constraint.p.norm()
- << " meters";
+ VLOG(1) << id_start << "->" << id_end << ": " << constraint.p.norm()
+ << " meters";
}
}
}
diff --git a/third_party/apriltag/common/workerpool.c b/third_party/apriltag/common/workerpool.c
index 29eccfc..a23832e 100644
--- a/third_party/apriltag/common/workerpool.c
+++ b/third_party/apriltag/common/workerpool.c
@@ -52,7 +52,7 @@
{
workerpool_t *wp = (workerpool_t*) p;
- int cnt = 0;
+ //int cnt = 0;
while (1) {
struct task *task;
@@ -63,13 +63,13 @@
// printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
pthread_cond_broadcast(&wp->endcond);
pthread_cond_wait(&wp->startcond, &wp->mutex);
- cnt = 0;
+ //cnt = 0;
// printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
}
zarray_get_volatile(wp->tasks, wp->taskspos, &task);
wp->taskspos++;
- cnt++;
+ //cnt++;
pthread_mutex_unlock(&wp->mutex);
// pthread_yield();
sched_yield();
diff --git a/third_party/apriltag/common/zarray.c b/third_party/apriltag/common/zarray.c
index 43e6a7e..fa1f8a2 100644
--- a/third_party/apriltag/common/zarray.c
+++ b/third_party/apriltag/common/zarray.c
@@ -41,7 +41,7 @@
return strcmp(a,b);
}
-void zarray_vmap(zarray_t *za, void (*f)())
+void zarray_vmap(zarray_t *za, void (*f)(void*))
{
assert(za != NULL);
assert(f != NULL);
diff --git a/third_party/apriltag/common/zhash.c b/third_party/apriltag/common/zhash.c
index 914f530..9414513 100644
--- a/third_party/apriltag/common/zhash.c
+++ b/third_party/apriltag/common/zhash.c
@@ -353,7 +353,7 @@
zit->last_entry--;
}
-void zhash_map_keys(zhash_t *zh, void (*f)())
+void zhash_map_keys(zhash_t *zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
@@ -369,7 +369,7 @@
}
}
-void zhash_vmap_keys(zhash_t * zh, void (*f)())
+void zhash_vmap_keys(zhash_t * zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
@@ -386,7 +386,7 @@
}
}
-void zhash_map_values(zhash_t * zh, void (*f)())
+void zhash_map_values(zhash_t * zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
@@ -401,7 +401,7 @@
}
}
-void zhash_vmap_values(zhash_t * zh, void (*f)())
+void zhash_vmap_values(zhash_t * zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
diff --git a/third_party/apriltag/common/zmaxheap.c b/third_party/apriltag/common/zmaxheap.c
index e04d03e..5cf5292 100644
--- a/third_party/apriltag/common/zmaxheap.c
+++ b/third_party/apriltag/common/zmaxheap.c
@@ -167,7 +167,7 @@
}
}
-void zmaxheap_vmap(zmaxheap_t *heap, void (*f)())
+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*))
{
assert(heap != NULL);
assert(f != NULL);
diff --git a/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl b/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl
index 59b542c..b167f6e 100644
--- a/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl
+++ b/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl
@@ -53,6 +53,12 @@
target_os_arch_key = _os_arch_pair(target_os, target_arch)
_check_os_arch_keys([host_os_arch_key, target_os_arch_key])
+ ## doesn't seem to have a feature for this.
+ llvm_version_split = llvm_version.split(".")
+ llvm_major_ver = int(llvm_version_split[0]) if len(llvm_version_split) else 0
+
+ llvm_subfolder = llvm_version_split[0] if llvm_major_ver > 14 else llvm_version
+
# A bunch of variables that get passed straight through to
# `create_cc_toolchain_config_info`.
# TODO: What do these values mean, and are they actually all correct?
@@ -129,7 +135,7 @@
resource_dir = [
"-resource-dir",
- "{}lib/clang/{}".format(target_toolchain_path_prefix, llvm_version),
+ "{}lib/clang/{}".format(target_toolchain_path_prefix, llvm_subfolder),
]
# Default compiler flags:
@@ -278,7 +284,7 @@
common_include_flags = [
"-nostdinc",
"-isystem",
- target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_version),
+ target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_subfolder),
"-isystem",
sysroot_path + "/usr/local/include",
"-isystem",
@@ -293,6 +299,7 @@
sysroot_path + "/usr/include",
]
compile_not_cxx_flags.extend(common_include_flags)
+
cxx_flags.extend([
"-nostdinc++",
"-isystem",
@@ -321,13 +328,12 @@
coverage_link_flags = ["-fprofile-instr-generate"]
## NOTE: framework paths is missing here; unix_cc_toolchain_config
- ## doesn't seem to have a feature for this.
# C++ built-in include directories:
cxx_builtin_include_directories = [
target_toolchain_path_prefix + "include/c++/v1",
- target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_version),
- target_toolchain_path_prefix + "lib64/clang/{}/include".format(llvm_version),
+ target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_subfolder),
+ target_toolchain_path_prefix + "lib64/clang/{}/include".format(llvm_subfolder),
]
sysroot_prefix = ""
@@ -354,8 +360,6 @@
# Tool paths:
# `llvm-strip` was introduced in V7 (https://reviews.llvm.org/D46407):
- llvm_version = llvm_version.split(".")
- llvm_major_ver = int(llvm_version[0]) if len(llvm_version) else 0
strip_binary = (tools_path_prefix + "bin/llvm-strip") if llvm_major_ver >= 7 else _host_tools.get_and_assert(host_tools_info, "strip")
# TODO: The command line formed on darwin does not work with llvm-ar.
diff --git a/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl b/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl
index 4cdbf75..031f4cf 100644
--- a/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl
+++ b/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl
@@ -181,6 +181,102 @@
"clang+llvm-13.0.0-x86_64-linux-gnu-ubuntu-16.04.tar.xz": "76d0bf002ede7a893f69d9ad2c4e101d15a8f4186fbfe24e74856c8449acd7c1",
"clang+llvm-13.0.0-x86_64-linux-gnu-ubuntu-20.04.tar.xz": "2c2fb857af97f41a5032e9ecadf7f78d3eff389a5cd3c9ec620d24f134ceb3c8",
"clang+llvm-13.0.0-aarch64-linux-gnu.tar.xz": "968d65d2593850ee9b37fcda074fb7641529bd45d2f976af6c8197de3c22612f",
+
+ # 13.0.1
+ "clang+llvm-13.0.1-aarch64-linux-gnu.tar.xz": "15ff2db12683e69e552b6668f7ca49edaa01ce32cb1cbc8f8ed2e887ab291069",
+ "clang+llvm-13.0.1-amd64-unknown-freebsd12.tar.xz": "8101c8d3a920bf930b33987ada5373f43537c5de8c194be0ea10530fd0ad5617",
+ "clang+llvm-13.0.1-amd64-unknown-freebsd13.tar.xz": "f1ba8ec77b5e82399af738ad9897a8aafc11c5692ceb331c8373eae77018d428",
+ "clang+llvm-13.0.1-armv7a-linux-gnueabihf.tar.xz": "1215720114538f57acbe2f3b0614c23f4fc551ba2976afa3779a3c01aaaf1221",
+ "clang+llvm-13.0.1-i386-unknown-freebsd12.tar.xz": "e3c921e0f130afa6a6ebac23c31b66b32563a5ec53a2f4ed4676f31a81379f70",
+ "clang+llvm-13.0.1-i386-unknown-freebsd13.tar.xz": "e85c46bd64a0217f3df1f42421a502648d6741ef29fd5d44674b87af119ce25d",
+ "clang+llvm-13.0.1-powerpc64le-linux-rhel-7.9.tar.xz": "ab659c290536182a99c064d4537d2fb1273bb2b1bf8c6a43866f033bf1ece4a8",
+ "clang+llvm-13.0.1-powerpc64le-linux-ubuntu-18.04.5.tar.xz": "7a4be2508aa0b4ee3f72c312af4b62ea14581a5db61aa703ea0822f46e5598cb",
+ "clang+llvm-13.0.1-x86_64-apple-darwin.tar.xz": "dec02d17698514d0fc7ace8869c38937851c542b02adf102c4e898f027145a4d",
+ "clang+llvm-13.0.1-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "84a54c69781ad90615d1b0276a83ff87daaeded99fbc64457c350679df7b4ff0",
+
+ # 14.0.0
+ "clang+llvm-14.0.0-aarch64-linux-gnu.tar.xz": "1792badcd44066c79148ffeb1746058422cc9d838462be07e3cb19a4b724a1ee",
+ "clang+llvm-14.0.0-amd64-pc-solaris2.11.tar.xz": "a708470fdbaadf530d6cfd56f92fde1328cb47ef8439ecf1a2126523e7c94a50",
+ "clang+llvm-14.0.0-amd64-unknown-freebsd12.tar.xz": "7eaff7ee2a32babd795599f41f4a5ffe7f161721ebf5630f48418e626650105e",
+ "clang+llvm-14.0.0-amd64-unknown-freebsd13.tar.xz": "b68d73fd57be385e7f06046a87381f7520c8861f492c294e6301d2843d9a1f57",
+ "clang+llvm-14.0.0-armv7a-linux-gnueabihf.tar.xz": "17d5f60c3d5f9494be7f67b2dc9e6017cd5e8457e53465968a54ec7069923bfe",
+ "clang+llvm-14.0.0-i386-unknown-freebsd12.tar.xz": "5ed9d93a8425132e8117d7061d09c2989ce6b2326f25c46633e2b2dee955bb00",
+ "clang+llvm-14.0.0-i386-unknown-freebsd13.tar.xz": "81f49eb466ce9149335ac8918a5f02fa724d562a94464ed13745db0165b4a220",
+ "clang+llvm-14.0.0-powerpc64-ibm-aix-7.2.tar.xz": "4ad5866de6c69d989cbbc989201b46dfdcd7d2b23a712fcad7baa09c204f10de",
+ "clang+llvm-14.0.0-powerpc64le-linux-rhel-7.9.tar.xz": "7a31de37959fdf3be897b01f284a91c28cd38a2e2fa038ff58121d1b6f6eb087",
+ "clang+llvm-14.0.0-powerpc64le-linux-ubuntu-18.04.tar.xz": "2d504c4920885c86b306358846178bc2232dfac83b47c3b1d05861a8162980e6",
+ "clang+llvm-14.0.0-sparcv9-sun-solaris2.11.tar.xz": "b342cdaaea3b44de5b0f45052e2df49bcdf69dcc8ad0c23ec5afc04668929681",
+ "clang+llvm-14.0.0-x86_64-apple-darwin.tar.xz": "cf5af0f32d78dcf4413ef6966abbfd5b1445fe80bba57f2ff8a08f77e672b9b3",
+ "clang+llvm-14.0.0-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "61582215dafafb7b576ea30cc136be92c877ba1f1c31ddbbd372d6d65622fef5",
+ "clang+llvm-14.0.0-x86_64-linux-sles12.4.tar.xz": "78f70cc94c3b6f562455b15cebb63e75571d50c3d488d53d9aa4cd9dded30627",
+
+ # 15.0.0
+ "clang+llvm-15.0.0-aarch64-linux-gnu.tar.xz": "527ed550784681f95ec7a1be8fbf5a24bd03d7da9bf31afb6523996f45670be3",
+ "clang+llvm-15.0.0-amd64-pc-solaris2.11.tar.xz": "5b9fd6a30ce6941adf74667d2076a49aa047fa040e3690f7af26c264d4ce58e7",
+ "clang+llvm-15.0.0-arm64-apple-darwin21.0.tar.xz": "cfd5c3fa07d7fccea0687f5b4498329a6172b7a15bbc45b547d0ac86bd3452a5",
+ "clang+llvm-15.0.0-armv7a-linux-gnueabihf.tar.xz": "58ce8877642fc1399736ffc81bc8ef6244440fc78d72e097a07475b8b25e2bf1",
+ "clang+llvm-15.0.0-powerpc64-ibm-aix-7.2.tar.xz": "c5f63401fa88ea96ca7110bd81ead1bf1a2575962e9cc84a6713ec29c02b1c10",
+ "clang+llvm-15.0.0-powerpc64le-linux-rhel-8.4.tar.xz": "c94448766b6b92cfc8f35e611308c9680a9ad2177f88d358c2b06e9b108d61bd",
+ "clang+llvm-15.0.0-powerpc64le-linux-ubuntu-18.04.6.tar.xz": "6bcedc3d18552732f219c1d0f8c4b0c917ff5f800400a31dabfe8d040cbf1f02",
+ "clang+llvm-15.0.0-sparc64-unknown-linux-gnu.tar.xz": "b5a8108040d5d5d69d6106fa89a6cffc71a16a3583b74c1f15c42f392a47a3d9",
+ "clang+llvm-15.0.0-sparcv9-sun-solaris2.11.tar.xz": "4354854976355ca6f4ac90231a97121844c4fc9f998c9850527390120c62f01f",
+ "clang+llvm-15.0.0-x86_64-apple-darwin.tar.xz": "8fb11e6ada98b901398b2e7b0378a3a59e88c88c754e95d8f6b54613254d7d65",
+
+ # 15.0.6
+ "clang+llvm-15.0.6-aarch64-linux-gnu.tar.xz": "8ca4d68cf103da8331ca3f35fe23d940c1b78fb7f0d4763c1c059e352f5d1bec",
+ "clang+llvm-15.0.6-arm64-apple-darwin21.0.tar.xz": "32bc7b8eee3d98f72dd4e5651e6da990274ee2d28c5c19a7d8237eb817ce8d91",
+ "clang+llvm-15.0.6-armv7a-linux-gnueabihf.tar.xz": "c12e9298f9a9ed3a96342e9ffb2c02146a0cd7535231fef57c7217bd3a36f53b",
+ "clang+llvm-15.0.6-powerpc64-ibm-aix-7.2.tar.xz": "6bc1c2fcc8069e28773f6a0d16624160cd6de01b8f15aab27652eedad665d462",
+ "clang+llvm-15.0.6-powerpc64le-linux-rhel-8.4.tar.xz": "c26e5563e6ff46a03bc45fe27547c69283b64cba2359ccd3a42f735c995c0511",
+ "clang+llvm-15.0.6-powerpc64le-linux-ubuntu-18.04.tar.xz": "7fc9f07ff0fcf191df93fe4adc1da555e43f62fe1d3ddafb15c943f72b1bda17",
+ "clang+llvm-15.0.6-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "38bc7f5563642e73e69ac5626724e206d6d539fbef653541b34cae0ba9c3f036",
+
+ # 15.0.7
+ "clang+llvm-15.0.7-arm64-apple-darwin22.0.tar.xz": "867c6afd41158c132ef05a8f1ddaecf476a26b91c85def8e124414f9a9ba188d",
+ "clang+llvm-15.0.7-powerpc64-ibm-aix-7.2.tar.xz": "6cbc7c7f4395abb9c1a5bdcab3811bd6b1a6c4d08756ba674bfbbd732e2b23ac",
+ "clang+llvm-15.0.7-powerpc64le-linux-rhel-8.4.tar.xz": "2163cc934437146dc30810a21a46327ba3983f123c3bea19be316a64135b6414",
+ "clang+llvm-15.0.7-powerpc64le-linux-ubuntu-18.04.tar.xz": "19a16d768e15966923b0cbf8fc7dc148c89e316857acd89ad3aff72dcfcd61f4",
+ "clang+llvm-15.0.7-x86_64-apple-darwin21.0.tar.xz": "d16b6d536364c5bec6583d12dd7e6cf841b9f508c4430d9ee886726bd9983f1c",
+
+ # 16.0.0
+ "clang+llvm-16.0.0-aarch64-linux-gnu.tar.xz": "b750ba3120e6153fc5b316092f19b52cf3eb64e19e5f44bd1b962cb54a20cf0a",
+ "clang+llvm-16.0.0-amd64-pc-solaris2.11.tar.xz": "b637b7da383d3417ac4862342911cb467fba2ec00f48f163eb8308f2bbb9b7ad",
+ "clang+llvm-16.0.0-amd64-unknown-freebsd13.tar.xz": "c4fe6293349b3ab7d802793103d1d44f58831884e63ff1b40ce29c3e7408257b",
+ "clang+llvm-16.0.0-arm64-apple-darwin22.0.tar.xz": "2041587b90626a4a87f0de14a5842c14c6c3374f42c8ed12726ef017416409d9",
+ "clang+llvm-16.0.0-powerpc64-ibm-aix-7.2.tar.xz": "e51209eeea3c3db41084d8625ab3357991980831e0b641d633ec23e9d858333f",
+ "clang+llvm-16.0.0-powerpc64le-linux-rhel-8.4.tar.xz": "eb56949af9a83a12754f7cf254886d30c4be8a1da4dd0f27db790a7fcd35a3bf",
+ "clang+llvm-16.0.0-powerpc64le-linux-ubuntu-18.04.tar.xz": "ae34b037cde14f19c3c431de5fc04e06fa43d2cce3f8d44a63659b48afdf1f7a",
+ "clang+llvm-16.0.0-sparc64-unknown-linux-gnu.tar.xz": "a2627fcb6d97405b38c9e4c17ccfdc5d61fdd1bee742dcce0726ed39e2dcd92c",
+ "clang+llvm-16.0.0-sparcv9-sun-solaris2.11.tar.xz": "45c2ac0c10c3876332407a1ea893dccbde77a490f4a9b54a00e4881681a3c5ea",
+ "clang+llvm-16.0.0-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "2b8a69798e8dddeb57a186ecac217a35ea45607cb2b3cf30014431cff4340ad1",
+
+ # 16.0.1
+ "clang+llvm-16.0.1-aarch64-linux-gnu.tar.xz": "83e38451772120b016432687c0a3aab391808442b86f54966ef44c73a26280ac",
+ "clang+llvm-16.0.1-amd64-unknown-freebsd13.tar.xz": "970359de2a1a09a93a9e1cf3405e5758dfe463567b20a168f9156bd72b7f8ac6",
+ "clang+llvm-16.0.1-arm64-apple-darwin22.0.tar.xz": "cb487fa991f047dc79ae36430cbb9ef14621c1262075373955b1d97215c75879",
+ "clang+llvm-16.0.1-powerpc64-ibm-aix-7.2.tar.xz": "c56d9cf643b7f39e40436e55b59b3bd88057ec0fa084bd8e06ac17fb20ea2a21",
+ "clang+llvm-16.0.1-powerpc64le-linux-rhel-8.4.tar.xz": "c89a9af64a35ee58ef4eac7b52c173707140dc7eac6839ff254b656de8eb6c3c",
+ "clang+llvm-16.0.1-powerpc64le-linux-ubuntu-20.04.tar.xz": "08b39f9e6c19086aaf029d155c42a4db96ce662f84d6e89d8c9037d3baeee036",
+
+ # 16.0.2
+ "clang+llvm-16.0.2-aarch64-linux-gnu.tar.xz": "de89d138cfb17e2d81fdaca2f9c5e0c042014beea6bcacde7f27db40b69c0bdc",
+ "clang+llvm-16.0.2-amd64-unknown-freebsd13.tar.xz": "0cd92b6a84e7477aa8070465f01eec8198e0b1e38d1b6da8c61859a633ec9a71",
+ "clang+llvm-16.0.2-arm64-apple-darwin22.0.tar.xz": "539861297b8aa6be8e89bf68268b07d79d7a1fde87f4b98f123709f13933f326",
+ "clang+llvm-16.0.2-powerpc64-ibm-aix-7.2.tar.xz": "8c9cbf29b261f1af905f41032b446fd78bd560b549ab31d05a16d0cc972df23d",
+ "clang+llvm-16.0.2-powerpc64le-linux-rhel-8.4.tar.xz": "fe21023b64d2298d65fea0f4832a27a9948121662b54a8c8ce8a9331c4039c36",
+ "clang+llvm-16.0.2-x86_64-linux-gnu-ubuntu-22.04.tar.xz": "9530eccdffedb9761f23cbd915cf95d861b1d95f340ea36ded68bd6312af912e",
+
+ # 16.0.3
+ "clang+llvm-16.0.3-aarch64-linux-gnu.tar.xz": "315fd821ddb3e4b10c4dfabe7f200d1d17902b6a5ccd5dd665a0cd454bca379f",
+ "clang+llvm-16.0.3-arm64-apple-darwin22.0.tar.xz": "b9068eee1cf1e17848241ea581a2abe6cb4a15d470ec515c100f8b52e4c6a7cb",
+ "clang+llvm-16.0.3-powerpc64-ibm-aix-7.2.tar.xz": "f0372ea5b665ca1b8524b933b84ccbe59e9441537388815b24323aa4aab7db2f",
+ "clang+llvm-16.0.3-powerpc64le-linux-rhel-8.4.tar.xz": "9804721c746d74a85ce935d938509277af728fad1548835f539660ff1380e04d",
+ "clang+llvm-16.0.3-x86_64-linux-gnu-ubuntu-22.04.tar.xz": "638d32fd0032f99bafaab3bae63a406adb771825a02b6b7da119ee7e71af26c6",
+
+ # 16.0.4
+ "clang+llvm-16.0.4-amd64-unknown-freebsd13.tar.xz": "cf9d73bcf05b8749c7f3efbe86654b8fe0209f28993eafe26c27eb85885593f7",
+ "clang+llvm-16.0.4-arm64-apple-darwin22.0.tar.xz": "429b8061d620108fee636313df55a0602ea0d14458c6d3873989e6b130a074bd",
+ "clang+llvm-16.0.4-x86_64-linux-gnu-ubuntu-22.04.tar.xz": "fd464333bd55b482eb7385f2f4e18248eb43129a3cda4c0920ad9ac3c12bdacf",
}
# Note: Unlike the user-specified llvm_mirror attribute, the URL prefixes in
@@ -201,6 +297,16 @@
"12.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
"12.0.1": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
"13.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "13.0.1": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "14.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "15.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "15.0.6": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "15.0.7": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.1": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.2": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.3": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.4": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
}
def _get_auth(ctx, urls):
diff --git a/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h b/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h
index f9c56ba..07c0d88 100644
--- a/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h
+++ b/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h
@@ -270,17 +270,12 @@
}
- Index count = 0;
// FIXME compute a reference value to filter zeros
for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
{
- ++ count;
-// std::cerr << "fill " << it.index() << ", " << col << "\n";
-// std::cout << it.value() << " ";
// FIXME use insertBack
res.insert(it.index(), col) = it.value();
}
-// std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n";
}
res.finalize();
other = res.markAsRValue();
diff --git a/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
index 6f75d50..7aecbca 100644
--- a/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
+++ b/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -75,8 +75,6 @@
// Identify the relaxed supernodes by postorder traversal of the etree
Index snode_start; // beginning of a snode
StorageIndex k;
- Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree
- Index nsuper_et = 0; // Number of relaxed snodes in the original etree
StorageIndex l;
for (j = 0; j < n; )
{
@@ -88,7 +86,6 @@
parent = et(j);
}
// Found a supernode in postordered etree, j is the last column
- ++nsuper_et_post;
k = StorageIndex(n);
for (Index i = snode_start; i <= j; ++i)
k = (std::min)(k, inv_post(i));
@@ -97,7 +94,6 @@
{
// This is also a supernode in the original etree
relax_end(k) = l; // Record last column
- ++nsuper_et;
}
else
{
@@ -107,7 +103,6 @@
if (descendants(i) == 0)
{
relax_end(l) = l;
- ++nsuper_et;
}
}
}
diff --git a/third_party/google-glog/bazel/glog.bzl b/third_party/google-glog/bazel/glog.bzl
index b4825d3..63f40d3 100644
--- a/third_party/google-glog/bazel/glog.bzl
+++ b/third_party/google-glog/bazel/glog.bzl
@@ -90,6 +90,7 @@
]
linux_or_darwin_copts = wasm_copts + [
+ "-Wno-unused-but-set-variable",
"-DGLOG_EXPORT=__attribute__((visibility(\\\"default\\\")))",
# For src/utilities.cc.
"-DHAVE_SYS_SYSCALL_H",
diff --git a/third_party/googletest/googletest.patch b/third_party/googletest/googletest.patch
index ad57dc5..25d8cf4 100644
--- a/third_party/googletest/googletest.patch
+++ b/third_party/googletest/googletest.patch
@@ -30,3 +30,17 @@
linkopts = select({
"//:qnx": [],
"//:windows": [],
+diff --git a/googletest/test/BUILD.bazel b/googletest/test/BUILD.bazel
+index 1890b6ff..9bd00bd2 100644
+--- a/googletest/test/BUILD.bazel
++++ b/googletest/test/BUILD.bazel
+@@ -151,6 +151,9 @@ cc_test(
+ name = "gtest_unittest",
+ size = "small",
+ srcs = ["gtest_unittest.cc"],
++ copts = [
++ "-Wno-unused-but-set-variable",
++ ],
+ shard_count = 2,
+ deps = ["//:gtest_main"],
+ )
diff --git a/third_party/pico-sdk/tools/pioasm/BUILD b/third_party/pico-sdk/tools/pioasm/BUILD
index 355834f..1a276c3 100644
--- a/third_party/pico-sdk/tools/pioasm/BUILD
+++ b/third_party/pico-sdk/tools/pioasm/BUILD
@@ -21,6 +21,7 @@
"-Wno-unused-parameter",
"-Wno-sign-compare",
"-fexceptions",
+ "-Wno-unused-but-set-variable",
],
includes = [
".",
diff --git a/third_party/rawrtc/usrsctp/BUILD b/third_party/rawrtc/usrsctp/BUILD
index 6d1fa69..2a4118f 100644
--- a/third_party/rawrtc/usrsctp/BUILD
+++ b/third_party/rawrtc/usrsctp/BUILD
@@ -50,6 +50,7 @@
] + compiler_select({
"clang": [
"-Wno-unused-but-set-variable",
+ "-Wno-deprecated-non-prototype",
],
"gcc": [
"-Wno-discarded-qualifiers",
diff --git a/tools/build_rules/autocxx.bzl b/tools/build_rules/autocxx.bzl
index d4b6be6..01a9197 100644
--- a/tools/build_rules/autocxx.bzl
+++ b/tools/build_rules/autocxx.bzl
@@ -138,6 +138,7 @@
gen_rs.add_all(["--outdir", out_rs_json.dirname])
gen_rs.add("--gen-rs-archive")
gen_rs.add("--gen-cpp")
+ #gen_rs.add("--auto-allowlist")
gen_rs.add_all(["--generate-exact", ctx.attr.sections_to_generate])
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.h b/y2014/control_loops/drivetrain/drivetrain_base.h
index a47ff5c..48143c3 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.h
+++ b/y2014/control_loops/drivetrain/drivetrain_base.h
@@ -6,8 +6,8 @@
namespace y2014 {
namespace control_loops {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace control_loops
} // namespace y2014
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.h b/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
index d41ff99..98cb31d 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -10,8 +10,8 @@
const double kDrivetrainEncoderRatio =
(17.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/;
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.h b/y2016/control_loops/drivetrain/drivetrain_base.h
index 4d5525a..0186a6c 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 56b10d7..0b12b14 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -42,11 +42,11 @@
// Define the following if we want to use a local www directory and feed in
// dummy data.
-//#define DASHBOARD_TESTING
+// #define DASHBOARD_TESTING
// Define the following if we want to read from the vision queue, which has
// caused problems in the past when auto aiming that still need to be addressed.
-//#define DASHBOARD_READ_VISION_QUEUE
+// #define DASHBOARD_READ_VISION_QUEUE
DataCollector::DataCollector(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.h b/y2017/control_loops/drivetrain/drivetrain_base.h
index 59ca4a9..c84934f 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.h
+++ b/y2017/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
index f7a958c..e917b2e 100644
--- a/y2017/vision/target_finder.cc
+++ b/y2017/vision/target_finder.cc
@@ -68,14 +68,12 @@
RangeImage t_img = Transpose(img);
int total = 0;
int split = 0;
- int count = t_img.mini();
for (const auto &row : t_img) {
if (row.size() == 1) {
total++;
} else if (row.size() == 2) {
split++;
}
- count++;
}
return (double)split / total;
}
diff --git a/y2018/control_loops/drivetrain/drivetrain_base.h b/y2018/control_loops/drivetrain/drivetrain_base.h
index a7ce43f..0592a25 100644
--- a/y2018/control_loops/drivetrain/drivetrain_base.h
+++ b/y2018/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.h b/y2019/control_loops/drivetrain/drivetrain_base.h
index 5cee1b5..3a8bd6b 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.h
+++ b/y2019/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.h b/y2020/control_loops/drivetrain/drivetrain_base.h
index c220088..e35c2af 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.h
+++ b/y2020/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2020/vision/sift/fast_gaussian_halide_generator.sh b/y2020/vision/sift/fast_gaussian_halide_generator.sh
index 793e56c..cf094b8 100755
--- a/y2020/vision/sift/fast_gaussian_halide_generator.sh
+++ b/y2020/vision/sift/fast_gaussian_halide_generator.sh
@@ -43,12 +43,12 @@
-isystem"${SYSROOT}/usr/include/c++/10" \
-isystem"${SYSROOT}/usr/include/${MULTIARCH}/c++/10" \
-isystem"${SYSROOT}/usr/include/c++/7/backward" \
- -isystem"${LLVM_TOOLCHAIN}/lib/clang/13.0.0/include" \
+ -isystem"${LLVM_TOOLCHAIN}/lib/clang/16/include" \
-isystem"${SYSROOT}/usr/include/${MULTIARCH}" \
-isystem"${SYSROOT}/usr/include" \
-isystem"${SYSROOT}/include" \
"--sysroot=${SYSROOT}" \
- -resource-dir "${LLVM_TOOLCHAIN}/lib/clang/13.0.0" \
+ -resource-dir "${LLVM_TOOLCHAIN}/lib/clang/16" \
-target "${TARGET}" \
-fuse-ld=lld \
-L"${LLVM_TOOLCHAIN}/lib" \
diff --git a/y2021_bot3/control_loops/drivetrain/drivetrain_base.h b/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
index 7250f93..f796d4e 100644
--- a/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.h b/y2022/control_loops/drivetrain/drivetrain_base.h
index 2983016..1f4cfe4 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.h
+++ b/y2022/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2022_bot3/control_loops/drivetrain/drivetrain_base.h b/y2022_bot3/control_loops/drivetrain/drivetrain_base.h
index cce197e..04c6c86 100644
--- a/y2022_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2022_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
index 74fd3aa..10cc438 100644
--- a/y2023/constants/7971.json
+++ b/y2023/constants/7971.json
@@ -4,7 +4,7 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-1_cam-23-01_ext_2023-02-19.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-08.json' %}
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.h b/y2023/control_loops/drivetrain/drivetrain_base.h
index 6404081..98f984e 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.h
+++ b/y2023/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index d5168c1..ef300e3 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -83,10 +83,10 @@
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
"//frc971/vision:target_mapper",
+ "//frc971/vision:visualize_robot",
"//third_party:opencv",
"//y2023/constants:constants_fbs",
"//y2023/constants:simulated_constants_sender",
- "@org_tuxfamily_eigen//:eigen",
],
)
@@ -203,6 +203,36 @@
)
cc_binary(
+ name = "calibrate_multi_cameras",
+ srcs = [
+ "calibrate_multi_cameras.cc",
+ ],
+ data = [
+ "//y2023:aos_config",
+ "//y2023/constants:constants.json",
+ "//y2023/vision:maps",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":aprilrobotics_lib",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/util:mcap_logger",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_mapper",
+ "//third_party:opencv",
+ "//y2023/constants:constants_fbs",
+ "//y2023/constants:simulated_constants_sender",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
name = "game_pieces_detector",
srcs = [
"game_pieces_main.cc",
diff --git a/y2023/vision/README.md b/y2023/vision/README.md
new file mode 100644
index 0000000..55d217f
--- /dev/null
+++ b/y2023/vision/README.md
@@ -0,0 +1,25 @@
+How to use the extrinsic calibration for camera-to-camera calibration
+
+This all assumes you have cameras that have been intrinsically
+calibrated, and that pi1 has a valid extrinsic calibration (from robot
+origin / IMU to the pi1 camera).
+
+It by default will compute the extrinsics for each of the other cameras (pi2,
+pi3, pi4) relative to the robot origin (IMU origin).
+
+Steps:
+* Place two Aruco Diamond markers about 1 meter apart
+ (center-to-center) at a height so that they will be in view of the
+ cameras when the robot is about 1-2m away
+* Start with the robot in a position that both markers are fully in
+ view by one camera
+* Enable logging for all cameras
+* Rotate the robot in place through a full circle, so that each camera sees both tags, and at times just one tag.
+* Stop the logging and copy the files to your laptop
+* Run the calibration code on the resulting files, e.g.,
+ * `bazel run -c opt //y2023/vision:calibrate_multi_cameras -- /data/PATH_TO_LOGS --team_number 971 --target_type charuco_diamond
+ * Can add "--visualize" flag to see the camera views and marker detections
+ * I've sometimes found it necessary to add the "--skip_missing_forwarding_entries" flag-- as guided by the logging messages
+* From the output, copy the calculated ("Updated") extrinsic into the
+ corresponding calibration file for the right robot and camera in the
+ calib_files directory
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json
deleted file mode 100644
index b175abb..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 7971, "intrinsics": [ 895.543945, 0.0, 645.250122, 0.0, 895.308838, 354.297241, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.482031, 0.00240341, 0.87615, 0.149719, -0.87612, -0.0100773, -0.481987, -0.000995038, 0.00767093, -0.999947, 0.00696306, 0.0062866, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.455658, 0.272167, 0.000796, -0.000206, -0.0975 ], "calibration_timestamp": 1358499769498335208, "camera_id": "23-02" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json
new file mode 100644
index 0000000..7f4e531
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 890.219788,
+ 0.0,
+ 647.802429,
+ 0.0,
+ 890.289368,
+ 365.760376,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.450615,
+ 0.25791,
+ 0.000498,
+ 0.000087,
+ -0.085233
+ ],
+"fixed_extrinsics": { "data": [ -0.482031, 0.00240341, 0.87615, 0.149719, -0.87612, -0.0100773, -0.481987, -0.000995038, 0.00767093, -0.999947, 0.00696306, 0.0062866, 0.0, 0.0, 0.0, 1.0 ] },
+ "calibration_timestamp": 1358499855721459233,
+ "camera_id": "23-15"
+}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json
deleted file mode 100644
index f7b554c..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 7971, "intrinsics": [ 892.089172, 0.0, 648.780701, 0.0, 892.362854, 342.340668, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.517483, 0.00906969, 0.855645, 0.156941, -0.855687, 0.00935422, 0.517409, 0.0833421, -0.00331132, -0.999915, 0.0126013, 0.0124048, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451178, 0.258187, 0.001071, 0.000017, -0.085526 ], "calibration_timestamp": 1358501967378322133, "camera_id": "23-03" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json b/y2023/vision/calib_files/calibration_pi-cam_23-02.json
similarity index 100%
rename from y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json
rename to y2023/vision/calib_files/calibration_pi-cam_23-02.json
diff --git a/y2023/vision/calibrate_multi_cameras.cc b/y2023/vision/calibrate_multi_cameras.cc
new file mode 100644
index 0000000..aaff033
--- /dev/null
+++ b/y2023/vision/calibrate_multi_cameras.cc
@@ -0,0 +1,456 @@
+#include <numeric>
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/mcap_logger.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/visualize_robot.h"
+// clang-format off
+// OpenCV eigen files must be included after Eigen includes
+#include "opencv2/aruco.hpp"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/features2d.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+// clang-format on
+#include "y2023/constants/simulated_constants_sender.h"
+#include "y2023/vision/aprilrobotics.h"
+#include "y2023/vision/vision_util.h"
+
+DEFINE_string(config, "",
+ "If set, override the log's config file with this one.");
+DEFINE_string(constants_path, "y2023/constants/constants.json",
+ "Path to the constant file");
+DEFINE_string(target_type, "charuco_diamond",
+ "Type of target being used [aruco, charuco, charuco_diamond]");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
+DEFINE_uint64(
+ wait_key, 1,
+ "Time in ms to wait between images (0 to wait indefinitely until click)");
+
+// Calibrate extrinsic relationship between cameras using two targets
+// seen jointly between cameras. Uses two types of information: 1)
+// when a single camera sees two targets, we estimate the pose between
+// targets, and 2) when two separate cameras each see a target, we can
+// use the pose between targets to estimate the pose between cameras.
+
+// We then create the extrinsics for the robot by starting with the
+// given extrinsic for camera 1 (between imu/robot and camera frames)
+// and then map each subsequent camera based on the data collected and
+// the extrinsic poses computed here.
+
+// TODO<Jim>: Should export a new extrinsic file for each camera
+// TODO<Jim>: Not currently using estimate from pi1->pi4-- should do full
+// estimation, and probably also include camera->imu extrinsics from all
+// cameras, not just pi1
+// TODO<Jim>: Should add ability to do this with apriltags, since they're on the
+// field
+
+namespace y2023 {
+namespace vision {
+using frc971::vision::DataAdapter;
+using frc971::vision::ImageCallback;
+using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
+using frc971::vision::TargetMapper;
+namespace calibration = frc971::vision::calibration;
+
+static constexpr double kImagePeriodMs =
+ 1.0 / 30.0 * 1000.0; // Image capture period in ms
+
+// Change reference frame from camera to robot
+Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
+ Eigen::Affine3d extrinsics) {
+ const Eigen::Affine3d H_robot_camera = extrinsics;
+ const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target;
+ return H_robot_target;
+}
+
+struct TimestampedPiDetection {
+ aos::distributed_clock::time_point time;
+ // Pose of target relative to robot
+ Eigen::Affine3d H_camera_target;
+ // name of pi
+ std::string pi_name;
+ int board_id;
+};
+
+TimestampedPiDetection last_observation;
+std::vector<std::pair<TimestampedPiDetection, TimestampedPiDetection>>
+ detection_list;
+std::vector<TimestampedPiDetection> two_board_extrinsics_list;
+
+// TODO<jim>: Implement variance calcs
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Vector3d> &translation_list,
+ std::vector<Eigen::Vector4d> &rotation_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr) {
+ Eigen::Vector3d avg_translation =
+ std::accumulate(translation_list.begin(), translation_list.end(),
+ Eigen::Vector3d{0, 0, 0}) /
+ translation_list.size();
+
+ // TODO<Jim>: Use QuaternionMean from quaternion_utils.cc (but this
+ // requires a fixed number of quaternions to be averaged
+ Eigen::Vector4d avg_rotation =
+ std::accumulate(rotation_list.begin(), rotation_list.end(),
+ Eigen::Vector4d{0, 0, 0, 0}) /
+ rotation_list.size();
+ // Normalize, so it's a valid quaternion
+ avg_rotation = avg_rotation / avg_rotation.norm();
+ Eigen::Quaterniond avg_rotation_q{avg_rotation[0], avg_rotation[1],
+ avg_rotation[2], avg_rotation[3]};
+ Eigen::Translation3d translation(avg_translation);
+ Eigen::Affine3d return_pose = translation * avg_rotation_q;
+ if (translation_variance != nullptr) {
+ *translation_variance = Eigen::Vector3d();
+ }
+ if (rotation_variance != nullptr) {
+ LOG(INFO) << *rotation_variance;
+ }
+
+ return return_pose;
+}
+
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Affine3d> &pose_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr) {
+ std::vector<Eigen::Vector3d> translation_list;
+ std::vector<Eigen::Vector4d> rotation_list;
+
+ for (Eigen::Affine3d pose : pose_list) {
+ translation_list.push_back(pose.translation());
+ Eigen::Quaterniond quat(pose.rotation().matrix());
+ rotation_list.push_back(
+ Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
+ }
+
+ return ComputeAveragePose(translation_list, rotation_list,
+ translation_variance, rotation_variance);
+}
+
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<TimestampedPiDetection> &pose_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr) {
+ std::vector<Eigen::Vector3d> translation_list;
+ std::vector<Eigen::Vector4d> rotation_list;
+
+ for (TimestampedPiDetection pose : pose_list) {
+ translation_list.push_back(pose.H_camera_target.translation());
+ Eigen::Quaterniond quat(pose.H_camera_target.rotation().matrix());
+ rotation_list.push_back(
+ Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
+ }
+ return ComputeAveragePose(translation_list, rotation_list,
+ translation_variance, rotation_variance);
+}
+
+void ProcessImage(aos::EventLoop *event_loop, cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof,
+ aos::distributed_clock::time_point distributed_eof,
+ frc971::vision::CharucoExtractor &charuco_extractor,
+ std::string pi_name) {
+ std::vector<cv::Vec4i> charuco_ids;
+ std::vector<std::vector<cv::Point2f>> charuco_corners;
+ bool valid = false;
+ std::vector<Eigen::Vector3d> rvecs_eigen;
+ std::vector<Eigen::Vector3d> tvecs_eigen;
+ charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(),
+ charuco_ids, charuco_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+
+ if (valid) {
+ if (tvecs_eigen.size() == 2) {
+ VLOG(2) << "Saw two boards in same view from " << pi_name;
+ // Handle when we see two boards at once
+ std::vector<TimestampedPiDetection> detections;
+ for (uint i = 0; i < tvecs_eigen.size(); i++) {
+ Eigen::Quaternion<double> rotation(
+ frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[i]));
+ Eigen::Translation3d translation(tvecs_eigen[i]);
+ Eigen::Affine3d H_camera_board = translation * rotation;
+ TimestampedPiDetection new_observation{
+ .time = distributed_eof,
+ .H_camera_target = H_camera_board,
+ .pi_name = pi_name,
+ .board_id = charuco_ids[i][0]};
+ detections.emplace_back(new_observation);
+ }
+ Eigen::Affine3d H_boardA_boardB =
+ detections[0].H_camera_target.inverse() *
+ detections[1].H_camera_target;
+
+ TimestampedPiDetection board_extrinsic{.time = distributed_eof,
+ .H_camera_target = H_boardA_boardB,
+ .pi_name = pi_name,
+ .board_id = charuco_ids[0][0]};
+
+ // Make sure we've got them in the right order (sorted by id)
+ if (detections[0].board_id < detections[1].board_id) {
+ two_board_extrinsics_list.push_back(board_extrinsic);
+ } else {
+ // Flip them around
+ board_extrinsic.H_camera_target =
+ board_extrinsic.H_camera_target.inverse();
+ board_extrinsic.board_id = charuco_ids[1][0];
+ two_board_extrinsics_list.push_back(board_extrinsic);
+ }
+ } else {
+ VLOG(1) << "Saw single board in camera " << pi_name;
+ Eigen::Quaternion<double> rotation(
+ frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[0]));
+ Eigen::Translation3d translation(tvecs_eigen[0]);
+ Eigen::Affine3d H_camera_board = translation * rotation;
+ TimestampedPiDetection new_observation{.time = distributed_eof,
+ .H_camera_target = H_camera_board,
+ .pi_name = pi_name,
+ .board_id = charuco_ids[0][0]};
+
+ VLOG(2) << "Checking versus last result from " << last_observation.pi_name
+ << " at time " << last_observation.time << " with delta time = "
+ << std::abs((distributed_eof - last_observation.time).count());
+ // Only take two observations if they're near enough to each other
+ // in time. This should be within +/- kImagePeriodMs of each other (e.g.,
+ // +/-16.666ms for 30 Hz capture). This should make sure
+ // we're always getting the closest images, and not miss too many possible
+ // pairs, between cameras
+ // TODO<Jim>: Should also check that (rotational) velocity of the robot is
+ // small
+ if (std::abs((distributed_eof - last_observation.time).count()) <
+ static_cast<int>(kImagePeriodMs / 2.0 * 1000000)) {
+ Eigen::Affine3d H_camera1_board = last_observation.H_camera_target;
+ Eigen::Affine3d H_camera1_camera2 =
+ H_camera1_board * H_camera_board.inverse();
+ VLOG(1) << "Storing observation between " << last_observation.pi_name
+ << ", target " << last_observation.board_id << " and "
+ << new_observation.pi_name << ", target "
+ << new_observation.board_id << " is "
+ << H_camera1_camera2.matrix();
+ // Sort by pi numbering
+ if (last_observation.pi_name < new_observation.pi_name) {
+ detection_list.push_back(
+ std::pair(last_observation, new_observation));
+ } else if (last_observation.pi_name > new_observation.pi_name) {
+ detection_list.push_back(
+ std::pair(new_observation, last_observation));
+ } else {
+ LOG(WARNING) << "Got 2 observations in a row from same pi. Probably "
+ "not too much of an issue???";
+ }
+ } else {
+ VLOG(2) << "Storing observation for " << pi_name << " at time "
+ << distributed_eof;
+ last_observation = new_observation;
+ }
+ }
+ }
+ if (FLAGS_visualize) {
+ std::string image_name = pi_name + " Image";
+ cv::Mat rgb_small;
+ cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5);
+ cv::imshow(image_name, rgb_small);
+ cv::waitKey(FLAGS_wait_key);
+ }
+}
+
+void ExtrinsicsMain(int argc, char *argv[]) {
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
+
+ std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config =
+ (FLAGS_config.empty()
+ ? std::nullopt
+ : std::make_optional(aos::configuration::ReadConfig(FLAGS_config)));
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)),
+ config.has_value() ? &config->message() : nullptr);
+
+ constexpr size_t kNumPis = 4;
+ for (size_t i = 1; i <= kNumPis; i++) {
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "frc971.vision.TargetMap");
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "foxglove.ImageAnnotations");
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
+ "y2023.Constants");
+ }
+ reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
+ reader.Register();
+
+ SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
+ FLAGS_constants_path);
+
+ LOG(INFO) << "Using target type " << FLAGS_target_type;
+ std::vector<std::string> node_list;
+ node_list.push_back("pi1");
+ node_list.push_back("pi2");
+ node_list.push_back("pi3");
+ node_list.push_back("pi4");
+
+ std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
+ std::vector<frc971::vision::CharucoExtractor *> charuco_extractors;
+ std::vector<frc971::vision::ImageCallback *> image_callbacks;
+ std::vector<Eigen::Affine3d> default_extrinsics;
+
+ for (uint i = 0; i < node_list.size(); i++) {
+ std::string node = node_list[i];
+ const aos::Node *pi =
+ aos::configuration::GetNode(reader.configuration(), node.c_str());
+
+ detection_event_loops.emplace_back(
+ reader.event_loop_factory()->MakeEventLoop(
+ (node + "_detection").c_str(), pi));
+
+ frc971::constants::ConstantsFetcher<y2023::Constants> constants_fetcher(
+ detection_event_loops.back().get());
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> intrinsics =
+ aos::RecursiveCopyFlatBuffer(y2023::vision::FindCameraCalibration(
+ constants_fetcher.constants(), node));
+
+ const calibration::CameraCalibration *calibration =
+ FindCameraCalibration(constants_fetcher.constants(), node);
+
+ frc971::vision::TargetType target_type =
+ frc971::vision::TargetTypeFromString(FLAGS_target_type);
+ frc971::vision::CharucoExtractor *charuco_ext =
+ new frc971::vision::CharucoExtractor(calibration, target_type);
+ charuco_extractors.emplace_back(charuco_ext);
+
+ cv::Mat extrinsics_cv = CameraExtrinsics(calibration).value();
+ Eigen::Matrix4d extrinsics_matrix;
+ cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
+ const auto ext_H_robot_pi = Eigen::Affine3d(extrinsics_matrix);
+ default_extrinsics.emplace_back(ext_H_robot_pi);
+
+ LOG(INFO) << "Got extrinsics for " << node << " as\n"
+ << default_extrinsics.back().matrix();
+
+ frc971::vision::ImageCallback *image_callback =
+ new frc971::vision::ImageCallback(
+ detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
+ [&reader, &charuco_extractors, &detection_event_loops, &node_list,
+ i](cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(
+ detection_event_loops[i].get()->node())
+ ->ToDistributedClock(eof);
+ ProcessImage(detection_event_loops[i].get(), rgb_image, eof,
+ pi_distributed_time, *charuco_extractors[i],
+ node_list[i]);
+ });
+
+ image_callbacks.emplace_back(image_callback);
+ }
+
+ const auto ext_H_robot_piA = default_extrinsics[0];
+ const auto ext_H_robot_piB = default_extrinsics[1];
+
+ reader.event_loop_factory()->Run();
+
+ for (auto node : node_list) {
+ std::vector<TimestampedPiDetection> pose_list;
+ for (auto ext : two_board_extrinsics_list) {
+ if (ext.pi_name == node) {
+ pose_list.push_back(ext);
+ }
+ }
+ Eigen::Affine3d avg_pose = ComputeAveragePose(pose_list);
+ VLOG(1) << "Estimate from " << node << " with " << pose_list.size()
+ << " observations is:\n"
+ << avg_pose.matrix();
+ }
+ Eigen::Affine3d avg_pose = ComputeAveragePose(two_board_extrinsics_list);
+ LOG(INFO) << "Estimate of two board pose using all nodes with "
+ << two_board_extrinsics_list.size() << " observations is:\n"
+ << avg_pose.matrix() << "\n";
+
+ LOG(INFO) << "Got " << detection_list.size() << " extrinsic observations";
+ Eigen::Affine3d H_target0_target1 = avg_pose;
+ int base_target_id = detection_list[0].first.board_id;
+ std::vector<Eigen::Affine3d> H_cameraA_cameraB_list;
+ std::vector<Eigen::Affine3d> updated_extrinsics;
+ // Use the first node's extrinsics as our base, and fix from there
+ updated_extrinsics.push_back(default_extrinsics[0]);
+ LOG(INFO) << "Default extrinsic for node " << node_list[0] << " is "
+ << default_extrinsics[0].matrix();
+ for (uint i = 0; i < node_list.size() - 1; i++) {
+ H_cameraA_cameraB_list.clear();
+ for (auto [poseA, poseB] : detection_list) {
+ if ((poseA.pi_name == node_list[i]) &&
+ (poseB.pi_name == node_list[i + 1])) {
+ Eigen::Affine3d H_cameraA_target0 = poseA.H_camera_target;
+ // If this pose isn't referenced to target0, correct that
+ if (poseA.board_id != base_target_id) {
+ H_cameraA_target0 = H_cameraA_target0 * H_target0_target1.inverse();
+ }
+
+ Eigen::Affine3d H_cameraB_target0 = poseB.H_camera_target;
+ if (poseB.board_id != base_target_id) {
+ H_cameraB_target0 = H_cameraB_target0 * H_target0_target1.inverse();
+ }
+ Eigen::Affine3d H_cameraA_cameraB =
+ H_cameraA_target0 * H_cameraB_target0.inverse();
+ H_cameraA_cameraB_list.push_back(H_cameraA_cameraB);
+ }
+ }
+ CHECK(H_cameraA_cameraB_list.size() > 0)
+ << "Failed with zero poses for node " << node_list[i];
+ if (H_cameraA_cameraB_list.size() > 0) {
+ Eigen::Affine3d avg_H_cameraA_cameraB =
+ ComputeAveragePose(H_cameraA_cameraB_list);
+ LOG(INFO) << "From " << node_list[i] << " to " << node_list[i + 1]
+ << " found " << H_cameraA_cameraB_list.size()
+ << " observations, and the average pose is:\n"
+ << avg_H_cameraA_cameraB.matrix();
+ Eigen::Affine3d default_H_cameraA_camera_B =
+ default_extrinsics[i].inverse() * default_extrinsics[i + 1];
+ LOG(INFO) << "Compare this to that from default values:\n"
+ << default_H_cameraA_camera_B.matrix();
+ // Next extrinsic is just previous one * avg_delta_pose
+ Eigen::Affine3d next_extrinsic =
+ updated_extrinsics.back() * avg_H_cameraA_cameraB;
+ LOG(INFO)
+ << "Difference between averaged and default delta poses has |T| = "
+ << (avg_H_cameraA_cameraB * default_H_cameraA_camera_B.inverse())
+ .translation()
+ .norm()
+ << " and |R| = "
+ << Eigen::AngleAxisd(
+ (avg_H_cameraA_cameraB * default_H_cameraA_camera_B.inverse())
+ .rotation())
+ .angle();
+ updated_extrinsics.push_back(next_extrinsic);
+ LOG(INFO) << "Default Extrinsic for " << node_list[i + 1] << " is \n"
+ << default_extrinsics[i + 1].matrix();
+ LOG(INFO) << "--> Updated Extrinsic for " << node_list[i + 1] << " is \n"
+ << next_extrinsic.matrix();
+ }
+ }
+
+ // Cleanup
+ for (uint i = 0; i < image_callbacks.size(); i++) {
+ delete charuco_extractors[i];
+ delete image_callbacks[i];
+ }
+}
+} // namespace vision
+} // namespace y2023
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::ExtrinsicsMain(argc, argv);
+}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 2ca6f66..e4099b3 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -20,38 +20,39 @@
#include "y2023/vision/aprilrobotics.h"
#include "y2023/vision/vision_util.h"
-DEFINE_string(json_path, "y2023/vision/maps/target_map.json",
- "Specify path for json with initial pose guesses.");
DEFINE_string(config, "",
"If set, override the log's config file with this one.");
DEFINE_string(constants_path, "y2023/constants/constants.json",
"Path to the constant file");
-DEFINE_string(output_dir, "y2023/vision/maps",
- "Directory to write solved target map to");
+DEFINE_string(dump_constraints_to, "/tmp/mapping_constraints.txt",
+ "Write the target constraints to this path");
+DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
+ "Write the mapping stats to this path");
DEFINE_string(field_name, "charged_up",
"Field name, for the output json filename and flatbuffer field");
-DEFINE_int32(team_number, 0,
- "Required: Use the calibration for a node with this team number");
-DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
-DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
+DEFINE_string(json_path, "y2023/vision/maps/target_map.json",
+ "Specify path for json with initial pose guesses.");
DEFINE_double(max_pose_error, 1e-6,
"Throw out target poses with a higher pose error than this");
DEFINE_double(
max_pose_error_ratio, 0.4,
"Throw out target poses with a higher pose error ratio than this");
-DEFINE_uint64(wait_key, 0,
- "Time in ms to wait between images, if no click (0 to wait "
- "indefinitely until click).");
+DEFINE_string(mcap_output_path, "", "Log to output.");
+DEFINE_string(output_dir, "y2023/vision/maps",
+ "Directory to write solved target map to");
DEFINE_double(pause_on_distance, 1.0,
"Pause if two consecutive implied robot positions differ by more "
"than this many meters");
+DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
DEFINE_uint64(skip_to, 1,
"Start at combined image of this number (1 is the first image)");
DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
-DEFINE_string(dump_constraints_to, "/tmp/mapping_constraints.txt",
- "Write the target constraints to this path");
-DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
- "Write the target constraints to this path");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
+DEFINE_uint64(wait_key, 1,
+ "Time in ms to wait between images, if no click (0 to wait "
+ "indefinitely until click).");
+
DECLARE_int32(frozen_target_id);
DECLARE_int32(min_target_id);
DECLARE_int32(max_target_id);
@@ -73,7 +74,6 @@
class TargetMapperReplay {
public:
TargetMapperReplay(aos::logger::LogReader *reader);
- ~TargetMapperReplay();
// Solves for the target poses with the accumulated detections if FLAGS_solve.
void MaybeSolve();
@@ -102,7 +102,6 @@
aos::logger::LogReader *reader_;
// April tag detections from all pis
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections_;
- std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors_;
VisualizeRobot vis_robot_;
// Set of node names which are currently drawn on the display
@@ -146,7 +145,6 @@
TargetMapperReplay::TargetMapperReplay(aos::logger::LogReader *reader)
: reader_(reader),
timestamped_target_detections_(),
- detectors_(),
vis_robot_(cv::Size(1280, 1000)),
drawn_nodes_(),
display_count_(0),
@@ -202,13 +200,6 @@
}
}
-TargetMapperReplay::~TargetMapperReplay() {
- // Clean up all the pointers
- for (auto &detector_ptr : detectors_) {
- detector_ptr.reset();
- }
-}
-
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
void TargetMapperReplay::HandleAprilTags(
@@ -268,8 +259,8 @@
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
if (FLAGS_visualize_solver) {
- // If we've already drawn in the current image,
- // display it before clearing and adding the new poses
+ // If we've already drawn this node_name in the current image,
+ // display the image before clearing and adding the new poses
if (drawn_nodes_.count(node_name) != 0) {
display_count_++;
cv::putText(vis_robot_.image_,
@@ -278,10 +269,20 @@
cv::Scalar(255, 255, 255));
if (display_count_ >= FLAGS_skip_to) {
+ VLOG(1) << "Showing image for node " << node_name
+ << " since we've drawn it already";
cv::imshow("View", vis_robot_.image_);
- cv::waitKey(max_delta_T_world_robot_ > FLAGS_pause_on_distance
- ? 0
- : FLAGS_wait_key);
+ // Pause if delta_T is too large, but only after first image (to make
+ // sure the delta's are correct
+ if (max_delta_T_world_robot_ > FLAGS_pause_on_distance &&
+ display_count_ > 1) {
+ LOG(INFO) << "Pausing since the delta between robot estimates is "
+ << max_delta_T_world_robot_ << " which is > threshold of "
+ << FLAGS_pause_on_distance;
+ cv::waitKey(0);
+ } else {
+ cv::waitKey(FLAGS_wait_key);
+ }
max_delta_T_world_robot_ = 0.0;
} else {
VLOG(1) << "At poses #" << std::to_string(display_count_);
@@ -293,21 +294,19 @@
Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
kFixedTargetMapper.GetTargetPoseById(target_pose_fbs->id())->pose);
Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
- VLOG(2) << node_name << ", " << target_pose_fbs->id()
+ VLOG(2) << node_name << ", id " << target_pose_fbs->id()
<< ", t = " << pi_distributed_time
<< ", pose_error = " << target_pose_fbs->pose_error()
<< ", pose_error_ratio = " << target_pose_fbs->pose_error_ratio()
- << ", robot_pos (x,y,z) + "
+ << ", robot_pos (x,y,z) = "
<< H_world_robot.translation().transpose();
- label << "id " << target_pose_fbs->id() << ": err "
+ label << "id " << target_pose_fbs->id() << ": err (% of max): "
<< (target_pose_fbs->pose_error() / FLAGS_max_pose_error)
- << " ratio " << target_pose_fbs->pose_error_ratio() << " ";
+ << " err_ratio: " << target_pose_fbs->pose_error_ratio() << " ";
- vis_robot_.DrawRobotOutline(H_world_robot,
- std::to_string(target_pose_fbs->id()),
+ vis_robot_.DrawRobotOutline(H_world_robot, node_name,
kPiColors.at(node_name));
-
vis_robot_.DrawFrameAxes(H_world_target,
std::to_string(target_pose_fbs->id()),
kPiColors.at(node_name));
@@ -318,27 +317,37 @@
max_delta_T_world_robot_ =
std::max(delta_T_world_robot, max_delta_T_world_robot_);
+ VLOG(1) << "Drew in info for robot " << node_name << " and target #"
+ << target_pose_fbs->id();
drew = true;
last_draw_time_ = pi_distributed_time;
last_H_world_robot_ = H_world_robot;
}
}
- if (drew) {
- size_t pi_number =
- static_cast<size_t>(node_name[node_name.size() - 1] - '0');
- cv::putText(vis_robot_.image_, label.str(),
- cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN, 1.0,
- kPiColors.at(node_name));
- drawn_nodes_.emplace(node_name);
- } else if (pi_distributed_time - last_draw_time_ >
- std::chrono::milliseconds(30) &&
- display_count_ >= FLAGS_skip_to) {
- // Clear the image if we haven't draw in a while
- vis_robot_.ClearImage();
- cv::imshow("View", vis_robot_.image_);
- cv::waitKey(FLAGS_wait_key);
- max_delta_T_world_robot_ = 0.0;
+ if (FLAGS_visualize_solver) {
+ if (drew) {
+ // Collect all the labels from a given node, and add the text
+ size_t pi_number =
+ static_cast<size_t>(node_name[node_name.size() - 1] - '0');
+ cv::putText(vis_robot_.image_, label.str(),
+ cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN,
+ 1.0, kPiColors.at(node_name));
+
+ drawn_nodes_.emplace(node_name);
+ } else if (pi_distributed_time - last_draw_time_ >
+ std::chrono::milliseconds(30) &&
+ display_count_ >= FLAGS_skip_to) {
+ cv::putText(vis_robot_.image_, "No detections", cv::Point(10, 0),
+ cv::FONT_HERSHEY_PLAIN, 1.0, kPiColors.at(node_name));
+ // Display and clear the image if we haven't draw in a while
+ VLOG(1) << "Displaying image due to time lapse";
+ cv::imshow("View", vis_robot_.image_);
+ cv::waitKey(FLAGS_wait_key);
+ vis_robot_.ClearImage();
+ max_delta_T_world_robot_ = 0.0;
+ drawn_nodes_.clear();
+ }
}
}
@@ -385,6 +394,8 @@
}),
target_constraints.end());
+ LOG(INFO) << "Solving for locations of tags with "
+ << target_constraints.size() << " constraints";
TargetMapper mapper(FLAGS_json_path, target_constraints);
mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index a741f3c..500b836 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -39,6 +39,12 @@
static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
+ // timeout to ensure code doesn't get stuck after releasing the "intake"
+ // button
+ static constexpr std::chrono::milliseconds kExtraIntakingTime() {
+ return std::chrono::milliseconds{100};
+ }
+
static double DrivetrainEncoderToMeters(int32_t in) {
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
diff --git a/y2023_bot3/control_loops/drivetrain/drivetrain_base.h b/y2023_bot3/control_loops/drivetrain/drivetrain_base.h
index 0c557c8..6922ea6 100644
--- a/y2023_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2023_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2023_bot3/control_loops/python/drivetrain.py b/y2023_bot3/control_loops/python/drivetrain.py
index 3d4469b..26cb043 100644
--- a/y2023_bot3/control_loops/python/drivetrain.py
+++ b/y2023_bot3/control_loops/python/drivetrain.py
@@ -14,14 +14,14 @@
# TODO(max): Change constants based on robot / CAD
kDrivetrain = drivetrain.DrivetrainParams(
- J=6.5,
- mass=68.0,
+ J=2.2,
+ mass=44.7,
# TODO(austin): Measure radius a bit better.
- robot_radius=0.39,
+ robot_radius=0.25,
wheel_radius=2.5 * 0.0254,
motor_type=control_loop.Falcon(),
- num_motors=3,
- G=(14.0 / 52.0) * (26.0 / 58.0),
+ num_motors=2,
+ G=(14.0 / 68.0) * (30.0 / 54.0),
q_pos=0.24,
q_vel=2.5,
efficiency=0.92,
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index e6a3082..0330182 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -54,12 +54,31 @@
gen_reflections = 1,
deps = [
"//frc971:can_configuration_fbs",
+ "//frc971/control_loops:can_falcon_fbs",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
],
)
cc_library(
+ name = "end_effector",
+ srcs = [
+ "end_effector.cc",
+ ],
+ hdrs = [
+ "end_effector.h",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:control_loop",
+ "//y2023_bot3:constants",
+ ],
+)
+
+cc_library(
name = "superstructure_lib",
srcs = [
"superstructure.cc",
@@ -70,6 +89,7 @@
data = [
],
deps = [
+ ":end_effector",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.cc b/y2023_bot3/control_loops/superstructure/end_effector.cc
new file mode 100644
index 0000000..a98d06f
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/end_effector.cc
@@ -0,0 +1,88 @@
+#include "y2023_bot3/control_loops/superstructure/end_effector.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+
+namespace y2023_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+
+EndEffector::EndEffector()
+ : state_(EndEffectorState::IDLE), timer_(aos::monotonic_clock::min_time) {}
+
+void EndEffector::RunIteration(
+ const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
+ bool beambreak_status, double *roller_voltage, bool preloaded_with_cube) {
+ *roller_voltage = 0.0;
+
+ if (roller_goal == RollerGoal::SPIT) {
+ state_ = EndEffectorState::SPITTING;
+ }
+
+ if (roller_goal == RollerGoal::SPIT_MID) {
+ state_ = EndEffectorState::SPITTING_MID;
+ }
+
+ // If we started off preloaded, skip to the loaded state.
+ // Make sure we weren't already there just in case.
+ if (preloaded_with_cube) {
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ case EndEffectorState::INTAKING:
+ state_ = EndEffectorState::LOADED;
+ break;
+ case EndEffectorState::LOADED:
+ break;
+ case EndEffectorState::SPITTING:
+ break;
+ case EndEffectorState::SPITTING_MID:
+ break;
+ }
+ }
+
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ // If idle and intake requested, intake
+ if (roller_goal == RollerGoal::INTAKE_CUBE) {
+ state_ = EndEffectorState::INTAKING;
+ timer_ = timestamp;
+ }
+ break;
+ case EndEffectorState::INTAKING:
+ // If intaking and beam break is not triggered, keep intaking
+ if (roller_goal == RollerGoal::INTAKE_CUBE) {
+ timer_ = timestamp;
+ }
+
+ if (beambreak_status) {
+ // Beam has been broken, switch to loaded.
+ state_ = EndEffectorState::LOADED;
+ break;
+ } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
+ // Intaking failed, waited 1 second with no beambreak
+ state_ = EndEffectorState::IDLE;
+ break;
+ }
+
+ *roller_voltage = kRollerCubeSuckVoltage();
+
+ break;
+ case EndEffectorState::LOADED:
+ timer_ = timestamp;
+ break;
+ case EndEffectorState::SPITTING:
+ *roller_voltage = kRollerCubeSpitVoltage();
+ break;
+ case EndEffectorState::SPITTING_MID:
+ *roller_voltage = kRollerCubeSpitMidVoltage();
+ }
+}
+
+void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3
\ No newline at end of file
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.h b/y2023_bot3/control_loops/superstructure/end_effector.h
new file mode 100644
index 0000000..ab4e117
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/end_effector.h
@@ -0,0 +1,39 @@
+#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "y2023_bot3/constants.h"
+#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2023_bot3 {
+namespace control_loops {
+namespace superstructure {
+
+class EndEffector {
+ public:
+ static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
+ static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
+ static constexpr double kRollerCubeSpitMidVoltage() { return 5.0; }
+
+ EndEffector();
+ void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
+ RollerGoal roller_goal, bool beambreak_status,
+ double *intake_roller_voltage, bool preloaded_with_cube);
+ EndEffectorState state() const { return state_; }
+ void Reset();
+
+ private:
+ EndEffectorState state_;
+
+ // variable which records the last time at which "intake" button was pressed
+ aos::monotonic_clock::time_point timer_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index 204bb2c..4b3d488 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023_bot3/constants.h"
#include "y2023_bot3/constants/constants_generated.h"
+#include "y2023_bot3/control_loops/superstructure/end_effector.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_position_generated.h"
@@ -27,6 +28,8 @@
double robot_velocity() const;
+ inline const EndEffector &end_effector() const { return end_effector_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -39,6 +42,8 @@
std::shared_ptr<const constants::Values> values_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ EndEffector end_effector_;
+
aos::Alliance alliance_ = aos::Alliance::kInvalid;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
index bcdf9ff..27a302c 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -2,8 +2,27 @@
namespace y2023_bot3.control_loops.superstructure;
-table Goal {
+enum PivotGoal: ubyte {
+ NEUTRAL = 0,
+ PICKUP_FRONT = 1,
+ PICKUP_BACK = 2,
+ SCORE_LOW_FRONT = 3,
+ SCORE_LOW_BACK = 4,
+ SCORE_MID_FRONT = 5,
+ SCORE_MID_BACK = 6,
+}
+enum RollerGoal: ubyte {
+ IDLE = 0,
+ INTAKE_CUBE = 1,
+ SPIT = 2,
+ SPIT_MID = 3,
+}
+
+table Goal {
+ pivot_goal:PivotGoal (id: 0);
+ roller_goal:RollerGoal (id: 1);
+ preloaded_with_cube:bool (id: 2);
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
index 98dbcf7..652d247 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -1,6 +1,10 @@
namespace y2023_bot3.control_loops.superstructure;
table Output {
+ // Pivot joint voltage
+ pivot_joint_voltage:double (id: 0);
+ // Roller voltage on the end effector (positive is spitting, negative is sucking)
+ roller_voltage:double (id: 1);
}
root_type Output;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
index dc6282f..a629e56 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -1,9 +1,18 @@
include "frc971/control_loops/control_loops.fbs";
include "frc971/can_configuration.fbs";
+include "frc971/control_loops/can_falcon.fbs";
namespace y2023_bot3.control_loops.superstructure;
table Position {
+ // The position of the pivot joint in radians
+ pivot_joint_position:frc971.PotAndAbsolutePosition (id: 0);
+
+ // If this is true, the cube beam break is triggered.
+ end_effector_cube_beam_break:bool (id: 1);
+
+ // Roller falcon data
+ roller_falcon:frc971.control_loops.CANFalcon (id: 2);
}
root_type Position;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index b36e0da..f3df83c 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -3,12 +3,32 @@
namespace y2023_bot3.control_loops.superstructure;
+enum EndEffectorState : ubyte {
+ // Not doing anything
+ IDLE = 0,
+ // Intaking the game object into the end effector
+ INTAKING = 1,
+ // The game object is loaded into the end effector
+ LOADED = 2,
+ // Waiting for the arm to be at shooting goal and then telling the
+ // end effector to spit
+ SPITTING = 3,
+ // Waiting for the arm to be at MID shooting goal and then telling the
+ // end effector to spit mid
+ SPITTING_MID = 4
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
+
+ // The current state of the arm.
+ pivot_joint_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
+
+ end_effector_state:EndEffectorState (id: 3);
}
root_type Status;
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index c1acc5f..2b5a9f4 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -257,16 +257,19 @@
void set_falcons(std::shared_ptr<Falcon> right_front,
std::shared_ptr<Falcon> right_back,
- std::shared_ptr<Falcon> right_under,
std::shared_ptr<Falcon> left_front,
std::shared_ptr<Falcon> left_back,
- std::shared_ptr<Falcon> left_under) {
+ std::shared_ptr<Falcon> roller_falcon) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
- right_under_ = std::move(right_under);
left_front_ = std::move(left_front);
left_back_ = std::move(left_back);
- left_under_ = std::move(left_under);
+ roller_falcon = std::move(roller_falcon);
+ }
+
+ std::optional<frc971::control_loops::CANFalconT> roller_falcon_data() {
+ std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
+ return roller_falcon_data_;
}
private:
@@ -281,8 +284,7 @@
auto builder = can_position_sender_.MakeBuilder();
- for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_}) {
+ for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
falcon->RefreshNontimesyncedSignals();
}
@@ -290,8 +292,7 @@
kCANFalconCount>
falcons;
- for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_}) {
+ for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
falcons.push_back(falcon->WritePosition(builder.fbb()));
}
@@ -318,8 +319,12 @@
aos::Sender<frc971::control_loops::drivetrain::CANPosition>
can_position_sender_;
- std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_;
+ std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_,
+ roller_falcon;
+
+ std::optional<frc971::control_loops::CANFalconT> roller_falcon_data_;
+
+ aos::stl_mutex roller_mutex_;
// Pointer to the timer handler used to modify the wakeup.
::aos::TimerHandler *timer_handler_;
@@ -366,6 +371,27 @@
void RunIteration() override {
superstructure_reading_->Set(true);
+ {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::control_loops::CANFalcon>
+ roller_falcon_offset;
+ auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
+ if (optional_roller_falcon.has_value()) {
+ roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
+ *builder.fbb(), &optional_roller_falcon.value());
+ }
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+ position_builder.add_end_effector_cube_beam_break(
+ end_effector_cube_beam_break_->Get());
+
+ if (!roller_falcon_offset.IsNull()) {
+ position_builder.add_roller_falcon(roller_falcon_offset);
+ }
+ builder.CheckOk(builder.Send(position_builder.Finish()));
+ }
{
auto builder = drivetrain_position_sender_.MakeBuilder();
@@ -453,7 +479,8 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_,
+ end_effector_cube_beam_break_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
@@ -546,16 +573,12 @@
void set_falcons(std::shared_ptr<Falcon> right_front,
std::shared_ptr<Falcon> right_back,
- std::shared_ptr<Falcon> right_under,
std::shared_ptr<Falcon> left_front,
- std::shared_ptr<Falcon> left_back,
- std::shared_ptr<Falcon> left_under) {
+ std::shared_ptr<Falcon> left_back) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
- right_under_ = std::move(right_under);
left_front_ = std::move(left_front);
left_back_ = std::move(left_back);
- left_under_ = std::move(left_under);
}
void set_right_inverted(ctre::phoenix6::signals::InvertedValue invert) {
@@ -567,8 +590,7 @@
}
void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
- for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_}) {
+ for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
falcon->PrintConfigs();
}
if (configuration.reapply()) {
@@ -578,13 +600,11 @@
private:
void WriteConfigs() {
- for (auto falcon :
- {right_front_.get(), right_back_.get(), right_under_.get()}) {
+ for (auto falcon : {right_front_.get(), right_back_.get()}) {
falcon->WriteConfigs(right_inverted_);
}
- for (auto falcon :
- {left_front_.get(), left_back_.get(), left_under_.get()}) {
+ for (auto falcon : {left_front_.get(), left_back_.get()}) {
falcon->WriteConfigs(left_inverted_);
}
}
@@ -601,8 +621,7 @@
right_control.UpdateFreqHz = 0_Hz;
right_control.EnableFOC = true;
- for (auto falcon :
- {left_front_.get(), left_back_.get(), left_under_.get()}) {
+ for (auto falcon : {left_front_.get(), left_back_.get()}) {
ctre::phoenix::StatusCode status =
falcon->talon()->SetControl(left_control);
@@ -612,8 +631,7 @@
}
}
- for (auto falcon :
- {right_front_.get(), right_back_.get(), right_under_.get()}) {
+ for (auto falcon : {right_front_.get(), right_back_.get()}) {
ctre::phoenix::StatusCode status =
falcon->talon()->SetControl(right_control);
@@ -630,9 +648,8 @@
stop_command.UpdateFreqHz = 0_Hz;
stop_command.EnableFOC = true;
- for (auto falcon :
- {right_front_.get(), right_back_.get(), right_under_.get(),
- left_front_.get(), left_back_.get(), left_under_.get()}) {
+ for (auto falcon : {right_front_.get(), right_back_.get(),
+ left_front_.get(), left_back_.get()}) {
falcon->talon()->SetControl(stop_command);
}
}
@@ -642,8 +659,7 @@
}
ctre::phoenix6::signals::InvertedValue left_inverted_, right_inverted_;
- std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_;
+ std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -679,14 +695,12 @@
std::make_shared<Falcon>(1, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> right_back =
std::make_shared<Falcon>(2, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> right_under =
- std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> left_front =
std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> left_back =
std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> left_under =
- std::make_shared<Falcon>(6, "Drivetrain Bus", &signals_registry);
+ std::shared_ptr<Falcon> roller_falcon =
+ std::make_shared<Falcon>(13, "Drivetrain Bus", &signals_registry);
// Thread 3.
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
@@ -694,8 +708,8 @@
CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop,
std::move(signals_registry));
- can_sensor_reader.set_falcons(right_front, right_back, right_under,
- left_front, left_back, left_under);
+ can_sensor_reader.set_falcons(right_front, right_back, left_front,
+ left_back, roller_falcon);
AddLoop(&can_sensor_reader_event_loop);
@@ -727,8 +741,8 @@
can_output_event_loop.set_name("CANOutputWriter");
DrivetrainWriter drivetrain_writer(&can_output_event_loop);
- drivetrain_writer.set_falcons(right_front, right_back, right_under,
- left_front, left_back, left_under);
+ drivetrain_writer.set_falcons(right_front, right_back, left_front,
+ left_back);
drivetrain_writer.set_right_inverted(
ctre::phoenix6::signals::InvertedValue::Clockwise_Positive);
drivetrain_writer.set_left_inverted(