Updates docs/arduino.md

Auto commit by GitBook Editor

docs/arduino.md

@@ -32,9 +32,76 @@ sudo systemctl restart clever
 
 Набор сервисов и топиков аналогичен обычному набору в `simple_offboard` и `mavros`.
 
-Пример кода контроля коптера по позиции:
+Пример программы, контролирующей коптер по позиции, с использованием сервисов `set_position` и `set_mode`:
 
-```c++
-```
+```cpp
+// Подключение библиотек для работы с rosseral
+#include <ros.h>
 
-Пример кода с объявлением прокси ко всем сервисам пакета Clever:
+// Подключение заголовочных файлов сообщений пакета Clever и MAVROS
+#include <clever/SetPosition.h>
+#include <mavros_msgs/SetMode.h>
+
+using namespace clever;
+using namespace mavros_msgs;
+
+ros::NodeHandle nh;
+
+// Объявление сервисов
+ros::ServiceClient<SetPosition::Request, SetPosition::Response> setPosition("/set_position");
+ros::ServiceClient<SetMode::Request, SetMode::Response> setMode("/mavros/set_mode");
+
+void setup()
+{
+  // Инициализация rosserial
+  nh.initNode();
+
+  // Инициализация сервисов
+  nh.serviceClient(setPosition);
+  nh.serviceClient(setMode);
+
+  // Ожидание подключение к Raspberry Pi
+  while(!nh.connected()) nh.spinOnce();
+  nh.loginfo("Startup complete");
+
+  // Пользовательская настройка
+  // <...>
+
+  // Тестовая программа
+  SetPosition::Request sp_req;
+  SetPosition::Response sp_res;
+  SetMode::Request sm_req;
+  SetMode::Response sm_res;
+
+  // Взлет на 2 метра:
+  nh.loginfo("Take off");
+  sp_req.yaw = 0;
+  sp_req.x = 0;
+  sp_req.y = 0;
+  sp_req.z = 2;
+  sp_req.frame_id = "fcu_horiz";
+  setPosition.call(sp_req, sp_res);
+
+ // Ждем 5 секунд
+  delay(5000);
+
+  // Пролет вперед на 3 метра:
+  nh.loginfo("Fly forward");
+  sp_req.x = 3;
+  sp_req.y = 0;
+  sp_req.z = 0;
+  setPosition.call(sp_req, sp_res);
+
+  // Ждем 5 секунд
+  delay(5000);
+
+  // Посадка
+  nh.loginfo("Land");
+  sm_req.custom_mode = "OFFBOARD";
+  setMode.call(sm_req, sm_res);
+}
+
+void loop()
+{
+}
+```