From cfaf0ada45a9b99a83221c6c51a50090c55a1483 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 23:14:05 +0400 Subject: [PATCH 1/9] Heading in VFR message fixed --- src/modules/mavlink/orb_listener.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34fa..2804a8191f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -248,7 +249,7 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t heading = _wrap_2pi(att.yaw) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } From e7f4d91022af363905b758ab39bde1addcb69517 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 23:15:02 +0400 Subject: [PATCH 2/9] geo: cleanup of wrap_XXX functions --- src/lib/geo/geo.c | 54 +++++++++++++++++++++++++++++++---------------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba7..9e533b92d8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -390,22 +390,22 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { + if (!isfinite(bearing)) { return bearing; } int c = 0; - - while (bearing > M_PI_F && c < 30) { + while (bearing > M_PI_F) { bearing -= M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } c = 0; - - while (bearing <= -M_PI_F && c < 30) { + while (bearing <= -M_PI_F) { bearing += M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } return bearing; @@ -418,12 +418,18 @@ __EXPORT float _wrap_2pi(float bearing) return bearing; } - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; + int c = 0; + while (bearing > M_TWOPI_F) { + bearing -= M_TWOPI_F; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; + c = 0; + while (bearing <= 0.0f) { + bearing += M_TWOPI_F; + if (c++ > 3) + return NAN; } return bearing; @@ -436,12 +442,18 @@ __EXPORT float _wrap_180(float bearing) return bearing; } + int c = 0; while (bearing > 180.0f) { - bearing = bearing - 360.0f; + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= -180.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; @@ -454,12 +466,18 @@ __EXPORT float _wrap_360(float bearing) return bearing; } - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; + int c = 0; + while (bearing > 360.0f) { + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= 0.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; From c31d065340a44ef121069b5b82395526b910d3e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 14 Nov 2013 11:59:18 +0400 Subject: [PATCH 3/9] Revert "Heading in VFR message fixed" This reverts commit cfaf0ada45a9b99a83221c6c51a50090c55a1483. --- src/modules/mavlink/orb_listener.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2804a8191f..abc91d34fa 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,7 +54,6 @@ #include #include #include -#include #include @@ -249,7 +248,7 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = _wrap_2pi(att.yaw) / M_PI_F * 180.0f; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } From 83df116c7aa21b6d68f2aa31c4526dd822495d70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Jan 2014 23:11:37 +0100 Subject: [PATCH 4/9] Hotfix: Move mixer variables in test routine into function --- src/systemcmds/tests/test_mixer.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 2a47551ee6..df382e2c68 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -65,20 +65,20 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; -static bool should_arm = false; -uint16_t r_page_servo_disarmed[output_max]; -uint16_t r_page_servo_control_min[output_max]; -uint16_t r_page_servo_control_max[output_max]; -uint16_t r_page_servos[output_max]; -uint16_t servo_predicted[output_max]; - -/* - * PWM limit structure - */ -pwm_limit_t pwm_limit; int test_mixer(int argc, char *argv[]) { + /* + * PWM limit structure + */ + pwm_limit_t pwm_limit; + static bool should_arm = false; + uint16_t r_page_servo_disarmed[output_max]; + uint16_t r_page_servo_control_min[output_max]; + uint16_t r_page_servo_control_max[output_max]; + uint16_t r_page_servos[output_max]; + uint16_t servo_predicted[output_max]; + warnx("testing mixer"); char *filename = "/etc/mixers/IO_pass.mix"; From 7d17a48b3c38c90509963beb7d776a44295b82d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 17:34:46 +0100 Subject: [PATCH 5/9] param set: fixed float format --- src/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 580fdc62f4..0cbba0a374 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -320,7 +320,7 @@ do_set(const char* name, const char* val) char* end; f = strtod(val,&end); param_set(param, &f); - printf(" -> new: %f\n", f); + printf(" -> new: %4.4f\n", (double)f); } From bb8be966fcaa484c0f8209da41760d4bc24d6c5f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:20:48 +0100 Subject: [PATCH 6/9] mc_pos_control: more safe tilt limiting --- .../mc_pos_control/mc_pos_control_main.cpp | 44 +++++++++++-------- 1 file changed, 26 insertions(+), 18 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 057cb051d8..3194534b9a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -839,30 +839,38 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } + float tilt_max = _params.tilt_max; + + /* adjust limits for landing mode */ + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + + /* limit min lift */ if (-thrust_sp(2) < thr_min) { thrust_sp(2) = -thr_min; saturation_z = true; } - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) - thr_min = 0.0f; - } - } - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } } } else { /* thrust compensation for altitude only control mode */ From 8a203951594282a297b2af402a82b85f0f927619 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:29:51 +0100 Subject: [PATCH 7/9] mc_pos_control: fixed yaw setpoint in AUTO --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3194534b9a..0d65b5b03c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -688,10 +688,16 @@ MulticopterPositionControl::task_main() _reset_lat_lon_sp = true; _reset_alt_sp = true; + /* update position setpoint */ _lat_sp = _pos_sp_triplet.current.lat; _lon_sp = _pos_sp_triplet.current.lon; _alt_sp = _pos_sp_triplet.current.alt; + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + } else { /* no waypoint, loiter, reset position setpoint if needed */ reset_lat_lon_sp(); @@ -711,7 +717,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = 0.0f; + _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); From f835980b468fe44c49051df0b58f830bafb256f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:32:46 +0100 Subject: [PATCH 8/9] mc_pos_control: more correct control flags usage --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0d65b5b03c..4fb9bd663e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -672,7 +672,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } - } else if (_control_mode.flag_control_auto_enabled) { + } else { /* always use AMSL altitude for AUTO */ select_alt(true); @@ -703,13 +703,9 @@ MulticopterPositionControl::task_main() reset_lat_lon_sp(); reset_alt_sp(); } - } else { - /* no control, reset setpoint */ - reset_lat_lon_sp(); - reset_alt_sp(); } - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); @@ -751,7 +747,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -848,7 +844,7 @@ MulticopterPositionControl::task_main() float tilt_max = _params.tilt_max; /* adjust limits for landing mode */ - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; From 8897894b19e8de4ad7960a0fa552ed12fc2f0200 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:14:21 +0100 Subject: [PATCH 9/9] commander, navigator, mc_att_control, mc_pos_control: code style fixed --- src/modules/commander/commander.cpp | 2 + .../commander/state_machine_helper.cpp | 2 + .../mc_att_control/mc_att_control_main.cpp | 6 ++- .../mc_pos_control/mc_pos_control_main.cpp | 11 ++++- src/modules/navigator/navigator_main.cpp | 5 ++- src/modules/navigator/navigator_mission.cpp | 44 ++++++++++++------- 6 files changed, 50 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713d..e9da69232d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,6 +369,7 @@ int arm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } @@ -381,6 +382,7 @@ int disarm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e3..e5d77b246a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -382,6 +382,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { status->set_nav_state = NAV_STATE_RTL; @@ -392,6 +393,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { status->set_nav_state = NAV_STATE_LAND; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0accb8552..db5e2e9bb0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; + } else { yaw_sp_move_rate += YAW_DEADZONE; } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; @@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4fb9bd663e..25d34c872d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global) { if (global != _use_global_alt) { _use_global_alt = global; + if (global) { /* switch from barometric to global altitude */ _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { /* switch from global to barometric altitude */ _alt_sp += _global_pos.baro_alt - _global_pos.alt; @@ -589,6 +591,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* select altitude source and update setpoint */ select_alt(_global_pos.global_valid); + if (!_use_global_alt) { alt = _global_pos.baro_alt; } @@ -845,9 +848,10 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) thr_min = 0.0f; } @@ -863,9 +867,11 @@ MulticopterPositionControl::task_main() if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { /* max horizontal thrust for given vertical thrust*/ float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; @@ -874,15 +880,18 @@ MulticopterPositionControl::task_main() } } } + } else { /* thrust compensation for altitude only control mode */ float att_comp; if (_att.R[2][2] > TILT_COS_MAX) { att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; saturation_z = true; + } else { att_comp = 1.0f; saturation_z = true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e961a8f942..6e4b5f0a06 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -689,7 +689,7 @@ Navigator::task_main() if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -747,7 +747,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached() if (_rtl_state == RTL_STATE_DESCEND) { /* hovering above home position, land if needed or loiter */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { dispatch(EVENT_LAND_REQUESTED); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 6576aae707..e72caf98e9 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -52,8 +52,8 @@ static const int ERROR = -1; -Mission::Mission() : - +Mission::Mission() : + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), @@ -65,7 +65,7 @@ Mission::Mission() : Mission::~Mission() { - + } void @@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool { /* try onboard mission first */ if (current_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } + _current_mission_type = MISSION_TYPE_ONBOARD; *onboard = true; *index = _current_onboard_mission_index; - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ _current_mission_type = MISSION_TYPE_NONE; return ERROR; } + _current_mission_type = MISSION_TYPE_OFFBOARD; *onboard = false; *index = _current_offboard_mission_index; @@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { /* try onboard mission first */ if (next_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; @@ -244,14 +254,16 @@ void Mission::move_to_next() { switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - case MISSION_TYPE_NONE: - default: - break; + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } } \ No newline at end of file