基于px4二次开发的断电续飞

第一次写博客，不知道写啥，贴出自己写的一段断电续飞的代码，欢迎大家给出建议。

有时间我会对这片博客不停完善，毕竟是第一篇博客哈！！！

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作为当前航点的经纬度信息并保存。