mirror of https://github.com/ArduPilot/ardupilot
Copter: move all commands logic into AUTO flightmode
This commit is contained in:
Peter Barker
Randy Mackay
parent
88abd34d40
commit
6c82f7671d
|
|
@ -34,8 +34,6 @@ Copter::Copter(void)
|
|||
super_simple_cos_yaw(1.0),
|
||||
super_simple_sin_yaw(0.0f),
|
||||
initial_armed_bearing(0),
|
||||
loiter_time_max(0),
|
||||
loiter_time(0),
|
||||
climb_rate(0),
|
||||
target_rangefinder_alt(0.0f),
|
||||
baro_alt(0),
|
||||
|
|
@ -47,8 +45,6 @@ Copter::Copter(void)
|
|||
yaw_look_at_heading(0),
|
||||
yaw_look_at_heading_slew(0),
|
||||
yaw_look_ahead_bearing(0.0f),
|
||||
condition_value(0),
|
||||
condition_start(0),
|
||||
G_Dt(MAIN_LOOP_SECONDS),
|
||||
inertial_nav(ahrs),
|
||||
pmTest1(0),
|
||||
|
|
|
|||
|
|
@ -221,6 +221,16 @@ private:
|
|||
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
|
||||
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)};
|
||||
|
||||
bool start_command(const AP_Mission::Mission_Command& cmd) {
|
||||
return mode_auto.start_command(cmd);
|
||||
}
|
||||
bool verify_command_callback(const AP_Mission::Mission_Command& cmd) {
|
||||
return mode_auto.verify_command_callback(cmd);
|
||||
}
|
||||
void exit_mission() {
|
||||
mode_auto.exit_mission();
|
||||
}
|
||||
|
||||
// Arming/Disarming mangement class
|
||||
AP_Arming_Copter arming{ahrs, barometer, compass, battery, inertial_nav, ins};
|
||||
|
||||
|
|
@ -369,20 +379,6 @@ private:
|
|||
int32_t _home_bearing;
|
||||
uint32_t _home_distance;
|
||||
|
||||
LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
|
||||
|
||||
|
||||
struct {
|
||||
PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)
|
||||
uint32_t hover_start_timestamp; // milliseconds
|
||||
float hover_throttle_level;
|
||||
uint32_t descend_start_timestamp; // milliseconds
|
||||
uint32_t place_start_timestamp; // milliseconds
|
||||
float descend_throttle_level;
|
||||
float descend_start_altitude;
|
||||
float descend_max; // centimetres
|
||||
} nav_payload_place;
|
||||
|
||||
// SIMPLE Mode
|
||||
// Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming
|
||||
// or in SuperSimple mode when the vehicle leaves a 20m radius from home.
|
||||
|
|
@ -395,14 +391,6 @@ private:
|
|||
// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable
|
||||
int32_t initial_armed_bearing;
|
||||
|
||||
// Loiter control
|
||||
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
|
||||
uint32_t loiter_time; // How long have we been loitering - The start time in millis
|
||||
|
||||
// Delay the next navigation command
|
||||
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
|
||||
uint32_t nav_delay_time_start;
|
||||
|
||||
// Battery Sensors
|
||||
AP_BattMonitor battery;
|
||||
|
||||
|
|
@ -456,10 +444,6 @@ private:
|
|||
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
|
||||
float auto_yaw_rate_cds;
|
||||
|
||||
// Delay Mission Scripting Command
|
||||
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
uint32_t condition_start;
|
||||
|
||||
// IMU variables
|
||||
// Integration time (in seconds) for the gyros (DCM algorithm)
|
||||
// Updated with the fast loop
|
||||
|
|
@ -727,21 +711,9 @@ private:
|
|||
void set_ekf_origin(const Location& loc);
|
||||
bool far_from_EKF_origin(const Location& loc);
|
||||
void set_system_time_from_GPS();
|
||||
void exit_mission();
|
||||
void do_RTL(void);
|
||||
bool verify_takeoff();
|
||||
bool verify_land();
|
||||
bool verify_payload_place();
|
||||
bool verify_loiter_unlimited();
|
||||
bool verify_loiter_time();
|
||||
bool verify_RTL();
|
||||
bool verify_wait_delay();
|
||||
bool verify_within_distance();
|
||||
bool verify_yaw();
|
||||
MAV_RESULT mavlink_compassmot(mavlink_channel_t chan);
|
||||
void delay(uint32_t ms);
|
||||
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
|
||||
void do_payload_place(const AP_Mission::Mission_Command& cmd);
|
||||
uint8_t get_default_auto_yaw_mode(bool rtl);
|
||||
void set_auto_yaw_mode(uint8_t yaw_mode);
|
||||
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle);
|
||||
|
|
@ -887,52 +859,6 @@ private:
|
|||
void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
|
||||
void print_hit_enter();
|
||||
void tuning();
|
||||
bool start_command(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_command(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
|
||||
|
||||
bool do_guided(const AP_Mission::Mission_Command& cmd);
|
||||
void do_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
void do_land(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
||||
void do_circle(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
||||
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
#if NAV_GUIDED == ENABLED
|
||||
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
|
||||
void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
||||
void do_yaw(const AP_Mission::Mission_Command& cmd);
|
||||
void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
||||
void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||
void do_roi(const AP_Mission::Mission_Command& cmd);
|
||||
void do_mount_control(const AP_Mission::Mission_Command& cmd);
|
||||
#if CAMERA == ENABLED
|
||||
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
#if PARACHUTE == ENABLED
|
||||
void do_parachute(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
#if GRIPPER_ENABLED == ENABLED
|
||||
void do_gripper(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
void do_winch(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
#if NAV_GUIDED == ENABLED
|
||||
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
|
||||
void log_init(void);
|
||||
void init_capabilities(void);
|
||||
void dataflash_periodic(void);
|
||||
|
|
|
|||
|
|
@ -676,7 +676,7 @@ GCS_MAVLINK_Copter::data_stream_send(void)
|
|||
|
||||
bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
||||
{
|
||||
return copter.do_guided(cmd);
|
||||
return copter.mode_auto.do_guided(cmd);
|
||||
}
|
||||
|
||||
void GCS_MAVLINK_Copter::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
#include "Copter.h"
|
||||
|
||||
// start_command - this function will be called when the ap_mission lib wishes to start a new command
|
||||
bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// To-Do: logging when new commands start/end
|
||||
if (should_log(MASK_LOG_CMD)) {
|
||||
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
|
||||
if (_copter.should_log(MASK_LOG_CMD)) {
|
||||
_copter.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
|
||||
}
|
||||
|
||||
switch(cmd.id) {
|
||||
|
|
@ -86,20 +86,20 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_SERVO:
|
||||
ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
|
||||
_copter.ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_RELAY:
|
||||
ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
|
||||
_copter.ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_REPEAT_SERVO:
|
||||
ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
|
||||
_copter.ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
|
||||
cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_REPEAT_RELAY:
|
||||
ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
|
||||
_copter.ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
|
||||
cmd.content.repeat_relay.cycle_time * 1000.0f);
|
||||
break;
|
||||
|
||||
|
|
@ -138,7 +138,7 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
break;
|
||||
|
||||
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
||||
camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
|
||||
_copter.camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
@ -179,9 +179,9 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
|
||||
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
|
||||
bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (control_mode == AUTO) {
|
||||
if (_copter.flightmode == &_copter.mode_auto) {
|
||||
bool cmd_complete = verify_command(cmd);
|
||||
|
||||
// send message to GCS
|
||||
|
|
@ -203,7 +203,7 @@ should move onto the next mission element.
|
|||
Return true if we do not recognize the command so that we move on to the next command
|
||||
*******************************************************************************/
|
||||
|
||||
bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
switch(cmd.id) {
|
||||
//
|
||||
|
|
@ -285,19 +285,19 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// exit_mission - function that is called once the mission completes
|
||||
void Copter::exit_mission()
|
||||
void Copter::ModeAuto::exit_mission()
|
||||
{
|
||||
// play a tone
|
||||
AP_Notify::events.mission_complete = 1;
|
||||
// if we are not on the ground switch to loiter or land
|
||||
if(!ap.land_complete) {
|
||||
// try to enter loiter but if that fails land
|
||||
if(!mode_auto.loiter_start()) {
|
||||
if(!loiter_start()) {
|
||||
set_mode(LAND, MODE_REASON_MISSION_END);
|
||||
}
|
||||
}else{
|
||||
// if we've landed it's safe to disarm
|
||||
init_disarm_motors();
|
||||
_copter.init_disarm_motors();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -306,10 +306,10 @@ void Copter::exit_mission()
|
|||
/********************************************************************************/
|
||||
|
||||
// do_RTL - start Return-to-Launch
|
||||
void Copter::do_RTL(void)
|
||||
void Copter::ModeAuto::do_RTL(void)
|
||||
{
|
||||
// start rtl in auto flight mode
|
||||
mode_auto.rtl_start();
|
||||
rtl_start();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
|
@ -317,16 +317,18 @@ void Copter::do_RTL(void)
|
|||
/********************************************************************************/
|
||||
|
||||
// do_takeoff - initiate takeoff navigation command
|
||||
void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// Set wp navigation target to safe altitude above current position
|
||||
mode_auto.takeoff_start(cmd.content.location);
|
||||
takeoff_start(cmd.content.location);
|
||||
}
|
||||
|
||||
// do_nav_wp - initiate move to next waypoint
|
||||
void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
const Location_Class ¤t_loc = _copter.current_loc;
|
||||
|
||||
// use current lat, lon if zero
|
||||
if (target_loc.lat == 0 && target_loc.lng == 0) {
|
||||
target_loc.lat = current_loc.lat;
|
||||
|
|
@ -350,7 +352,7 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
|||
loiter_time_max = cmd.p1;
|
||||
|
||||
// Set wp navigation target
|
||||
mode_auto.wp_start(target_loc);
|
||||
wp_start(target_loc);
|
||||
|
||||
// if no delay set the waypoint as "fast"
|
||||
if (loiter_time_max == 0 ) {
|
||||
|
|
@ -360,10 +362,11 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
// terrain_adjusted_location: returns a Location with lat/lon from cmd
|
||||
// and altitude from our current altitude adjusted for location
|
||||
Location_Class Copter::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const
|
||||
Location_Class Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const
|
||||
{
|
||||
// convert to location class
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
const Location_Class ¤t_loc = _copter.current_loc;
|
||||
|
||||
// decide if we will use terrain following
|
||||
int32_t curr_terr_alt_cm, target_terr_alt_cm;
|
||||
|
|
@ -380,7 +383,7 @@ Location_Class Copter::terrain_adjusted_location(const AP_Mission::Mission_Comma
|
|||
}
|
||||
|
||||
// do_land - initiate landing procedure
|
||||
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// To-Do: check if we have already landed
|
||||
|
||||
|
|
@ -391,18 +394,18 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
Location_Class target_loc = terrain_adjusted_location(cmd);
|
||||
|
||||
mode_auto.wp_start(target_loc);
|
||||
wp_start(target_loc);
|
||||
}else{
|
||||
// set landing state
|
||||
land_state = LandStateType_Descending;
|
||||
|
||||
// initialise landing controller
|
||||
mode_auto.land_start();
|
||||
land_start();
|
||||
}
|
||||
}
|
||||
|
||||
// do_payload_place - initiate placing procedure
|
||||
void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// if location provided we fly to that location at current altitude
|
||||
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
|
||||
|
|
@ -411,22 +414,23 @@ void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
Location_Class target_loc = terrain_adjusted_location(cmd);
|
||||
|
||||
mode_auto.wp_start(target_loc);
|
||||
wp_start(target_loc);
|
||||
} else {
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
|
||||
// initialise placing controller
|
||||
mode_auto.payload_place_start();
|
||||
payload_place_start();
|
||||
}
|
||||
nav_payload_place.descend_max = cmd.p1;
|
||||
}
|
||||
|
||||
// do_loiter_unlimited - start loitering with no end conditions
|
||||
// note: caller should set yaw_mode
|
||||
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// convert back to location
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
const Location_Class ¤t_loc = _copter.current_loc;
|
||||
|
||||
// use current location if not provided
|
||||
if (target_loc.lat == 0 && target_loc.lng == 0) {
|
||||
|
|
@ -452,13 +456,14 @@ void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// start way point navigator and provide it the desired location
|
||||
mode_auto.wp_start(target_loc);
|
||||
wp_start(target_loc);
|
||||
}
|
||||
|
||||
// do_circle - initiate moving in a circle
|
||||
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Location_Class circle_center(cmd.content.location);
|
||||
const Location_Class ¤t_loc = _copter.current_loc;
|
||||
|
||||
// default lat/lon to current position if not provided
|
||||
// To-Do: use stopping point or position_controller's target instead of current location to avoid jerk?
|
||||
|
|
@ -476,7 +481,7 @@ void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
|
|||
} else {
|
||||
// default to current altitude above origin
|
||||
circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
||||
_copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -484,12 +489,12 @@ void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
|
|||
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
|
||||
|
||||
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
|
||||
mode_auto.circle_movetoedge_start(circle_center, circle_radius_m);
|
||||
circle_movetoedge_start(circle_center, circle_radius_m);
|
||||
}
|
||||
|
||||
// do_loiter_time - initiate loitering at a point for a given time period
|
||||
// note: caller should set yaw_mode
|
||||
void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// re-use loiter unlimited
|
||||
do_loiter_unlimited(cmd);
|
||||
|
|
@ -500,9 +505,11 @@ void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// do_spline_wp - initiate move to next waypoint
|
||||
void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
const Location_Class ¤t_loc = _copter.current_loc;
|
||||
|
||||
// use current lat, lon if zero
|
||||
if (target_loc.lat == 0 && target_loc.lng == 0) {
|
||||
target_loc.lat = current_loc.lat;
|
||||
|
|
@ -569,25 +576,25 @@ void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// set spline navigation target
|
||||
mode_auto.spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
|
||||
spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
|
||||
}
|
||||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
// do_nav_guided_enable - initiate accepting commands from external nav computer
|
||||
void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.p1 > 0) {
|
||||
// initialise guided limits
|
||||
mode_guided.limit_init_time_and_pos();
|
||||
_copter.mode_guided.limit_init_time_and_pos();
|
||||
|
||||
// set spline navigation target
|
||||
mode_auto.nav_guided_start();
|
||||
nav_guided_start();
|
||||
}
|
||||
}
|
||||
#endif // NAV_GUIDED
|
||||
|
||||
// do_nav_delay - Delay the next navigation command
|
||||
void Copter::do_nav_delay(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
nav_delay_time_start = millis();
|
||||
|
||||
|
|
@ -603,19 +610,19 @@ void Copter::do_nav_delay(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
#if PARACHUTE == ENABLED
|
||||
// do_parachute - configure or release parachute
|
||||
void Copter::do_parachute(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_parachute(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
switch (cmd.p1) {
|
||||
case PARACHUTE_DISABLE:
|
||||
parachute.enabled(false);
|
||||
_copter.parachute.enabled(false);
|
||||
Log_Write_Event(DATA_PARACHUTE_DISABLED);
|
||||
break;
|
||||
case PARACHUTE_ENABLE:
|
||||
parachute.enabled(true);
|
||||
_copter.parachute.enabled(true);
|
||||
Log_Write_Event(DATA_PARACHUTE_ENABLED);
|
||||
break;
|
||||
case PARACHUTE_RELEASE:
|
||||
parachute_release();
|
||||
_copter.parachute_release();
|
||||
break;
|
||||
default:
|
||||
// do nothing
|
||||
|
|
@ -626,7 +633,7 @@ void Copter::do_parachute(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
#if GRIPPER_ENABLED == ENABLED
|
||||
// do_gripper - control gripper
|
||||
void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_gripper(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// Note: we ignore the gripper num parameter because we only support one gripper
|
||||
switch (cmd.content.gripper.action) {
|
||||
|
|
@ -647,17 +654,18 @@ void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
// do_guided_limits - pass guided limits to guided controller
|
||||
void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
mode_guided.limit_set(cmd.p1 * 1000, // convert seconds to ms
|
||||
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
|
||||
cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm
|
||||
cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
|
||||
_copter.mode_guided.limit_set(
|
||||
cmd.p1 * 1000, // convert seconds to ms
|
||||
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
|
||||
cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm
|
||||
cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
|
||||
}
|
||||
#endif
|
||||
|
||||
// control winch based on mission command
|
||||
void Copter::do_winch(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// Note: we ignore the gripper num parameter because we only support one gripper
|
||||
switch (cmd.content.winch.action) {
|
||||
|
|
@ -684,14 +692,14 @@ void Copter::do_winch(const AP_Mission::Mission_Command& cmd)
|
|||
/********************************************************************************/
|
||||
|
||||
// verify_takeoff - check if we have completed the takeoff
|
||||
bool Copter::verify_takeoff()
|
||||
bool Copter::ModeAuto::verify_takeoff()
|
||||
{
|
||||
// have we reached our target altitude?
|
||||
return wp_nav->reached_wp_destination();
|
||||
}
|
||||
|
||||
// verify_land - returns true if landing has been completed
|
||||
bool Copter::verify_land()
|
||||
bool Copter::ModeAuto::verify_land()
|
||||
{
|
||||
bool retval = false;
|
||||
|
||||
|
|
@ -703,7 +711,7 @@ bool Copter::verify_land()
|
|||
Vector3f dest = wp_nav->get_wp_destination();
|
||||
|
||||
// initialise landing controller
|
||||
mode_auto.land_start(dest);
|
||||
land_start(dest);
|
||||
|
||||
// advance to next state
|
||||
land_state = LandStateType_Descending;
|
||||
|
|
@ -736,7 +744,7 @@ bool Copter::verify_land()
|
|||
#endif
|
||||
|
||||
// verify_payload_place - returns true if placing has been completed
|
||||
bool Copter::verify_payload_place()
|
||||
bool Copter::ModeAuto::verify_payload_place()
|
||||
{
|
||||
const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
|
||||
const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
|
||||
|
|
@ -866,7 +874,7 @@ bool Copter::verify_payload_place()
|
|||
case PayloadPlaceStateType_Ascending_Start: {
|
||||
Location_Class target_loc = inertial_nav.get_position();
|
||||
target_loc.alt = nav_payload_place.descend_start_altitude;
|
||||
mode_auto.wp_start(target_loc);
|
||||
wp_start(target_loc);
|
||||
nav_payload_place.state = PayloadPlaceStateType_Ascending;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
|
@ -889,7 +897,7 @@ bool Copter::verify_payload_place()
|
|||
#undef debug
|
||||
|
||||
// verify_nav_wp - check if we have reached the next way point
|
||||
bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we have reached the waypoint
|
||||
if( !wp_nav->reached_wp_destination() ) {
|
||||
|
|
@ -913,13 +921,13 @@ bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
}
|
||||
|
||||
bool Copter::verify_loiter_unlimited()
|
||||
bool Copter::ModeAuto::verify_loiter_unlimited()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// verify_loiter_time - check if we have loitered long enough
|
||||
bool Copter::verify_loiter_time()
|
||||
bool Copter::ModeAuto::verify_loiter_time()
|
||||
{
|
||||
// return immediately if we haven't reached our destination
|
||||
if (!wp_nav->reached_wp_destination()) {
|
||||
|
|
@ -936,13 +944,13 @@ bool Copter::verify_loiter_time()
|
|||
}
|
||||
|
||||
// verify_circle - check if we have circled the point enough
|
||||
bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we've reached the edge
|
||||
if (mode_auto.mode() == Auto_CircleMoveToEdge) {
|
||||
if (mode() == Auto_CircleMoveToEdge) {
|
||||
if (wp_nav->reached_wp_destination()) {
|
||||
Vector3f curr_pos = inertial_nav.get_position();
|
||||
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
|
||||
const Vector3f curr_pos = _copter.inertial_nav.get_position();
|
||||
Vector3f circle_center = _copter.pv_location_to_vector(cmd.content.location);
|
||||
|
||||
// set target altitude if not provided
|
||||
if (is_zero(circle_center.z)) {
|
||||
|
|
@ -956,7 +964,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// start circling
|
||||
mode_auto.circle_start();
|
||||
circle_start();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
@ -968,13 +976,13 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
|
|||
// verify_RTL - handles any state changes required to implement RTL
|
||||
// do_RTL should have been called once first to initialise all variables
|
||||
// returns true with RTL has completed successfully
|
||||
bool Copter::verify_RTL()
|
||||
bool Copter::ModeAuto::verify_RTL()
|
||||
{
|
||||
return (mode_rtl.state_complete() && (mode_rtl.state() == RTL_FinalDescent || mode_rtl.state() == RTL_Land));
|
||||
return (_copter.mode_rtl.state_complete() && (_copter.mode_rtl.state() == RTL_FinalDescent || _copter.mode_rtl.state() == RTL_Land));
|
||||
}
|
||||
|
||||
// verify_spline_wp - check if we have reached the next way point using spline
|
||||
bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we have reached the waypoint
|
||||
if( !wp_nav->reached_wp_destination() ) {
|
||||
|
|
@ -997,7 +1005,7 @@ bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
// verify_nav_guided - check if we have breached any limits
|
||||
bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// if disabling guided mode then immediately return true so we move to next command
|
||||
if (cmd.p1 == 0) {
|
||||
|
|
@ -1005,12 +1013,12 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// check time and position limits
|
||||
return mode_guided.limit_check();
|
||||
return _copter.mode_guided.limit_check();
|
||||
}
|
||||
#endif // NAV_GUIDED
|
||||
|
||||
// verify_nav_delay - check if we have waited long enough
|
||||
bool Copter::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max,0)) {
|
||||
nav_delay_time_max = 0;
|
||||
|
|
@ -1024,18 +1032,18 @@ bool Copter::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
|
|||
// Condition (May) commands
|
||||
/********************************************************************************/
|
||||
|
||||
void Copter::do_wait_delay(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
condition_start = millis();
|
||||
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
|
||||
}
|
||||
|
||||
void Copter::do_within_distance(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
condition_value = cmd.content.distance.meters * 100;
|
||||
}
|
||||
|
||||
void Copter::do_yaw(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
set_auto_yaw_look_at_heading(
|
||||
cmd.content.yaw.angle_deg,
|
||||
|
|
@ -1049,7 +1057,7 @@ void Copter::do_yaw(const AP_Mission::Mission_Command& cmd)
|
|||
// Verify Condition (May) commands
|
||||
/********************************************************************************/
|
||||
|
||||
bool Copter::verify_wait_delay()
|
||||
bool Copter::ModeAuto::verify_wait_delay()
|
||||
{
|
||||
if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) {
|
||||
condition_value = 0;
|
||||
|
|
@ -1058,9 +1066,9 @@ bool Copter::verify_wait_delay()
|
|||
return false;
|
||||
}
|
||||
|
||||
bool Copter::verify_within_distance()
|
||||
bool Copter::ModeAuto::verify_within_distance()
|
||||
{
|
||||
if (flightmode->wp_distance() < (uint32_t)MAX(condition_value,0)) {
|
||||
if (wp_distance() < (uint32_t)MAX(condition_value,0)) {
|
||||
condition_value = 0;
|
||||
return true;
|
||||
}
|
||||
|
|
@ -1068,7 +1076,7 @@ bool Copter::verify_within_distance()
|
|||
}
|
||||
|
||||
// verify_yaw - return true if we have reached the desired heading
|
||||
bool Copter::verify_yaw()
|
||||
bool Copter::ModeAuto::verify_yaw()
|
||||
{
|
||||
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
|
||||
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
|
||||
|
|
@ -1076,7 +1084,7 @@ bool Copter::verify_yaw()
|
|||
}
|
||||
|
||||
// check if we are within 2 degrees of the target heading
|
||||
if (labs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200) {
|
||||
if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.yaw_look_at_heading)) <= 200) {
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
|
|
@ -1088,10 +1096,10 @@ bool Copter::verify_yaw()
|
|||
/********************************************************************************/
|
||||
|
||||
// do_guided - start guided mode
|
||||
bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// only process guided waypoint if we are in guided mode
|
||||
if (control_mode != GUIDED && !(control_mode == AUTO && mode_auto.mode() == Auto_NavGuided)) {
|
||||
if (_copter.control_mode != GUIDED && !(_copter.control_mode == AUTO && mode() == Auto_NavGuided)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1102,7 +1110,7 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
|
|||
{
|
||||
// set wp_nav's destination
|
||||
Location_Class dest(cmd.content.location);
|
||||
return mode_guided.set_destination(dest);
|
||||
return _copter.mode_guided.set_destination(dest);
|
||||
}
|
||||
|
||||
case MAV_CMD_CONDITION_YAW:
|
||||
|
|
@ -1117,19 +1125,19 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
|
|||
return true;
|
||||
}
|
||||
|
||||
void Copter::do_change_speed(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.content.speed.target_ms > 0) {
|
||||
wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
|
||||
}
|
||||
}
|
||||
|
||||
void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
|
||||
set_home_to_current_location(false);
|
||||
_copter.set_home_to_current_location(false);
|
||||
} else {
|
||||
set_home(cmd.content.location, false);
|
||||
_copter.set_home(cmd.content.location, false);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1137,42 +1145,43 @@ void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
|
|||
// this involves either moving the camera to point at the ROI (region of interest)
|
||||
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
|
||||
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
|
||||
void Copter::do_roi(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
set_auto_yaw_roi(cmd.content.location);
|
||||
_copter.set_auto_yaw_roi(cmd.content.location);
|
||||
}
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
|
||||
// do_digicam_configure Send Digicam Configure message with the camera library
|
||||
void Copter::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
camera.configure(cmd.content.digicam_configure.shooting_mode,
|
||||
cmd.content.digicam_configure.shutter_speed,
|
||||
cmd.content.digicam_configure.aperture,
|
||||
cmd.content.digicam_configure.ISO,
|
||||
cmd.content.digicam_configure.exposure_type,
|
||||
cmd.content.digicam_configure.cmd_id,
|
||||
cmd.content.digicam_configure.engine_cutoff_time);
|
||||
_copter.camera.configure(
|
||||
cmd.content.digicam_configure.shooting_mode,
|
||||
cmd.content.digicam_configure.shutter_speed,
|
||||
cmd.content.digicam_configure.aperture,
|
||||
cmd.content.digicam_configure.ISO,
|
||||
cmd.content.digicam_configure.exposure_type,
|
||||
cmd.content.digicam_configure.cmd_id,
|
||||
cmd.content.digicam_configure.engine_cutoff_time);
|
||||
}
|
||||
|
||||
// do_digicam_control Send Digicam Control message with the camera library
|
||||
void Copter::do_digicam_control(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_digicam_control(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
camera.control(cmd.content.digicam_control.session,
|
||||
cmd.content.digicam_control.zoom_pos,
|
||||
cmd.content.digicam_control.zoom_step,
|
||||
cmd.content.digicam_control.focus_lock,
|
||||
cmd.content.digicam_control.shooting_cmd,
|
||||
cmd.content.digicam_control.cmd_id);
|
||||
_copter.camera.control(cmd.content.digicam_control.session,
|
||||
cmd.content.digicam_control.zoom_pos,
|
||||
cmd.content.digicam_control.zoom_step,
|
||||
cmd.content.digicam_control.focus_lock,
|
||||
cmd.content.digicam_control.shooting_cmd,
|
||||
cmd.content.digicam_control.cmd_id);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// point the camera to a specified angle
|
||||
void Copter::do_mount_control(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
|
||||
_copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -271,6 +271,14 @@ public:
|
|||
|
||||
void payload_place_start();
|
||||
|
||||
// only out here temporarily
|
||||
bool start_command(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
||||
void exit_mission();
|
||||
|
||||
// for GCS_MAVLink to call:
|
||||
bool do_guided(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "AUTO"; }
|
||||
|
|
@ -286,6 +294,8 @@ protected:
|
|||
|
||||
private:
|
||||
|
||||
bool verify_command(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
void takeoff_run();
|
||||
void wp_run();
|
||||
void spline_run();
|
||||
|
|
@ -306,6 +316,86 @@ private:
|
|||
|
||||
AP_Mission &mission;
|
||||
AC_Circle *&circle_nav;
|
||||
|
||||
Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
|
||||
|
||||
void do_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
void do_land(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
||||
void do_circle(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
||||
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
#if NAV_GUIDED == ENABLED
|
||||
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
|
||||
void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
||||
void do_yaw(const AP_Mission::Mission_Command& cmd);
|
||||
void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
||||
void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||
void do_roi(const AP_Mission::Mission_Command& cmd);
|
||||
void do_mount_control(const AP_Mission::Mission_Command& cmd);
|
||||
#if CAMERA == ENABLED
|
||||
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
#if PARACHUTE == ENABLED
|
||||
void do_parachute(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
#if GRIPPER_ENABLED == ENABLED
|
||||
void do_gripper(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
void do_winch(const AP_Mission::Mission_Command& cmd);
|
||||
void do_payload_place(const AP_Mission::Mission_Command& cmd);
|
||||
void do_RTL(void);
|
||||
|
||||
bool verify_takeoff();
|
||||
bool verify_land();
|
||||
bool verify_payload_place();
|
||||
bool verify_loiter_unlimited();
|
||||
bool verify_loiter_time();
|
||||
bool verify_RTL();
|
||||
bool verify_wait_delay();
|
||||
bool verify_within_distance();
|
||||
bool verify_yaw();
|
||||
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||
#if NAV_GUIDED == ENABLED
|
||||
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
|
||||
|
||||
// Loiter control
|
||||
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
|
||||
uint32_t loiter_time; // How long have we been loitering - The start time in millis
|
||||
|
||||
// Delay the next navigation command
|
||||
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
|
||||
uint32_t nav_delay_time_start;
|
||||
|
||||
// Delay Mission Scripting Command
|
||||
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
uint32_t condition_start;
|
||||
|
||||
LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
|
||||
|
||||
struct {
|
||||
PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)
|
||||
uint32_t hover_start_timestamp; // milliseconds
|
||||
float hover_throttle_level;
|
||||
uint32_t descend_start_timestamp; // milliseconds
|
||||
uint32_t place_start_timestamp; // milliseconds
|
||||
float descend_throttle_level;
|
||||
float descend_start_altitude;
|
||||
float descend_max; // centimetres
|
||||
} nav_payload_place;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -807,7 +807,7 @@ void Copter::ModeAuto::payload_place_start()
|
|||
void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
|
||||
{
|
||||
_mode = Auto_NavPayloadPlace;
|
||||
_copter.nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
|
||||
// initialise loiter target destination
|
||||
wp_nav->init_loiter_target(destination);
|
||||
|
|
@ -866,7 +866,7 @@ void Copter::ModeAuto::payload_place_run()
|
|||
// set motors to full range
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
switch (_copter.nav_payload_place.state) {
|
||||
switch (nav_payload_place.state) {
|
||||
case PayloadPlaceStateType_FlyToLocation:
|
||||
case PayloadPlaceStateType_Calibrating_Hover_Start:
|
||||
case PayloadPlaceStateType_Calibrating_Hover:
|
||||
|
|
|
|||
Loading…
Reference in New Issue