fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 11:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move MISSION_STATE read/write from mavlink to navigator and commander

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-13 23:40:48 +02:00
parent d84cbd008c
commit 91b590ef58
6 changed files with 80 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+18 -4
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <drivers/drv_led.h>
@@ -89,6 +90,7 @@
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include <dataman/dataman.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@@ -691,6 +693,11 @@ int commander_thread_main(int argc, char *argv[])
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR);
}
/* armed topic */
orb_advert_t armed_pub;
@@ -708,10 +715,17 @@ int commander_thread_main(int argc, char *argv[])
struct home_position_s home;
memset(&home, 0, sizeof(home));
if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR);
/* init mission state */
mission_s mission;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i",
mission.dataman_id, mission.count, mission.current_index);
orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
close(mission_pub);
} else {
mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed");
}
}
mavlink_log_info(mavlink_fd, "[cmd] started");
src/modules/dataman/dataman.h
+1 -11
View File
@@ -53,7 +53,7 @@ extern "C" {
DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */
DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */
DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */
DM_KEY_MISSION_STATE, /* Persistent mission state */
DM_KEY_MISSION_STATE, /* Persistent mission state */
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
@@ -127,16 +127,6 @@ extern "C" {
dm_reset_reason restart_type /* The last reset type */
);
/* NOTE: The following structure defines the persistent system state data stored in the single
entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that
needs to survive restarts. This definition is application specific so it doesn't really belong
in this header, but till I find it a better home here it is */
typedef struct {
char offboard_waypoint_id; /* the index of the active offboard waypoint data */
/* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */
} persistent_system_state_t;
#ifdef __cplusplus
}
#endif
src/modules/mavlink/mavlink_main.cpp
+23 -96
View File
@@ -18,7 +18,6 @@
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
@@ -964,8 +963,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
{
persistent_system_state_t sys_state;
/* init WPM state */
state->size = 0;
state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
@@ -980,40 +977,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT;
int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state));
if (sys_state_size == sizeof(sys_state)) {
if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) {
state->dataman_id = sys_state.offboard_waypoint_id;
if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); }
/* count waypoints in current waypoints storage */
dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
struct mission_item_s mission_item;
int seq = 0;
while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
seq++;
}
if (_verbose) { warnx("WPM init: found %d items", seq); }
state->size = seq;
} else {
if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); }
}
} else {
if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); }
}
/* init mission topic */
mission.count = state->size;
mission.dataman_id = state->dataman_id;
mission.current_index = -1; // TODO store current index in dataman?
publish_mission();
}
/*
@@ -1081,10 +1045,10 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
struct mission_item_s mission_item;
if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
/* create mission_item_s from mavlink_mission_item_t */
mavlink_mission_item_t wp;
@@ -1103,7 +1067,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); }
if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); }
}
}
@@ -1214,9 +1178,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mission.current_index = wpc.seq;
publish_mission();
/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); }
@@ -1392,6 +1353,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
_wpm->current_count = wpc.count;
_wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission
mission.current_index = -1;
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
@@ -1442,10 +1405,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
break;
}
dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); }
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
@@ -1453,6 +1416,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
break;
}
/* waypoint marked as current */
if (wp.current) {
mission.current_index = wp.seq;
}
if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
_wpm->current_wp_id = wp.seq + 1;
@@ -1463,65 +1431,23 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
/* write terminator item */
if (_wpm->current_count < _wpm->max_size) {
if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
break;
}
}
/* update WPM state */
_wpm->dataman_id = _wpm->current_dataman_id;
_wpm->size = _wpm->current_count;
/* offboard mission data saved correctly, now update the persistent system state */
persistent_system_state_t state;
bool dm_result;
/* update mission topic */
mission.dataman_id = _wpm->dataman_id;
mission.count = _wpm->current_count;
/* since we are doing a read-modify-write we must lock the item type */
dm_lock(DM_KEY_MISSION_STATE);
/* first read in the current state data, there may eventually be data other than the offboard index
* and we must preserve it */
ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state));
if (dm_state_size != sizeof(state)) {
warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state));
}
/* set new dataman storage ID */
state.offboard_waypoint_id = _wpm->current_dataman_id;
/* write back to dataman */
dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state);
dm_unlock(DM_KEY_MISSION_STATE);
if (dm_result) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
/* update WPM state */
_wpm->dataman_id = _wpm->current_dataman_id;
_wpm->size = _wpm->current_count;
/* update mission topic */
mission.dataman_id = _wpm->dataman_id;
mission.count = _wpm->current_count;
mission.current_index = 0;
publish_mission();
}
publish_mission();
} else {
/* request next item */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
break;
}
@@ -1545,6 +1471,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
_wpm->size = 0;
/* update mission topic */
mission.dataman_id = _wpm->dataman_id;
mission.count = 0;
mission.current_index = -1;
src/modules/navigator/mission.cpp
+33
View File
@@ -430,6 +430,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
return false;
}
void
Mission::save_offboard_mission_state()
{
mission_s mission_state;
/* lock MISSION_STATE item */
dm_lock(DM_KEY_MISSION_STATE);
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
/* check if state actually changed to save flash write cycles */
if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id ||
mission_state.count != _offboard_mission.count ||
mission_state.current_index != _current_offboard_mission_index) {
mission_state.dataman_id = _offboard_mission.dataman_id;
mission_state.count = _offboard_mission.count;
mission_state.current_index = _current_offboard_mission_index;
/* write modifyed state only if changed */
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state");
}
}
/* unlock MISSION_STATE item */
dm_unlock(DM_KEY_MISSION_STATE);
}
void
Mission::report_mission_item_reached()
{
@@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
publish_mission_result();
save_offboard_mission_state();
}
void
src/modules/navigator/mission.h
+5
View File
@@ -141,6 +141,11 @@ private:
bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item);
/**
* Save current offboard mission state to dataman
*/
void save_offboard_mission_state();
/**
* Report that a mission item has been reached
*/
src/modules/navigator/navigator_main.cpp
-7
View File
@@ -227,13 +227,6 @@ Navigator::task_main()
warnx("Could not clear geofence");
}
/* Get the last offboard mission id */
persistent_system_state_t sys_state;
if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) {
if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1))
_mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id);
}
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));