void Search::SetExample() { // Test changing the missions waypoint count? search_mission.count = 2; search_mission.current_seq = 0; getCoord(15.0); dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 0, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s)); offboard = (offboard == 0) ? 1 : 0; getCoord(15.0, 90); next_point.nav_cmd = NAV_CMD_WAYPOINT; dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 1, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s)); offboard = (offboard == 0) ? 1 : 0; orb_publish(ORB_ID(offboard_mission), mission_pub, &search_mission); }
void Mission::reset_offboard_mission(struct mission_s &mission) { dm_lock(DM_KEY_MISSION_STATE); if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { /* set current item to 0 */ mission.current_seq = 0; /* reset jump counters */ if (mission.count > 0) { dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id); for (unsigned index = 0; index < mission.count; index++) { struct mission_item_s item; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, index, &item, len) != len) { PX4_WARN("could not read mission item during reset"); break; } if (item.nav_cmd == NAV_CMD_DO_JUMP) { item.do_jump_current_count = 0; if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET, &item, len) != len) { PX4_WARN("could not save mission item during reset"); break; } } } } } else { mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: could not read mission"); /* initialize mission state in dataman */ mission.dataman_id = 0; mission.count = 0; mission.current_seq = 0; } dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } dm_unlock(DM_KEY_MISSION_STATE); }
/** * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. */ int MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) { struct mission_s mission; mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; /* update mission state in dataman */ int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); if (res == sizeof(mission_s)) { /* update active mission state */ _dataman_id = dataman_id; _count = count; _current_seq = seq; /* mission state saved successfully, publish offboard_mission topic */ if (_offboard_mission_pub < 0) { _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); } return OK; } else { warnx("ERROR: can't save mission state"); _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); return ERROR; } }
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)); if (read_res == sizeof(mission_s)) { /* data read successfully, check dataman ID and items count */ if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { /* navigator may modify only sequence, write modified state only if it changed */ if (mission_state.current_seq != _current_offboard_mission_index) { if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { warnx("ERROR: can't save mission state"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state"); } } } } else { /* invalid data, this must not happen and indicates error in offboard_mission publisher */ mission_state.dataman_id = _offboard_mission.dataman_id; mission_state.count = _offboard_mission.count; mission_state.current_seq = _current_offboard_mission_index; warnx("ERROR: invalid mission state"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: invalid mission state"); /* write modified 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)) { warnx("ERROR: can't save mission state"); mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state"); } } /* unlock MISSION_STATE item */ dm_unlock(DM_KEY_MISSION_STATE); }
void Geofence::addPoint(int argc, char *argv[]) { int ix, last; double lon, lat; struct fence_vertex_s vertex; char *end; if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { dm_clear(DM_KEY_FENCE_POINTS); publishFence(0); return; } if (argc < 3) { PX4_WARN("Specify: -clear | sequence latitude longitude [-publish]"); } ix = atoi(argv[0]); if (ix >= DM_KEY_FENCE_POINTS_MAX) { PX4_WARN("Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); } lat = strtod(argv[1], &end); lon = strtod(argv[2], &end); last = 0; if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { last = 1; } vertex.lat = (float)lat; vertex.lon = (float)lon; if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { if (last) { publishFence((unsigned)ix + 1); } return; } PX4_WARN("can't store fence point"); }
/** * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. */ int MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) { struct mission_s mission; mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; /* update mission state in dataman */ int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); if (res == sizeof(mission_s)) { /* update active mission state */ _dataman_id = dataman_id; _count = count; _current_seq = seq; _my_dataman_id = _dataman_id; /* mission state saved successfully, publish offboard_mission topic */ if (_offboard_mission_pub == nullptr) { _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); } return OK; } else { warnx("WPM: ERROR: can't save mission state"); if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD"); } return ERROR; } }
static int dm_write_shared_word(struct usbnet *dev, int phy, u8 reg, __le16 value) { int ret, i; mutex_lock(&dev->phy_mutex); ret = dm_write(dev, DM_SHARED_DATA, 2, &value); if (ret < 0) goto out; dm_write_reg(dev, DM_SHARED_ADDR, phy ? (reg | 0x40) : reg); dm_write_reg(dev, DM_SHARED_CTRL, phy ? 0x1a : 0x12); for (i = 0; i < DM_TIMEOUT; i++) { u8 tmp; udelay(1); ret = dm_read_reg(dev, DM_SHARED_CTRL, &tmp); if (ret < 0) goto out; /* ready */ if ((tmp & 1) == 0) break; } if (i == DM_TIMEOUT) { netdev_err(dev->net, "%s write timed out!\n", phy ? "phy" : "eeprom"); ret = -EIO; goto out; } dm_write_reg(dev, DM_SHARED_CTRL, 0x0); out: mutex_unlock(&dev->phy_mutex); return ret; }
void Search::SetMission(double search_radius) { // Get the amount of waypoints needed for mission. // Note: mission should already have a size of 1. For endpoint. // Note: we won't need to manage the home waypoint ourselves. // Note: We assume the last waypoint is the upper left corner of the square search pattern. // Note: Only test on the field where the home position is correctly set // Home position is not part of mission, Index 0 = First new waypoint /* Waypoint ID * Waypoint Count = Waypoint ID + 1*/ dm_clear(DM_KEY_WAYPOINTS_OFFBOARD(offboard)); offboard = 0; int wp_id = 0; // Note: Acceptance Radius not accounted for (it's in meters, not degrees)) while (next_point.lat - last.lat > search_radius && next_point.lon - last.lon > search_radius) { if (wp_id % 2 == 0) { // Do Latitudinal shifts here if (next_point.lat - last.lat < search_radius) { // Going Up next_point.lat = last.lat; } else { // Going down next_point.lat = home.lat; } } else { // Do longitudinal shifts here next_point.lon += -1 * search_radius; } if (wp_id) next_point.nav_cmd = NAV_CMD_WAYPOINT; dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), wp_id++, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s)); offboard = (offboard == 0) ? 1 : 0; } search_mission.count = wp_id + 1; warnx("Waypoint count is: %d", search_mission.count); search_mission.current_seq = 0; orb_publish(ORB_ID(offboard_mission), mission_pub, &search_mission); }
void BottleDrop::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); bool updated = false; float z_0; // ground properties float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV float ground_distance = _alt_clearance; // Replace by closer estimate in loop // constant float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] float m = 0.5f; // mass of bottle [kg] float rho = 1.2f; // air density [kg/m^3] float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] // Definition float h_0; // height over target float az; // acceleration in z direction[m/s^2] float vz; // velocity in z direction [m/s] float z; // fallen distance [m] float h; // height over target [m] float ax; // acceleration in x direction [m/s^2] float vx; // ground speed in x direction [m/s] float x; // traveled distance in x direction [m] float vw; // wind speed [m/s] float vrx; // relative velocity in x direction [m/s] float v; // relative speed vector [m/s] float Fd; // Drag force [N] float Fdx; // Drag force in x direction [N] float Fdz; // Drag force in z direction [N] float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) float x_l, y_l; // local position in projected coordinates float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); param_t param_cd = param_find("BD_OBJ_CD"); param_t param_mass = param_find("BD_OBJ_MASS"); param_t param_surface = param_find("BD_OBJ_SURFACE"); param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); param_get(param_cd, &cd); param_get(param_mass, &m); param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct parameter_update_s update; memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); struct mission_item_s flight_vector_s {}; struct mission_item_s flight_vector_e {}; flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; flight_vector_s.altitude_is_relative = false; flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; flight_vector_s.altitude_is_relative = false; struct wind_estimate_s wind; // wakeup source(s) struct pollfd fds[1]; // Setup of loop fds[0].fd = _command_sub; fds[0].events = POLLIN; // Whatever state the bay is in, we want it closed on startup lock_release(); close_bay(); while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } /* vehicle commands updated */ if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } orb_check(vehicle_global_position_sub, &updated); if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } if (_global_pos.timestamp == 0) { continue; } const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { // Get wind estimate orb_check(_wind_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } // Get vehicle position orb_check(vehicle_global_position_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } // Get parameter updates orb_check(parameter_update_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); // update all parameters param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); } orb_check(_command_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); ground_distance = _global_pos.alt - _target_position.alt; // Distance to drop position and angle error to approach vector // are relevant in all states greater than target valid (which calculates these positions) if (_drop_state > DROP_STATE_TARGET_VALID) { distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } switch (_drop_state) { case DROP_STATE_INIT: // do nothing break; case DROP_STATE_TARGET_VALID: { az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] z = 0; // fallen distance [m] h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] h = h_0; // height over target [m] ax = 0; // acceleration in x direction [m/s^2] vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] x = 0; // traveled distance in x direction [m] vw = 0; // wind speed [m/s] vrx = 0; // relative velocity in x direction [m/s] v = groundspeed_body; // relative speed vector [m/s] Fd = 0; // Drag force [N] Fdx = 0; // Drag force in x direction [N] Fdz = 0; // Drag force in z direction [N] // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while (h > 0.05f) { // z-direction vz = vz + az * dt_freefall_prediction; z = z + vz * dt_freefall_prediction; h = h_0 - z; // x-direction vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); vx = vx + ax * dt_freefall_prediction; x = x + vx * dt_freefall_prediction; vrx = vx + vw; // drag force v = sqrtf(vz * vz + vrx * vrx); Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; // acceleration az = g - Fdz / m; ax = -Fdx / m; } // compute drop vector x = groundspeed_body * t_signal + x; x_t = 0.0f; y_t = 0.0f; float wind_direction_n, wind_direction_e; if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen wind_direction_n = 1.0f; wind_direction_e = 0.0f; } else { wind_direction_n = wind.windspeed_north / windspeed_norm; wind_direction_e = wind.windspeed_east / windspeed_norm; } x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, &(flight_vector_s.lat), &(flight_vector_s.lon)); flight_vector_s.altitude = _drop_position.alt; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; // Save WPs in datamanager const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { warnx("ERROR: could not save onboard WP"); } if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { warnx("ERROR: could not save onboard WP"); } _onboard_mission.count = 2; _onboard_mission.current_seq = 0; if (_onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); _drop_state = DROP_STATE_TARGET_SET; } break; case DROP_STATE_TARGET_SET: { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (isfinite(distance_real) && distance_real < distance_open_door && fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } } } break; case DROP_STATE_BAY_OPEN: { if (_drop_approval) { map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); x_f = x_l + _global_pos.vel_n * dt_runs; y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); if (isfinite(distance_real) && (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } } break; case DROP_STATE_DROPPED: /* 2s after drop, reset and close everything again */ if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { _drop_state = DROP_STATE_INIT; _drop_approval = false; lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); // remove onboard mission _onboard_mission.current_seq = -1; _onboard_mission.count = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; case DROP_STATE_BAY_CLOSED: // do nothing break; } counter++; // update_actuators(); // run at roughly 100 Hz usleep(sleeptime_us); dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } warnx("exiting."); _main_task = -1; _exit(0); }
void MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); if (wp.seq != _transfer_seq) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } /* don't send request here, it will be performed in eventloop after timeout */ return; } } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; } struct mission_item_s mission_item; int ret = parse_mavlink_mission_item(&wp, &mission_item); if (ret != OK) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; } dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_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, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; } /* waypoint marked as current */ if (wp.current) { _transfer_current_seq = wp.seq; } if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _transfer_seq = wp.seq + 1; if (_transfer_seq == _transfer_count) { /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } _mavlink->send_statustext_info("WPM: Transfer complete."); _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); } _transfer_in_progress = false; } else { /* request next item */ send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } } }
bool Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { /* select onboard/offboard mission */ int *mission_index_ptr; dm_item_t dm_item; struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; /* do not work on empty missions */ if (mission->count == 0) { return false; } /* move to next item if there is one */ if (mission_index_next < ((int)mission->count - 1)) { mission_index_next++; } if (onboard) { /* onboard mission */ mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds - if they are equal, we just reached the end */ if (*mission_index_ptr != (int)mission->count) { mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); } return false; } const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ struct mission_item_s mission_item_tmp; /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; } /* 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) { /* do DO_JUMP as many times as requested */ if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) { /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR DO JUMP waypoint could not be written"); return false; } report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count); } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { if (is_current) { mavlink_log_critical(_navigator->get_mavlink_fd(), "DO JUMP repetitions completed"); } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } } else { /* if it's not a DO_JUMP, then we were successful */ memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); return true; } } /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR DO JUMP is cycling, giving up"); return false; }
int Geofence::loadFromFile(const char *filename) { FILE *fp; char line[120]; int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); if (fp == NULL) { return ERROR; } /* create geofence points from valid lines and store in DM */ for (;;) { /* get a line, bail on error/EOF */ if (fgets(line, sizeof(line), fp) == NULL) { break; } /* Trim leading whitespace */ size_t textStart = 0; while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { continue; } /* if there is only a linefeed, skip it */ if (line[0] == '\n') { continue; } if (gotVertical) { /* Parse the line as a geofence point */ struct fence_vertex_s vertex; /* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */ if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') { /* Handle degree minute second format */ float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { warnx("Scanf to parse DMS geofence vertex failed."); goto error; } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); goto error; } } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { goto error; } warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; } else { /* Parse the line as the vertical limits */ if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { goto error; } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } } /* Check if import was successful */ if (gotVertical && pointCounter > 0) { _vertices_count = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "Geofence import error"); } error: fclose(fp); return rc; }
static int task_main(int argc, char *argv[]) { char buffer[DM_MAX_DATA_SIZE]; hrt_abstime wstart, wend, rstart, rend; warnx("Starting dataman test task %s", argv[1]); /* try to read an invalid item */ int my_id = atoi(argv[1]); /* try to read an invalid item */ if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { warnx("%d read an invalid item failed", my_id); goto fail; } /* try to read an invalid index */ if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { warnx("%d read an invalid index failed", my_id); goto fail; } srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; wstart = hrt_absolute_time(); for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); goto fail; } usleep(rand() & ((64 * 1024) - 1)); } rstart = hrt_absolute_time(); wend = rstart; for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } if (buffer[0] == my_id) { hit++; if (len2 != len) { warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); goto fail; } if (buffer[1] != i) { warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); goto fail; } } else { miss++; } } rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); sem_post(sems + my_id); return 0; fail: warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); sem_post(sems + my_id); return -1; }
static int task_main(int argc, char *argv[]) { char buffer[DM_MAX_DATA_SIZE]; PX4_INFO("Starting dataman test task %s", argv[1]); /* try to read an invalid item */ int my_id = atoi(argv[1]); /* try to read an invalid item */ if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { PX4_ERR("%d read an invalid item failed", my_id); goto fail; } /* try to read an invalid index */ if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { PX4_ERR("%d read an invalid index failed", my_id); goto fail; } srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; hrt_abstime wstart = hrt_absolute_time(); for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; unsigned hash = i ^ my_id; unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); //PX4_INFO("ret: %d", ret); if (ret != len) { PX4_WARN("task %d: write failed, index %d, length %d", my_id, hash, len); goto fail; } if (i % (NUM_MISSIONS_TEST / 10) == 0) { PX4_INFO("task %d: %.0f%%", my_id, (double)i * 100.0f / NUM_MISSIONS_TEST); } usleep(rand() & ((64 * 1024) - 1)); } hrt_abstime rstart = hrt_absolute_time(); hrt_abstime wend = rstart; for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { unsigned hash = i ^ my_id; unsigned len2; unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { PX4_WARN("task %d: read failed length test, index %d", my_id, hash); goto fail; } if (buffer[0] == my_id) { hit++; if (len2 != len) { PX4_WARN("task %d: read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); goto fail; } if (buffer[1] != i) { PX4_WARN("task %d: data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); goto fail; } } else { miss++; } } hrt_abstime rend = hrt_absolute_time(); PX4_INFO("task %d pass, hit %d, miss %d, io time read %llums. write %llums.", my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_TEST / 1000, (wend - wstart) / NUM_MISSIONS_TEST / 1000); px4_sem_post(sems + my_id); return 0; fail: PX4_ERR("test_dataman FAILED: task %d, buffer %02x %02x %02x %02x %02x %02x", my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); px4_sem_post(sems + my_id); task_returned_error[my_id] = true; return -1; }