diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index cab743f661..a52746801c 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -15,6 +15,7 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd); static void do_set_home(const AP_Mission::Mission_Command& cmd); static void do_roi(const AP_Mission::Mission_Command& cmd); static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); +static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); // start_command - this function will be called when the ap_mission lib wishes to start a new command static bool start_command(const AP_Mission::Mission_Command& cmd) @@ -264,21 +265,58 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd) static void do_nav_wp(const AP_Mission::Mission_Command& cmd) { Vector3f local_pos = pv_location_to_vector(cmd.content.location); - // Set wp navigation target - auto_wp_start(local_pos); + + // this will be used to remember the time in millis after we reach or pass the WP. + loiter_time = 0; + // this is the delay, stored in seconds + loiter_time_max = abs(cmd.p1); + + // decide if this is a straight or spline segment + if (!cmd.content.location.flags.spline) { + // Set wp navigation target + auto_wp_start(local_pos); + // if no delay set the waypoint as "fast" + if (loiter_time_max == 0 ) { + wp_nav.set_fast_waypoint(true); + } + }else{ + // determine segment start and end type + bool stopped_at_start = true; + AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP; + AP_Mission::Mission_Command temp_cmd; + Vector3f next_spline_destination; // end of next segment if it is a spline segment + + // if previous command was a wp_nav command with no delay set stopped_at_start to false + // To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself? + uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index(); + if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) { + if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) { + if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT && temp_cmd.p1 == 0) { + stopped_at_start = false; + } + } + } + + // if there is no delay at the end of this segment get next nav command + if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index, temp_cmd)) { + // if the next nav command is a waypoint set end type to spline or straight + if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) { + if (temp_cmd.content.location.flags.spline) { + seg_end_type = AC_WPNav::SEGMENT_END_SPLINE; + next_spline_destination = pv_location_to_vector(temp_cmd.content.location); + }else{ + seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT; + } + } + } + + // set spline navigation target + auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_spline_destination); + } // initialise original_wp_bearing which is used to check if we have missed the waypoint wp_bearing = wp_nav.get_wp_bearing_to_destination(); original_wp_bearing = wp_bearing; - - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; - // this is the delay, stored in seconds - loiter_time_max = cmd.p1; - // if no delay set the waypoint as "fast" - if (loiter_time_max == 0 ) { - wp_nav.set_fast_waypoint(true); - } } // do_land - initiate landing procedure @@ -518,6 +556,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd) // To-Do: adjust waypoint target altitude to new provided altitude break; case Auto_WP: + case Auto_Spline: // To-Do; reset origin to current location + stopping distance at new altitude break; case Auto_Land: diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 0fef3df67c..ea33db5b50 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -61,6 +61,10 @@ static void auto_run() case Auto_Circle: auto_circle_run(); break; + + case Auto_Spline: + auto_spline_run(); + break; } } @@ -169,6 +173,62 @@ static void auto_wp_run() } } +// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller +static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination) +{ + auto_mode = Auto_Spline; + + // initialise wpnav + wp_nav.set_spline_destination(destination,stopped_at_start, seg_end_type, next_spline_destination); + + // initialise yaw + // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI + if (auto_yaw_mode != AUTO_YAW_ROI) { + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + } +} + +// auto_spline_run - runs the auto spline controller +// called by auto_run at 100hz or more +static void auto_spline_run() +{ + // if not auto armed set throttle to zero and exit immediately + if(!ap.auto_armed) { + // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off + // (of course it would be better if people just used take-off) + attitude_control.init_targets(); + attitude_control.set_throttle_out(0, false); + // tell motors to do a slow start + motors.slow_start(true); + return; + } + + // process pilot's yaw input + float target_yaw_rate = 0; + if (!failsafe.radio) { + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + if (target_yaw_rate != 0) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } + } + + // run waypoint controller + wp_nav.update_spline(); + + // call z-axis position controller (wpnav should have already updated it's alt target) + pos_control.update_z_controller(); + + // call attitude controller + if (auto_yaw_mode == AUTO_YAW_HOLD) { + // roll & pitch from waypoint controller, yaw rate from pilot + attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + }else{ + // roll, pitch from waypoint controller, yaw heading from auto_heading() + attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); + } +} + // auto_land_start - initialises controller to implement a landing static void auto_land_start() { diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 64ac13117f..432b6d1e43 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -183,7 +183,8 @@ enum AutoMode { Auto_WP, Auto_Land, Auto_RTL, - Auto_Circle + Auto_Circle, + Auto_Spline }; // RTL states