第一次写博客,不知道写啥,贴出自己写的一段断电续飞的代码,欢迎大家给出建议。
有时间我会对这片博客不停完善,毕竟是第一篇博客哈!!!
px4断电续飞主要是在任务模式中,在任务结束时调用该函数。任务结束会调用 void Mission::on_inactivation(),该函数位于src/modules/navigator/mission.cpp中,所以将编写的set_current_position_as_takeoff_item()函数在该函数中调用就可以实现断电续飞的功能。
本函数设计的思路是:读取当前任务点的序号,即_current_mission_index(说明:px4中以_开头的都是表示当前的意思),将之前的任务点都设置成DO_JUMP模式,这样,这些任务点就会被跳过。然后通过_navigator->get_global_position()函数获得当前的位置信息,包括经纬度和高度,赋值给当前的任务点,这样就实现了断电续飞的功能。
bool
Mission::set_current_position_as_takeoff_item()
{
int *mission_index_ptr = &_current_mission_index;//获去当前任务点的序号
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "first item index num: %d.",
*mission_index_ptr);
const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;//指定要读取的数据的类型
/* do not work on empty missions */
if (_mission.count == 0) {
return false;
}
for (int offset = 2; offset < 20; offset++) {
//首先判断是否在正确的任务点序号类
if ((*mission_index_ptr) - offset < 0 || *mission_index_ptr >= (int)_mission.count) {
if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item index out of bound, index: %d, max: %d.",
*mission_index_ptr - offset, _mission.count);
}
break;
}
const ssize_t len = sizeof(struct mission_item_s);
struct mission_item_s mission_item_tmp;
//读取航点信息
if (dm_read(dm_item, (*mission_index_ptr)-offset, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.");
return false;
}
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "index: %d, nav_cmd: %d.",
*mission_index_ptr - offset, mission_item_tmp.nav_cmd);
//对航点进行判断,如果不是DO_JUMP类型的航点设置为DO_JUMP类型从而跳过该航点
if (mission_item_tmp.nav_cmd != NAV_CMD_DO_JUMP) {
mission_item_tmp.nav_cmd = NAV_CMD_DO_JUMP;
mission_item_tmp.do_jump_mission_index = *mission_index_ptr;
//写回到SD卡中
if (dm_write(dm_item, (*mission_index_ptr)-offset, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.");
return false;
}
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "here is JUMP setpoint");
}
}
ssize_t len = sizeof(struct mission_item_s);
struct mission_item_s item;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "the index to write %d.",*mission_index_ptr-1);
//读取当前任务点的航点信息
if (dm_read(dm_item, *mission_index_ptr-1, &item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.");
return false;
}
//通过_navigator->get_global_position()获取当前的位置信息,并赋值给当前航点
item.lat = _navigator->get_global_position()->lat;
item.lon = _navigator->get_global_position()->lon;
item.altitude = _navigator->get_global_position()->alt/100;//高度数据要进行100倍的转化
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "lat %lf. lon %lf. alt %lf",
item.lat,item.lon,(double)item.altitude);
//写回到当前航点,实现更改
if (dm_write(dm_item, *mission_index_ptr-1, DM_PERSIST_POWER_ON_RESET, &item, len) == len) {
/* not supposed to happen unless the datamanager can't access the dataman */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "CURRENT waypoint be written.");
return true;
}
/* we have given up, we don't want to cycle forever */
return false;
}
主要是读取已经飞过的航点,将已经飞过的航点都设置为JUMP模式,然后读取当前的local_position作为当前航点的经纬度信息并保存。