Compare commits

..

1 Commits

Author SHA1 Message Date
Oleg Kalachev
ab3f5e6c69 docs: remove unwanted code blocks indentation in network article 2019-07-08 19:58:47 +03:00
73 changed files with 1144 additions and 1519 deletions

View File

@@ -13,32 +13,17 @@
"MD040": false,
"MD044": {
"names": [
"COEX",
"Copter Express",
"Коптер Экспресс",
"Клевер",
"MAVLink",
"ROS",
"ROS Kinetic",
"OpenCV",
"Gazebo",
"GitHub",
"FPV",
"PPM",
"PWM",
"Python",
"C++",
"JavaScript",
"Node.js",
"Django",
"Flask",
"HTTP",
"HTTPS",
"WebSocket",
"RPC",
"PX4",
"ArduPilot",
"jMAVSim",
"px4.io",
"logs.px4.io",
"QGroundControl",
@@ -46,23 +31,16 @@
"WireShark",
"FlightPlot",
"OFFBOARD",
"ACRO",
"RPY",
"LPE",
"EKF2",
"IMU",
"VPE",
"SITL",
"PID",
"Wi-Fi",
"Raspberry Pi",
"RPi",
"Linux",
"GNU",
"GNU/Linux",
"Windows",
"Docker",
"RFC",
"Travis",
"travis-ci.org",
"travis-ci.com",
@@ -73,7 +51,6 @@
"Raspbian",
"Raspbian Jesse",
"Raspbian Stretch",
"Raspbian Buster",
"Pixhawk",
"Pixracer",
"Arduino",
@@ -82,19 +59,12 @@
"LIRC",
"GPIO",
"HC-SR04",
"RCW-0001",
"RealSense",
"NUC",
"NVIDIA",
"Jetson",
"Jetson Nano",
"STM",
"LED",
"USB",
"FAT32",
"uORB",
"SSH",
"PuTTY",
"API",
"UART",
"GND",

View File

@@ -62,7 +62,6 @@ jobs:
- markdownlint -V
script:
- markdownlint docs
- ./check_files_size.py
- gitbook install
- gitbook build
deploy:
@@ -85,14 +84,6 @@ jobs:
- pip install GitPython PyGithub
script:
- PYTHONUNBUFFERED=1 python ./gen_changelog.py
- stage: Build
name: Editorconfig-lint
language: generic
before_script:
- wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
- chmod +x ec-linux-amd64
script:
- ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt"
stages:
- Build
- Annotate

View File

@@ -59,15 +59,6 @@ To complete `mavros` install you'll need to install `geographiclib` datasets:
curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
```
You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
```bash
cd ~/catkin_ws/src/clever/clever/config
sudo cp 99-px4fmu.rules /lib/udev/rules.d
```
Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
## Running
Enable systemd service `roscore` (if not running):
@@ -89,8 +80,6 @@ To start connection to the flight controller, use:
roslaunch clever clever.launch
```
> Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
Also, you can enable and start the systemd service:
```bash

View File

@@ -23,18 +23,19 @@ function throttle(func, ms) {
}
function postAppMessage(msg) {
if (window.webkit != undefined) {
if (window.webkit.messageHandlers.appInterface != undefined) {
window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
}
} else if (window.appInterface != undefined) {
window.appInterface.postMessage(JSON.stringify(msg));
}
if (window.webkit != undefined) {
if (window.webkit.messageHandlers.appInterface != undefined) {
window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
}
}
else if (window.appInterface != undefined) {
window.appInterface.postMessage(JSON.stringify(msg));
}
}
function callNativeApp(name, msg) {
try {
postAppMessage(msg);
postAppMessage(msg);
return true;
} catch(err) {
console.warn('The native context does not exist yet');
@@ -108,12 +109,12 @@ function stickTouchStart(e) {
function stickTouchMove(e) {
for (touch of e.changedTouches) {
onStickTouchMove(touch);
}
//onStickTouchMove(e.changedTouches[0]);
rcPublishThrottled();
e.stopPropagation();
e.preventDefault();
onStickTouchMove(touch);
}
//onStickTouchMove(e.changedTouches[0]);
rcPublishThrottled();
e.stopPropagation();
e.preventDefault();
}
function stickTouchEnd(e) {
@@ -135,4 +136,4 @@ stickRight.addEventListener('touchmove', stickTouchMove);
stickLeft.addEventListener('touchstart', stickTouchStart);
stickRight.addEventListener('touchstart', stickTouchStart);
stickLeft.addEventListener('touchend', stickTouchEnd);
stickRight.addEventListener('touchend', stickTouchEnd);
stickRight.addEventListener('touchend', stickTouchEnd);

View File

@@ -173,7 +173,7 @@ target_link_libraries(aruco_pose
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

View File

@@ -185,13 +185,9 @@ private:
// TODO: check IDs are unique
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
// check if such static transform exists
if (!tf_buffer_.canTransform(transform.header.frame_id, transform.child_frame_id, transform.header.stamp)) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
}
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_.sendTransform(transform);
}
}
array_.markers.push_back(marker);

View File

@@ -77,7 +77,7 @@ private:
visualization_msgs::MarkerArray vis_array_;
std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
int image_width_, image_height_, image_margin_;
bool auto_flip_, image_axis_;
bool auto_flip_;
public:
virtual void onInit()
@@ -104,7 +104,6 @@ public:
nh_priv_.param("image_width", image_width_, 2000);
nh_priv_.param("image_height", image_height_, 2000);
nh_priv_.param("image_margin", image_margin_, 200);
nh_priv_.param("image_axis", image_axis_, true);
nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
@@ -482,15 +481,14 @@ publish_debug:
cv_bridge::CvImage msg;
if (!board_->ids.empty()) {
_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
_drawPlanarBoard(board_, size, image, image_margin_, 1);
} else {
// empty map
image.create(size, CV_8UC1);
image.setTo(cv::Scalar::all(255));
msg.encoding = sensor_msgs::image_encodings::MONO8;
}
msg.encoding = sensor_msgs::image_encodings::MONO8;
msg.image = image;
img_pub_.publish(msg.toImageMsg());
}

View File

@@ -23,12 +23,12 @@ static void _projectPoints( InputArray objectPoints,
void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
int borderBits, bool drawAxis) {
int borderBits) {
CV_Assert(outSize.area() > 0);
CV_Assert(marginSize >= 0);
_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
_img.create(outSize, CV_8UC1);
Mat out = _img.getMat();
out.setTo(Scalar::all(255));
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
@@ -90,9 +90,6 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
side = std::max(side, 10);
dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
if (drawAxis) {
cvtColor(marker, marker, COLOR_GRAY2RGB);
}
// interpolate tiny marker to marker position in markerZone
inCorners[0] = Point2f(-0.5f, -0.5f);
@@ -104,81 +101,74 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
BORDER_TRANSPARENT);
}
// draw axis
if (drawAxis) {
Size wholeSize; Point ofs;
out.locateROI(wholeSize, ofs);
auto out_copy = _img.getMat();
cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
line(out_copy, center, center + Point(300, 0), Scalar(255, 0, 0), 10); // x axis
line(out_copy, center, center + Point(0, -300), Scalar(0, 255, 0), 10); // y axis
line(out_copy, center, center + Point(-200, 200), Scalar(0, 0, 255), 10); // z axis
}
}
/* Draw a (potentially partially visible) line. */
static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
int thickness = 1, int lineType = LINE_8, int shift = 0)
int thickness = 1, int lineType = LINE_8, int shift = 0)
{
// If both points are behind the screen, don't draw anything
if (p1.z <= 0 && p2.z <= 0) {
return;
}
Point2f p1p{p1.x, p1.y};
Point2f p2p{p2.x, p2.y};
// If points are on the different sides of the plane, compute intersection point
if (p1.z * p2.z < 0) {
// Compute intersection point with the screen
// We denote alpha as such:
// xi = (1 - alpha) * x1 + alpha * x2
// yi = (1 - alpha) * y1 + alpha * y2
// zi = (1 - alpha) * z1 + alpha * z2 = 0
// Thus, alpha can be expressed as
// alpha = z1 / (z1 - z2)
float alpha = p1.z / (p1.z - p2.z);
Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
if (p1.z < 0) {
p1p = pi;
} else {
p2p = pi;
}
}
line(image, p1p, p2p, color, thickness, lineType, shift);
// If both points are behind the screen, don't draw anything
if (p1.z <= 0 && p2.z <= 0)
{
return;
}
Point2f p1p{p1.x, p1.y};
Point2f p2p{p2.x, p2.y};
// If points are on the different sides of the plane, compute intersection point
if (p1.z * p2.z < 0)
{
// Compute intersection point with the screen
// We denote alpha as such:
// xi = (1 - alpha) * x1 + alpha * x2
// yi = (1 - alpha) * y1 + alpha * y2
// zi = (1 - alpha) * z1 + alpha * z2 = 0
// Thus, alpha can be expressed as
// alpha = z1 / (z1 - z2)
float alpha = p1.z / (p1.z - p2.z);
Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
if (p1.z < 0)
{
p1p = pi;
}
else
{
p2p = pi;
}
}
line(image, p1p, p2p, color, thickness, lineType, shift);
}
void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
InputArray _rvec, InputArray _tvec, float length) {
CV_Assert(_image.getMat().total() != 0 &&
(_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
CV_Assert(length > 0);
CV_Assert(_image.getMat().total() != 0 &&
(_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
CV_Assert(length > 0);
// project axis points
std::vector<Point3f> axisPoints;
axisPoints.push_back(Point3f(0, 0, 0));
axisPoints.push_back(Point3f(length, 0, 0));
axisPoints.push_back(Point3f(0, length, 0));
axisPoints.push_back(Point3f(0, 0, length));
std::vector<Point3f> imagePointsZ;
_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
// project axis points
std::vector< Point3f > axisPoints;
axisPoints.push_back(Point3f(0, 0, 0));
axisPoints.push_back(Point3f(length, 0, 0));
axisPoints.push_back(Point3f(0, length, 0));
axisPoints.push_back(Point3f(0, 0, length));
std::vector< Point3f > imagePointsZ;
_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
// draw axis lines
linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
// draw axis lines
linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
}
static CvMat _cvMat(const cv::Mat& m)
{
CvMat self;
CV_DbgAssert(m.dims <= 2);
self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
self.step = (int)m.step[0];
self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
return self;
CvMat self;
CV_DbgAssert(m.dims <= 2);
self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
self.step = (int)m.step[0];
self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
return self;
}
static void _projectPoints( InputArray _opoints,
@@ -190,47 +180,47 @@ static void _projectPoints( InputArray _opoints,
OutputArray _jacobian,
double aspectRatio )
{
Mat opoints = _opoints.getMat();
int npoints = opoints.checkVector(3), depth = opoints.depth();
CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
Mat opoints = _opoints.getMat();
int npoints = opoints.checkVector(3), depth = opoints.depth();
CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;
CV_Assert(_ipoints.needed());
CV_Assert( _ipoints.needed() );
_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
Mat imagePoints = _ipoints.getMat();
CvMat c_imagePoints = _cvMat(imagePoints);
CvMat c_objectPoints = _cvMat(opoints);
Mat cameraMatrix = _cameraMatrix.getMat();
_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
Mat imagePoints = _ipoints.getMat();
CvMat c_imagePoints = _cvMat(imagePoints);
CvMat c_objectPoints = _cvMat(opoints);
Mat cameraMatrix = _cameraMatrix.getMat();
Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
CvMat c_cameraMatrix = _cvMat(cameraMatrix);
CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
CvMat c_cameraMatrix = _cvMat(cameraMatrix);
CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
double dc0buf[5] = {0};
Mat dc0(5, 1, CV_64F, dc0buf);
Mat distCoeffs = _distCoeffs.getMat();
if (distCoeffs.empty())
distCoeffs = dc0;
CvMat c_distCoeffs = _cvMat(distCoeffs);
int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
double dc0buf[5]={0};
Mat dc0(5,1,CV_64F,dc0buf);
Mat distCoeffs = _distCoeffs.getMat();
if( distCoeffs.empty() )
distCoeffs = dc0;
CvMat c_distCoeffs = _cvMat(distCoeffs);
int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
Mat jacobian;
if (_jacobian.needed())
{
_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
jacobian = _jacobian.getMat();
pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
}
Mat jacobian;
if( _jacobian.needed() )
{
_jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
jacobian = _jacobian.getMat();
pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10+ndistCoeffs)));
}
_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
&c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
_cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
&c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
}
namespace _detail

View File

@@ -3,7 +3,6 @@
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
int marginSize, int borderBits, bool drawAxis); // editorconfig-checker-disable-line
void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
cv::InputArray rvec, cv::InputArray tvec, float length); // editorconfig-checker-disable-line
cv::InputArray rvec, cv::InputArray tvec, float length);

View File

@@ -30,7 +30,8 @@ static void param(ros::NodeHandle nh, const std::string& param_name, T& param_va
}
}
static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo,
cv::Mat& matrix, cv::Mat& dist)
{
for (unsigned int i = 0; i < 3; ++i)
for (unsigned int j = 0; j < 3; ++j)

View File

@@ -24,7 +24,7 @@ def approx(expected):
def test_markers(node):
markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
assert len(markers.markers) == 5
assert len(markers.markers) == 4
assert markers.header.frame_id == 'main_camera_optical'
assert markers.markers[0].id == 2
@@ -45,46 +45,28 @@ def test_markers(node):
assert markers.markers[0].c4.x == approx(415.557739258)
assert markers.markers[0].c4.y == approx(429.442260742)
assert markers.markers[4].id == 3
assert markers.markers[4].length == approx(0.1)
assert markers.markers[4].pose.position.x == approx(-0.1805169666)
assert markers.markers[4].pose.position.y == approx(-0.200697302327)
assert markers.markers[4].pose.position.z == approx(0.585767514823)
assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
assert markers.markers[4].c1.x == approx(129.557723999)
assert markers.markers[4].c1.y == approx(49.557723999)
assert markers.markers[4].c2.x == approx(223.442276001)
assert markers.markers[4].c2.y == approx(49.557723999)
assert markers.markers[4].c3.x == approx(223.442276001)
assert markers.markers[4].c3.y == approx(143.442276001)
assert markers.markers[4].c4.x == approx(129.557723999)
assert markers.markers[4].c4.y == approx(143.442276001)
assert markers.markers[3].id == 3
assert markers.markers[3].length == approx(0.1)
assert markers.markers[3].pose.position.x == approx(-0.1805169666)
assert markers.markers[3].pose.position.y == approx(-0.200697302327)
assert markers.markers[3].pose.position.z == approx(0.585767514823)
assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
assert markers.markers[3].c1.x == approx(129.557723999)
assert markers.markers[3].c1.y == approx(49.557723999)
assert markers.markers[3].c2.x == approx(223.442276001)
assert markers.markers[3].c2.y == approx(49.557723999)
assert markers.markers[3].c3.x == approx(223.442276001)
assert markers.markers[3].c3.y == approx(143.442276001)
assert markers.markers[3].c4.x == approx(129.557723999)
assert markers.markers[3].c4.y == approx(143.442276001)
assert markers.markers[1].id == 1
assert markers.markers[1].length == approx(0.33)
assert markers.markers[3].id == 4
assert markers.markers[3].length == approx(0.33)
assert markers.markers[2].id == 100
assert markers.markers[2].id == 4
assert markers.markers[2].length == approx(0.33)
assert markers.markers[2].pose.position.x == approx(-1.37600105389)
assert markers.markers[2].pose.position.y == approx(-0.323028530991)
assert markers.markers[2].pose.position.z == approx(2.94611272668)
assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
assert markers.markers[2].c1.x == approx(52.557723999)
assert markers.markers[2].c1.y == approx(205.557723999)
assert markers.markers[2].c2.x == approx(113.442276001)
assert markers.markers[2].c2.y == approx(205.557723999)
assert markers.markers[2].c3.x == approx(113.442276001)
assert markers.markers[2].c3.y == approx(265.442260742)
assert markers.markers[2].c4.x == approx(52.557723999)
assert markers.markers[2].c4.y == approx(265.442260742)
def test_markers_frames(node, tf_buffer):
marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
@@ -100,24 +82,24 @@ def test_map_markers_frames(node, tf_buffer):
stamp = rospy.get_rostime()
timeout = rospy.Duration(5)
marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_1', stamp, timeout)
assert marker_1.transform.translation.x == approx(0)
assert marker_1.transform.translation.y == approx(0)
assert marker_1.transform.translation.z == approx(0)
marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_4', stamp, timeout)
assert marker_4.transform.translation.x == approx(1)
assert marker_4.transform.translation.y == approx(1)
assert marker_4.transform.translation.z == approx(0)
marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_map_12', stamp, timeout)
assert marker_12.transform.translation.x == approx(0.2)
assert marker_12.transform.translation.y == approx(0.5)
assert marker_12.transform.translation.z == approx(0)
def test_visualization(node):
vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
assert len(vis.markers) == 11
assert len(vis.markers) == 9
def test_debug(node):
img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)

View File

@@ -23,7 +23,7 @@
<param name="type" value="map"/>
<param name="map" value="$(find aruco_pose)/test/basic.txt"/>
<param name="markers/frame_id" value="aruco_map"/>
<param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
<param name="markers/child_frame_id_prefix" value="aruco_map_"/>
</node>
<param name="test_module" value="$(find aruco_pose)/test/basic.py"/>

Binary file not shown.

Before

Width:  |  Height:  |  Size: 2.1 KiB

After

Width:  |  Height:  |  Size: 2.1 KiB

View File

@@ -1,12 +1,12 @@
[Unit]
Description=Clever ROS package
Requires=roscore.service
After=network.target
After=roscore.service
[Service]
User=pi
ExecStart=/bin/sh -c ". /home/pi/catkin_ws/devel/setup.sh; \
ROS_HOSTNAME=`hostname`.local exec roslaunch clever clever.launch --wait --screen --skip-log-check"
EnvironmentFile=/lib/systemd/system/roscore.env
ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
Restart=on-failure
RestartSec=3

35
builder/assets/clever_rename Executable file
View File

@@ -0,0 +1,35 @@
#!/usr/bin/env bash
# Set Clever hostname to the specified value
set -e
NEW_NAME_OPT=$1
if [[ -z ${NEW_NAME_OPT} ]]; then
echo "Please specify new name for this Clever"
exit 1
fi
NEW_NAME=$(echo ${NEW_NAME_OPT} | tr '[:upper:]' '[:lower:]')
echo "Setting name to ${NEW_NAME}"
echo "Backing up /etc/hostname"
cp /etc/hostname /etc/hostname.bak
echo "Writing new /etc/hostname"
echo ${NEW_NAME} > /etc/hostname
echo "Backing up /etc/hosts"
cp /etc/hosts /etc/hosts.bak
echo "Rewriting /etc/hosts with new values"
sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_NAME}'/g' /etc/hosts
echo "Changing hostname in /lib/systemd/system/roscore.env"
sed -i 's/ROS_HOSTNAME=.*/ROS_HOSTNAME='${NEW_NAME}'.local/g' /lib/systemd/system/roscore.env
echo "Changing hostname in .bashrc"
sed -i 's/export ROS_HOSTNAME=.*/export ROS_HOSTNAME='${NEW_NAME}'.local/g' /home/pi/.bashrc
echo "Done, reboot your Clever to see the results"

View File

@@ -38,11 +38,7 @@ echo_stamp() {
echo_stamp "Rename SSID"
NEW_SSID='CLEVER-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
sudo sed -i.OLD "s/CLEVER/${NEW_SSID}/" /etc/wpa_supplicant/wpa_supplicant.conf
echo_stamp "Rename hostname to $NEW_SSID"
hostnamectl set-hostname $NEW_SSID
sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_SSID}' '${NEW_SSID}'.local/g' /etc/hosts
# .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces down
clever_rename ${NEW_SSID}
echo_stamp "Harware setup"
/root/hardware_setup.sh

View File

@@ -0,0 +1,10 @@
ROS_ROOT=/opt/ros/kinetic/share/ros
ROS_DISTRO=kinetic
ROS_PACKAGE_PATH=/home/pi/catkin_ws/src:/opt/ros/kinetic/share
ROS_PORT=11311
ROS_MASTER_URI=http://localhost:11311
CMAKE_PREFIX_PATH=/home/pi/catkin_ws/devel:/opt/ros/kinetic
PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
LD_LIBRARY_PATH=/opt/ros/kinetic/lib
PYTHONPATH=/home/pi/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages
ROS_HOSTNAME=raspberrypi.local

View File

@@ -4,7 +4,8 @@ After=network.target
[Service]
User=pi
ExecStart=/bin/sh -c ". /opt/ros/kinetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
EnvironmentFile=/lib/systemd/system/roscore.env
ExecStart=/opt/ros/kinetic/bin/roscore
Restart=on-failure
RestartSec=3

View File

@@ -109,14 +109,16 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.
[[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
# Clever
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.service' '/lib/systemd/system/'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
# ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
# Add PX4 udev rules
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/clever/config/99-px4fmu.rules' '/lib/udev/rules.d/'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
# Add rename script
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever_rename' '/usr/local/bin/clever_rename'
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'

View File

@@ -192,7 +192,7 @@ cat << EOF >> /home/pi/.bashrc
LANG='C.UTF-8'
LC_ALL='C.UTF-8'
ROS_DISTRO='kinetic'
export ROS_HOSTNAME=\`hostname\`.local
export ROS_HOSTNAME='raspberrypi.local'
source /opt/ros/kinetic/setup.bash
source /home/pi/catkin_ws/devel/setup.bash
EOF

View File

@@ -114,11 +114,11 @@ mjpg-streamer=2.0 \
echo_stamp "Updating kernel to fix camera bug"
apt-get install --no-install-recommends -y \
raspberrypi-kernel=1.20190709~stretch-1 \
raspberrypi-bootloader=1.20190709~stretch-1 \
libraspberrypi-bin=1.20190709~stretch-1 \
libraspberrypi-dev=1.20190709~stretch-1 \
libraspberrypi0=1.20190709~stretch-1 \
raspberrypi-kernel=1.20190517-1 \
raspberrypi-bootloader=1.20190517-1 \
libraspberrypi-bin=1.20190517-1 \
libraspberrypi-dev=1.20190517-1 \
libraspberrypi0=1.20190517-1 \
wireless-regdb=2018.05.09-0~rpt1 \
wpasupplicant=2:2.6-21~bpo9~rpt1

View File

@@ -1,31 +0,0 @@
#!/usr/bin/env python3
import os
import sys
def human_size(num, suffix='B'):
for unit in ['', 'Ki', 'Mi', 'Gi', 'Ti', 'Pi', 'Ei', 'Zi']:
if abs(num) < 1024.0:
return "%3.1f %s%s" % (num, unit, suffix)
num /= 1024.0
return "%.1f %s%s" % (num, 'Yi', suffix)
SIZE_LIMIT = 600 * 1024
EXCLUDE = 'rviz.png', 'ssid.png', 'sitl_docker_demo.png', 'qgc-params.png', 'butterfly.png', \
'Clever main.png', 'fpv_3.png', '1_4.png', 'fpv_4.png', 'detal1.png', 'lockradio.png', \
'qground.png', 'allElements.png', 'download-log.png', 'explosion.png', 'rqt.png', \
'cl3_mountBEC.JPG', 'cl3_mountRpiCamera.JPG'
code = 0
for root, dirs, files in os.walk('docs/'):
for f in files:
if f not in EXCLUDE:
path = os.path.join(root, f)
size = os.path.getsize(path)
if size > SIZE_LIMIT:
print('\x1b[1;31mFile too large ({}): {}\x1b[0m'.format(human_size(size), path), \
file=sys.stderr)
code = 1
sys.exit(code)

View File

@@ -227,19 +227,6 @@ target_link_libraries(clever
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
# Only install udev rules when building a Debian package
# FIXME: Other operating systems may have other prefixes
string(FIND ${CMAKE_INSTALL_PREFIX} "/opt/ros" _PREFIX_INDEX)
if (${_PREFIX_INDEX} EQUAL 0)
message(STATUS "Building as a Debian package - adding udev rules as installable files")
install(FILES
config/99-px4fmu.rules
DESTINATION /lib/udev/rules.d
)
else()
message(STATUS "Building in a user workspace - not adding udev rules")
endif()
#############
## Testing ##
#############

View File

@@ -22,11 +22,10 @@
<remap from="markers" to="aruco_detect/markers"/>
<param name="map" value="$(find aruco_pose)/map/map.txt"/>
<param name="known_tilt" value="map"/>
<param name="image_axis" value="true"/>
<param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
<param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
<param name="markers/frame_id" value="aruco_map"/>
<param name="markers/child_frame_id_prefix" value="aruco_"/>
<param name="markers/child_frame_id_prefix" value="aruco_map_"/>
</node>
<!-- vpe publisher from aruco markers -->

View File

@@ -1,4 +1,4 @@
flask==1.1.1
flask==0.12.3
docopt==0.6.2
geopy==1.11.0
smbus2==0.2.1

View File

@@ -1,5 +1,4 @@
#!/usr/bin/env python
# coding=utf-8
# Copyright (C) 2018 Copter Express Technologies
#
@@ -145,45 +144,6 @@ def mavlink_exec(cmd, timeout=3.0):
return mavlink_recv
BOARD_ROTATIONS = {
0: 'no rotation',
1: 'yaw 45°',
2: 'yaw 90°',
3: 'yaw 135°',
4: 'yaw 180°',
5: 'yaw 225°',
6: 'yaw 270°',
7: 'yaw 315°',
8: 'roll 180°',
9: 'roll 180°, yaw 45°',
10: 'roll 180°, yaw 90°',
11: 'roll 180°, yaw 135°',
12: 'pitch 180°',
13: 'roll 180°, yaw 225°',
14: 'roll 180°, yaw 270°',
15: 'roll 180°, yaw 315°',
16: 'roll 90°',
17: 'roll 90°, yaw 45°',
18: 'roll 90°, yaw 90°',
19: 'roll 90°, yaw 135°',
20: 'roll 270°',
21: 'roll 270°, yaw 45°',
22: 'roll 270°, yaw 90°',
23: 'roll 270°, yaw 135°',
24: 'pitch 90°',
25: 'pitch 270°',
26: 'roll 270°, yaw 270°',
27: 'roll 180°, pitch 270°',
28: 'pitch 90°, yaw 180',
29: 'pitch 90°, roll 90°',
30: 'yaw 293°, pitch 68°, roll 90°',
31: 'pitch 90°, roll 270°',
32: 'pitch 9°, yaw 180°',
33: 'pitch 45°',
34: 'pitch 315°',
}
@check('FCU')
def check_fcu():
try:
@@ -229,13 +189,6 @@ def check_fcu():
else:
failure('unknown selected estimator: %s', est)
rot = get_param('SENS_BOARD_ROT')
if rot is not None:
try:
info('board rotation: %s', BOARD_ROTATIONS[rot])
except KeyError:
failure('unknown board rotation %s', rot)
except rospy.ROSException:
failure('no MAVROS state (check wiring)')
@@ -287,7 +240,7 @@ def check_camera(name):
if not optical or not cable:
info('%s: custom camera orientation detected', name)
else:
info('camera is oriented %s, camera cable goes %s', optical, cable)
info('camera is oriented %s, camera cable goes %s', optical, cable)
except tf2_ros.TransformException:
failure('cannot transform from base_link to camera frame')
@@ -397,8 +350,8 @@ def check_vpe():
if delay != 0:
failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
get_param('EKF2_EVA_NOISE'),
get_param('EKF2_EVP_NOISE'))
get_param('EKF2_EVA_NOISE'),
get_param('EKF2_EVP_NOISE'))
if not vis:
return
@@ -606,16 +559,10 @@ def check_cpu_usage():
@check('clever.service')
def check_clever_service():
try:
output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split(),
stderr=subprocess.STDOUT)
except subprocess.CalledProcessError as e:
failure('systemctl returned %s: %s', e.returncode, e.output)
return
output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split())
if 'inactive' in output:
failure('clever.service is not running, try sudo systemctl restart clever')
return
j = journal.Reader()
j.this_boot()
j.add_match(_SYSTEMD_UNIT='clever.service')
@@ -638,10 +585,7 @@ def check_clever_service():
@check('Image')
def check_image():
try:
info('version: %s', open('/etc/clever_version').read().strip())
except IOError:
info('no /etc/clever_version file, not the Clever image?')
info('version: %s', open('/etc/clever_version').read().strip())
@check('Preflight status')
@@ -650,7 +594,7 @@ def check_preflight_status():
mavlink_exec('\n')
cmdr_output = mavlink_exec('commander check')
if cmdr_output == '':
failure('no data from FCU')
failure('No data from FCU')
return
cmdr_lines = cmdr_output.split('\n')
r = re.compile(r'^(.*)(Preflight|Prearm) check: (.*)')

View File

@@ -155,9 +155,9 @@ void handleLocalPosition(const PoseStamped& pose)
// wait for transform without interrupting publishing setpoints
inline bool waitTransform(const string& target, const string& source,
const ros::Time& stamp, const ros::Duration& timeout) // editorconfig-checker-disable-line
const ros::Time& stamp, const ros::Duration& timeout)
{
ros::Rate r(100);
ros::Rate r(10);
auto start = ros::Time::now();
while (ros::ok()) {
if (ros::Time::now() - start > timeout) return false;
@@ -201,29 +201,31 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
res.mode = state.mode;
}
try {
waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
res.x = transform.transform.translation.x;
res.y = transform.transform.translation.y;
res.z = transform.transform.translation.z;
waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);
double yaw, pitch, roll;
tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
res.yaw = yaw;
res.pitch = pitch;
res.roll = roll;
} catch (const tf2::TransformException& e) {
ROS_DEBUG("simple_offboard: %s", e.what());
if (!TIMEOUT(local_position, local_position_timeout)) {
try {
// transform pose
PoseStamped pose;
tf_buffer.transform(local_position, pose, req.frame_id);
res.x = pose.pose.position.x;
res.y = pose.pose.position.y;
res.z = pose.pose.position.z;
// Tait-Bryan angles, order z-y-x
double yaw, pitch, roll;
tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll);
res.yaw = yaw;
res.pitch = pitch;
res.roll = roll;
} catch (const tf2::TransformException& e) {}
}
if (!TIMEOUT(velocity, velocity_timeout)) {
try {
// transform velocity
waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout);
Vector3Stamped vec, vec_out;
vec.header.stamp = velocity.header.stamp;
vec.header.frame_id = velocity.header.frame_id;
vec.header = velocity.header;
vec.vector = velocity.twist.linear;
tf_buffer.transform(vec, vec_out, req.frame_id);
@@ -349,10 +351,6 @@ PoseStamped globalToLocal(double lat, double lon)
x_offset = distance * sin(azimuth_radians);
y_offset = distance * cos(azimuth_radians);
if (!waitTransform(local_frame, fcu_frame, global_position.header.stamp, ros::Duration(0.2))) {
throw std::runtime_error("No local position");
}
auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);
PoseStamped pose;
@@ -481,12 +479,10 @@ inline void checkState()
throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
}
#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
uint8_t& success, string& message) // editorconfig-checker-disable-line
float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
uint8_t& success, string& message)
{
auto stamp = ros::Time::now();
@@ -494,20 +490,6 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
if (busy)
throw std::runtime_error("Busy");
ENSURE_FINITE(x);
ENSURE_FINITE(y);
ENSURE_FINITE(z);
ENSURE_FINITE(vx);
ENSURE_FINITE(vy);
ENSURE_FINITE(vz);
ENSURE_FINITE(pitch);
ENSURE_FINITE(roll);
ENSURE_FINITE(pitch_rate);
ENSURE_FINITE(roll_rate);
ENSURE_FINITE(lat);
ENSURE_FINITE(lon);
ENSURE_FINITE(thrust);
busy = true;
// Checks
@@ -557,9 +539,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
if (sp_type == NAVIGATE_GLOBAL) {
// Calculate x and from lat and lot in request's frame
auto pose_local = globalToLocal(lat, lon);
pose_local.header.stamp = stamp; // TODO: fix
auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
auto xy_in_req_frame = tf_buffer.transform(globalToLocal(lat, lon), frame_id);
x = xy_in_req_frame.pose.position.x;
y = xy_in_req_frame.pose.position.y;
}
@@ -763,7 +743,7 @@ int main(int argc, char **argv)
// Telemetry subscribers
auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);

View File

@@ -1,197 +0,0 @@
#!/usr/bin/env python
import math
import rospy
import pytest
from pytest import approx
from numpy import isfinite
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from sensor_msgs.msg import NavSatFix
from clever import srv
from std_srvs.srv import Trigger
import tf2_ros
import tf2_geometry_msgs
def roughly(expected):
return approx(expected, abs=1e-1)
def very_roughly(expected):
return approx(expected, abs=1)
@pytest.fixture()
def node():
return rospy.init_node('clever_test', anonymous=True)
def wait_service(name, service_class):
res = rospy.ServiceProxy(name, service_class)
res.wait_for_service(5)
return res
@pytest.fixture
def get_telemetry():
return wait_service('get_telemetry', srv.GetTelemetry)
@pytest.fixture
def navigate():
return wait_service('navigate', srv.Navigate)
@pytest.fixture
def navigate():
res = rospy.ServiceProxy('navigate', srv.Navigate)
res.wait_for_service(5)
return res
@pytest.fixture
def land():
return wait_service('land', Trigger)
@pytest.fixture
def tf_buffer():
buf = tf2_ros.Buffer()
tf2_ros.TransformListener(buf)
return buf
def wait_connection():
start = rospy.get_rostime()
while rospy.get_rostime() - start <= rospy.Duration(15):
state = rospy.wait_for_message('mavros/state', State, timeout=10)
if state.connected:
return True
assert False, "no connection to SITL"
def minimal_rate(func, rate):
start = rospy.get_rostime()
i = 0
while rospy.get_rostime() - start <= rospy.Duration(2):
func()
i += 1
result_rate = i / 2
assert result_rate >= rate, 'Rate too low: %s Hz' % result_rate
def test_state_initial(node):
wait_connection()
state = rospy.wait_for_message('mavros/state', State, timeout=10)
assert state.connected == True
assert state.armed == False
def test_telem_initial(node, get_telemetry):
# wait for local position
rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=15)
rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=30)
telem = get_telemetry()
assert telem.frame_id == 'map'
assert telem.connected == True
assert telem.armed == False
assert telem.mode != ''
assert telem.x == roughly(0.0)
assert telem.y == roughly(0.0)
assert telem.z == roughly(0.0)
assert telem.lat == approx(47.397742)
assert telem.lon == approx(8.545594)
assert telem.vx == roughly(0.0)
assert telem.vy == roughly(0.0)
assert telem.vz == roughly(0.0)
assert telem.pitch == roughly(0.0)
assert telem.roll == roughly(0.0)
assert telem.yaw == roughly(1.56)
assert isfinite(telem.voltage)
assert isfinite(telem.cell_voltage)
def test_telem_rate(node, get_telemetry):
minimal_rate(lambda: get_telemetry(), 20)
minimal_rate(lambda: get_telemetry(frame_id='body'), 20)
minimal_rate(lambda: get_telemetry(frame_id='base_link'), 200)
def test_takeoff_without_auto_arm(node, navigate):
res = navigate(z=2, frame_id='body')
assert res.success == False
assert res.message == 'Copter is not in OFFBOARD mode, use auto_arm?'
def test_takeoff(node, navigate, get_telemetry, tf_buffer):
res = navigate(z=2, speed=1, frame_id='body', auto_arm=True)
assert res.success == True, res.message
rospy.sleep(5)
telem = get_telemetry()
assert telem.z == very_roughly(2.0)
assert telem.x == very_roughly(0.0)
assert telem.y == very_roughly(0.0)
assert telem.pitch == roughly(0.0)
assert telem.roll == roughly(0.0)
assert telem.yaw == roughly(1.56)
def test_navigate_nans(node, navigate):
res = navigate(x=float('nan'))
assert res.success == False
assert res.message == 'x argument cannot be NaN or Inf'
res = navigate(y=float('nan'))
assert res.success == False
assert res.message == 'y argument cannot be NaN or Inf'
res = navigate(z=float('nan'))
assert res.success == False
assert res.message == 'z argument cannot be NaN or Inf'
res = navigate(x=float('inf'))
assert res.success == False
assert res.message == 'x argument cannot be NaN or Inf'
res = navigate(y=float('inf'))
assert res.success == False
assert res.message == 'y argument cannot be NaN or Inf'
res = navigate(z=float('inf'))
assert res.success == False
assert res.message == 'z argument cannot be NaN or Inf'
res = navigate(x=float('-inf'))
assert res.success == False
assert res.message == 'x argument cannot be NaN or Inf'
res = navigate(y=float('-inf'))
assert res.success == False
assert res.message == 'y argument cannot be NaN or Inf'
res = navigate(z=float('-inf'))
assert res.success == False
assert res.message == 'z argument cannot be NaN or Inf'
def test_navigate(node, navigate, get_telemetry, tf_buffer):
res = navigate(x=1, y=2, z=3, speed=1)
assert res.success == True, res.message
nav_target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(0.2))
assert nav_target.transform.translation.x == approx(1)
assert nav_target.transform.translation.y == approx(2)
assert nav_target.transform.translation.z == approx(3)
rospy.sleep(10)
telem = get_telemetry()
assert telem.x == very_roughly(1.0)
assert telem.y == very_roughly(2.0)
assert telem.z == very_roughly(3.0)
assert telem.pitch == roughly(0.0)
assert telem.roll == roughly(0.0)
assert telem.yaw == roughly(0.0)
res = navigate(x=-1, y=-2, z=-1, frame_id='body', speed=1)
assert res.success == True, res.message
nav_target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(0.2))
assert nav_target.transform.translation.x == very_roughly(0)
assert nav_target.transform.translation.y == very_roughly(0)
assert nav_target.transform.translation.z == very_roughly(2)
rospy.sleep(10)
telem = get_telemetry()
assert telem.x == very_roughly(0)
assert telem.y == very_roughly(0)
assert telem.z == very_roughly(2)
assert telem.pitch == roughly(0)
assert telem.roll == roughly(0)
assert telem.yaw == roughly(0)
# TODO
# test navigate_global, set_velocity, set_attitude, set_rates
def test_land(node, get_telemetry, land):
res = land()
assert res.success, res.message
telem = get_telemetry()
assert telem.mode == 'AUTO.LAND'
assert telem.armed == True, 'Drone unexpectedly disarmed while landing'
rospy.sleep(12)
telem = get_telemetry()
assert telem.mode == 'AUTO.LAND'
assert telem.armed == False, 'Drone is not disarmed after landing'

View File

@@ -1,20 +0,0 @@
<launch>
<arg name="ip" default="127.0.0.1"/>
<!-- mavros -->
<include file="$(find clever)/launch/mavros.launch">
<arg name="fcu_conn" value="udp"/>
<arg name="fcu_ip" value="$(arg ip)"/>
<arg name="gcs_bridge" value="false"/>
<arg name="viz" value="false"/>
<arg name="respawn" default="false"/>
</include>
<node name="simple_offboard" pkg="clever" type="simple_offboard" required="true" output="screen">
<param name="reference_frames/body" value="map"/>
<param name="reference_frames/base_link" value="map"/>
</node>
<param name="test_module" value="$(find clever)/test/sitl.py"/>
<test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner" time-limit="120.0"/>
</launch>

View File

@@ -3,7 +3,7 @@ var titleEl = document.querySelector('title');
var modeEl = document.querySelector('.mode');
var batteryEl = document.querySelector('.battery');
var url = 'ws://' + location.hostname + ':9090';
var url = 'ws://' + location.host + ':9090';
var ros = new ROSLIB.Ros({ url: url });
function speak(txt) {

View File

@@ -1,5 +1,5 @@
var ros = new ROSLIB.Ros({
url : 'ws://' + location.hostname + ':9090'
url : 'ws://' + location.host + ':9090'
});
var titleEl = document.querySelector('title');

Binary file not shown.

Before

Width:  |  Height:  |  Size: 259 KiB

BIN
docs/assets/Clever main.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 6.4 MiB

View File

@@ -0,0 +1,735 @@
# Onboard parameters for Vehicle 1
#
# Stack: PX4 Pro
# Vehicle: Multi-Rotor
# Version: 1.7.0
# Git Revision: cbdb08bb6109c1ea
#
# Vehicle-Id Component-Id Name Value Type
1 1 ATT_ACC_COMP 1 6
1 1 ATT_BIAS_MAX 0.050000000745058060 9
1 1 ATT_EXT_HDG_M 0 6
1 1 ATT_MAG_DECL 0.000000000000000000 9
1 1 ATT_MAG_DECL_A 1 6
1 1 ATT_W_ACC 0.200000002980232239 9
1 1 ATT_W_EXT_HDG 0.100000001490116119 9
1 1 ATT_W_GYRO_BIAS 0.100000001490116119 9
1 1 ATT_W_MAG 0.100000001490116119 9
1 1 BAT_A_PER_V 15.391030311584472656 9
1 1 BAT_CAPACITY -1.000000000000000000 9
1 1 BAT_CNT_V_CURR 0.000805664050858468 9
1 1 BAT_CNT_V_VOLT 0.000805664050858468 9
1 1 BAT_CRIT_THR 0.070000000298023224 9
1 1 BAT_EMERGEN_THR 0.050000000745058060 9
1 1 BAT_LOW_THR 0.150000005960464478 9
1 1 BAT_N_CELLS 4 6
1 1 BAT_R_INTERNAL -1.000000000000000000 9
1 1 BAT_SOURCE 0 6
1 1 BAT_V_CHARGED 4.199999809265136719 9
1 1 BAT_V_DIV 10.887768745422363281 9
1 1 BAT_V_EMPTY 3.400000095367431641 9
1 1 BAT_V_LOAD_DROP 0.300000011920928955 9
1 1 BAT_V_OFFS_CURR 0.000000000000000000 9
1 1 CAL_ACC0_EN 1 6
1 1 CAL_ACC0_ID 1246218 6
1 1 CAL_ACC0_XOFF 0.081070423126220703 9
1 1 CAL_ACC0_XSCALE 0.996176421642303467 9
1 1 CAL_ACC0_YOFF -0.182177066802978516 9
1 1 CAL_ACC0_YSCALE 1.000774979591369629 9
1 1 CAL_ACC0_ZOFF 0.506614208221435547 9
1 1 CAL_ACC0_ZSCALE 0.991645038127899170 9
1 1 CAL_ACC1_EN 1 6
1 1 CAL_ACC1_ID 1114634 6
1 1 CAL_ACC1_XOFF 0.864228248596191406 9
1 1 CAL_ACC1_XSCALE 1.004844427108764648 9
1 1 CAL_ACC1_YOFF 0.980560302734375000 9
1 1 CAL_ACC1_YSCALE 0.980703771114349365 9
1 1 CAL_ACC1_ZOFF 0.873121738433837891 9
1 1 CAL_ACC1_ZSCALE 0.989114820957183838 9
1 1 CAL_ACC2_XOFF 0.000000000000000000 9
1 1 CAL_ACC2_XSCALE 1.000000000000000000 9
1 1 CAL_ACC2_YOFF 0.000000000000000000 9
1 1 CAL_ACC2_YSCALE 1.000000000000000000 9
1 1 CAL_ACC2_ZOFF 0.000000000000000000 9
1 1 CAL_ACC2_ZSCALE 1.000000000000000000 9
1 1 CAL_AIR_CMODEL 0 6
1 1 CAL_AIR_TUBED_MM 1.500000000000000000 9
1 1 CAL_AIR_TUBELEN 0.200000002980232239 9
1 1 CAL_GYRO0_EN 1 6
1 1 CAL_GYRO0_ID 2163722 6
1 1 CAL_GYRO0_XOFF 0.013700588606297970 9
1 1 CAL_GYRO0_XSCALE 1.000000000000000000 9
1 1 CAL_GYRO0_YOFF 0.049494847655296326 9
1 1 CAL_GYRO0_YSCALE 1.000000000000000000 9
1 1 CAL_GYRO0_ZOFF -0.032555881887674332 9
1 1 CAL_GYRO0_ZSCALE 1.000000000000000000 9
1 1 CAL_GYRO1_EN 1 6
1 1 CAL_GYRO1_ID 2228490 6
1 1 CAL_GYRO1_XOFF -0.007464688271284103 9
1 1 CAL_GYRO1_XSCALE 1.000000000000000000 9
1 1 CAL_GYRO1_YOFF 0.008172192610800266 9
1 1 CAL_GYRO1_YSCALE 1.000000000000000000 9
1 1 CAL_GYRO1_ZOFF -0.010663406923413277 9
1 1 CAL_GYRO1_ZSCALE 1.000000000000000000 9
1 1 CAL_GYRO2_XOFF 0.000000000000000000 9
1 1 CAL_GYRO2_XSCALE 1.000000000000000000 9
1 1 CAL_GYRO2_YOFF 0.000000000000000000 9
1 1 CAL_GYRO2_YSCALE 1.000000000000000000 9
1 1 CAL_GYRO2_ZOFF 0.000000000000000000 9
1 1 CAL_GYRO2_ZSCALE 1.000000000000000000 9
1 1 CAL_MAG0_EN 1 6
1 1 CAL_MAG0_ID 131594 6
1 1 CAL_MAG0_ROT -1 6
1 1 CAL_MAG0_XOFF -0.020210459828376770 9
1 1 CAL_MAG0_XSCALE 1.033145427703857422 9
1 1 CAL_MAG0_YOFF 0.100117556750774384 9
1 1 CAL_MAG0_YSCALE 1.017574071884155273 9
1 1 CAL_MAG0_ZOFF -0.113783776760101318 9
1 1 CAL_MAG0_ZSCALE 0.933741152286529541 9
1 1 CAL_MAG1_ID 0 6
1 1 CAL_MAG1_ROT -1 6
1 1 CAL_MAG1_XOFF 0.000000000000000000 9
1 1 CAL_MAG1_XSCALE 1.000000000000000000 9
1 1 CAL_MAG1_YOFF 0.000000000000000000 9
1 1 CAL_MAG1_YSCALE 1.000000000000000000 9
1 1 CAL_MAG1_ZOFF 0.000000000000000000 9
1 1 CAL_MAG1_ZSCALE 1.000000000000000000 9
1 1 CAL_MAG2_ID 0 6
1 1 CAL_MAG2_ROT -1 6
1 1 CAL_MAG2_XOFF 0.000000000000000000 9
1 1 CAL_MAG2_XSCALE 1.000000000000000000 9
1 1 CAL_MAG2_YOFF 0.000000000000000000 9
1 1 CAL_MAG2_YSCALE 1.000000000000000000 9
1 1 CAL_MAG2_ZOFF 0.000000000000000000 9
1 1 CAL_MAG2_ZSCALE 1.000000000000000000 9
1 1 CAL_MAG3_ID 0 6
1 1 CAL_MAG3_ROT -1 6
1 1 CAL_MAG3_XOFF 0.000000000000000000 9
1 1 CAL_MAG3_XSCALE 1.000000000000000000 9
1 1 CAL_MAG3_YOFF 0.000000000000000000 9
1 1 CAL_MAG3_YSCALE 1.000000000000000000 9
1 1 CAL_MAG3_ZOFF 0.000000000000000000 9
1 1 CAL_MAG3_ZSCALE 1.000000000000000000 9
1 1 CAL_MAG_SIDES 63 6
1 1 CBRK_AIRSPD_CHK 0 6
1 1 CBRK_ENGINEFAIL 284953 6
1 1 CBRK_FLIGHTTERM 121212 6
1 1 CBRK_GPSFAIL 240024 6
1 1 CBRK_IO_SAFETY 22027 6
1 1 CBRK_RATE_CTRL 0 6
1 1 CBRK_SUPPLY_CHK 894281 6
1 1 CBRK_USB_CHK 197848 6
1 1 CBRK_VELPOSERR 0 6
1 1 COM_ARM_AUTH 256010 6
1 1 COM_ARM_EKF_AB 0.004999999888241291 9
1 1 COM_ARM_EKF_GB 0.000869999988935888 9
1 1 COM_ARM_EKF_HGT 1.000000000000000000 9
1 1 COM_ARM_EKF_POS 0.500000000000000000 9
1 1 COM_ARM_EKF_VEL 0.500000000000000000 9
1 1 COM_ARM_EKF_YAW 0.500000000000000000 9
1 1 COM_ARM_IMU_ACC 0.699999988079071045 9
1 1 COM_ARM_IMU_GYR 0.200000002980232239 9
1 1 COM_ARM_MIS_REQ 0 6
1 1 COM_ARM_SWISBTN 0 6
1 1 COM_ARM_WO_GPS 1 6
1 1 COM_DISARM_LAND 1 6
1 1 COM_DL_LOSS_T 10 6
1 1 COM_DL_REG_T 0 6
1 1 COM_EF_C2T 5.000000000000000000 9
1 1 COM_EF_THROT 0.500000000000000000 9
1 1 COM_EF_TIME 10.000000000000000000 9
1 1 COM_FLIGHT_UUID 34 6
1 1 COM_FLTMODE1 8 6
1 1 COM_FLTMODE2 -1 6
1 1 COM_FLTMODE3 -1 6
1 1 COM_FLTMODE4 1 6
1 1 COM_FLTMODE5 -1 6
1 1 COM_FLTMODE6 2 6
1 1 COM_HOME_H_T 5.000000000000000000 9
1 1 COM_HOME_V_T 10.000000000000000000 9
1 1 COM_LOW_BAT_ACT 2 6
1 1 COM_OBL_ACT 0 6
1 1 COM_OBL_RC_ACT 0 6
1 1 COM_OF_LOSS_T 0.000000000000000000 9
1 1 COM_POSCTL_NAVL 0 6
1 1 COM_POS_FS_DELAY 1 6
1 1 COM_POS_FS_GAIN 10 6
1 1 COM_POS_FS_PROB 30 6
1 1 COM_RC_ARM_HYST 1000 6
1 1 COM_RC_IN_MODE 0 6
1 1 COM_RC_LOSS_T 0.500000000000000000 9
1 1 COM_RC_OVERRIDE 0 6
1 1 COM_RC_STICK_OV 12.000000000000000000 9
1 1 FW_CLMBOUT_DIFF 10.000000000000000000 9
1 1 FW_MAN_P_SC 1.000000000000000000 9
1 1 FW_MAN_R_SC 1.000000000000000000 9
1 1 FW_MAN_Y_SC 1.000000000000000000 9
1 1 GF_ACTION 1 6
1 1 GF_ALTMODE 0 6
1 1 GF_COUNT -1 6
1 1 GF_MAX_HOR_DIST 0.000000000000000000 9
1 1 GF_MAX_VER_DIST 0.000000000000000000 9
1 1 GF_SOURCE 0 6
1 1 GPS_DUMP_COMM 0 6
1 1 GPS_UBX_DYNMODEL 7 6
1 1 IMU_ACCEL_CUTOFF 30.000000000000000000 9
1 1 IMU_GYRO_CUTOFF 30.000000000000000000 9
1 1 LED_RGB_MAXBRT 15 6
1 1 LNDMC_ALT_MAX 10000.000000000000000000 9
1 1 LNDMC_FFALL_THR 2.000000000000000000 9
1 1 LNDMC_FFALL_TTRI 0.300000011920928955 9
1 1 LNDMC_ROT_MAX 20.000000000000000000 9
1 1 LNDMC_THR_RANGE 0.100000001490116119 9
1 1 LNDMC_XY_VEL_MAX 1.500000000000000000 9
1 1 LNDMC_Z_VEL_MAX 0.500000000000000000 9
1 1 LND_FLIGHT_T_HI 0 6
1 1 LND_FLIGHT_T_LO 959960540 6
1 1 LPE_ACC_XY 0.012000000104308128 9
1 1 LPE_ACC_Z 0.019999999552965164 9
1 1 LPE_BAR_Z 3.000000000000000000 9
1 1 LPE_EPH_MAX 3.000000000000000000 9
1 1 LPE_EPV_MAX 5.000000000000000000 9
1 1 LPE_FAKE_ORIGIN 1 6
1 1 LPE_FGYRO_HP 0.001000000047497451 9
1 1 LPE_FLW_OFF_Z 0.000000000000000000 9
1 1 LPE_FLW_QMIN 150 6
1 1 LPE_FLW_R 7.000000000000000000 9
1 1 LPE_FLW_RR 7.000000000000000000 9
1 1 LPE_FLW_SCALE 1.299999952316284180 9
1 1 LPE_FUSION 28 6
1 1 LPE_GPS_DELAY 0.289999991655349731 9
1 1 LPE_GPS_VXY 0.250000000000000000 9
1 1 LPE_GPS_VZ 0.250000000000000000 9
1 1 LPE_GPS_XY 1.000000000000000000 9
1 1 LPE_GPS_Z 3.000000000000000000 9
1 1 LPE_LAND_VXY 0.050000000745058060 9
1 1 LPE_LAND_Z 0.029999999329447746 9
1 1 LPE_LAT 47.397743225097656250 9
1 1 LPE_LDR_OFF_Z 0.000000000000000000 9
1 1 LPE_LDR_Z 0.029999999329447746 9
1 1 LPE_LON 8.545594215393066406 9
1 1 LPE_PN_B 0.001000000047497451 9
1 1 LPE_PN_P 0.100000001490116119 9
1 1 LPE_PN_T 0.001000000047497451 9
1 1 LPE_PN_V 0.100000001490116119 9
1 1 LPE_SNR_OFF_Z 0.000000000000000000 9
1 1 LPE_SNR_Z 0.050000000745058060 9
1 1 LPE_T_MAX_GRADE 1.000000000000000000 9
1 1 LPE_VIC_P 0.001000000047497451 9
1 1 LPE_VIS_DELAY 0.000000000000000000 9
1 1 LPE_VIS_XY 0.100000001490116119 9
1 1 LPE_VIS_Z 0.100000001490116119 9
1 1 LPE_VXY_PUB 0.300000011920928955 9
1 1 LPE_X_LP 5.000000000000000000 9
1 1 LPE_Z_PUB 1.000000000000000000 9
1 1 MAV_BROADCAST 0 6
1 1 MAV_COMP_ID 1 6
1 1 MAV_FWDEXTSP 1 6
1 1 MAV_PROTO_VER 0 6
1 1 MAV_RADIO_ID 0 6
1 1 MAV_SYS_ID 1 6
1 1 MAV_TEST_PAR 1 6
1 1 MAV_TYPE 2 6
1 1 MAV_USEHILGPS 0 6
1 1 MC_ACRO_EXPO 0.689999997615814209 9
1 1 MC_ACRO_P_MAX 360.000000000000000000 9
1 1 MC_ACRO_R_MAX 360.000000000000000000 9
1 1 MC_ACRO_SUPEXPO 0.699999988079071045 9
1 1 MC_ACRO_Y_MAX 360.000000000000000000 9
1 1 MC_BAT_SCALE_EN 0 6
1 1 MC_PITCHRATE_D 0.003000000026077032 9
1 1 MC_PITCHRATE_FF 0.000000000000000000 9
1 1 MC_PITCHRATE_I 0.045000001788139343 9
1 1 MC_PITCHRATE_MAX 220.000000000000000000 9
1 1 MC_PITCHRATE_P 0.144999995827674866 9
1 1 MC_PITCH_P 6.500000000000000000 9
1 1 MC_PITCH_TC 0.200000002980232239 9
1 1 MC_PR_INT_LIM 0.300000011920928955 9
1 1 MC_RATT_TH 0.800000011920928955 9
1 1 MC_ROLLRATE_D 0.003000000026077032 9
1 1 MC_ROLLRATE_FF 0.000000000000000000 9
1 1 MC_ROLLRATE_I 0.045000001788139343 9
1 1 MC_ROLLRATE_MAX 220.000000000000000000 9
1 1 MC_ROLLRATE_P 0.144999995827674866 9
1 1 MC_ROLL_P 6.500000000000000000 9
1 1 MC_ROLL_TC 0.200000002980232239 9
1 1 MC_RR_INT_LIM 0.300000011920928955 9
1 1 MC_TPA_BREAK_D 1.000000000000000000 9
1 1 MC_TPA_BREAK_I 1.000000000000000000 9
1 1 MC_TPA_BREAK_P 1.000000000000000000 9
1 1 MC_TPA_RATE_D 0.000000000000000000 9
1 1 MC_TPA_RATE_I 0.000000000000000000 9
1 1 MC_TPA_RATE_P 0.000000000000000000 9
1 1 MC_YAWRATE_D 0.000000000000000000 9
1 1 MC_YAWRATE_FF 0.000000000000000000 9
1 1 MC_YAWRATE_I 0.100000001490116119 9
1 1 MC_YAWRATE_MAX 200.000000000000000000 9
1 1 MC_YAWRATE_P 0.200000002980232239 9
1 1 MC_YAWRAUTO_MAX 45.000000000000000000 9
1 1 MC_YAW_FF 0.500000000000000000 9
1 1 MC_YAW_P 2.799999952316284180 9
1 1 MC_YR_INT_LIM 0.300000011920928955 9
1 1 MIS_ALTMODE 1 6
1 1 MIS_DIST_1WP 900.000000000000000000 9
1 1 MIS_DIST_WPS 900.000000000000000000 9
1 1 MIS_LTRMIN_ALT -1.000000000000000000 9
1 1 MIS_ONBOARD_EN 1 6
1 1 MIS_TAKEOFF_ALT 2.500000000000000000 9
1 1 MIS_YAWMODE 1 6
1 1 MIS_YAW_ERR 12.000000000000000000 9
1 1 MIS_YAW_TMT -1.000000000000000000 9
1 1 MNT_MODE_IN -1 6
1 1 MOT_SLEW_MAX 0.000000000000000000 9
1 1 MPC_ACC_DOWN_MAX 5.000000000000000000 9
1 1 MPC_ACC_HOR 5.000000000000000000 9
1 1 MPC_ACC_HOR_MAX 5.000000000000000000 9
1 1 MPC_ACC_UP_MAX 5.000000000000000000 9
1 1 MPC_ALT_MODE 0 6
1 1 MPC_CRUISE_90 3.000000000000000000 9
1 1 MPC_DEC_HOR_SLOW 5.000000000000000000 9
1 1 MPC_HOLD_DZ 0.100000001490116119 9
1 1 MPC_HOLD_MAX_XY 0.800000011920928955 9
1 1 MPC_HOLD_MAX_Z 0.600000023841857910 9
1 1 MPC_JERK_MAX 0.000000000000000000 9
1 1 MPC_JERK_MIN 1.000000000000000000 9
1 1 MPC_LAND_ALT1 10.000000000000000000 9
1 1 MPC_LAND_ALT2 5.000000000000000000 9
1 1 MPC_LAND_SPEED 0.500000000000000000 9
1 1 MPC_MANTHR_MAX 0.899999976158142090 9
1 1 MPC_MANTHR_MIN 0.079999998211860657 9
1 1 MPC_MAN_TILT_MAX 35.000000000000000000 9
1 1 MPC_MAN_Y_MAX 200.000000000000000000 9
1 1 MPC_THR_HOVER 0.500000000000000000 9
1 1 MPC_THR_MAX 0.899999976158142090 9
1 1 MPC_THR_MIN 0.119999997317790985 9
1 1 MPC_TILTMAX_AIR 45.000000000000000000 9
1 1 MPC_TILTMAX_LND 12.000000000000000000 9
1 1 MPC_TKO_RAMP_T 0.400000005960464478 9
1 1 MPC_TKO_SPEED 1.500000000000000000 9
1 1 MPC_VELD_LP 5.000000000000000000 9
1 1 MPC_VEL_MANUAL 10.000000000000000000 9
1 1 MPC_XY_CRUISE 5.000000000000000000 9
1 1 MPC_XY_MAN_EXPO 0.000000000000000000 9
1 1 MPC_XY_P 0.949999988079071045 9
1 1 MPC_XY_VEL_D 0.009999999776482582 9
1 1 MPC_XY_VEL_I 0.019999999552965164 9
1 1 MPC_XY_VEL_MAX 8.000000000000000000 9
1 1 MPC_XY_VEL_P 0.090000003576278687 9
1 1 MPC_Z_MAN_EXPO 0.000000000000000000 9
1 1 MPC_Z_P 1.000000000000000000 9
1 1 MPC_Z_VEL_D 0.000000000000000000 9
1 1 MPC_Z_VEL_I 0.019999999552965164 9
1 1 MPC_Z_VEL_MAX_DN 1.000000000000000000 9
1 1 MPC_Z_VEL_MAX_UP 3.000000000000000000 9
1 1 MPC_Z_VEL_P 0.200000002980232239 9
1 1 NAV_ACC_RAD 2.000000000000000000 9
1 1 NAV_AH_ALT 600.000000000000000000 9
1 1 NAV_AH_LAT -265847810 6
1 1 NAV_AH_LON 1518423250 6
1 1 NAV_DLL_ACT 0 6
1 1 NAV_DLL_AH_T 120.000000000000000000 9
1 1 NAV_DLL_CHSK 0 6
1 1 NAV_DLL_CH_ALT 600.000000000000000000 9
1 1 NAV_DLL_CH_LAT -266072120 6
1 1 NAV_DLL_CH_LON 1518453890 6
1 1 NAV_DLL_CH_T 120.000000000000000000 9
1 1 NAV_DLL_N 2 6
1 1 NAV_FORCE_VT 1 6
1 1 NAV_FT_DST 8.000000000000000000 9
1 1 NAV_FT_FS 1 6
1 1 NAV_FT_RS 0.500000000000000000 9
1 1 NAV_FW_ALT_RAD 10.000000000000000000 9
1 1 NAV_GPSF_LT 30.000000000000000000 9
1 1 NAV_GPSF_P 0.000000000000000000 9
1 1 NAV_GPSF_R 15.000000000000000000 9
1 1 NAV_GPSF_TR 0.699999988079071045 9
1 1 NAV_LOITER_RAD 50.000000000000000000 9
1 1 NAV_MC_ALT_RAD 0.800000011920928955 9
1 1 NAV_MIN_FT_HT 8.000000000000000000 9
1 1 NAV_RCL_ACT 3 6
1 1 NAV_RCL_LT 120.000000000000000000 9
1 1 NAV_TRAFF_AVOID 1 6
1 1 PWM_AUX_DIS1 -1 6
1 1 PWM_AUX_DIS2 -1 6
1 1 PWM_AUX_DIS3 -1 6
1 1 PWM_AUX_DIS4 -1 6
1 1 PWM_AUX_DIS5 -1 6
1 1 PWM_AUX_DIS6 -1 6
1 1 PWM_AUX_DISARMED 1500 6
1 1 PWM_AUX_MAX 2000 6
1 1 PWM_AUX_MIN 1000 6
1 1 PWM_AUX_REV1 0 6
1 1 PWM_AUX_REV2 0 6
1 1 PWM_AUX_REV3 0 6
1 1 PWM_AUX_REV4 0 6
1 1 PWM_AUX_REV5 0 6
1 1 PWM_AUX_REV6 0 6
1 1 PWM_AUX_TRIM1 0.000000000000000000 9
1 1 PWM_AUX_TRIM2 0.000000000000000000 9
1 1 PWM_AUX_TRIM3 0.000000000000000000 9
1 1 PWM_AUX_TRIM4 0.000000000000000000 9
1 1 PWM_AUX_TRIM5 0.000000000000000000 9
1 1 PWM_AUX_TRIM6 0.000000000000000000 9
1 1 PWM_DISARMED 900 6
1 1 PWM_MAIN_DIS1 -1 6
1 1 PWM_MAIN_DIS2 -1 6
1 1 PWM_MAIN_DIS3 -1 6
1 1 PWM_MAIN_DIS4 -1 6
1 1 PWM_MAIN_DIS5 -1 6
1 1 PWM_MAIN_DIS6 -1 6
1 1 PWM_MAIN_DIS7 -1 6
1 1 PWM_MAIN_DIS8 -1 6
1 1 PWM_MAIN_REV1 0 6
1 1 PWM_MAIN_REV2 0 6
1 1 PWM_MAIN_REV3 0 6
1 1 PWM_MAIN_REV4 0 6
1 1 PWM_MAIN_REV5 0 6
1 1 PWM_MAIN_REV6 0 6
1 1 PWM_MAIN_REV7 0 6
1 1 PWM_MAIN_REV8 0 6
1 1 PWM_MAIN_TRIM1 0.000000000000000000 9
1 1 PWM_MAIN_TRIM2 0.000000000000000000 9
1 1 PWM_MAIN_TRIM3 0.000000000000000000 9
1 1 PWM_MAIN_TRIM4 0.000000000000000000 9
1 1 PWM_MAIN_TRIM5 0.000000000000000000 9
1 1 PWM_MAIN_TRIM6 0.000000000000000000 9
1 1 PWM_MAIN_TRIM7 0.000000000000000000 9
1 1 PWM_MAIN_TRIM8 0.000000000000000000 9
1 1 PWM_MAX 1950 6
1 1 PWM_MIN 1075 6
1 1 PWM_RATE 400 6
1 1 PWM_SBUS_MODE 0 6
1 1 RC10_DZ 0.000000000000000000 9
1 1 RC10_MAX 2000.000000000000000000 9
1 1 RC10_MIN 1000.000000000000000000 9
1 1 RC10_REV 1.000000000000000000 9
1 1 RC10_TRIM 1500.000000000000000000 9
1 1 RC11_DZ 0.000000000000000000 9
1 1 RC11_MAX 2000.000000000000000000 9
1 1 RC11_MIN 1000.000000000000000000 9
1 1 RC11_REV 1.000000000000000000 9
1 1 RC11_TRIM 1500.000000000000000000 9
1 1 RC12_DZ 0.000000000000000000 9
1 1 RC12_MAX 2000.000000000000000000 9
1 1 RC12_MIN 1000.000000000000000000 9
1 1 RC12_REV 1.000000000000000000 9
1 1 RC12_TRIM 1500.000000000000000000 9
1 1 RC13_DZ 0.000000000000000000 9
1 1 RC13_MAX 2000.000000000000000000 9
1 1 RC13_MIN 1000.000000000000000000 9
1 1 RC13_REV 1.000000000000000000 9
1 1 RC13_TRIM 1500.000000000000000000 9
1 1 RC14_DZ 0.000000000000000000 9
1 1 RC14_MAX 2000.000000000000000000 9
1 1 RC14_MIN 1000.000000000000000000 9
1 1 RC14_REV 1.000000000000000000 9
1 1 RC14_TRIM 1500.000000000000000000 9
1 1 RC15_DZ 0.000000000000000000 9
1 1 RC15_MAX 2000.000000000000000000 9
1 1 RC15_MIN 1000.000000000000000000 9
1 1 RC15_REV 1.000000000000000000 9
1 1 RC15_TRIM 1500.000000000000000000 9
1 1 RC16_DZ 0.000000000000000000 9
1 1 RC16_MAX 2000.000000000000000000 9
1 1 RC16_MIN 1000.000000000000000000 9
1 1 RC16_REV 1.000000000000000000 9
1 1 RC16_TRIM 1500.000000000000000000 9
1 1 RC17_DZ 0.000000000000000000 9
1 1 RC17_MAX 2000.000000000000000000 9
1 1 RC17_MIN 1000.000000000000000000 9
1 1 RC17_REV 1.000000000000000000 9
1 1 RC17_TRIM 1500.000000000000000000 9
1 1 RC18_DZ 0.000000000000000000 9
1 1 RC18_MAX 2000.000000000000000000 9
1 1 RC18_MIN 1000.000000000000000000 9
1 1 RC18_REV 1.000000000000000000 9
1 1 RC18_TRIM 1500.000000000000000000 9
1 1 RC1_DZ 10.000000000000000000 9
1 1 RC1_MAX 1999.000000000000000000 9
1 1 RC1_MIN 1000.000000000000000000 9
1 1 RC1_REV 1.000000000000000000 9
1 1 RC1_TRIM 1499.000000000000000000 9
1 1 RC2_DZ 10.000000000000000000 9
1 1 RC2_MAX 2000.000000000000000000 9
1 1 RC2_MIN 1001.000000000000000000 9
1 1 RC2_REV 1.000000000000000000 9
1 1 RC2_TRIM 1499.000000000000000000 9
1 1 RC3_DZ 10.000000000000000000 9
1 1 RC3_MAX 1989.000000000000000000 9
1 1 RC3_MIN 1000.000000000000000000 9
1 1 RC3_REV 1.000000000000000000 9
1 1 RC3_TRIM 1000.000000000000000000 9
1 1 RC4_DZ 10.000000000000000000 9
1 1 RC4_MAX 1999.000000000000000000 9
1 1 RC4_MIN 1018.000000000000000000 9
1 1 RC4_REV 1.000000000000000000 9
1 1 RC4_TRIM 1501.000000000000000000 9
1 1 RC5_DZ 10.000000000000000000 9
1 1 RC5_MAX 2000.000000000000000000 9
1 1 RC5_MIN 1000.000000000000000000 9
1 1 RC5_REV 1.000000000000000000 9
1 1 RC5_TRIM 1500.000000000000000000 9
1 1 RC6_DZ 10.000000000000000000 9
1 1 RC6_MAX 2000.000000000000000000 9
1 1 RC6_MIN 1000.000000000000000000 9
1 1 RC6_REV 1.000000000000000000 9
1 1 RC6_TRIM 1500.000000000000000000 9
1 1 RC7_DZ 10.000000000000000000 9
1 1 RC7_MAX 2000.000000000000000000 9
1 1 RC7_MIN 1000.000000000000000000 9
1 1 RC7_REV 1.000000000000000000 9
1 1 RC7_TRIM 1500.000000000000000000 9
1 1 RC8_DZ 10.000000000000000000 9
1 1 RC8_MAX 2000.000000000000000000 9
1 1 RC8_MIN 1000.000000000000000000 9
1 1 RC8_REV 1.000000000000000000 9
1 1 RC8_TRIM 1500.000000000000000000 9
1 1 RC9_DZ 0.000000000000000000 9
1 1 RC9_MAX 2000.000000000000000000 9
1 1 RC9_MIN 1000.000000000000000000 9
1 1 RC9_REV 1.000000000000000000 9
1 1 RC9_TRIM 1500.000000000000000000 9
1 1 RC_ACRO_TH 0.500000000000000000 9
1 1 RC_ARMSWITCH_TH 0.250000000000000000 9
1 1 RC_ASSIST_TH 0.250000000000000000 9
1 1 RC_AUTO_TH 0.750000000000000000 9
1 1 RC_CHAN_CNT 8 6
1 1 RC_FAILS_THR 0 6
1 1 RC_FLT_CUTOFF 10.000000000000000000 9
1 1 RC_FLT_SMP_RATE 50.000000000000000000 9
1 1 RC_GEAR_TH 0.250000000000000000 9
1 1 RC_KILLSWITCH_TH 0.250000000000000000 9
1 1 RC_LOITER_TH 0.500000000000000000 9
1 1 RC_MAN_TH 0.500000000000000000 9
1 1 RC_MAP_ACRO_SW 0 6
1 1 RC_MAP_ARM_SW 0 6
1 1 RC_MAP_AUX1 0 6
1 1 RC_MAP_AUX2 0 6
1 1 RC_MAP_AUX3 0 6
1 1 RC_MAP_AUX4 0 6
1 1 RC_MAP_AUX5 0 6
1 1 RC_MAP_FAILSAFE 0 6
1 1 RC_MAP_FLAPS 0 6
1 1 RC_MAP_FLTMODE 5 6
1 1 RC_MAP_GEAR_SW 0 6
1 1 RC_MAP_KILL_SW 6 6
1 1 RC_MAP_LOITER_SW 0 6
1 1 RC_MAP_MAN_SW 0 6
1 1 RC_MAP_MODE_SW 0 6
1 1 RC_MAP_OFFB_SW 0 6
1 1 RC_MAP_PARAM1 0 6
1 1 RC_MAP_PARAM2 0 6
1 1 RC_MAP_PARAM3 0 6
1 1 RC_MAP_PITCH 2 6
1 1 RC_MAP_POSCTL_SW 0 6
1 1 RC_MAP_RATT_SW 0 6
1 1 RC_MAP_RETURN_SW 0 6
1 1 RC_MAP_ROLL 1 6
1 1 RC_MAP_STAB_SW 0 6
1 1 RC_MAP_THROTTLE 3 6
1 1 RC_MAP_TRANS_SW 0 6
1 1 RC_MAP_YAW 4 6
1 1 RC_OFFB_TH 0.500000000000000000 9
1 1 RC_POSCTL_TH 0.500000000000000000 9
1 1 RC_RATT_TH 0.500000000000000000 9
1 1 RC_RETURN_TH 0.500000000000000000 9
1 1 RC_RSSI_PWM_CHAN 0 6
1 1 RC_RSSI_PWM_MAX 1000 6
1 1 RC_RSSI_PWM_MIN 2000 6
1 1 RC_STAB_TH 0.500000000000000000 9
1 1 RC_TRANS_TH 0.250000000000000000 9
1 1 RTL_DESCEND_ALT 10.000000000000000000 9
1 1 RTL_LAND_DELAY 0.000000000000000000 9
1 1 RTL_MIN_DIST 5.000000000000000000 9
1 1 RTL_RETURN_ALT 30.000000000000000000 9
1 1 SDLOG_DIRS_MAX 0 6
1 1 SDLOG_MODE 0 6
1 1 SDLOG_PROFILE 3 6
1 1 SDLOG_UTC_OFFSET 0 6
1 1 SENS_BARO_QNH 1013.250000000000000000 9
1 1 SENS_BOARD_ROT 0 6
1 1 SENS_BOARD_X_OFF 0.576516091823577881 9
1 1 SENS_BOARD_Y_OFF 2.084044218063354492 9
1 1 SENS_BOARD_Z_OFF 0.000000000000000000 9
1 1 SENS_DPRES_ANSC 0.000000000000000000 9
1 1 SENS_DPRES_OFF 0.000000000000000000 9
1 1 SENS_EN_LL40LS 0 6
1 1 SENS_EN_MB12XX 0 6
1 1 SENS_EN_SF0X 0 6
1 1 SENS_EN_SF1XX 0 6
1 1 SENS_EN_THERMAL -1 6
1 1 SENS_EN_TRANGER 0 6
1 1 SYS_AUTOCONFIG 0 6
1 1 SYS_AUTOSTART 4001 6
1 1 SYS_CAL_ACCEL 0 6
1 1 SYS_CAL_BARO 0 6
1 1 SYS_CAL_GYRO 0 6
1 1 SYS_CAL_TDEL 24 6
1 1 SYS_CAL_TMAX 10 6
1 1 SYS_CAL_TMIN 5 6
1 1 SYS_COMPANION 921600 6
1 1 SYS_FMU_TASK 0 6
1 1 SYS_HITL 0 6
1 1 SYS_LOGGER 1 6
1 1 SYS_MC_EST_GROUP 1 6
1 1 SYS_PARAM_VER 1 6
1 1 SYS_RESTART_TYPE 0 6
1 1 SYS_STCK_EN 1 6
1 1 SYS_USE_IO 1 6
1 1 TC_A0_ID 0 6
1 1 TC_A0_SCL_0 1.000000000000000000 9
1 1 TC_A0_SCL_1 1.000000000000000000 9
1 1 TC_A0_SCL_2 1.000000000000000000 9
1 1 TC_A0_TMAX 100.000000000000000000 9
1 1 TC_A0_TMIN 0.000000000000000000 9
1 1 TC_A0_TREF 25.000000000000000000 9
1 1 TC_A0_X0_0 0.000000000000000000 9
1 1 TC_A0_X0_1 0.000000000000000000 9
1 1 TC_A0_X0_2 0.000000000000000000 9
1 1 TC_A0_X1_0 0.000000000000000000 9
1 1 TC_A0_X1_1 0.000000000000000000 9
1 1 TC_A0_X1_2 0.000000000000000000 9
1 1 TC_A0_X2_0 0.000000000000000000 9
1 1 TC_A0_X2_1 0.000000000000000000 9
1 1 TC_A0_X2_2 0.000000000000000000 9
1 1 TC_A0_X3_0 0.000000000000000000 9
1 1 TC_A0_X3_1 0.000000000000000000 9
1 1 TC_A0_X3_2 0.000000000000000000 9
1 1 TC_A1_ID 0 6
1 1 TC_A1_SCL_0 1.000000000000000000 9
1 1 TC_A1_SCL_1 1.000000000000000000 9
1 1 TC_A1_SCL_2 1.000000000000000000 9
1 1 TC_A1_TMAX 100.000000000000000000 9
1 1 TC_A1_TMIN 0.000000000000000000 9
1 1 TC_A1_TREF 25.000000000000000000 9
1 1 TC_A1_X0_0 0.000000000000000000 9
1 1 TC_A1_X0_1 0.000000000000000000 9
1 1 TC_A1_X0_2 0.000000000000000000 9
1 1 TC_A1_X1_0 0.000000000000000000 9
1 1 TC_A1_X1_1 0.000000000000000000 9
1 1 TC_A1_X1_2 0.000000000000000000 9
1 1 TC_A1_X2_0 0.000000000000000000 9
1 1 TC_A1_X2_1 0.000000000000000000 9
1 1 TC_A1_X2_2 0.000000000000000000 9
1 1 TC_A1_X3_0 0.000000000000000000 9
1 1 TC_A1_X3_1 0.000000000000000000 9
1 1 TC_A1_X3_2 0.000000000000000000 9
1 1 TC_A2_ID 0 6
1 1 TC_A2_SCL_0 1.000000000000000000 9
1 1 TC_A2_SCL_1 1.000000000000000000 9
1 1 TC_A2_SCL_2 1.000000000000000000 9
1 1 TC_A2_TMAX 100.000000000000000000 9
1 1 TC_A2_TMIN 0.000000000000000000 9
1 1 TC_A2_TREF 25.000000000000000000 9
1 1 TC_A2_X0_0 0.000000000000000000 9
1 1 TC_A2_X0_1 0.000000000000000000 9
1 1 TC_A2_X0_2 0.000000000000000000 9
1 1 TC_A2_X1_0 0.000000000000000000 9
1 1 TC_A2_X1_1 0.000000000000000000 9
1 1 TC_A2_X1_2 0.000000000000000000 9
1 1 TC_A2_X2_0 0.000000000000000000 9
1 1 TC_A2_X2_1 0.000000000000000000 9
1 1 TC_A2_X2_2 0.000000000000000000 9
1 1 TC_A2_X3_0 0.000000000000000000 9
1 1 TC_A2_X3_1 0.000000000000000000 9
1 1 TC_A2_X3_2 0.000000000000000000 9
1 1 TC_A_ENABLE 0 6
1 1 TC_B0_ID 0 6
1 1 TC_B0_SCL 1.000000000000000000 9
1 1 TC_B0_TMAX 75.000000000000000000 9
1 1 TC_B0_TMIN 5.000000000000000000 9
1 1 TC_B0_TREF 40.000000000000000000 9
1 1 TC_B0_X0 0.000000000000000000 9
1 1 TC_B0_X1 0.000000000000000000 9
1 1 TC_B0_X2 0.000000000000000000 9
1 1 TC_B0_X3 0.000000000000000000 9
1 1 TC_B0_X4 0.000000000000000000 9
1 1 TC_B0_X5 0.000000000000000000 9
1 1 TC_B1_ID 0 6
1 1 TC_B1_SCL 1.000000000000000000 9
1 1 TC_B1_TMAX 75.000000000000000000 9
1 1 TC_B1_TMIN 5.000000000000000000 9
1 1 TC_B1_TREF 40.000000000000000000 9
1 1 TC_B1_X0 0.000000000000000000 9
1 1 TC_B1_X1 0.000000000000000000 9
1 1 TC_B1_X2 0.000000000000000000 9
1 1 TC_B1_X3 0.000000000000000000 9
1 1 TC_B1_X4 0.000000000000000000 9
1 1 TC_B1_X5 0.000000000000000000 9
1 1 TC_B2_ID 0 6
1 1 TC_B2_SCL 1.000000000000000000 9
1 1 TC_B2_TMAX 75.000000000000000000 9
1 1 TC_B2_TMIN 5.000000000000000000 9
1 1 TC_B2_TREF 40.000000000000000000 9
1 1 TC_B2_X0 0.000000000000000000 9
1 1 TC_B2_X1 0.000000000000000000 9
1 1 TC_B2_X2 0.000000000000000000 9
1 1 TC_B2_X3 0.000000000000000000 9
1 1 TC_B2_X4 0.000000000000000000 9
1 1 TC_B2_X5 0.000000000000000000 9
1 1 TC_B_ENABLE 0 6
1 1 TC_G0_ID 0 6
1 1 TC_G0_SCL_0 1.000000000000000000 9
1 1 TC_G0_SCL_1 1.000000000000000000 9
1 1 TC_G0_SCL_2 1.000000000000000000 9
1 1 TC_G0_TMAX 100.000000000000000000 9
1 1 TC_G0_TMIN 0.000000000000000000 9
1 1 TC_G0_TREF 25.000000000000000000 9
1 1 TC_G0_X0_0 0.000000000000000000 9
1 1 TC_G0_X0_1 0.000000000000000000 9
1 1 TC_G0_X0_2 0.000000000000000000 9
1 1 TC_G0_X1_0 0.000000000000000000 9
1 1 TC_G0_X1_1 0.000000000000000000 9
1 1 TC_G0_X1_2 0.000000000000000000 9
1 1 TC_G0_X2_0 0.000000000000000000 9
1 1 TC_G0_X2_1 0.000000000000000000 9
1 1 TC_G0_X2_2 0.000000000000000000 9
1 1 TC_G0_X3_0 0.000000000000000000 9
1 1 TC_G0_X3_1 0.000000000000000000 9
1 1 TC_G0_X3_2 0.000000000000000000 9
1 1 TC_G1_ID 0 6
1 1 TC_G1_SCL_0 1.000000000000000000 9
1 1 TC_G1_SCL_1 1.000000000000000000 9
1 1 TC_G1_SCL_2 1.000000000000000000 9
1 1 TC_G1_TMAX 100.000000000000000000 9
1 1 TC_G1_TMIN 0.000000000000000000 9
1 1 TC_G1_TREF 25.000000000000000000 9
1 1 TC_G1_X0_0 0.000000000000000000 9
1 1 TC_G1_X0_1 0.000000000000000000 9
1 1 TC_G1_X0_2 0.000000000000000000 9
1 1 TC_G1_X1_0 0.000000000000000000 9
1 1 TC_G1_X1_1 0.000000000000000000 9
1 1 TC_G1_X1_2 0.000000000000000000 9
1 1 TC_G1_X2_0 0.000000000000000000 9
1 1 TC_G1_X2_1 0.000000000000000000 9
1 1 TC_G1_X2_2 0.000000000000000000 9
1 1 TC_G1_X3_0 0.000000000000000000 9
1 1 TC_G1_X3_1 0.000000000000000000 9
1 1 TC_G1_X3_2 0.000000000000000000 9
1 1 TC_G2_ID 0 6
1 1 TC_G2_SCL_0 1.000000000000000000 9
1 1 TC_G2_SCL_1 1.000000000000000000 9
1 1 TC_G2_SCL_2 1.000000000000000000 9
1 1 TC_G2_TMAX 100.000000000000000000 9
1 1 TC_G2_TMIN 0.000000000000000000 9
1 1 TC_G2_TREF 25.000000000000000000 9
1 1 TC_G2_X0_0 0.000000000000000000 9
1 1 TC_G2_X0_1 0.000000000000000000 9
1 1 TC_G2_X0_2 0.000000000000000000 9
1 1 TC_G2_X1_0 0.000000000000000000 9
1 1 TC_G2_X1_1 0.000000000000000000 9
1 1 TC_G2_X1_2 0.000000000000000000 9
1 1 TC_G2_X2_0 0.000000000000000000 9
1 1 TC_G2_X2_1 0.000000000000000000 9
1 1 TC_G2_X2_2 0.000000000000000000 9
1 1 TC_G2_X3_0 0.000000000000000000 9
1 1 TC_G2_X3_1 0.000000000000000000 9
1 1 TC_G2_X3_2 0.000000000000000000 9
1 1 TC_G_ENABLE 0 6
1 1 THR_MDL_FAC 0.000000000000000000 9
1 1 TRIG_MODE 0 6
1 1 TRIM_PITCH 0.000000000000000000 9
1 1 TRIM_ROLL 0.000000000000000000 9
1 1 TRIM_YAW 0.000000000000000000 9
1 1 VT_B_DEC_MSS 2.000000000000000000 9
1 1 VT_B_REV_DEL 0.000000000000000000 9

Binary file not shown.

Before

Width:  |  Height:  |  Size: 177 KiB

After

Width:  |  Height:  |  Size: 122 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 94 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 32 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 118 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 40 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 46 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 175 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 235 KiB

View File

@@ -1,23 +0,0 @@
# 3D-scanning drone
<img src="../assets/3d_drone_1.jpg" title="3D-scanning drone">
The project was created in collaboration with Texel inc. that develops 3D-scanners for scanning people.
Our fellows from Texel provided a module consisting of a Raspberry Pi and a PrimeSense 3D-sensor.
We provided a Clever 3 drone that's capable of autonomous flight and wrote a flight program.
To make it all work we conducted many tests, made changes in the drone's design and tuned the drone properly.
## Video
<iframe width="966" height="543" src="https://www.youtube.com/embed/aqBION3TVhg" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
## Team
The project was made by:
* Timofei Kondratiev [Copter Express] - drone assembly, writing and debugging the program, conducting tests;
* Anton Maltsev [Copter Express] - modeling of the protection of the propellers;
* Andrei Poskonin [Texel] - modifying the Texel's software to work on Raspberry Pi.

View File

@@ -45,20 +45,16 @@
* [Computer vision basics](camera.md)
* [LED strip](leds.md)
* [Using rviz and rqt](rviz.md)
* [Working with GPIO](gpio.md)
* [Interfacing with a sonar](sonar.md)
* [Interfacing with a laser rangefinder](laser.md)
* [PX4 Simulation](sitl.md)
* [Software autorun](autolaunch.md)
* [Hostname](hostname.md)
* [Controlling the copter from Arduino](arduino.md)
* [Using an external 3G modem](3g.md)
* Clever-based projects
* [Copter spheric guard](shield.md)
* [Face recognition system](face_recognition.md)
* [Android RC app](android.md)
* [3D-scanning drone](3dscan.md)
* [Human pose estimation drone control](human_pose_estimation_drone_control.md)
* [Copter Hack 2018](copterhack2018.md)
* [Copter Hack 2017](copterhack2017.md)
* Supplementary materials

View File

@@ -10,12 +10,6 @@
## Configuration
Set the `aruco` argument in `~/catkin_ws/src/clever/clever/launch/aruco.launch` to `true`:
```xml
<arg name="aruco" default="true"/>
```
In order to enable map detection set `aruco_map` and `aruco_detect` arguments to `true` in `~/catkin_ws/src/clever/clever/launch/aruco.launch`:
```xml

View File

@@ -10,13 +10,7 @@ Using this module along with [map-based navigation](aruco_map.md) is also possib
## Setup
Set the `aruco` argument in `~/catkin_ws/src/clever/clever/launch/aruco.launch` to `true`:
```xml
<arg name="aruco" default="true"/>
```
For enabling detection set the `aruco_detect` argument in `~/catkin_ws/src/clever/clever/launch/aruco.launch` to `true`:
Set the `aruco_detect` argument in `~/catkin_ws/src/clever/clever/launch/aruco.launch` to `true` to automatically launch the module:
```xml
<arg name="aruco_detect" default="true"/>

View File

@@ -11,7 +11,7 @@ An unmanned aerial vehicle with an electronic stabilization system and the numbe
## Flight controller / autopilot
**1\.** A specialized circuit-board designed for controlling a multicopter, an aircraft or another apparatus. Examples:
Pixhawk, ArduPilot, Naze32, CC3D.
Pixhawk, Ardupilot, Naze32, CC3D.
**2\.** Software for the multicopter control circuit-board. Examples: PX4, APM, CleanFlight.
@@ -41,7 +41,7 @@ Clever may also be [controlled from a smartphone](rc.md).
## Arming
Armed is the state of copter readiness for the flight. When the gas stick is lifted, or when an external command with the target point is sent, the copter will fly. Usually, a copter starts rotating its propellers when it is switched to the "armed" state, even if the gas stick is down.
Armed is the state of copter readiness for the fligh. When the gas stick is lifted, or when an external command with the target point is sent, the copter will fly. Usually, a copter starts rotating its propellers when it is switched to the "armed" state, even if the gas stick is down.
The opposite state is Disarmed.

View File

@@ -1,98 +0,0 @@
# Working with GPIO
A GPIO (General-Purpose Input/Output) pin is a programmable digital signal pin on a circuit board or a microcontroller that may act as an input or an output. Raspberry Pi has a set of easily accessible GPIO pins, some of which have hardware <abbr title="Pulse-width modulation">PWM</abbr>.
> **Info** Use the [pinout](https://pinout.xyz) for figuring out, which Raspberry Pi's pins support GPIO and PWM.
The [`pigpio`](http://abyz.me.uk/rpi/pigpio) library for interfacing with the GPIO pins is already preinstalled on [the RPi image](microsd_images.md). To interact with this library, run the appropriate daemon:
```bash
sudo systemctl start pigpiod.service
```
To enable automatic launch of the daemon, run:
```bash
sudo systemctl enable pigpiod.service
```
Example of working with the library:
```python
import time
import pigpio
# initializing connection to pigpiod
pi = pigpio.pi()
# set pin 11 mode for output
pi.set_mode(11, pigpio.OUTPUT)
# set signal of pin 11 to high
pi.write(11, 1)
time.sleep(2)
# set signal on pin 11 to low
pi.write(11, 0)
# ...
# setting pin 12 mode for input
pi.set_mode(12, pigpio.INPUT)
# read the state of pin 12
level = pi.read(12)
```
To find out the pins' numbers, consult the [Raspberry Pi pinout](https://pinout.xyz).
## Connecting servos
Servo motors are typically controlled with PWM signal. Extreme positions of servos are reached with signal widths approximately equal to 1000 and 2000 µs. Values for a specific servo can be determined experimentally.
Connect the signal wire of the servo to one of GPIO-pins of the Raspberry. To control a servo connected to the pin 13 use a code like this:
```python
import time
import pigpio
pi = pigpio.pi()
# set mode of pin 13 to output
pi.set_mode(13, pigpio.OUTPUT)
# set pin 13 to output PWM signal 1000 us
pi.set_servo_pulsewidth(13, 1000)
time.sleep(2)
# set pin 13 to output PWM signal 2000 us
pi.set_servo_pulsewidth(13, 2000)
```
## Connecting an electromagnet
![GPIO Mosfet Magnet Connection](../assets/gpio_mosfet_magnet.png)
To connect an electromagnet use a field-effect transistor (MOSFET). Connect the MOSFET to one of GPIO-pins of the Raspberry Pi. To control the magnet connected to the pin 15 use a code like this:
```python
import time
import pigpio
pi = pigpio.pi()
# set mode of pin 15 for output
pi.set_mode(15, pigpio.OUTPUT)
# enable the magnet
pi.write(15, 1)
time.sleep(2)
# disable the magnet
pi.write(15, 0)
```
> **Note** A more [comprehensive description](https://elinux.org/RPi_Low-level_peripherals) of the Raspberry Pi GPIO pins and [additional examples](https://elinux.org/RPi_GPIO_Interface_Circuits) of circuits are available at the [Embedded Linux wiki](https://elinux.org/RPi_Hub).

View File

@@ -1,31 +0,0 @@
# Hostname
[By default](microsd_images.md) the hostname of the Clever drone is set to `clever-xxxx`, where `xxxx` are random numbers. These numbers are the same as in the [Wi-Fi SSID](wifi.md).
Thus, Clever is accessible on machines that support mDNS as `clever-xxxx.local`. You can use this name to access Clever over SSH:
```bash
ssh pi@clever-xxxx.local
```
Also, this name can be used in place of IP-address to open Clever web pages in browser, accessing ROS master, etc.
## Changing hostname
In some situations it is necessary to change Clever's hostname. You can use the `hostnamectl` utility for that:
```bash
sudo hostnamectl set-hostname newname
```
Where `newname` is the new name of the machine. `hostnamectl` utility will change the name in `/etc/hostname` file.
You should also put the new name to `/etc/hosts` file:
```txt
127.0.1.1 newname newname.local
```
Setting `newname.local` is necessary to allow ROS to resolve this name in situations where all the network interfaces are down (when Wi-Fi is turned off or disconnected).
> **Note** Changing the hostname does not affect the Wi-Fi SSID (and vice versa, changing the Wi-Fi SSID won't affect the hostname).

View File

@@ -1,152 +0,0 @@
# Human Pose Estimation drone control
## Introduction
Human pose estimation is one of the computer vision applications in order to estimate all the joints and the different poses of the human body through a special camera and a special hardware or process the images from a regular camera by machine learning and deep learning techniques. This project is about controlling the drone through the poses of a person in front of regular camera without needing to an external hardware or hard build dependencies on your computer.
## Demo
<iframe width="770" height="421" src="https://www.youtube.com/embed/ucPONeHg2lk" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
<iframe width="853" height="480" src="https://www.youtube.com/embed/EDcTtPLxzoU" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
In the demo video, we were using Ubuntu 18.04 and clever4 drone.
## Development
We used posenet from tensorflow.js as our human pose estimation module because it is easier to use, build, fast and compatible with different environments(Hardware and OS). You can find the work of posenet for this project [here](https://github.com/hany606/tfjs-posenet). Websockets were used as communication protocol between the browser and a running server on the drone.
## Architecture
The image below is a visualization of our architecture for the project.
![Architecture](../assets/human_pose_estimation_project_architecture.png)
This figure is made from [here](https://www.draw.io/)
## Dependencies
Before you test it you need to install on your laptop:
- Install Nodejs from [here](https://nodejs.org/en/download/). For [Ubuntu installation](https://tecadmin.net/install-latest-nodejs-npm-on-ubuntu/)
- Install Yarn package manager from [here](https://yarnpkg.com/lang/en/docs/install/). [Usual problem](https://github.com/yarnpkg/yarn/issues/3189) while installing and using yarn with Ubuntu.
- Have an experience in manual control on the drone in case of any weird behavior happen.
- Worked before with COEX drones, if this is your first time to work with COEX drones check [this](https://clever.copterexpress.com/en/).
and you are ready to build and use the required codes.
## Setup & installation
### In your main laptop
#### (It has been tested until now only on Ubuntu 18.04)
- Clone the repo of posenet in your computer or download it if you are using Windows without GitHub
```sh
git clone https://github.com/hany606/tfjs-posenet.git
```
- Do the steps of running and setup as it is described in the README [here](https://github.com/hany606/tfjs-posenet/tree/master/posenet)
### In the Raspberry Pi of the drone (Main controller)
- Access the Raspberry Pi
- [Switch to Client mode](https://clever.copterexpress.com/en/network.html) and ensure that the network has internet connection.
Notice: I have already made a bash script based on that tutorial, it is in COEX-Internship19/helpers/ called .to_client.bash
To run it:
```sh
chmod +x .to_client.bash
./.to_client <NAME_OF_NETWORK> <PASSWORD>
```
- Install the tornado library to make a WebSocket server
```sh
sudo pip install tornado
```
- Clone the main repo on the Raspberry Pi of the drone
```sh
git clone https://github.com/hany606/COEX-Internship19.git
```
- Go to the project directory
```sh
cd COEX-Internship19/projects/Human_pose_estimation_drone_control/
```
- Run the server to test that everything is correct and run the posenet, you should see a lot of data is printed in the terminal (if you are running the human pose estimation code on your main computer, just refresh the page in the browser after running the below command in Raspberry Pi)
```sh
python websocket_server_test.py
```
- Close the server using Ctrl+C
- To run the main file
```sh
python main_drone.py
```
## How to use it
- Run the server first from the Raspberry Pi from the correct directory
```sh
python main_drone.py
```
- Run Human pose estimation module on your laptop with WebSocket by
```sh
yarn websocket
```
Or refresh the page if you already run it.
- You should see the instructions on the screen of the terminal of the Raspberry Pi right now.
- Firstly, you should be visible for the camera and it is better to have a clear background without many details.
- Secondly, you should do initial pose as it is described in the images below.
- You can perform any pose and try to keep it until your drone finish doing this move that is corresponding to the pose.
- After you do the pose return to the initial pose in order to give the drone the command to listen to another pose.
- If you want to stop the program, land the drone and don't return to the initial pose and press Ctrl+C to stop the drone.
## Poses
![Poses](../assets/human_pose_estimation_project_poses.jpg)
Animation is created by [this](https://justsketchme.web.app/)
## Notes
- Websockets are used to communicate between the page on the browser that runs posenet and the drone.
- As the model of posenet is already pre-trained and using tensorflow.js. So, it is quite fast and can run on different computers without any problems thanks to yarn, parcel and tensorflow.js, and we have configured the code of posenet to the minimal configuration to not require a lot of computation power.
- This project has been built in 1 week of working, it took a lot of time trying to make openpose and google colab working but unfortunately I had many errors and one I decided to move to posenet everything was pretty easy.
- If you have any comments about the codes to try to improve it, I will be happy if you can contact me through telegram: @hany606 or email: h.hamed.elanwar@gmail.com or do pull requests.
- If you have implemented any of the applications below, or do some improvements, we will be very happy for that.
## Future application
- [Drone wars](https://web.facebook.com/COEXDrones/photos/pcb.1129309377266616/1129308437266710/?type=3&theater): Control the drone during the drones battle using human poses. It requires high speed interaction and more precise control.
- Control a drone that draw graffiti using human poses and draw in real-time.
- Playing with balls like ping pong game with the drones. It may require 3D Human Pose estimation Algorithms.
- Control two drones by your arms and do some task together.
## Acknowledgments
- This project was part of an internship in COEX in July 2019. if you found any bugs or problems, you can contact me through telegram: @hany606 or email: h.hamed.elanwar@gmail.com.
- The above applications were thought by me and my internship supervisor Timofey.
## References
- [Human pose estimation guide](https://blog.nanonets.com/human-pose-estimation-2d-guide/)
- [Clever drones tutorials](https://clever.copterexpress.com/en/)
- [Posenet GitHub repo](https://github.com/tensorflow/tfjs-models/tree/master/posenet)
- [Posenet meduim article](https://medium.com/tensorflow/real-time-human-pose-estimation-in-the-browser-with-tensorflow-js-7dd0bc881cd5)
- [Tensorflow.js demos](https://www.tensorflow.org/js/demos)
- [Posenet overview](https://www.tensorflow.org/lite/models/pose_estimation/overview)

View File

@@ -52,4 +52,4 @@ Messages published in the topics may be viewed with the `rostopic` utility, e.g.
`/mavros/setpoint_raw/attitude` — sends [SET\_ATTITUDE\_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET) message. Allows setting the target attitude /angular velocity and throttle level. The values to be set are selected using the `type_mask` field
`/mavros/setpoint_raw/global` — sends [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Allows setting the target attitude in global coordinates \(latitude, longitude, altitude\) and flight speed. **Not supported in PX4** \([issue](https://github.com/PX4/Firmware/issues/7552)\).
`/mavros/setpoint_raw/global` — sends [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Allows setting the target attitude in global coordinates \(latitude, longitude, altitude\) and flight speed. **May not be supported in earlier releases of PX4** \([issue](https://github.com/PX4/Firmware/issues/7552)\).

View File

@@ -1,21 +1,22 @@
# Configuring Wi-Fi
The Raspberry Pi Wi-Fi adapter has two main operating modes:
The Raspberry Pi Wi-Fi adapter of has two main operating modes:
1. **Client mode** RPi connects to an existing Wi-Fi network.
2. **Access point mode** RPi creates a Wi-Fi network that you can connect to.
On our [RPi image](microsd_images.md) the Wi-Fi adapter is confuigured to use the [access point mode](Wi-Fi.md) by default.
When using [the RPi image](microsd_images.md), the Wi-Fi adapter works in the [access point mode] by default (Wi-Fi.md).
## Changing the password or SSID (of the network name)
1. Edit the `/etc/wpa_supplicant/wpa_supplicant.conf` file (using [SSH connection](ssh.md)):
1. Edit file `/etc/wpa_supplicant/wpa_supplicant.conf` (using [SSH connection](ssh.md)):
```bash
sudo nano /etc/wpa_supplicant/wpa_supplicant.conf
```
In order to change the name of the Wi-Fi network, change the value of the `ssid` parameter; to change the password, change the `psk` parameter. For example:
To change the name of the Wi-Fi network, change the value of parameter `ssid`, to change the password, change parameter `psk`. For example:
```txt
network={
@@ -32,9 +33,9 @@ On our [RPi image](microsd_images.md) the Wi-Fi adapter is confuigured to use th
2. Restart Raspberry Pi.
> **Caution** The Wi-Fi network password should be **at least** 8 characters.
> **Caution** The length of the password for the Wi-Fi network should be **at least** 8 characters.
>
> If your `wpa_supplicant.conf` is not valid, Raspberry Pi will not allow Wi-Fi connections!
> In case of incorrect settings `wpa_supplicant.conf`, Raspberry Pi will stop distributing Wi-Fi!
## Switching adapter to the client mode
@@ -45,27 +46,39 @@ On our [RPi image](microsd_images.md) the Wi-Fi adapter is confuigured to use th
sudo systemctl disable dnsmasq
```
2. Enable DHCP client on the wireless interface to obtain IP address. In order to do this, remove the following lines from the `etc/dhcpcd.conf` file:
2. Enable obtaining IP address on the wireless interface by the DHCP client.
For this purpose, remove the following lines
```conf
interface wlan0
static ip_address=192.168.11.1/24
```
3. Configure `wpa_supplicant` to connect to an existing access point. Change your `/etc/wpa_supplicant/wpa_supplicant.conf` to contain the following:
from file `/etc/dhcpcd.conf` manually, or run the following commands.
```bash
sudo sed -i 's/interface wlan0//' /etc/dhcpcd.conf
sudo sed -i 's/static ip_address=192.168.11.1\/24//' /etc/dhcpcd.conf
```
3. Configure `wpa_supplicant` to connect to an existing access point.
```bash
cat << EOF | sudo tee /etc/wpa_supplicant/wpa_supplicant.conf
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
network={
ssid="SSID"
psk="password"
ssid="CLEVER"
psk="cleverwifi"
}
EOF
```
where `SSID` is the name of the network, and `password` is its password.
where `CLEVER` is the name of the network, and `cleverwifi` is the password.
4. Restart the `dhcpcd` service.
@@ -75,22 +88,35 @@ On our [RPi image](microsd_images.md) the Wi-Fi adapter is confuigured to use th
## Switching the adapter to the access point mode
1. Enable the static IP address in the wireless interface. Add the following lines to your `/etc/dhcpcd.conf` file:
1. Enable the static IP address in the wireless interface.
For this purpose, add the following lines
```conf
interface wlan0
static ip_address=192.168.11.1/24
```
2. Configure wpa_supplicant to work in the access point mode. Change your `/etc/wpa_supplicant/wpa_supplicant.conf` file to contain the following:
to file `/etc/dhcpcd.conf` manually, or run the following command.
```bash
cat << EOF | sudo tee -a /etc/dhcpcd.conf
interface wlan0
static ip_address=192.168.11.1/24
EOF
```
2. Configure wpa_supplicant to work in the access point mode.
```bash
cat << EOF | sudo tee /etc/wpa_supplicant/wpa_supplicant.conf
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
network={
ssid="CLEVER-1234"
ssid="CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)"
psk="cleverwifi"
mode=2
proto=RSN
@@ -99,23 +125,23 @@ On our [RPi image](microsd_images.md) the Wi-Fi adapter is confuigured to use th
group=CCMP
auth_alg=OPEN
}
EOF
```
where `CLEVER-1234` is the network name and `cleverwifi` is the password.
3. Restart the `dhcpcd` service.
3. Enable the `dnsmasq` service.
```bash
sudo systemctl restart dhcpcd
```
4. Enable the `dnsmasq` service.
```bash
sudo systemctl enable dnsmasq
sudo systemctl start dnsmasq
```
4. Restart the `dhcpcd` service.
```bash
sudo systemctl restart dhcpcd
```
___
Below you can read more about how RPi networking is organized.
@@ -124,16 +150,16 @@ Below you can read more about how RPi networking is organized.
Network operation in the [image](microsd_images.md) is supported by two pre-installed services:
* **networking** — the service enables all network interfaces at startup [5].
* **dhcpcd** — the service ensures that configuration of addressing and routing on the interfaces is obtained dynamically or specified statically in the config file.
* **networking** — the service enables all network interfaces at the moment of start [5].
* **dhcpcd** — the service ensures configuration of addressing and routing on the interfaces obtained dynamically, or specified statically in the config file.
To work in the router (access point) mode, RPi requires a DHCP server. It is used to automatically send the settings of the current network to connected clients. `isc-dhcp-server` or `dnsmasq` may be used for this.
To work in the router (access point) mode, RPi requires a DHCP server. It is used to automatically send the settings of the current network to connected clients. `isc-dhcp-server` or `dnsmasq` may be used as such server.
### dhcpcd
Starting with Raspbian Jessie, network settings are no longer defined in the `/etc/network/interfaces` file. Now `dhcpcd` is used for sending addressing and routing settings[4].
Starting with Raspbian Jessy, network settings are no longer defined in the `/etc/network/interfaces` file. Now `dhcpcd` is used for sending addressing and routing settings[4].
By default, a dhcp client is enabled in all interfaces. Settings for network interfaces are changed in the `/etc/dhcpcd.conf` file. An access point should have a static IP address. To specify one, add the following lines to the end of the file:
By default, a dhcp client is enabled in all interfaces. Settings of the interfaces are changed in the `/etc/dhcpcd.conf` file. To start an access point, specify a static IP address. To do this, add the following lines to the end of the file:
```
interface wlan0
@@ -144,9 +170,9 @@ static ip_address=192.168.11.1/24
### wpa_supplicant
**wpa_supplicant** — the service configures the Wi-Fi adapter. The `wpa_supplicant` service does not run as a standalone service (although it exists as such), but is instead launched as a `dhcpcd` child process.
**wpa_supplicant** — the service configures the Wi-Fi adapter. The `wpa_supplicant` service runs not as standalone (although it exists as such), and runs as a `dhcpcd` child process.
By default the config file is `/etc/wpa_supplicant/wpa_supplicant.conf`.
By default, the config file should contain path `/etc/wpa_supplicant/wpa_supplicant.conf`.
An example of the configuration file:
```conf
@@ -166,7 +192,7 @@ network={
}
```
Inside the config file, general `wpa_supplicant` settings, and the settings for the adapter configuration are specified. The configuration file also contains `network` section with the basic settings of the Wi-Fi network, such as network SSID, password, adapter operating mode. There may be several `network` sections, but only the first valid one is used. For example, if the first section contains a connection to an unavailable network, the adapter will be configured according to a next valid section, if there is one. Read more about the syntax of `wpa_supplicant.conf` [TODO WIKI].
Inside the config file, general `wpa_supplicant` settings, and the settings for the adapter configuration are specified. The configuration file also contains `network` section with the basic settings of the Wi-Fi network, such as network SSID, password, adapter operating mode. There may be several such sections, but only the first one is the working one. For example, if the first section contains connection to an unavailable network, the adapter will be configured according to a next valid section, if there is one. Read more about the syntax of `wpa_supplicant.conf` [TODO WIKI].
#### wpa_passphrase

View File

@@ -23,7 +23,7 @@ ROS_MASTER_URI=http://192.168.11.1:11311 rviz
If connection is not established, make sure the `.bashrc` of Clever contains line:
```(bash)
export ROS_HOSTNAME=`hostname`.local
export ROS_IP=192.168.11.1
```
Using rviz

View File

@@ -180,7 +180,7 @@ To unlock the copter, disable the safety button
1. Go to the Power menu.
2. Set the Number of cells — 4S.
3. Set parameter Full Voltage (per cell) - 4.20 V; Empty Voltage (per cell) - 3.50V.
3. Set parameter Full Voltage (per cell) - 4.20 V.
To save the changes, restart Pihxawk:

View File

@@ -3,9 +3,9 @@ PX4 Simulation
Main article: https://dev.px4.io/en/simulation/
PX4 simulation is possible in Linux and macOS with the use of physical environment simulation systems [jMAVSim](https://pixhawk.org/dev/hil/jmavsim) and [the Gazebo](http://gazebosim.org).
PX4 simulation is possible in Linux and macOS with the use of physical environment simulation systems [jMavSim](https://pixhawk.org/dev/hil/jmavsim) and [the Gazebo](http://gazebosim.org).
jMAVSim is a lightweight environment intended only for testing multi-rotor aircraft systems; Gazebo is a versatile environment for all types of robots.
jMavSim is a lightweight environment intended only for testing multi-rotor aircraft systems; Gazebo is a versatile environment for all types of robots.
Launching PX4 SITL
--
@@ -17,12 +17,12 @@ git clone https://github.com/PX4/Firmware.git
cd Firmware
```
jMAVSim
jMavSim
--
Main article: https://dev.px4.io/en/simulation/jmavsim.html
For simulation using the jMAVSim lightweight environment, use the following command:
For simulation using the jMavSim lightweight environment, use the following command:
```(bash)
make posix_sitl_default jmavsim

View File

@@ -4,6 +4,15 @@ Code examples
Python
---
<!-- markdownlint-disable MD031 -->
> **Note** When using cyrillic simbols with UTF-8 encoding, specify the encoding in the beginning of the program:
> ```python
> # -*- coding: utf-8 -*-
> ```
<!-- markdownlint-enable MD031 -->
### # {#distance}
Calculating the distance between two points (**important**: the points are to be in the same [system of coordinates](frames.md)):
@@ -37,7 +46,7 @@ start = get_telemetry()
print navigate(z=z, speed=0.5, frame_id='body', auto_arm=True)
# Waiting for takeoff
while not rospy.is_shutdown():
while True:
# Checking current altitude
if get_telemetry().z - start.z + z < tolerance:
# Takeoff complete
@@ -45,20 +54,6 @@ while not rospy.is_shutdown():
rospy.sleep(0.2)
```
This code can be wrapped in a function:
```python
def takeoff_wait(alt, speed=0.5, tolerance=0.2):
start = get_telemetry()
print navigate(z=alt, speed=speed, frame_id='body', auto_arm=True)
while not rospy.is_shutdown():
if get_telemetry().z - start.z + z < tolerance:
break
rospy.sleep(0.2)
```
### # {#block-nav}
Flying towards a point and waiting for copter's arrival:
@@ -71,7 +66,7 @@ frame_id='aruco_map'
print navigate(frame_id=frame_id, x=1, y=2, z=3, speed=0.5)
# Wait for the copter to arrive at the requested point
while not rospy.is_shutdown():
while True:
telem = get_telemetry(frame_id=frame_id)
# Calculating the distance to the requested point
if get_distance(1, 2, 3, telem.x, telem.y, telem.z) < tolerance:
@@ -80,25 +75,6 @@ while not rospy.is_shutdown():
rospy.sleep(0.2)
```
### # {#block-land}
Landing and waiting until the copter lands:
```python
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
This code can be wrapped in a function:
```python
def land_wait():
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
### # {#disarm}
Quadcopter disarm (disabling propellers **the copter will fall down**):

View File

@@ -1,23 +0,0 @@
# Дрон для 3D-сканирования человека
<img src="../assets/3d_drone_1.jpg" title="Сканирующий дрон">
Проект был создан совместно с компанией Texel, которая разрабатывает стационарные 3D-сканеры для создания модели человека.
Коллеги из Texel предоставили модуль, состоящий из Raspberry Pi с установленным ПО для сканирования и камеры для захвата 3D-изображения PrimeSense.
С нашей стороны был предоставлен квадрокоптер Клевер 3 с установленным оборудованием для автономного полета, а также написана полетная программа.
Мы провели множество тестов и настроек, внесли много изменений в конструкции дрона, чтобы в итоге добиться результата.
## Видео
<iframe width="966" height="543" src="https://www.youtube.com/embed/aqBION3TVhg" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
## Команда
Над проектом работали:
* Тимофей Кондратьев [Copter Express] - сборка дрона, написание и отладка программы, проведение тестов;
* Антон Мальцев [Copter Express] - моделирование защиты пропеллеров;
* Андрей Посконин [Texel] - импорт ПО Texel на Raspberr Pi, совместные тесты;

View File

@@ -54,22 +54,18 @@
* [Работа с SITL](sitl.md)
* [Контейнер с симулятором](sitl_docker.md)
* [Автозапуск ПО](autolaunch.md)
* [Имя хоста](hostname.md)
* [Взаимодействие с Arduino](arduino.md)
* [3G-модем](3g.md)
* [Установка ROS Kinetic](ros-install.md)
* Проекты на базе Клевера
* [Генератор ArUco карт](arucogenmap.md)
* [Модель аэротакси в городе](bigchallenges.md)
* [Шаровая защита коптера](shield.md)
* [Распознавание лиц](face_recognition.md)
* [Подсчет количества объектов c камеры](object_counting.md)
* [Пульт на Андроид](android.md)
* [Блочный конструктор полета](clever_blocks.md)
* [Дрон для 3D-сканирования человека](3dscan.md)
* [CopterHack-2018](copterhack2018.md)
* [CopterHack-2017](copterhack2017.md)
* [Робокросс-2019](robocross2019.md)
* Дополнительные материалы
* [Олимпиада НТИ 2019](nti2019.md)
* [Вклад в Клевер](contributing.md)

View File

@@ -10,12 +10,6 @@
## Конфигурирование
Аргумент `aruco` в файле `~/catkin_ws/src/clever/clever/launch/clever.launch` должен быть в значении `true`:
```xml
<arg name="aruco" default="true"/>
```
Для включения распознавания карт маркеров аргументы `aruco_map` и `aruco_detect` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должны быть в значении `true`:
```xml

View File

@@ -10,13 +10,7 @@
## Настройка
Аргумент `aruco` в файле `~/catkin_ws/src/clever/clever/launch/clever.launch` должен быть в значении `true`:
```xml
<arg name="aruco" default="true"/>
```
Для включения распознавания маркеров аргумент `aruco_detect` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должен быть в значении `true`:
Для включения модуля аргумент `aruco_detect` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должен быть в значении `true`:
```xml
<arg name="aruco_detect" default="true"/>

View File

@@ -1,246 +0,0 @@
# Аэротакси
Проект команды Human Express на проектной программе "Большие Вызовы"
## Суть проекта
Крупные города страдают из-за пробок и перегруженности транспорта. Пробки на дорогах и перегруженность транспорта влечёт за собой многие неудобства. Одной из таких проблем является отсутствие возможности быстро добраться из точки А в точку Б. При этом воздушное пространство практически не используется. Предлагаемое решение заключается в создании системы, которая в режиме реального времени наблюдает и контролирует движение беспилотных летательных аппаратов. Такое решение приводит к полной автоматизации процесса полёта и исключает возможность воздушно-транспортных происшествий. В результате проделанной работы удалось сделать систему, которая состоит из нескольких дронов и сервера. Сервер прокладывает маршрут дронам из начальной точки в заданную по проложенным дорогам. Также в работу сервера входит логистика, благодаря которой беспилотники не сталкиваются в полёте.
<iframe width="966" height="543" src="https://www.youtube.com/embed/nq1DKjacs6U" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
<iframe width="966" height="543" src="https://www.youtube.com/embed/QdgZ4lzPmwQ" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
## Настройка сервера
Чтобы скачать проект на сервер выполните команду
```bash
git clone https://github.com/Tennessium/HUEX
```
Перед началом работы с системой необходимо подключить устройство к сети WiFi (например, раздать со смартфона) установить на него необходимые библиотеки
```bash
cd HUEX/server
pip install -r requirements.txt
```
В случае необходимости вы можете изменить высоту эшелонов и разрешенные IP-адреса для доступа к Центру управления полетами в файле *server/consts.py* и настроить поле с метками в *server/static/map.txt*.
Теперь можно запустить сервер написав в командную строку
```bash
python manage.py runserver 0.0.0.0:8000
```
Чтобы перейти на веб страницу наберите в адресной строке ip адрес сервера в локальной сети и укажите порт 8000 (`http://ip:8000`).
[Как узнать ip адрес устройства](https://remontka.pro/ip-adres/)
## Настройка коптеров
В первую очередь подготовьте SD-карту с образом Clever ([Инструкция](https://clever.copterexpress.com/ru/microsd_images.html))
Чтобы скачать проект на Raspberry Pi в коптере выполните команду
```bash
git clone https://github.com/Tennessium/HUEX
```
Перед началом работы с системой необходимо перевести коптеры в режим клиента и подключить к сети WiFi. Вы можете воспользоваться [этим мануалом](https://clever.copterexpress.com/ru/network.html#переключение-адаптера-в-режим-клиента)
Однако, для упрощения развертывания системы на нескольких коптреах, рекомендуется использование нашего скрипта, лежащего в папке *copter/setup/*
- Перейдите в папку
```bash
cd HUEX/copter/setup/
```
- Используя любой редактор, в файле *networkData.txt* измените SSID и пароль сети
- Запустите скрипт
```bash
sudo bash networkEdit.sh
```
- Перезагрузите Raspberry Pi
Произведите установку и настройку ROS-пакета для LED-ленты
```bash
cd ~/catkin_ws/src
git clone https://github.com/bart02/ros-led-lib.git led
cd led
```
Воспользовавшись nano ledsub.py, измените переменную LED_COUNT на число светодиодов на вашей ленте
```bash
chmod +x ledsub.py
cd ~/catkin_ws
catkin_make
sudo systemctl enable /home/pi/catkin_ws/src/led/led.service
sudo systemctl start led
```
Установите необходимые пакеты
```bash
cd HUEX/clever
pip install -r requirements.txt
```
В файле *copter/consts.py* укажите IP-адрес сервера.
Для запуска основного скрипта воспользуйтсь нашим systemd-сервисом.
```bash
sudo systemctl enable /home/pi/HUEX/clever/setup/taxi.service
sudo systemctl start taxi.service
```
Скрипт будет запускаться автоматически при старте системы.
Для остановки можно воспользоваться командой
```bash
sudo systemctl stop taxi.service
```
## Веб-интерфейс центра управления полётами
<img src="../assets/cup.png" alt=""/>
В данном веб интерфейсе можно следить за полётами всех дронов на карте (масштабировать с помощью колёсика, передвигаять с помощью Alt). При нажатии на лебедя в правом верхнем углу все коптеры аварийно садятся, А при нажатии на значок "обновить" все коптеры автоматически удаляются, что приводит к удалению всех комманд и посадке активных на текущий момент коптеров.
С помощью инструментов в правом нижнем углу можно строить новые основания и рёбра.
## Веб-интерфейс заказа
Чтобы перейти на веб страницу наберите в адресной строке `http://ip:8000/m`, где *ip* - адрес сервера в сети.
## Визуализатор
Для наглядности работы системы был разработан 3D-визуализатор воздушного пространства. Он отображает систему эшелонов, поле ArUco-маркеров; позиции коптеров и направления их движения в real-time.
Запускать визуализатор можно после старта сервера. Компьютер должен быть подключен к той же сети, что и сервер.
```bash
cd viz
python main.py
```
Программа автоматически подгрузит карты маркеров и эшелонов. Если одна из них изменяется в процессе эксплуатации, перезагрузите программу для внесения изменений.
<img src="../assets/viz.png" alt=""/>
>Камеру можно передвигать при помощи клавиш *WASD* и поворачивать при помощи стрелок.
## DDoS-скрипт
Во время смены был написан простой скрипт, предназначенный для тестирования логистики. Его суть заключается в бесконечном заказе такси между случайными точками. Перед тем как запустить программу [*DDos.py*](https://github.com/Tennessium/HUEX/blob/master/DDos.py) замените параметр *static_path* в пятой строке на *ip* вашего сервера.
## API
На случай если вы захотите реализовать свою "обёртку" вы можете реализовать взаимодействие с сервером по средствам *HTTP/HTTPS* запросов
### /get
Возвращает телеметрию всех доступных коптеров.
Пример:
```json
{
"message": "OK",
"drones": [
{
"ip": "192.168.1.101",
"led": "#FF0000",
"status": "land",
"pose": {
"x": 0.24,
"y": 0.38,
"z": 0,
"yaw": 0
},
"voltage": 4.12,
"nextp": {
"led": "#FF0000",
"status": "land",
"pose": {
"x": 0.24,
"y": 0.38,
"z": 1.5,
"yaw": 0
}
}
}
]
}
```
Где
- **ip** - ip адрес коптера
<!-- markdownlint-disable MD044 -->
- **led** - цвет светодиодной ленты
<!-- markdownlint-restore MD044 -->
- **status** - *fly* или *land* - текущий статус коптера
- **pose** - позиция коптера (*x*, *y*, *z*, и *yaw*)
- **voltage** - напряжение на одной банке
<!-- markdownlint-disable MD044 -->
- **nextp** - отдаваемая коптеру команда на полёт (*led*, *status*, *pose* как выше)
<!-- markdownlint-restore MD044 -->
### /static/roads.json
Текущая карта дорог первого эшелона
Пример:
```json
{
"points": [
{
"x": 2.1,
"y": 3.5
},
{
"x": 0.6,
"y": 0.4
},
{
"x": 2.4,
"y": 0.5
}
],
"lines": [
{
"1": 2,
"2": 1
},
{
"1": 1,
"2": 0
}
]
}
```
Где
- **poits** - массив вершин графа и их координат
- **lines** - массив рёбер графа (*1* - точка из которой выходит ребро, *2* - точка куда это ребро направлено)
### /ask_taxi?o=x&t=y
Заказ такси из точки под номером *x* в точку под номером *y*. Точки *x* и *y* берутся из **/static/roads.json**
### /get_dist?o=x&t=y
Возвращает расстояние и цену за пролёт между точками.
Пример:
```json
{"dist": 5.3, "cost": 309}
```
Где
- **dist** - Дистанция в метрах
- **cost** - Цена в рублях (150₽ + 30₽ * *n* метров)

View File

@@ -34,61 +34,10 @@
Более подробную информацию о Pull Request'ах смотрите [на GitHub](https://help.github.com/articles/about-pull-requests/) (англ.) или в [документации по git](https://git-scm.com/book/ru/v2/GitHub-Внесение-собственного-вклада-в-проекты) (русск.).
## Добавление статьи в GitBook
## Ваш проект с Клевером
> **Note** Если вы реализовали собственный интересный проект на Клевере, вы можете добавить статью о нем в раздел "Проекты на базе Клевера".
Если вы реализовали собственный интересный проект на Клевере, вы можете добавить статью о нем в раздел "Проекты на базе Клевера".
Подготовьте вашу статью и пришлите Pull Request с ней в [репозиторий Клевера](https://github.com/CopterExpress/clever).
Подготовьте вашу статью и пришлите Pull Request с ней в [репозиторий Клевера]( https://github.com/CopterExpress/clever).
1. Сделайте форк репозитория Клевера:
<img src="../assets/github-fork.png" alt="GitHub Fork">
2. Склонируйте форк на компьютер:
```bash
git clone https://github.com/<USERNAME>/clever.git
```
3. Перейдите в директорию с форком и создайте новую ветку с названием вашей статьи (например `new-article`):
```bash
git checkout -b new-article
```
4. Напишите новую статью в разделе `docs/ru` или `docs/en` в формате [Markdown](https://ru.wikipedia.org/wiki/Markdown) (например `docs/ru/new_article.md`).
5. Поместите дополнительные визуальные материалы в папку `docs/assets` и оформите на них ссылки в вашей статье.
6. Добавьте статью в файл оглавления `SUMMARY.md` в том разделе, где вы её написали (например в `docs/ru/SUMMARY.md`):
```bash
...
* Дополнительные материалы
* [Олимпиада НТИ 2019](nti2019.md)
* [Вклад в Клевер](contributing.md)
* [Новая статья](new_article.md)
* [Сборка и модификация образа Клевера](image_building.md)
* [Прошивка ESC контроллеров](esc_firmware.md)
...
```
7. Сохраните состояние ваших изменений локально:
```bash
git add docs/
git commit -m "Add new article for Clever"
```
8. Загрузите вашу новую ветку с изменениями на ваш GitHub репозиторий с форком Клевера:
```bash
git push -u origin new-article
```
9. Перейдите на web страницу вашего форка и сделайте `pull request` вашей ветки в master Клевера:
<img src="../assets/github-pull-request.png" alt="GitHub Pull Request">
<img src="../assets/github-pull-request-create.png" alt="GitHub Create Pull">
10. Дождитесь комментариев на свою статью, сделайте правки, если потребуется.
11. Порадуйтесь своей новой полезной статье, опубликованной на https://clever.copterexpress.com!
<!-- TODO -->

View File

@@ -11,7 +11,7 @@
## Полетный контроллер / автопилот
**1\.** Специализированная плата, спроектированная для управления мультикоптером, самолетом или другим аппаратом. Примеры:
Pixhawk, ArduPilot, Naze32, CC3D.
Pixhawk, Ardupilot, Naze32, CC3D.
**2\.** Программное обеспечение для платы управления мультикоптером. Примеры: PX4, APM, CleanFlight.

View File

@@ -1,31 +0,0 @@
# Имя хоста
[По умолчанию](microsd_images.md) на Клевере установлено имя хоста (hostname) `clever-xxxx`, где `xxxx` случайные цифры. Имя хоста соответствует SSID [точки доступа Wi-Fi](wifi.md).
Таким образом, Клевер доступен на машинах, поддерживающих mDNS, под именем `clever-xxxx.local`. Вы можете использовать это имя для SSH-доступа на Клевер:
```bash
ssh pi@clever-xxxx.local
```
Также это имя может быть использовано вместо IP-адреса для открытия страницы Клевера в браузере и т. д.
## Изменение имени хоста
В некоторых ситуациях необходимо изменение имени хоста Клевера. Для это используйте утилиту `hostnamectl`:
```bash
sudo hostnamectl set-hostname newname
```
Где `newname` новое имя машины. Утилита `hostnamectl` поменяет имя в файле `/etc/hostname`.
Также необходимо прописывание нового имени в файл `/etc/hosts`:
```txt
127.0.1.1 newname newname.local
```
Прописывание `newname.local` необходимо, чтобы ROS смог разрешить это имя в ситуациях, когда все сетевые интерфейсы неактивны (отключение/разрыв связи Wi-Fi).
> **Note** Изменение имени хоста не повлечёт за собой изменений SSID точки доступа Wi-Fi (и наоборот, изменение SSID точки доступа не поменяет имя хоста).

View File

@@ -48,8 +48,8 @@ MAVROS подписывается на определенные ROS-топики
### Топики для посылки raw-пакетов
`/mavros/setpoint_raw/local` — отправка пакета [SET\_POSITION\_TARGET\_LOCAL\_NED](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED). Позволяет установить целевую позицию/целевую скорость и целевое рысканье/угловую скорость по рысканью. Выбор устанавливаемых величин осуществляется с помощью поля `type_mask`.
`/mavros/setpoint_raw/local` — отправка пакета [SET\_POSITION\_TARGET\_LOCAL\_NED](https://pixhawk.ethz.ch/mavlink/#SET_POSITION_TARGET_LOCAL_NED). Позволяет установить целевую позицию/целевую скорость и целевое рысканье/угловую скорость по рысканью. Выбор устанавливаемых величин осуществляется с помощью поля `type_mask`.
`/mavros/setpoint_raw/attitude` — отправка пакета [SET\_ATTITUDE\_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET). Позвлояет установить целевую ориенатацию /угловые скорости и уровень газа. Выбор устанавливаемых величин осуществляется с помощью поля `type_mask`
`/mavros/setpoint_raw/attitude` — отправка пакета [SET\_ATTITUDE\_TARGET](https://pixhawk.ethz.ch/mavlink/#SET_ATTITUDE_TARGET). Позвлояет установить целевую ориенатацию /угловые скорости и уровень газа. Выбор устанавливаемых величин осуществляется с помощью поля `type_mask`
`/mavros/setpoint_raw/global` — отправка пакета [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Позволяет установить целевую позицию в глобальных координатах \(ширина, долгота, высота\), а также скорости полета. **Не поддерживается в PX4** \([issue](https://github.com/PX4/Firmware/issues/7552)\).
`/mavros/setpoint_raw/global` — отправка пакета [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://pixhawk.ethz.ch/mavlink/#SET_POSITION_TARGET_GLOBAL_INT). Позволяет установить целевую позицию в глобальных координатах \(ширина, долгота, высота\), а также скорости полета. **Не поддерживается в PX4** \([issue](https://github.com/PX4/Firmware/issues/7552)\).

View File

@@ -7,28 +7,30 @@ Wi-Fi адаптер на Raspberry Pi имеет два основных реж
При использовании [образа для RPi](microsd_images.md) по умолчанию Wi-Fi адаптер работает в [режиме точки доступа](wifi.md).
<!-- markdownlint-disable MD029 -->
## Изменение пароля или SSID (имени сети)
1. Отредактируйте файл `/etc/wpa_supplicant/wpa_supplicant.conf` (используя [SSH-соединение](ssh.md)):
```bash
sudo nano /etc/wpa_supplicant/wpa_supplicant.conf
```
```bash
sudo nano /etc/wpa_supplicant/wpa_supplicant.conf
```
Измените значение параметра `ssid`, чтобы изменить название Wi-Fi сети, и параметра `psk`, чтобы изменить пароль. Например:
Измените значение параметра `ssid`, чтобы изменить название Wi-Fi сети, и параметра `psk`, чтобы изменить пароль. Например:
```txt
network={
ssid="my-super-ssid"
psk="cleverwifi123"
mode=2
proto=RSN
key_mgmt=WPA-PSK
pairwise=CCMP
group=CCMP
auth_alg=OPEN
}
```
```txt
network={
ssid="my-super-ssid"
psk="cleverwifi123"
mode=2
proto=RSN
key_mgmt=WPA-PSK
pairwise=CCMP
group=CCMP
auth_alg=OPEN
}
```
2. Перезагрузите Raspberry Pi.
@@ -40,81 +42,89 @@ Wi-Fi адаптер на Raspberry Pi имеет два основных реж
1. Выключите службу `dnsmasq`.
```bash
sudo systemctl stop dnsmasq
sudo systemctl disable dnsmasq
```
```bash
sudo systemctl stop dnsmasq
sudo systemctl disable dnsmasq
```
2. Включите получение IP адреса на беспроводном интерфейсе DHCP клиентом. Для этого удалите из файла `/etc/dhcpcd.conf` строки:
```conf
interface wlan0
static ip_address=192.168.11.1/24
```
```conf
interface wlan0
static ip_address=192.168.11.1/24
```
3. Настройте `wpa_supplicant` для подключения к существующей точке доступа. Для этого замените содержимое файла `/etc/wpa_supplicant/wpa_supplicant.conf` на:
```
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
```
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
network={
ssid="SSID"
psk="password"
}
```
network={
ssid="SSID"
psk="password"
}
```
где `SSID` название сети, а `password` пароль.
где `SSID` название сети, а `password` пароль.
4. Перезапустите службу `dhcpcd`.
```bash
sudo systemctl restart dhcpcd
```
```bash
sudo systemctl restart dhcpcd
```
## Переключение адаптера в режим точки доступа
1. Включите статический IP адрес на беспроводном интерфейсе. Для этого добавьте в файл `/etc/dhcpcd.conf` строки:
```conf
interface wlan0
static ip_address=192.168.11.1/24
```
```conf
interface wlan0
static ip_address=192.168.11.1/24
```
2. Настроите `wpa_supplicant` на работу в режиме точки доступа. Для этого замените содержимое файла `/etc/wpa_supplicant/wpa_supplicant.conf` на:
```
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
```
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
update_config=1
country=GB
network={
ssid="CLEVER-1234"
psk="cleverwifi"
mode=2
proto=RSN
key_mgmt=WPA-PSK
pairwise=CCMP
group=CCMP
auth_alg=OPEN
}
```
network={
ssid="CLEVER-1234"
psk="cleverwifi"
mode=2
proto=RSN
key_mgmt=WPA-PSK
pairwise=CCMP
group=CCMP
auth_alg=OPEN
}
```
где `CLEVER-1234` название сети, а `cleverwifi` пароль.
где `CLEVER-1234` название сети, а `cleverwifi` пароль.
3. Включите службу `dnsmasq`.
3. Перезагрузите `systemd`.
```bash
sudo systemctl enable dnsmasq
sudo systemctl start dnsmasq
```
```bash
sudo systemctl daemon-reload
```
4. Перезапустите службу `dhcpcd`.
4. Включите службу `dnsmasq`.
```bash
sudo systemctl start dhcpcd
```
```bash
sudo systemctl enable dnsmasq
sudo systemctl start dnsmasq
```
5. Перезагрузите службу `dhcpcd`.
```bash
sudo systemctl restart dhcpcd
```
<!-- markdownlint-enable MD029 -->
___
@@ -131,7 +141,7 @@ ___
### dhcpcd
Начиная с Raspbian Jessie настройки сети больше не задаются в файле `/etc/network/interfaces`. Теперь за выдачу адресации и настройку маршрутизации отвечает `dhcpcd` [4].
Начиная с Raspbian Jesse настройки сети больше не задаются в файле `/etc/network/interfaces`. Теперь за выдачу адресации и настройку маршрутизации отвечает `dhcpcd` [4].
По умолчанию на всех интерфейсах включен dhcp-клиент. Настройки интерфейсов меняются в файле `/etc/dhcpcd.conf`. Для того, чтобы поднять точку доступа необходимо прописать статический ip-адрес. Для этого в конец файла необходимо добавить следующие строки:

View File

@@ -1,25 +0,0 @@
# Робокросс-2019
В июле 2019 команда Коптер Экспресс в 4-й раз подряд одержала победу на ежегодных испытаниях беспилотных транспортных средств "[Робокросс](http://russianrobotics.ru/activities/robokross-2019/)". Испытания проводятся на полигоне ГАЗ под Нижним Новгородом.
Основной задачей испытаний в категории БПЛА было локализовать и уничтожить цель — красный воздушный шар — в автономном режиме.
## Видео
<iframe width="560" height="315" src="https://www.youtube.com/embed/zMh5THdHuX8" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
## Реализация
Команда использовала квадрокоптер на базе рамы F450 и [платформу Клевер](https://github.com/CopterExpress/clever). Полный итоговый исходный код доступен для изучения [на GitHub](https://github.com/CopterExpress/robocross2019/).
Разработанный пакет `robocross2019` разбит на модули: ROS-нодлет `red_dead_detection` распознает красный шар, `balloon.py` реализует высокоуровневую логику полета коптера.
## red_dead_detection
Нодлет `red_dead_detection` обеспечивает обнаружение красного шара на изображении с камеры квадрокоптера, смотрящей вперед (топики `/front_camera/image_raw` и `/front_camera/camera_info`). Используется простейший метод фильтрации изображения по цвету. Затем вычисляется геометрический центр полученных участков и производится компенсация искажений камеры (`cv::undistortPoints`).
Используя известное фокусное расстояние камеры (из топика `camera_info`) вычисляется вектор, направленный в сторону цели. Полученный вектор публикуется в топик `/red_dead_detection/direction`, причем его система координат (`frame_id` связана с расположением передней камеры `front_camera_optical`).
## balloon.py
Для полета к шару используется вектор направления `red_dead_detection/direction`, который используется как setpoint по скорости дрона. Угол по рысканью также устанавливается по направлению к шару. Цель считается уничтоженной, когда на протяжении заданного количества кадров общая площадь участка с красными пикселями меньше определенного порога.

View File

@@ -23,7 +23,7 @@ ROS_MASTER_URI=http://192.168.11.1:11311 rviz
Если соединение не устанавливается, необходимо убедиться, что в `.bashrc` Клевера присутствует строка:
```bash
export ROS_HOSTNAME=`hostname`.local
export ROS_HOSTNAME=raspberrypi.local
```
Использование rviz

View File

@@ -182,7 +182,7 @@
1. Заходим в меню Power.
2. Устанавливаем количество банок Number of cells - 4S.
3. Устанавливаем параметры: Full Voltage (per cell) - 4.20V; Empty Voltage (per cell) - 3.50V.
3. Устанавливаем параметр Full Voltage (per cell) - 4.20V.
Чтобы изменения сохранились, необходимо перезагрузить Pixhawk:

View File

@@ -4,9 +4,9 @@
Основная статья: https://dev.px4.io/en/simulation/
Симуляция PX4 возможна в ОС GNU/Linux и macOS с использованием систем симуляции физической среды [jMAVSim](https://pixhawk.org/dev/hil/jmavsim) и [Gazebo](http://gazebosim.org).
Симуляция PX4 возможна в ОС Linux и macOS с использованием систем симуляции физической среды [jMavSim](https://pixhawk.org/dev/hil/jmavsim) и [Gazebo](http://gazebosim.org).
jMAVSim является легковесной средой, предназначенной только для тестирование мультироторных летательных систем; Gazebo универсальная среда для любых типов роботов.
jMavSim является легковесной средой, предназначенной только для тестирование мультироторных летательных систем; Gazebo универсальная среда для любых типов роботов.
## Запуск PX4 SITL
@@ -17,11 +17,11 @@ git clone https://github.com/PX4/Firmware.git
cd Firmware
```
## jMAVSim
## jMavSim
Основная статья: https://dev.px4.io/en/simulation/jmavsim.html
Для симуляции с использованием легковесной среды jMAVSim используйте команду:
Для симуляции с использованием легковесной среды jMavSim используйте команду:
```bash
make posix_sitl_default jmavsim

View File

@@ -54,7 +54,7 @@ start = get_telemetry()
print navigate(z=z, speed=0.5, frame_id='body', auto_arm=True)
# Ожидаем взлета
while not rospy.is_shutdown():
while True:
# Проверяем текущую высоту
if get_telemetry().z - start.z + z < tolerance:
# Взлет завершен
@@ -62,20 +62,6 @@ while not rospy.is_shutdown():
rospy.sleep(0.2)
```
Вышеприведенный код может быть обернут в функцию:
```python
def takeoff_wait(alt, speed=0.5, tolerance=0.2):
start = get_telemetry()
print navigate(z=alt, speed=speed, frame_id='body', auto_arm=True)
while not rospy.is_shutdown():
if get_telemetry().z - start.z + z < tolerance:
break
rospy.sleep(0.2)
```
### # {#block-nav}
Лететь в точку и ждать пока коптер долетит в нее:
@@ -88,7 +74,7 @@ frame_id='aruco_map'
print navigate(frame_id=frame_id, x=1, y=2, z=3, speed=0.5)
# Ждем, пока коптер долетит до запрошенной точки
while not rospy.is_shutdown():
while True:
telem = get_telemetry(frame_id=frame_id)
# Вычисляем расстояние до заданной точки
if get_distance(1, 2, 3, telem.x, telem.y, telem.z) < tolerance:
@@ -103,7 +89,7 @@ while not rospy.is_shutdown():
def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
while not rospy.is_shutdown():
while True:
telem = get_telemetry(frame_id=frame_id)
if get_distance(x, y, z, telem.x, telem.y, telem.z) < tolerance:
break
@@ -116,7 +102,7 @@ def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
while not rospy.is_shutdown():
while True:
telem = get_telemetry(frame_id='navigate_target')
if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
break
@@ -124,25 +110,6 @@ def navigate_wait(x, y, z, speed, frame_id, tolerance=0.2):
Такой код может быть использован для полета в том числе с использованием фрейма `body`.
### # {#block-land}
Посадка и ожидание окончания посадки:
```python
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
Вышеприведенный код может быть обернут в функцию:
```python
def land_wait():
land()
while get_telemetry().armed:
rospy.sleep(0.2)
```
### # {#disarm}
Дизарм коптера (выключение винтов, **коптер упадет**):
@@ -360,7 +327,7 @@ def flip():
while True:
telem = get_telemetry()
if abs(telem.roll) > math.pi/2:
if -math.pi + 0.1 < telem.roll < -0.2:
break
rospy.loginfo('finish flip')

View File

@@ -5,7 +5,7 @@
Для доступа по SSH необходимо [подключиться к Raspberry Pi по Wi-Fi](wifi.md) (также возможно подключение через Ethernet-кабель).
В GNU/Linux или macOS необходимо запустить Терминал и выполнить команду:
В Linux или macOS необходимо запустить Терминал и выполнить команду:
```bash
ssh pi@192.168.11.1