Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
gps.cpp
- Committer:
- osaka
- Date:
- 2021-11-12
- Revision:
- 107:46e039e12182
- Parent:
- 100:7589b663d462
File content as of revision 107:46e039e12182:
#include "global.hpp" void getGPSval() { gpsUpdateFlag = false; gps.Update(); if (gps.iTOW_VELNED != itow_velned) { itow_velned = gps.iTOW_VELNED; vi.x = gps.VelocityNED.x; vi.y = gps.VelocityNED.y; vi.z = gps.VelocityNED.z; } if (gps.iTOW_POSLLH != itow_posllh) { itow_posllh = gps.iTOW_POSLLH; gps.Calculate(); change_refpos(); pi.x = gps.PositionNED.x; pi.y = gps.PositionNED.y; pi.z = gps.PositionNED.z; gpsUpdateFlag = true; } } //20m以上の変位で基準位置更新 void change_refpos() { float x_dist = gps.PositionNED.x - pi.x; float y_dist = gps.PositionNED.y - pi.y; float z_dist = gps.PositionNED.z - pi.z; float dist2 = x_dist*x_dist + y_dist*y_dist + z_dist*z_dist; if (dist2 >= 400.0f) { autopilot.set_dest(destination.x - gps.PositionNED.x, destination.y - gps.PositionNED.y); autopilot.set_turn(turn_center.x - gps.PositionNED.x, turn_center.y - gps.PositionNED.y, turn_radius); gps.CalculateUnit(); eskf.setPihat(0.0f, 0.0f, 0.0f); //palt0 = palt; } else { //何もしない } }