第一次写博客，不知道写啥，贴出自己写的一段断电续飞的代码，欢迎大家给出建议。
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;
/* read mission item from datamanager */
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);
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
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;
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;
}
item.lat = _navigator->get_global_position()->lat;
item.lon = _navigator->get_global_position()->lon;
item.altitude = _navigator->get_global_position()->alt/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作为当前航点的经纬度信息并保存。