-
Notifications
You must be signed in to change notification settings - Fork 1
APM에서 PX4FLOW용 모드를 만들기 위한 사전 준비
현재 APM 소스상에서는 Optical Flow 관련 센서 리드 파트는 존제하지만, 실제로 사용하는 곳은 없다. 모드상에서도 Optical Flow 센서를 사용하는 부분은 찾을 수 없었고, Mission planner 상에서도 선택하는 항목이 없었다.
APM 공식 문서상에는 다음과 같이 친절하게(?) 설명되어있다.
물론 안된다.
문서상에는 3.3버전부터 실험적으로 진행되고 있다고 말하고 있지만, 3.3에서는 드랍된지 오래이며, OpticalFlow 센서 관련 응용 파트는 3.2.1 버전에 남아있다.
일단 그 응용파트, 사용된 모드에 관해 알아보도록 하자.
모드명은 ofloiter 모드이며, OpticalFlow 센서를 이용해 제자리에 머물며 앞뒤좌우 컨트롤을 허용하는 모드다.
기존에 있는 loiter 모드와 상당히 유사하다. 차이점이라면 OpticalFlow 센서를 사용하느냐, GPS를 사용하느냐 이다.
쨋든 위치정보를 센서로 부터 읽어내어, 현제 위치와 어긋날 경우 원하는 자세 정보를 만들어내서 보정하게끔 되어있다.
APM에서는 모드를 별도 cpp 파일로 만들어 별도로 관리하게끔 되어있다. 'control'이라는 수식을 앞에 붙여 모드 관련 소스임을 나타낸다.
loiter 모드는 control_loiter 로 명시되어있다.
모드의 구성요소는 크개 두가지이다.
- init
말 그대로 초기화 하는 과정을 기술하는 것으로, 여타 init 의 의미와 다를바 없다. - run
반복적으로 호출되며 동작을 하는 부분으로, 실질적인 동작 관련 소스가 나타나있다.
모드를 개발하는 사람이 원하는 동작을 기술해두고, 차후 해당 모드를 선택하면(트랜시버의 스틱이나 GCS 명령어로 변경) 동작하는 방식
위 두 구성요소는 각자
- '모드명_init'
- '모드명_run'
과 같이 함수명을 정해 각각의 함수로 만들고, 차후 이를 타 소스에 등록해 둠으로써 활용할 수 있다.
(해당 과정은 실제로 모드를 만들어 등록을 해볼 때 문서로 정리해둬야 할 것 같다. 이 부분은 APM쪽 문서도 잘 되있더라)
이 과정에 대한 자세한 영문 문서는 APM쪽에서 잘 정리해 두었다.
주의
아래 버전부터는 필자가 러프하게 코드를 대강 보고 해석한 부분입니다.
정확히 맞지 않을 수 있습니다.
실제로 내가 보고싶어하는 ofloiter 모드는 3.2.1버전 까지만 있으며, loiter 모드와 매우 흡사하다.
살펴본 바로는 GPS관련 파트를 OpticalFlow관련 파트로 바꿔치기 한 것으로 보이는데, 3.3 이상 버전과 좀 차이가 나기 때문에 정리해둘 필요성이 있다.
(이 모드 하나 보자고 업데이트도 안되는 구버전 바짓가랑이 붙잡고 있을수도 없기 때문에,,,)
// loiter_init - initialise loiter controller
static bool loiter_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and accelerationj
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
}else{
return false;
}
}각 함수의 내용물까지 세세하게 뜯어볼 생각은 없다.(지금은)
주석은 꽤 잘 달려있는 편이라서 주석에 따라 각 부분이 하는 것을 정리해보면
- 현제 위치를 타겟으로 설정
- 수직축(상하)속도의 최대/최솟값, 가속도값 관련 설정
- 멈출 높이 설정(정해진 값이 있는듯하다)
이 과정은 GPS가 제대로 잡혀있을때 1회 한다.
// loiter_run - runs the loiter controller
// should be called at 100hz or more
static void loiter_run()
{
float target_yaw_rate = 0;
float target_climb_rate = 0;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || !inertial_nav.position_ok()) {
wp_nav.init_loiter_target();
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
pos_control.set_alt_target_to_current_alt();
return;
}
// process pilot inputs
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
} else {
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
wp_nav.clear_pilot_desired_acceleration();
}
// relax loiter target if we might be landed
if (land_complete_maybe()) {
wp_nav.loiter_soften_for_landing();
}
// when landed reset targets and output zero throttle
if (ap.land_complete) {
wp_nav.init_loiter_target();
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
pos_control.set_alt_target_to_current_alt();
}else{
// run loiter controller
wp_nav.update_loiter();
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// body-frame rate controller is run directly from 100hz loop
// run altitude controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
}
}부분별로 살펴보도록 하자. (이렇게 보니까 또 기네)
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || !inertial_nav.position_ok()) {
wp_nav.init_loiter_target();
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
pos_control.set_alt_target_to_current_alt();
return;
}만약 arm이 되어있지 않을 경우,
- 현재 heading, 고도 값을 타겟값으로 초기화
- throttle 값을 0으로 초기화
- 이후 내용을 실행하지 않고 즉시 나옴
// process pilot inputs
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
} else {
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
wp_nav.clear_pilot_desired_acceleration();
}radio failsafe 플래그를 확인후, failsafe가 아니라면,
- update_simple_mode
사용자 인풋을 rotate(백터값 회전을 의미하는 듯)시킨다. - 사용자 roll, pitch 명령어를 가져와 세팅하고, yaw 값을 가져와 별도로 저장해둔다.
- 수직 속도 명령어를 가져와 셋팅한다.
만약 사용자 takeoff 명령을 했다면,
- landing 관련 플래그를 설정하는 함수 호출(인듯함)
- throttle 값을 takeoff시 값으로 설정(정해진 듯 하다)
그리고 failsafe 플래그라면 가속도 명령값을 저장하던 변수들을 초기화 하는 것 같다.
// relax loiter target if we might be landed
if (land_complete_maybe()) {
wp_nav.loiter_soften_for_landing();
}주석도 애매하게 써있는데, 함수명도 애매하다(maybe라니).
아무래도 불시착?(어쩌다 땅바닥 착지해야 한다든지) 어쨋든 사용자가 의도하지 않은 landing이 발생하면 실행되는 듯 하고,
이 경우 갑자기 추락하는것을 방지하기 위한 코드인듯 하다.
// when landed reset targets and output zero throttle
if (ap.land_complete) {
wp_nav.init_loiter_target();
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
pos_control.set_alt_target_to_current_alt();이 경우는 의도된 landing일 경우 실행되는 파트인 듯 하다.
landing이 완료되면,
- 무언가 target값을 초기화 하는 부분이 있다(그냥 0으로 초기화하는지, 디폴트 값이 있는지는 모르겠다).
- arm이 되어있지 않은 경우와 비슷하게, 현제 yaw값을 타겟값으로 설정한다.
- throttle 값을 최솟값~takeoff값 사이에 놓도록 확인하는 부분이 있는 듯 하다(사고 방지 차원인가?)
- 현재 고도값을 타겟값으로 지정한다.
그냥 땅에 붙어있을때마다 값을 초기화 하는 듯 하다. 뜰 때마다 새로운 값을 쓰는 것 같다.
}else{
// run loiter controller
wp_nav.update_loiter();
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// body-frame rate controller is run directly from 100hz loop
// run altitude controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
}맨 마지막에 실제로 날아댕길때 쓰는 소스가 있다. 대충 요약하면 이렇다.
- loiter 제어에 쓰일 자세값을 만들어 낸다. 어떤식으로 만드는지는 추상화 해놔서 보이지 않지만, GPS관련 부분에서 담당하고 있기 때문에 GPS를 쓰는것은 확실하다.
- 만들어진 자세값을 자세제어기에 넣는다.
- sonar 센서가 사용가능할때 땅표면을 확인(거리에 따라 PID 와 같은 제어기를 이용하는 듯 하다)
리턴값은 수직 속도로 보이며, 땅과 너무 가까워지면 멀어지는 방향으로 수직 속도를 올린다든지 할 것 같다. - sonar 센서를 통해 만들어낸 수직 속도값을 타겟값으로 지정, 수직축 컨트롤러를 실행시킨다.
순서상으로는 앞뒤좌우 컨트롤 -> 상하 컨트롤 순이다. 정말 계산하는 파트는 추상화 해놨고, 필요에 따라 열어봐야 할 듯.
솔직히 말해서, loiter 모드와 정말 차이가 없다. 차이점이라면 다음과 같다.
기존 GPS 관련(wp_nav 로 인스턴스화 된 GPS 관련 파트)부분들이 OpticalFlow 관련 함수들로 대체되었다.
때문에 내용이 거의 같고, 다음과 같이 소스상에 대체된 부분이 있다고만 언급해도 될 듯하다.
기존 GPS를 이용한 loiter 모드의 init함수 중 일부
static bool loiter_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();OpticalFlow 센서값을 활용한 init 함수 중 일부
static bool ofloiter_init(bool ignore_checks)
{
if (g.optflow_enabled || ignore_checks) {GPS_ok() 가 g.opticalflow_enabled 로 바뀌었다.
그리고 현재 위치를 GPS값으로 초기화 하는 부분은, OpticalFlow로써는 현제 위치를 특정할 수 없기 때문에 필요없는 부분이라 없어진 것 같다.
그 이후 나오는 소스도 다 이런식으로 g.~~ 로 되어있다.
여기서 이 'g' 라는 녀석은 parameters 객체를 인스턴스화 한 것이다.
3.2.1 버전의 ArduCopter.pde 파일에 전역으로 인스턴스화 되있다.
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
//
// Global parameters are all contained within the 'g' class.
//
static Parameters g;(왜 하필 이름을 g 로 해놔서 찾기도 어렵게 해놨어)
(것보다 opticalflow 함수관련 부분을 왜 여기다 떄려박아 논건대;)
(그냥 ofloiter 성능이 안나온게 아니라 이게 뭣같아서 드랍시킨거 아닐까)
그리고 이 3.2.1 버전의 Parameter Class 내부에 다음과 같이 OpticalFlow 관련 파트가 있다.
AC_PID pid_optflow_roll;
AC_PID pid_optflow_pitch;
~~
pid_optflow_roll (OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX),
pid_optflow_pitch (OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX),뭐 대충 이런식으로 기술되있는데, 최신버전에서 확인결과 관련 파트는 전부 제거되있었다.
(응용하는 부분을 찾을수 없었던게 당연함)
즉, 최신버전에서 Optical Flow 센서를 활용하기 위해서는 구버전 소스를 뒤적뒤적 찾아다가 전생시키던지, 직접 개발하는 수밖에 없는것 같다(뭐 그럴것 같더라;)
그런대 최신버전에서는 구버전에서 보이지 않던 몇가지가 듬성듬성 보이기 때문에, 단순히 OpticalFlow 관련 파트를 전생시켜서 가능할지는 좀더 봐야 한다.
최신버전의 loiter 모드를 마이너한 부분을 제외하고 가장 크게 바뀐점을 들라면 다음이 있다.
- switch, case 문을 활용해 코드 가독성을 높혔다.
- 그 외 짜잘한 부분에서 최신 코드베이스에 따라 포인터명, 함수명 등이 바뀌었다.
이는 3.3버전부터 이루어진 리팩토링 작업으로 보이며, 확실히 가독성은 좋아졌지만 성능이나 구동 방법은 크개 바뀌지않았다.
사실 내용물은 이전 내용과 별반 다르지 않기 때문에 어떤식으로 리팩토링 했는지만 소개한다.
// Loiter State Machine Determination
if (!motors.armed() || !motors.get_interlock()) {
loiter_state = Loiter_MotorStopped;
} else if (takeoff_state.running || takeoff_triggered) {
loiter_state = Loiter_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
loiter_state = Loiter_Landed;
} else {
loiter_state = Loiter_Flying;
}위와 같이 state 변수를 하나 정한 뒤, 조건에 따라 state 값을 변화시킨다.
이후 이 loiter_state 변수는 switch문에서 활용한다.
switch (loiter_state) {
case Loiter_MotorStopped:
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
#else
wp_nav.init_loiter_target();
attitude_control.reset_rate_controller_I_terms();
attitude_control.set_yaw_target_to_current_heading();
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
pos_control.update_z_controller();
break;
case Loiter_Takeoff:
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
break;
case Loiter_Landed:
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
wp_nav.init_loiter_target();
attitude_control.reset_rate_controller_I_terms();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control.update_z_controller();
break;
case Loiter_Flying:
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.update_z_controller();
break;
}위 소스를 보면 각 4파트로 나눠 동작을 기술하였고, case 별로 나눠서 가독성은 올라갔다.
다만 이전 소스와 좀 차이가 나고, 추가된 부분이 살짝식 듬성듬성 있다.
이에 관해서는 사실 나는 그럴 생각인데, 절대위치를 활용한 waypoint 기능을 더 이용하고 싶긴 한다. 왜냐하면, 그러면 내가 딱 정한 포인트에 정지하거나, 원하는 만큼만 이동하도록 제한할 수 있기 때문이다.
일단 이전 소스에서 가져올 건 다 가져와서 활용할 생각이다.
곂치는 부분이 꽤 되기 때문에 이러한 부분은 크게 문제가 될 것이라 생각하지 않지만, 3.3이후 버전에서 추가된 파트, 특히 GPS를 활용하면서 추가된 부분은 직접 구현해줘야 한다. 문제는 내가 그쪽에 관해서 잘 모름.
(내 머리가 안좋아서 이해를 못하는 건가?)
이 부분에 관해서는 선행 예시가 없었기 때문에, 어떻게 될지 모르겠다.
필연적으로 시뮬레이터나 실재 실험환경이 구성되어서 눈으로 보면서 확인해야 되는데, 시뮬레이터 써본적도 없고 실험환경 구성도 안됨
(할만한 마땅한 곳이 있어야지;)
아 씨 어떻게 해야할지 모르겠다;