Ardupilot飞控添加使用诺瓦泰双天线GPS航向角的设置
一、添加诺瓦泰GPS heading角数据包解析代码
1、打开libraries\AP_GPS\AP_GPS_NOVA.h，添加如下代码：
struct PACKED heading
{
uint32_t solstat;
uint32_t postype;
float length;
float heading;
float pitch;
float resv1;
float hdg_std_dev;
float ptch_std;
uint8_t stnID[4];
uint8_t svstracked;
uint8_t svslnsolution;
uint8_t obsmask;
uint8_t multi;
uint8_t resv2;
uint8_t extsolstat;
uint8_t resv3;
uint8_t sigmask;
};
在 msgbuffer中作如下修改：
union PACKED msgbuffer {
bestvel bestvelu;
bestpos bestposu;
psrdop psrdopu;
heading headingu;
uint8_t bytes[256];
};
2、打开libraries\AP_GPS\AP_GPS_NOVA.cpp，找到bool AP_GPS_NOVA::process_message(void)函数，添加如下代码：
if (messageid == 971) // heading
{
const heading &headingu = nova_msg.data.headingu;
state.heading = (float) (headingu.heading);
state.last_gps_fixed_time_ms = AP_HAL::millis();
return false;
}
同时，在本函数的开始处找到bestpos数据解析，定位到相应的位置并作如下修改：
if (bestposu.solstat == 0) // have a solution
{
switch (bestposu.postype)
{
case 16:
state.status = AP_GPS::GPS_OK_FIX_3D;
state.last_gps_fixed_time_ms = AP_HAL::millis();
break;
case 17: // psrdiff
case 18: // waas
case 20: // omnistar
case 68: // ppp_converg
case 69: // ppp
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
state.last_gps_fixed_time_ms = AP_HAL::millis();
break;
case 32: // l1 float
case 33: // iono float
case 34: // narrow float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
state.last_gps_fixed_time_ms = AP_HAL::millis();
break;
case 48: // l1 int
case 50: // narrow int
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
state.last_gps_fixed_time_ms = AP_HAL::millis();
break;
case 0: // NONE
case 1: // FIXEDPOS
case 2: // FIXEDHEIGHT
default:
state.status = AP_GPS::NO_FIX;
break;
}
}
struct GPS_State {
uint8_t instance; // the instance number of this GPS
// all the following fields must all be filled by the backend driver
GPS_Status status; ///< driver fix status
uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
uint16_t time_week; ///< GPS week number
Location location; ///< last fix location
float ground_speed; ///< ground speed in m/sec
float ground_course; ///< ground course in degrees
uint16_t hdop; ///< horizontal dilution of precision in cm
uint16_t vdop; ///< vertical dilution of precision in cm
uint8_t num_sats; ///< Number of visible satellites
Vector3f velocity; ///< 3D velocity in m/s, in NED format
float speed_accuracy; ///< 3D velocity accuracy estimate in m/s
float horizontal_accuracy; ///< horizontal accuracy estimate in m
float vertical_accuracy; ///< vertical accuracy estimate in m
bool have_vertical_velocity:1; ///< does GPS give vertical velocity? Set to true only once available.
bool have_speed_accuracy:1; ///< does GPS give speed accuracy? Set to true only once available.
bool have_horizontal_accuracy:1; ///< does GPS give horizontal position accuracy? Set to true only once available.
bool have_vertical_accuracy:1; ///< does GPS give vertical position accuracy? Set to true only once available.
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
float heading; //RTKGPS heading
uint32_t last_gps_fixed_time_ms;
};
并在AP_GPS类的public中添加如下函数：
// GPS heading in degrees
float gps_heading(uint8_t instance) const {
return state[instance].heading;
}
int32_t gps_heading() const {
return gps_heading(primary_instance)*100;
}
//get gps fixed time
uint32_t gps_last_fixed_time(uint8_t instance) const {
return state[instance].last_gps_fixed_time_ms;
}
uint32_t gps_last_fixed_time() const {
return gps_last_fixed_time(primary_instance);
}
打开libraries\AP_AHRS\AP_AHRS_DCM.cpp文件，找到void AP_AHRS_DCM::drift_correction_yaw(void)函数，找到else if并将其作如下替换：
else if (have_gps()) {
//we are using GPS heading for yaw
if ( _gps.gps_last_fixed_time() != _gps_last_update) {
yaw_deltat = (_gps.gps_last_fixed_time() - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps.gps_last_fixed_time();
new_value = true;
float gps_heading_rad = ToRad(_gps.gps_heading()*0.01f);
float yaw_error_rad = wrap_PI(gps_heading_rad - yaw);
yaw_error = sinf(yaw_error_rad);
if (!_flags.have_initial_yaw ||
yaw_deltat > 20 ||
fabsf(yaw_error_rad) >= 1.047f){
// reset DCM matrix based on current yaw
_dcm_matrix.from_euler(roll, pitch, gps_heading_rad);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
yaw_error = 0;
}
}
}
打开libraries\AP_AHRS\AP_AHRS_NavEKF.cpp文件，找到 void AP_AHRS_NavEKF::update_EKF2(void)函数,并找到yaw=eulers.z;将其注释掉。//yaw=eulers.z
打开libraries\AP_NavEKF2\AP_NavEKF2_core.cpp文件，找到void NavEKF2_core::UpdateFiler(bool predict)函数，定位到SelectMafFusion();在其下面一行添加：调用recordYawReset();函数。
sudo apt-get install git
2、下载和安装交叉编译工具
wgeth ttps://launchpadlibrarian.net/218827486/gcc-arm-none-eabi-4_9-2015q3-20150921-linux.tar.bz2
tar -xjvf gcc-arm-none-eabi-4_9-2015q3-20150921-linux.tar.bz2
sudo apt-get install lsb-core
export PATH=/home/your user name/gcc-arm-none-eabi-4_9-2015q3/bin:$PATH
4、克隆飞控源码
git clone https://github.com/ArduPilot/ardupilot.git
cd ardupilot
git submodule update --init --recursive
5、运行unbuntu系统环境安装工具脚本
Tools/scripts/install-prereqs-ubuntu.sh -y
6、. ~/.profile
7、编译
cd ArduCopter
make px4-v2
8、清空编译文件
make px4-clean
通过MissonPlanner地面站将编译好的固件ArduCopter-v2.px4（在ArduCopter目录下）,烧写到pixhawk飞控板子，然后点击配置（CONFIG/TUNING）,选择全部参数树（Full Parameter Tree），点击最右边刷新参数按钮（Refresh Params）。
1、使用诺瓦泰GPS的设置
在参数表中，找到GPS，点击“+”号打开，找到GPS_TYPE，将其值设为15（即：使用诺瓦泰GPS），然后点击右边写入参数按钮（Write Params）。
同样，在参数表中，找到COMPASS，点击“+”号打开，找到COMPASS_USE,COMPASS_USE2,
COMPASS_USE3,将它们全部设为0（不使用罗盘），然后点击右边写入参数按钮（Write Params）。
并将EKF3关掉：
1、初始化GPS，执行该命令后GPS波特率变为9600
freset
2、设置GPS串口波特率为115200
com com2 115200 n 8 1 n off off
com com1 115200 n 8 1 n off off
dualantennaalign enable 5 1
位置信息：
log com1 bestposb ontime 0.2
速度信息：
log com1 bestvelb ontime 0.2
精度因子：
log com1 psrdopb ontime 0.2
航向角信息：
log com1 headingb onchanged
INTERFACEMODE COM2 NOVATELX NONE OFF
PSRDIFFTIMEOUT 15
RTKTIMEOUT 10
saveconfig
GPS的TX接到飞控板子的RX；
GPS的GND接飞控的GND；
GPS的RX空置不用。
