AC_PrecLand.cpp 17 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450
  1. #include <AP_HAL/AP_HAL.h>
  2. #include <AP_Scheduler/AP_Scheduler.h>
  3. #include <AP_AHRS/AP_AHRS.h>
  4. #include "AC_PrecLand.h"
  5. #include "AC_PrecLand_Backend.h"
  6. #include "AC_PrecLand_Companion.h"
  7. #include "AC_PrecLand_IRLock.h"
  8. #include "AC_PrecLand_SITL_Gazebo.h"
  9. #include "AC_PrecLand_SITL.h"
  10. #include <AP_AHRS/AP_AHRS.h>
  11. extern const AP_HAL::HAL& hal;
  12. const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
  13. // @Param: ENABLED
  14. // @DisplayName: Precision Land enabled/disabled and behaviour
  15. // @Description: Precision Land enabled/disabled and behaviour
  16. // @Values: 0:Disabled, 1:Enabled
  17. // @User: Advanced
  18. AP_GROUPINFO_FLAGS("ENABLED", 0, AC_PrecLand, _enabled, 0, AP_PARAM_FLAG_ENABLE),
  19. // @Param: TYPE
  20. // @DisplayName: Precision Land Type
  21. // @Description: Precision Land Type
  22. // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo, 4:SITL
  23. // @User: Advanced
  24. AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0),
  25. // @Param: YAW_ALIGN
  26. // @DisplayName: Sensor yaw alignment
  27. // @Description: Yaw angle from body x-axis to sensor x-axis.
  28. // @Range: 0 360
  29. // @Increment: 1
  30. // @User: Advanced
  31. // @Units: cdeg
  32. AP_GROUPINFO("YAW_ALIGN", 2, AC_PrecLand, _yaw_align, 0),
  33. // @Param: LAND_OFS_X
  34. // @DisplayName: Land offset forward
  35. // @Description: Desired landing position of the camera forward of the target in vehicle body frame
  36. // @Range: -20 20
  37. // @Increment: 1
  38. // @User: Advanced
  39. // @Units: cm
  40. AP_GROUPINFO("LAND_OFS_X", 3, AC_PrecLand, _land_ofs_cm_x, 0),
  41. // @Param: LAND_OFS_Y
  42. // @DisplayName: Land offset right
  43. // @Description: desired landing position of the camera right of the target in vehicle body frame
  44. // @Range: -20 20
  45. // @Increment: 1
  46. // @User: Advanced
  47. // @Units: cm
  48. AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0),
  49. // @Param: EST_TYPE
  50. // @DisplayName: Precision Land Estimator Type
  51. // @Description: Specifies the estimation method to be used
  52. // @Values: 0:RawSensor, 1:KalmanFilter
  53. // @User: Advanced
  54. AP_GROUPINFO("EST_TYPE", 5, AC_PrecLand, _estimator_type, 1),
  55. // @Param: ACC_P_NSE
  56. // @DisplayName: Kalman Filter Accelerometer Noise
  57. // @Description: Kalman Filter Accelerometer Noise, higher values weight the input from the camera more, accels less
  58. // @Range: 0.5 5
  59. // @User: Advanceds
  60. AP_GROUPINFO("ACC_P_NSE", 6, AC_PrecLand, _accel_noise, 2.5f),
  61. // @Param: CAM_POS_X
  62. // @DisplayName: Camera X position offset
  63. // @Description: X position of the camera in body frame. Positive X is forward of the origin.
  64. // @Units: m
  65. // @User: Advanced
  66. // @Param: CAM_POS_Y
  67. // @DisplayName: Camera Y position offset
  68. // @Description: Y position of the camera in body frame. Positive Y is to the right of the origin.
  69. // @Units: m
  70. // @User: Advanced
  71. // @Param: CAM_POS_Z
  72. // @DisplayName: Camera Z position offset
  73. // @Description: Z position of the camera in body frame. Positive Z is down from the origin.
  74. // @Units: m
  75. // @User: Advanced
  76. AP_GROUPINFO("CAM_POS", 7, AC_PrecLand, _cam_offset, 0.0f),
  77. // @Param: BUS
  78. // @DisplayName: Sensor Bus
  79. // @Description: Precland sensor bus for I2C sensors.
  80. // @Values: -1:DefaultBus,0:InternalI2C,1:ExternalI2C
  81. // @User: Advanced
  82. AP_GROUPINFO("BUS", 8, AC_PrecLand, _bus, -1),
  83. // @Param: LAG
  84. // @DisplayName: Precision Landing sensor lag
  85. // @Description: Precision Landing sensor lag, to cope with variable landing_target latency
  86. // @Range: 0.02 0.250
  87. // @Increment: 1
  88. // @Units: s
  89. // @User: Advanced
  90. // @RebootRequired: True
  91. AP_GROUPINFO("LAG", 9, AC_PrecLand, _lag, 0.02f), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms)
  92. AP_GROUPEND
  93. };
  94. // Default constructor.
  95. // Note that the Vector/Matrix constructors already implicitly zero
  96. // their values.
  97. //
  98. AC_PrecLand::AC_PrecLand()
  99. {
  100. // set parameters to defaults
  101. AP_Param::setup_object_defaults(this, var_info);
  102. }
  103. // perform any required initialisation of landing controllers
  104. // update_rate_hz should be the rate at which the update method will be called in hz
  105. void AC_PrecLand::init(uint16_t update_rate_hz)
  106. {
  107. // exit immediately if init has already been run
  108. if (_backend != nullptr) {
  109. return;
  110. }
  111. // default health to false
  112. _backend = nullptr;
  113. _backend_state.healthy = false;
  114. // create inertial history buffer
  115. // constrain lag parameter to be within bounds
  116. _lag = constrain_float(_lag, 0.02f, 0.25f);
  117. // calculate inertial buffer size from lag and minimum of main loop rate and update_rate_hz argument
  118. const uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag * MIN(update_rate_hz, AP::scheduler().get_loop_rate_hz())), 1);
  119. // instantiate ring buffer to hold inertial history, return on failure so no backends are created
  120. _inertial_history = new ObjectArray<inertial_data_frame_s>(inertial_buffer_size);
  121. if (_inertial_history == nullptr) {
  122. return;
  123. }
  124. // instantiate backend based on type parameter
  125. switch ((enum PrecLandType)(_type.get())) {
  126. // no type defined
  127. case PRECLAND_TYPE_NONE:
  128. default:
  129. return;
  130. // companion computer
  131. case PRECLAND_TYPE_COMPANION:
  132. _backend = new AC_PrecLand_Companion(*this, _backend_state);
  133. break;
  134. // IR Lock
  135. case PRECLAND_TYPE_IRLOCK:
  136. _backend = new AC_PrecLand_IRLock(*this, _backend_state);
  137. break;
  138. #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
  139. case PRECLAND_TYPE_SITL_GAZEBO:
  140. _backend = new AC_PrecLand_SITL_Gazebo(*this, _backend_state);
  141. break;
  142. case PRECLAND_TYPE_SITL:
  143. _backend = new AC_PrecLand_SITL(*this, _backend_state);
  144. break;
  145. #endif
  146. }
  147. // init backend
  148. if (_backend != nullptr) {
  149. _backend->init();
  150. }
  151. }
  152. // update - give chance to driver to get updates from sensor
  153. void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
  154. {
  155. // exit immediately if not enabled
  156. if (_backend == nullptr || _inertial_history == nullptr) {
  157. return;
  158. }
  159. // append current velocity and attitude correction into history buffer
  160. struct inertial_data_frame_s inertial_data_newest;
  161. const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
  162. _ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt);
  163. inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned();
  164. Vector3f curr_vel;
  165. nav_filter_status status;
  166. if (!_ahrs.get_velocity_NED(curr_vel) || !_ahrs.get_filter_status(status)) {
  167. inertial_data_newest.inertialNavVelocityValid = false;
  168. } else {
  169. inertial_data_newest.inertialNavVelocityValid = status.flags.horiz_vel;
  170. }
  171. curr_vel.z = -curr_vel.z; // NED to NEU
  172. inertial_data_newest.inertialNavVelocity = curr_vel;
  173. inertial_data_newest.time_usec = AP_HAL::micros64();
  174. _inertial_history->push_force(inertial_data_newest);
  175. // update estimator of target position
  176. if (_backend != nullptr && _enabled) {
  177. _backend->update();
  178. run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
  179. }
  180. }
  181. bool AC_PrecLand::target_acquired()
  182. {
  183. _target_acquired = _target_acquired && (AP_HAL::millis()-_last_update_ms) < 2000;
  184. return _target_acquired;
  185. }
  186. bool AC_PrecLand::get_target_position_cm(Vector2f& ret)
  187. {
  188. if (!target_acquired()) {
  189. return false;
  190. }
  191. Vector2f curr_pos;
  192. if (!AP::ahrs().get_relative_position_NE_origin(curr_pos)) {
  193. return false;
  194. }
  195. ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm
  196. ret.y = (_target_pos_rel_out_NE.y + curr_pos.y) * 100.0f; // m to cm
  197. return true;
  198. }
  199. void AC_PrecLand::get_target_position_measurement_cm(Vector3f& ret)
  200. {
  201. ret = _target_pos_rel_meas_NED*100.0f;
  202. return;
  203. }
  204. bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret)
  205. {
  206. if (!target_acquired()) {
  207. return false;
  208. }
  209. ret = _target_pos_rel_out_NE*100.0f;
  210. return true;
  211. }
  212. bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret)
  213. {
  214. if (!target_acquired()) {
  215. return false;
  216. }
  217. ret = _target_vel_rel_out_NE*100.0f;
  218. return true;
  219. }
  220. // handle_msg - Process a LANDING_TARGET mavlink message
  221. void AC_PrecLand::handle_msg(const mavlink_message_t &msg)
  222. {
  223. // run backend update
  224. if (_backend != nullptr) {
  225. _backend->handle_msg(msg);
  226. }
  227. }
  228. //
  229. // Private methods
  230. //
  231. void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
  232. {
  233. const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
  234. switch (_estimator_type) {
  235. case ESTIMATOR_TYPE_RAW_SENSOR: {
  236. // Return if there's any invalid velocity data
  237. for (uint8_t i=0; i<_inertial_history->available(); i++) {
  238. const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
  239. if (!inertial_data->inertialNavVelocityValid) {
  240. _target_acquired = false;
  241. return;
  242. }
  243. }
  244. // Predict
  245. if (target_acquired()) {
  246. _target_pos_rel_est_NE.x -= inertial_data_delayed->inertialNavVelocity.x * inertial_data_delayed->dt;
  247. _target_pos_rel_est_NE.y -= inertial_data_delayed->inertialNavVelocity.y * inertial_data_delayed->dt;
  248. _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x;
  249. _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y;
  250. }
  251. // Update if a new Line-Of-Sight measurement is available
  252. if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
  253. _target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x;
  254. _target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y;
  255. _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x;
  256. _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y;
  257. _last_update_ms = AP_HAL::millis();
  258. _target_acquired = true;
  259. }
  260. // Output prediction
  261. if (target_acquired()) {
  262. run_output_prediction();
  263. }
  264. break;
  265. }
  266. case ESTIMATOR_TYPE_KALMAN_FILTER: {
  267. // Predict
  268. if (target_acquired()) {
  269. const float& dt = inertial_data_delayed->dt;
  270. const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED;
  271. _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt);
  272. _ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt);
  273. }
  274. // Update if a new Line-Of-Sight measurement is available
  275. if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
  276. float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f);
  277. if (!target_acquired()) {
  278. // reset filter state
  279. if (inertial_data_delayed->inertialNavVelocityValid) {
  280. _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f));
  281. _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.y, sq(2.0f));
  282. } else {
  283. _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f));
  284. _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f));
  285. }
  286. _last_update_ms = AP_HAL::millis();
  287. _target_acquired = true;
  288. } else {
  289. float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var);
  290. float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var);
  291. if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
  292. _outlier_reject_count = 0;
  293. _ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var);
  294. _ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var);
  295. _last_update_ms = AP_HAL::millis();
  296. _target_acquired = true;
  297. } else {
  298. _outlier_reject_count++;
  299. }
  300. }
  301. }
  302. // Output prediction
  303. if (target_acquired()) {
  304. _target_pos_rel_est_NE.x = _ekf_x.getPos();
  305. _target_pos_rel_est_NE.y = _ekf_y.getPos();
  306. _target_vel_rel_est_NE.x = _ekf_x.getVel();
  307. _target_vel_rel_est_NE.y = _ekf_y.getVel();
  308. run_output_prediction();
  309. }
  310. break;
  311. }
  312. }
  313. }
  314. bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
  315. {
  316. if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
  317. _last_backend_los_meas_ms = _backend->los_meas_time_ms();
  318. _backend->get_los_body(target_vec_unit_body);
  319. // Apply sensor yaw alignment rotation
  320. float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
  321. float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
  322. Matrix3f Rz = Matrix3f(
  323. cos_yaw_align, -sin_yaw_align, 0,
  324. sin_yaw_align, cos_yaw_align, 0,
  325. 0, 0, 1
  326. );
  327. target_vec_unit_body = Rz*target_vec_unit_body;
  328. return true;
  329. } else {
  330. return false;
  331. }
  332. }
  333. bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid)
  334. {
  335. Vector3f target_vec_unit_body;
  336. if (retrieve_los_meas(target_vec_unit_body)) {
  337. const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
  338. Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body;
  339. bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
  340. bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
  341. if (target_vec_valid && alt_valid) {
  342. float dist, alt;
  343. if (_backend->distance_to_target() > 0.0f) {
  344. dist = _backend->distance_to_target();
  345. alt = dist * target_vec_unit_ned.z;
  346. } else {
  347. alt = MAX(rangefinder_alt_m, 0.0f);
  348. dist = alt / target_vec_unit_ned.z;
  349. }
  350. // Compute camera position relative to IMU
  351. Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index());
  352. Vector3f cam_pos_ned = inertial_data_delayed->Tbn * (_cam_offset.get() - accel_body_offset);
  353. // Compute target position relative to IMU
  354. _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned;
  355. return true;
  356. }
  357. }
  358. return false;
  359. }
  360. void AC_PrecLand::run_output_prediction()
  361. {
  362. _target_pos_rel_out_NE = _target_pos_rel_est_NE;
  363. _target_vel_rel_out_NE = _target_vel_rel_est_NE;
  364. // Predict forward from delayed time horizon
  365. for (uint8_t i=1; i<_inertial_history->available(); i++) {
  366. const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
  367. _target_vel_rel_out_NE.x -= inertial_data->correctedVehicleDeltaVelocityNED.x;
  368. _target_vel_rel_out_NE.y -= inertial_data->correctedVehicleDeltaVelocityNED.y;
  369. _target_pos_rel_out_NE.x += _target_vel_rel_out_NE.x * inertial_data->dt;
  370. _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data->dt;
  371. }
  372. const AP_AHRS &_ahrs = AP::ahrs();
  373. const Matrix3f& Tbn = (*_inertial_history)[_inertial_history->available()-1]->Tbn;
  374. Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(_ahrs.get_primary_accel_index());
  375. // Apply position correction for CG offset from IMU
  376. Vector3f imu_pos_ned = Tbn * accel_body_offset;
  377. _target_pos_rel_out_NE.x += imu_pos_ned.x;
  378. _target_pos_rel_out_NE.y += imu_pos_ned.y;
  379. // Apply position correction for body-frame horizontal camera offset from CG, so that vehicle lands lens-to-target
  380. Vector3f cam_pos_horizontal_ned = Tbn * Vector3f(_cam_offset.get().x, _cam_offset.get().y, 0);
  381. _target_pos_rel_out_NE.x -= cam_pos_horizontal_ned.x;
  382. _target_pos_rel_out_NE.y -= cam_pos_horizontal_ned.y;
  383. // Apply velocity correction for IMU offset from CG
  384. Vector3f vel_ned_rel_imu = Tbn * (_ahrs.get_gyro() % (-accel_body_offset));
  385. _target_vel_rel_out_NE.x -= vel_ned_rel_imu.x;
  386. _target_vel_rel_out_NE.y -= vel_ned_rel_imu.y;
  387. // Apply land offset
  388. Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f;
  389. _target_pos_rel_out_NE.x += land_ofs_ned_m.x;
  390. _target_pos_rel_out_NE.y += land_ofs_ned_m.y;
  391. }