APM에서 PX4FLOW를 사용할 수 있을까 - whdlgp/look_into_pixhawk_with_apm GitHub Wiki
APM에서 PX4FLOW 센서를 사용
사용하려고니 좀 걸리는 점이 있다.
- 일단 APM상에 optical flow 관련 부분이 있긴 한데, 사용하는 모드가 확인이 안됬다.
- 공식 문서를보면 OF_Loiter 모드를 사용하면 된다는데 일단 드랍된 모드인듯,,,
- 펌웨어가 pixhawk 쪽에서 사용하는 펌웨어랑 다르니 할말이 없다(
뭔짓거리야;)
게다가 APM 쪽에서도 '자기내 드론은 인도어용으로는 적합하지 않으니 GPS를 쓰라'는 분위기고, 사실상 APM에서 optical flow는 사장된듯한 분위기다. 하지만 소스상에서는 optical flow 센서값을 업데이트 하는 부분이 멀쩡이 테스트로 돌아가고 있다.
때문에 APM 에서의 optical flow 부분을 먼저 보기로 했다.
APM 에서 optical flow 센서
소스 상에서는 테스크 스케줄러에서 update_optical_flow 라는 함수가 200Hz 로 돌게끔 작성되어있다.
(센서값 업데이트만 하는거냐?)
(그래서 어디다가 쓰는데?)
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
#endif
update_optical_flow 내부
// called at 200hz
#if OPTFLOW == ENABLED
void Copter::update_optical_flow(void)
{
static uint32_t last_of_update = 0;
// exit immediately if not enabled
if (!optflow.enabled()) {
return;
}
// read from sensor
optflow.update();
// write to log and send to EKF if new data has arrived
if (optflow.last_update() != last_of_update) {
last_of_update = optflow.last_update();
uint8_t flowQuality = optflow.quality();
Vector2f flowRate = optflow.flowRate();
Vector2f bodyRate = optflow.bodyRate();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}
}
}
#endif // OPTFLOW == ENABLED
돌아가는 테스크 자체는 간단해보인다. 크게 두가지 부분으로 나눌 수 있다.
- 센서값 업데이트
- AHRS(EKF 쪽으로)로 값 보내기
(
보낸다고 했지 저기 관련 모드가 있다고 까지 한적은 없다)
일단 EKF(extended kalman filter? what?)쪽으로 보내는 부분은 뒤로하고, 실제 센서값 읽는 방식이 pixhawk 쪽과 다른지가 우선이다.
optical flow 의 백엔드와 프론트엔드
void OpticalFlow::update(void)
{
if (backend != NULL) {
backend->update();
}
// only healthy if the data is less than 0.5s old
_flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
}
여기서 백엔드와 프론트엔드 개념이라는게 등장한다.
(서버사이드와 프론트사이드?)
(웹하냐 ㅡㅡ)
소스코드를 보면 PX4FLOW 이외에 다른 optical flow 센서도 지원하는 모양이며, 백엔드는 이런 센서값에 따라 업데이트 하는 방식의 다름을 대응하기 위한 것인 듯 하다.
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
backend = new AP_OpticalFlow_PX4(*this);
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
backend = new AP_OpticalFlow_HIL(*this);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
backend = new AP_OpticalFlow_Linux(*this, hal.i2c_mgr->get_device(HAL_OPTFLOW_PX4FLOW_I2C_BUS, HAL_OPTFLOW_PX4FLOW_I2C_ADDRESS));
#endif
사용하는 센서에 따라 backend 포인터를 다르게 가리키도록 하고 있다. 프론트엔드는 optical_flow Class 자기 자신을 의미하며, 백엔드는 센서별로 다르게 동작하는 부분을 구현하는 셈이다.
백엔드
백엔드 원형을 상속받아 실제 내용물을 구현한 Class 의 원형은 다음과 같다.
class AP_OpticalFlow_PX4 : public OpticalFlow_backend
{
public:
/// constructor
AP_OpticalFlow_PX4(OpticalFlow &_frontend);
// init - initialise the sensor
void init();
// update - read latest values from sensor and fill in x,y and totals.
void update(void);
private:
int _fd; // file descriptor for sensor
uint64_t _last_timestamp; // time of last update (used to avoid processing old reports)
};
이전에 Pixhawk 보드는 Nuttx OS(유사 POSIX)를 사용한다고 하였는데, 센서값을 읽는것도 파일 입출력을 통해 읽는 듯 하다.
_fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
// check for failure to open device
if (_fd == -1) {
return;
}
// change to 10Hz update
if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) {
hal.console->printf("Unable to set flow rate to 10Hz\n");
}
파일 입출력을 통해 파일 디스크립터를 초기화 하고, 무슨 설정값을 설정하는 모양이다.
(이전에 센서 사용법에서는 안나오던 설정인데?)
하여튼, 이 초기화된 파일 디스크립터 _fd를 통해 PX4FLOW의 센서값을 읽어드린다.
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_PX4::update(void)
{
// return immediately if not initialised
if (_fd == -1) {
return;
}
struct optical_flow_s report;
while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) &&
report.timestamp != _last_timestamp) {
struct OpticalFlow::OpticalFlow_state state;
state.device_id = report.sensor_id;
state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
float yawAngleRad = _yawAngleRad();
float cosYaw = cosf(yawAngleRad);
float sinYaw = sinf(yawAngleRad);
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
// rotate sensor measurements from sensor to body frame through sensor yaw angle
state.flowRate.x = flowScaleFactorX * integralToRate * (cosYaw * float(report.pixel_flow_x_integral) - sinYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the X body axis
state.flowRate.y = flowScaleFactorY * integralToRate * (sinYaw * float(report.pixel_flow_x_integral) + cosYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the Y body axis
state.bodyRate.x = integralToRate * (cosYaw * float(report.gyro_x_rate_integral) - sinYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the X body axis
state.bodyRate.y = integralToRate * (sinYaw * float(report.gyro_x_rate_integral) + cosYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the Y body axis
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
_last_timestamp = report.timestamp;
_update_frontend(state);
}
}
optical_flow_s 구조체 report를 하나 선언하는데, 이 구조체로 먼저 파일을 읽는다.
간략하게 데이터가 어떻게 이동되는지 적어보면
- optical_flow_s 구조체인 report로 센서값을 저장한다.
- OpticalFlow::OpticalFlow_state 구조체인 state로 값을 가공해 저장한다.
- 이를 frontend로 보낸다.
각각 구조체의 원형은 다음과 같다.
#ifdef __cplusplus
struct __EXPORT optical_flow_s {
#else
struct optical_flow_s {
#endif
uint64_t timestamp;
uint8_t sensor_id;
float pixel_flow_x_integral;
float pixel_flow_y_integral;
float gyro_x_rate_integral;
float gyro_y_rate_integral;
float gyro_z_rate_integral;
float ground_distance_m;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t frame_count_since_last_readout;
int16_t gyro_temperature;
uint8_t quality;
#ifdef __cplusplus
#endif
};
struct OpticalFlow_state {
uint8_t device_id; // device id
uint8_t surface_quality; // image quality (below TBD you can't trust the dx,dy values returned)
Vector2f flowRate; // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
Vector2f bodyRate; // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
};
여기서 눈에띄는 점은 optical_flow_s 구조체의 내용물이 pixhawk쪽 펌웨어에서 integral 값으로 응답을 받을때와 거의 유사하다는 것이다. 다만, 받는순서가 좀 다른 듯 하다.
Pixhawk는 uORB를 사용하고 있기 때문에 실제로 read 함수를 통해 동작하는 부분까지 정확히 파악해야 알 수 있을 것 같다.
일단 센서값 업데이트 하는 부분은 여기까지,,,(관련 부분이 어디있는지 못찾겠어 썋)
그럼 EKF로 보내는 값은 어떻게 되는건데?
솔직히 모르겠다. 아니, 찾아보고 있긴 한데 뭐랄까 분석툴도 잘 안먹고 미묘하게 찾기 힘들게(귀찮게) 되있어서,,,
(그냥 솔직히 보고 싶지않아 귀찮아 뼤에ㅔㅔㅔㅔㅔㅔㅔㅔㅔㅔㅔㅔ)
현제 APM쪽에서 사용하는 PX4FLOW 파트도 유사한 데이터를 받을 것이란 결과는 나왔지만, 실제로 데이터가 보내지는 순서, 받는 방법등이 아직 찾아지지 않았다. 이는 uORB와 관련이 있을 것 같은데, 솔직히 여기서 좀 막혔다. uORB자체는 뭐 어렵진 않은데, 그걸 이사람들이 어떤 함수나 라이브러리로 사용하는지 종이 안잡힌다.
- 실제 PX4FLOW로 I2C를 통해 데이터를 주고받는 파트의 확인이 필요하다.
- EKF로 데이터가 가서 이게 어떻게 가공되서 모드에서 사용할 수 있는지 확인이 필요하다.