APM 태스트용 간단한 문구를 프린트하는 모드
APM에 만들어져 있는 모드들을 보면 어느정도 일정한 컨밴션을 가지고 있는 것 같다.
- init, run 함수가 기본 단위이다.
- Land, failsafe, takeoff 등 상황별로 나누어 switch 문 등으로 정리한다.
모드를 initialize 화 하기 위한 절차도 어느정도 정해진 듯 하지만, 현 시점에서 모드에서 호출하는 함수, 매서드들 하나하나의 의미는 정확하게 파악하지 못했다(솔직히 말해서 요즘 하기 좀 싫어졌는데, 갑자기 또 뽕들었는지 하게됬다.).
때문에 다음과 같은 동작을 확인할 수 있는 간단한 모드를 만들어봤다.
- mavproxy GCS에서 자유롭게 실행, 정지시킬 수 있는가.
- 모드의 initialize 시점은?
- 모드의 run 부분은 돌아가는 period가 어느정도인가. (PX4의 경우 매인처럼 0.25s?)
이후 다른 모드를 참조하고, library를 참조하면서 다음과 같은 부분을 참고하여 모드를 제작할 예정이다.
- 자세제어(기울기각도 제어)를 하는 모드를 먼저 만들어 본다
stabilize 모드를 참고하면 도움이 될 것같다(stabilize 모드가 자세제어 관련 모드이기 때문에) - 자세제어가 어느정도 갖추어지면, 이를 활용해 위치제어 모드를 만든다.
이를 위해 optical flow센서를 이용해 현 위치를 어느정도 신뢰성 있게 추정할 수 있어야 한다.
관건은 'optical flow센서를 이용해 현 위치를 어느정도 신뢰성 있게 추정' 하는 방법이 어떤게 있을 까 인데, 관련 문서들을 이전부터 찾아보았지만 명쾌한 답을 못찾겠다. 계속 고민해보아야 할 부분인것.
일단 태스트용 모드를 하나 만들기로 했다.
APM 은 기본적으로 각 기체가 하나의 Class이고, 기체의 기능들은 이 Class의 매써드로 포함하게끔 되어있다.
내가 사용하는 기체는 Copter 이며, 이 Copter는 다양한 모드들을 포함하고 있다.
모드들은 init, run 등의 기본적인 틀과 util이라 할 수 있는 추가적인 기능(run, init에서 사용하는)을 가지고 있고, 이는 Copter Class의 매써드로 표현된다.
대충 그림을 그리면 이렇게 되겠다.
각 모드들의 init, run은 원형은 Copter Class에 정의되어 있고, 내용물은 별도로 "control_모드명.cpp"로 만들어 관리하고 있다. 모드를 추가하기 위해서는 기본적으로 다음과 같은 작업이 필요하다.
- 추가할 모드가 몇번째 모드가 될 지 enum 등에 추가해주어야 한다.
- 해당 모드가 선택됬을 때 run, init 함수를 호출할 수 있도록 추가해주어야 한다.
- 호출될 함수(매써드)를 Copter Class에 원형을 선언해두고, 별도로 파일을 만들어 내용물을 채워넣는다.
우선 관련 enum 부분을 추가하고, 관련함수가 호출될 수 있도록 만들어주자. 모드 이름은 TESTSPACE로 하였다.(이런저런 동작실험을 하기위한 공간, 정도의 의미다.)
ArduCopter/defines.h
// Auto Pilot Modes enumeration
enum control_mode_t {
STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude
TESTSPACE = 21, // add for new test mode and test the try new something
};맨 아래 TESTSPACE 라는 이름으로 정해두었고, 모드 순서상 번호는 21번이다.
(이 번호 기억해두자)
ArduCopter/Copter.h 에 private 위치에 다음과 같이 init, run 관련 매서드 원형을 추가해주자.
// support for AP_Avoidance custom flight mode, AVOID_ADSB
bool avoid_adsb_init(bool ignore_checks);
void avoid_adsb_run();
bool avoid_adsb_set_velocity(const Vector3f& velocity_neu);
// add for new test mode and test the try new something
bool testspace_init(bool ignore_checks);
void testspace_run();
void ekf_check();
bool ekf_over_threshold();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
void esc_calibration_startup_check();ignore_checks라는 인자는 다른 모드들이 어떻게 활용하는지 보면서 알아야 될 것 같은데, init 과정을 할지 안할지 결정하는 듯 하다.
위 기본적으로 있는 모드들을 보면 init, run 이외에 모드 내에서 활용할 함수도 Copter Class 의 매서드로 되어있음을 알 수 있는데, 이 건에 관해서는 본인도 이를 따라야할지 말아야할지 좀 고민스럽다.
(그야 이 모드에서만 쓰는 내용들인데, 굳이 Copter Class에 원형을 선언해서 할 필요가 있을까,,,)
특별한 일이 없으면 따르긴 할거다.
이후 ArduCopter/flight_mode.cpp 에 몇가지 좀더 추가해주어야 한다.
set_mode 라는 매써드에 다음과 같이 init을 호출할 수 있도록 해주자.
case GUIDED_NOGPS:
success = guided_nogps_init(ignore_checks);
break;
case TESTSPACE:
success = testspace_init(ignore_checks);
break;
default:
success = false;
break;
}update_flight_mode 라는 매써드에 init과 마찬가지로 run을 호출할 수 있도록 해주자.
case GUIDED_NOGPS:
guided_nogps_run();
break;
case TESTSPACE:
testspace_run();
break;
default:
break;이는 필수적인 내용인지는 모르겠는데, Serial port로 모드명을 프린트할 일이 생길 시 프린트 할 수 있도록 추가해주는 부분이 있다.
print_flight_mode 라는 매써드에 추가해주자.
case GUIDED_NOGPS:
port->print("GUIDED_NOGPS");
break;
case TESTSPACE:
port->print("TESTSPACE");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;근대 mavproxy 에서는 가능한 모드에 저 TESTSPACE가 뜨진 않던데, 어디에 쓰는건지,,, 흠.
이제 init, run 원형을 선언, 추가해줬으니 그 내용물을 작성할때다.
ArduCopter 디렉토리 내에 "control_testspace.cpp" 파일을 추가해줬다.
파일 내용은 다음과 같다.
/*
* control_testspace.cpp
*
* Created on: 2017. 4. 5.
* Author: ygcho
*/
#include "Copter.h"
bool Copter::testspace_init(bool ignore_checks)
{
hal.console->printf("TestSpace mode init\n");
return true;
}
void Copter::testspace_run()
{
static uint8_t count = 0;
count++;
if(count > 100)
{
hal.console->printf("TestSpace mode run\n");
hal.console->printf("runtime : %lu\n", (unsigned long)AP_HAL::millis());
count = 0;
}
}간단하게 호출되는 여부를 판단하기 위한 print문과, run 함수가 어느정도 주기로 도는지 확인할 수 있는 print문을 작성해두었다. print내용은 mavproxy의 console창을 통해 확인할 것이다.
이제 모드를 추가했으면 빌드할 시간이다.
빌드하는 방법은 앞선 문서에서 다루었듯이, makefile을 통해 px4-v2를 타겟으로 빌드할수도 있으며 waf(sim_vehicle)를 이용해 시뮬레이터용으로 빌드할 수도있다.
현재는 기체 상태보다는 print문이 정상적으로 출력되는지 볼 것이기 때문에 다음과 같이 빌드하였다.
sim_vehicle.py --console --map --aircraft test
map은 그냥 멋지라고 추가했다.
빌드가 완료된 후에는 mavproxy가 자동으로 실행되며, 시뮬레이터와 붙는다.
mavproxy가 시뮬레이터와 정상적으로 붙었다면, mode를 변경해보자.
앞서 추가한 모드가 21번으로 되어있음을 기억해 두었을것이다. mavproxy의 경우 모드명으로도 모드변경이 가능하지만, mode TESTSPACE와 같이 변경시 변경이 안된다.
(공식 문서상으로는 GCS 개발하는 사람들한태 추가해달라고 해줘야 한댄다 ㅡㅡ)
때문에 직접 추가한 모드를 mavproxy로 실행하려면 모드명이 아닌 숫자로 변경해주어야 한다.
STABILIZE> mode 21
STABILIZE> Mode(21)>
"mode 21" 이라는 명령을 주면 모드명 대신 "Mode(모드번호)" 가 뜨게 된다. 아마 이부분에 아까 작성한 "TESTSPACE" print 파트가 들어갈탠데, GCS 개발하는 쪽에서 안해주면 뜨는걸 보는 일은 없을듯하다.
뭐 사용에는 문제가 없다.
Console 을 보면 다음과 같이 모드가 변경됨과 동시에 init, run이 실행됨을 알 수있다.
TestSpace mode init
Got MAVLink msg: COMMAND_ACK {command : 11, result : 0}
TestSpace mode run
runtime : 353616
TestSpace mode run
runtime : 353870
Mode Mode(21)
TestSpace mode run
runtime : 354122
TestSpace mode run
runtime : 354374
대충 실행 순서를 보면
- init을 먼저 실행
- mavlink로 명령을 받았음을 보냄
- run을 주기적으로 실행
이런 순서인 듯 하다. 주기를 보면 평균적으로 250ms 정도가 나오는데, 매인 loop가 400Hz로 도는것을 고려하면 매인 loop에서 별도 prescale 없이 바로 실행하는 것으로 보인다.