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;
}
}
- 打开libraries\AP_GPS\AP_GPS.h，找到结构体GPS_State，作如下添加：
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);
}
- 修改yaw漂移修正函数
打开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;
}
}
}
- 修改AP_AHRS_NavEKF.cpp文件:
打开libraries\AP_AHRS\AP_AHRS_NavEKF.cpp文件，找到 void AP_AHRS_NavEKF::update_EKF2(void)函数,并找到yaw=eulers.z;将其注释掉。//yaw=eulers.z
- 修改AP_NavEKF2_core.cpp文件：
打开libraries\AP_NavEKF2\AP_NavEKF2_core.cpp文件，找到void NavEKF2_core::UpdateFiler(bool predict)函数，定位到SelectMafFusion();在其下面一行添加：调用recordYawReset();函数。
- Ubuntu系统编译飞控代码
- 安装git
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）。
- 使用EKF2
并将EKF3关掉：
- 诺瓦泰GPS输出配置
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接线
GPS的TX接到飞控板子的RX；
GPS的GND接飞控的GND；
GPS的RX空置不用。