PX4를 Mavlink로 제어해보자.
대상
- Mavlink를 이용하여 px4를 제어하는 코드를 만들고 싶은 분
Tech Map
- mavlink
- px4
- python
사전
- px4_overview, px4_hardware, nuttx101 , px4_code
내용
- 왜 mavlink를 사용하나?
- mavlink소개
- 사용 예
- mavlink api
- mavlink demo
- Q&A
왜 mavlink를 공부하나?
비행체에 상태를 주고 받고 명령을 내리기 위해 사용하는 인터페이스 px4, apm, 유일한 공식 인터페이스. dji도 지원.
- QGroundcontrol
- Mission Planner
- mavelous (web based) https://github.com/wiseman/mavelous
- https://github.com/MenchiG/DJI-Onboard-SDK-for-Mavlink
UAV Challenge
Canberrauav team: Ardupilot based
swissfang: PX4
TU Delft: Paparazzi
Mavlink
HTTP of Drones
Mavlink Generator --from Lorenz Meier(2009)
Mavlink Packet 구성
17 bytes (6 bytes header + 9 bytes payload + 2 bytes checksum)
Send Command and Receive ACK
QGroundcontrol에서 WAYPOINT_CLEAR_ALL 명령을 내리고, 비행FC에서 WAYPOINT 명령을 잘받으면 다시 QGroundcontrol에게 잘받았다는 ACK명령을 보낸다.
Packet 좀더
System ID, subsystem ID and Message ID
- FC의 System ID: 1-
- GCS의 System ID: 255
Mavlink Message (HEARTBEAT) 주고받는 코드 예시
https://pixhawk.ethz.ch/mavlink/#HEARTBEAT
- HEARTBEAT Data structure
MAVLINK_MSG_ID_HEARTBEAT 0
typedef struct __mavlink_heartbeat_t {
uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags.*/
uint8_t type; /*< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)*/
uint8_t autopilot; /*< Autopilot type / class. defined in MAV_AUTOPILOT ENUM*/
uint8_t base_mode; /*< System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h*/
uint8_t system_status; /*< System status flag, see MAV_STATE ENUM*/
uint8_t mavlink_version; /*< MAVLink version*/
}) mavlink_heartbeat_t;
- HEARTBEAT Packing function
mavlink_msg_heartbeat_pack()
- HEARTBEAT Decoding function
mavlink_msg_heart_decord()
Mavlink client code (from GCS)
Sending a heartbeat message
sysId = 2; //The system ID reflects the current aircraft. MAVLink can be used with multiple aircraft simultaneously
compId = 1; //The component ID reflects the onboard component, e.g. the IMU, the autopilot or the onboard computer.
//This allows to use MAVLink both for onboard-communication and for off-board telemetry.
targetId = 1; // The target id, the vehicle you are trying to control
mavlink_message_t message;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_heartbeat_pack(sysId, compId, &message, MAV_TYPE_GCS, MAV_AUTOPILOT_GENERIC, 0, 0, MAV_STATE_ACTIVE);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &message);
// write packet via serial or socket
int returnval = write(fd, buf, len);
Arduino 예제 https://github.com/baptistelabat/robokite/blob/master/Arduino/ArduinoMAVLink/ArduinoMAVLink.ino
Linux UDP 예제 https://github.com/mavlink/mavlink/blob/master/examples/linux/mavlink_udp.c
Mavlink receiver code (in Flight Controller)
Receiving a heartbeat message in PX4
Firmware/src/modules/mavlink/mavlink_receiver.cpp
void MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
...
case MAVLINK_MSG_ID_HEARTBEAT:
handle_message_heartbeat(msg);
break;
...
}
void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = tstatus.timestamp;
if (_telemetry_status_pub == nullptr) {
int multi_instance;
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
}
}
- PX4 mavlink APP https://github.com/PX4/Firmware/tree/master/src/modules/mavlink
- APM mavlink library https://github.com/ArduPilot/ardupilot/tree/master/libraries/GCS_MAVLink https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/commands_logic.cpp
mavlink api and tools
- Pymavlink
- Dronekit
- MAVROS
- Mavproxy
- Python, C or C++ or Java and Matlab ... so on
Pymavlink
python으로 구현된 mavlink 라이브러리, mavlink 프로토콜 정의와 패킷 인코딩 디코딩 그리고 mavlink 사용하는데 유용한 utils를 제공한다.
http://mavlink.org/messages/common#MANUAL_CONTROL http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE
https://github.com/PX4/Firmware/blob/master/Tools/mavlink_px4.py
def manual_control_send(self, target, x, y, z, r, buttons):
'''
This message provides an API for manually controlling the vehicle
using standard joystick axes nomenclature, along with
a joystick-like input device. Unused axes can be
disabled an buttons are also transmit as boolean
values of their
target : The system to be controlled. (uint8_t)
x : X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. (int16_t)
y : Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. (int16_t)
z : Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. (int16_t)
r : R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. (int16_t)
buttons : A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. (uint16_t)
'''
def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw):
'''
The RAW values of the RC channels sent to the MAV to override info
received from the RC radio. A value of -1 means no
change to that channel. A value of 0 means control of
that channel should be released back to the RC radio.
The standard PPM modulation is as follows: 1000
microseconds: 0%, 2000 microseconds: 100%. Individual
receivers/transmitters might violate this
specification.
target_system : System ID (uint8_t)
target_component : Component ID (uint8_t)
chan1_raw : RC channel 1 value, in microseconds (uint16_t)
chan2_raw : RC channel 2 value, in microseconds (uint16_t)
chan3_raw : RC channel 3 value, in microseconds (uint16_t)
chan4_raw : RC channel 4 value, in microseconds (uint16_t)
chan5_raw : RC channel 5 value, in microseconds (uint16_t)
chan6_raw : RC channel 6 value, in microseconds (uint16_t)
chan7_raw : RC channel 7 value, in microseconds (uint16_t)
chan8_raw : RC channel 8 value, in microseconds (uint16_t)
'''
참고
- https://github.com/PX4/Firmware/blob/master/Tools/mavlink_px4.py
- https://github.com/mavlink/c_uart_interface_example
Pymavlink 사용 예시(Mode 바꾸기)
from pymavlink import mavutil
import time, sys, argparse, math
connection_string = '127.0.0.1:14540'
mav = mavutil.mavlink_connection('udp:'+connection_string)
mav.wait_heartbeat()
print("HEARTBEAT OK\n")
mav.mav.command_long_send(1, 1, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, mavutil.mavlink.MAV_MODE_MANUAL_ARMED, 0, 0, 0, 0, 0, 0)
msg = mav.recv_match(type=['COMMAND_ACK'],blocking=True)
print(msg)
미션 수행 중인 기체 이륙한 곳으로 돌아오기(Gazebo)
Pymavlink 사용 예시(Mission 업로드 그리고 비행)
1. Make a mavlink connection (udp 14540, udp 14550, serialport)
2. Get HOME location (take off location) from vehicle
3. Send waypoints to vehicle and verify vehicle's waypoints
4. Change MISSION flight mode
5. Send ARM command and then start mission
6. Read vehicle status and send HEARTBEAT to vehicle per at least 1 sec.
7. Send DISARM command
https://gist.github.com/donghee/8d8377ba51aa11721dcaa7c811644169
Dronekit
pymavlink + alpha
pip install dronekit
Dronekit 사용 예시(vehicle 상태)
vehicle_state.py
git clone http://github.com/dronekit/dronekit-python.git
cd dronekit-python/examples/vehicle_state/
python vehicle_state.py --connect 127.0.0.1:14540
Get all vehicle attribute values:
Autopilot Firmware version: APM:Copter-3.3.0-alpha64
Major version number: 3
Minor version number: 3
Patch version number: 0
Release type: rc
Release version: 0
Stable release?: True
Autopilot capabilities
Supports MISSION_FLOAT message type: True
Supports PARAM_FLOAT message type: True
Supports MISSION_INT message type: False
Supports COMMAND_INT message type: False
Supports PARAM_UNION message type: False
Supports ftp for file transfers: False
Supports commanding attitude offboard: False
Supports commanding position and velocity targets in local NED frame: True
Supports set position + velocity targets in global scaled integers: True
Supports terrain protocol / data handling: True
Supports direct actuator control: False
Supports the flight termination command: True
Supports mission_float message type: True
Supports onboard compass calibration: False
Global Location: LocationGlobal:lat=-35.363261,lon=149.1652299,alt=None
Global Location (relative altitude): LocationGlobalRelative:lat=-35.363261,lon=149.1652299,alt=0.0
Local Location: LocationLocal:north=None,east=None,down=None
Attitude: Attitude:pitch=0.00294387154281,yaw=-0.11805768311,roll=0.00139428151306
Velocity: [-0.03, 0.02, 0.0]
GPS: GPSInfo:fix=3,num_sat=10
Gimbal status: Gimbal: pitch=None, roll=None, yaw=None
Battery: Battery:voltage=12.587,current=0.0,level=100
EKF OK?: False
Last Heartbeat: 0.769999980927
Rangefinder: Rangefinder: distance=None, voltage=None
Rangefinder distance: None
Rangefinder voltage: None
Heading: 353
Is Armable?: False
System status: STANDBY
Groundspeed: 0.0
Airspeed: 0.0
Mode: STABILIZE
Armed: False
Takeoff and land in dronekit
https://gist.github.com/donghee/21cf1d3b35c6bb1aebad4bb8d9448fe8
2:13
Simple Mission in dronekit
https://gist.github.com/donghee/a41121eaa78e5a6e51462ade507dc9d7
Offboard mode
실시간으로 기체의 포지션을 바꾸고 싶은 경우 사용
Offboard mode in dronekit
Python MAV_FRAME_LOCAL_NED
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0b0000111111111000, # type_mask (only positions enabled)
north, east, down, # x, y, z positions (or North, East, Down in the MAV_FRAME_BODY_NED frame
0, 0, 0, # x, y, z velocity in m/s (not used)
0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle
vehicle.send_mavlink(msg)
Dronekit 관련 프로젝트
비전이용하여 랜딩마커위에 랜딩하기
- https://github.com/djnugent/Precland https://github.com/djnugent/Precland/blob/master/Common/VN_vehicle_control.py dronekit 사용안하고 pymavlink로 dronekit 비슷한걸 만듬.
- https://github.com/djnugent/dronedirect/blob/master/dronedirect/__init__.py
Mavros
ROS module for mavlink
참고: https://wiki.dronecode.org/_media/elc05.flying_penguins_-_clay_mcclure.pdf
Topics
/mavros/state
/mavros/imu/data
/mavros/global_position/global
/mavros/local_position/local
/mavros/setpoint_position/local_position
/mavros/setpoint_velocity/cmd_vel
Services
/mavros/cmd/arming
/mavros/cmd/land
/mavros/cmd/takeoff
/mavros/set_mode
/mavros/set_stream_rate
MAVROS 예시
조이스틱으로 자세제어 명령 보내는 mavlink 코드(in mavros) 읽기
- https://github.com/mavlink/mavros/blob/master/mavros_extras/scripts/mavteleop
- get_pub_attitude_pose()
- https://github.com/mavlink/mavros/blob/master/mavros/src/mavros/setpoint.py
- SetpointAttitudePlugin "attitude"
- https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_attitude.cpp
- pose_cb
- https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_attitude.cpp
- send_attitude_target()
- https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_attitude.cpp
- set_attitude_target()
- https://github.com/mavlink/mavros/blob/master/mavros/include/mavros/setpoint_mixin.h
- SET_ATTITUDE_TARGET
http://mavlink.org/messages/common#SET_ATTITUDE_TARGET
참고
- https://github.com/baptistelabat/robokite/blob/master/Motors/motorJoy.py
- https://github.com/UAVenture/ros-setups/blob/master/intel-edison/setpoint_demo.py
- https://mzahana.gitbooks.io/indoor-nav-at-risclab/content/ros_with_hardware_experiment.html
Mavproxy
telemetry 포트를 udp socket으로 연결.
pip install mavproxy
mavproxy.py --master=/dev/tty.SLAB_USBtoUART --baudrate 57600 --out udp:127.0.0.1:14540 --out udp:127.0.0.1:14550
C
mavlink 에서 제공하는 uart 인터페이스
https://github.com/mavlink/c_uart_interface_example/blob/master/autopilot_interface.cpp
setpoint 메시지 보내기
start_write_thread(void) -> write_thread() -> write_setpoint() -> write_message(message)
Offboard mode in C
offboard mode: px4 flight stack을 제어할 수 있다.
SET_POSITION_TARGET_LOCAL_NED and the SET_ATTITUDE_TARGET mavlink 커맨드 사용
참고: https://pixhawk.ethz.ch/mavlink/#SET_POSITION_TARGET_LOCAL_NED
https://github.com/mavlink/c_uart_interface_example/blob/master/mavlink_control.cpp
void
commands(Autopilot_Interface &api)
{
// --------------------------------------------------------------------------
// START OFFBOARD MODE
// --------------------------------------------------------------------------
api.enable_offboard_control();
usleep(100); // give some time to let it sink in
// now the autopilot is accepting setpoint commands
// --------------------------------------------------------------------------
// SEND OFFBOARD COMMANDS
// --------------------------------------------------------------------------
printf("SEND OFFBOARD COMMANDS\n");
// initialize command data strtuctures
mavlink_set_position_target_local_ned_t sp;
mavlink_set_position_target_local_ned_t ip = api.initial_position;
// autopilot_interface.h provides some helper functions to build the command
// Example 1 - Set Velocity
// set_velocity( -1.0 , // [m/s]
// -1.0 , // [m/s]
// 0.0 , // [m/s]
// sp );
// Example 2 - Set Position
set_position( ip.x - 5.0 , // [m]
ip.y - 5.0 , // [m]
ip.z , // [m]
sp );
// Example 1.2 - Append Yaw Command
set_yaw( ip.yaw , // [rad]
sp );
// SEND THE COMMAND
api.update_setpoint(sp);
// NOW pixhawk will try to move
// Wait for 8 seconds, check position
for (int i=0; i < 8; i++)
{
mavlink_local_position_ned_t pos = api.current_messages.local_position_ned;
printf("%i CURRENT POSITION XYZ = [ % .4f , % .4f , % .4f ] \n", i, pos.x, pos.y, pos.z);
sleep(1);
}
printf("\n");
// --------------------------------------------------------------------------
// STOP OFFBOARD MODE
// --------------------------------------------------------------------------
api.disable_offboard_control();
// now pixhawk isn't listening to setpoint commands
// --------------------------------------------------------------------------
// GET A MESSAGE
// --------------------------------------------------------------------------
printf("READ SOME MESSAGES \n");
// copy current messages
Mavlink_Messages messages = api.current_messages;
// local position in ned frame
mavlink_local_position_ned_t pos = messages.local_position_ned;
printf("Got message LOCAL_POSITION_NED (spec: https://pixhawk.ethz.ch/mavlink/#LOCAL_POSITION_NED)\n");
printf(" pos (NED): %f %f %f (m)\n", pos.x, pos.y, pos.z );
// hires imu
mavlink_highres_imu_t imu = messages.highres_imu;
printf("Got message HIGHRES_IMU (spec: https://pixhawk.ethz.ch/mavlink/#HIGHRES_IMU)\n");
printf(" ap time: %llu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc , imu.yacc , imu.zacc );
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n" , imu.xmag , imu.ymag , imu.zmag );
printf(" baro: %f (mBar) \n" , imu.abs_pressure);
printf(" altitude: %f (m) \n" , imu.pressure_alt);
printf(" temperature: %f C \n" , imu.temperature );
printf("\n");
// --------------------------------------------------------------------------
// END OF COMMANDS
// --------------------------------------------------------------------------
return;
}
프로젝트 소개
mavlink로 gps 메시지 보내기
GPS MAVLink message using Pozyx potision
https://github.com/ShingoMatsuura/IndoorLoiter/blob/master/IndoorLoiter.ino
web based groundcontrol
mavelous (web based) https://github.com/wiseman/mavelous
ODM
https://github.com/OpenDroneMap/WebODM
참고
- http://dev.px4.io/dronekit-example.html
- http://python.dronekit.io/guide/auto_mode.html
- https://github.com/mavlink/c_uart_interface_example/
- https://mhageh.gitbooks.io/c-uart-interface-technical-details/
Canberrauav UAV