Copter: support for NAV_CMD_PLACE
This commit is contained in:
Peter Barker
parent
20b83861e9
commit
289aba4350
@ -381,6 +381,17 @@ private:
|
||||
uint32_t wp_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;
|
||||
|
||||
// Auto
|
||||
AutoMode auto_mode; // controls which auto controller is run
|
||||
|
||||
@ -777,6 +788,7 @@ private:
|
||||
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();
|
||||
@ -803,6 +815,14 @@ private:
|
||||
void auto_land_start();
|
||||
void auto_land_start(const Vector3f& destination);
|
||||
void auto_land_run();
|
||||
void do_payload_place(const AP_Mission::Mission_Command& cmd);
|
||||
void auto_payload_place_start();
|
||||
void auto_payload_place_start(const Vector3f& destination);
|
||||
void auto_payload_place_run();
|
||||
bool auto_payload_place_run_should_run();
|
||||
void auto_payload_place_run_loiter();
|
||||
void auto_payload_place_run_descend();
|
||||
void auto_payload_place_run_release();
|
||||
void auto_rtl_start();
|
||||
void auto_rtl_run();
|
||||
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
|
||||
@ -1098,6 +1118,8 @@ private:
|
||||
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);
|
||||
|
||||
@ -25,6 +25,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
|
||||
do_land(cmd);
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint
|
||||
do_payload_place(cmd);
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
|
||||
do_loiter_unlimited(cmd);
|
||||
break;
|
||||
@ -186,7 +190,6 @@ Return true if we do not recognize the command so that we move on to the next co
|
||||
bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
switch(cmd.id) {
|
||||
|
||||
//
|
||||
// navigation commands
|
||||
//
|
||||
@ -199,6 +202,9 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||
case MAV_CMD_NAV_LAND:
|
||||
return verify_land();
|
||||
|
||||
case MAV_CMD_NAV_PAYLOAD_PLACE:
|
||||
return verify_payload_place();
|
||||
|
||||
case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
return verify_loiter_unlimited();
|
||||
|
||||
@ -334,6 +340,27 @@ 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
|
||||
{
|
||||
// convert to location class
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
|
||||
// decide if we will use terrain following
|
||||
int32_t curr_terr_alt_cm, target_terr_alt_cm;
|
||||
if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
|
||||
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
|
||||
curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
|
||||
// if using terrain, set target altitude to current altitude above terrain
|
||||
target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
|
||||
} else {
|
||||
// set target altitude to current altitude above home
|
||||
target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
}
|
||||
return target_loc;
|
||||
}
|
||||
|
||||
// do_land - initiate landing procedure
|
||||
void Copter::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
@ -344,20 +371,8 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
// set state to fly to location
|
||||
land_state = LandStateType_FlyToLocation;
|
||||
|
||||
// convert to location class
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
Location_Class target_loc = terrain_adjusted_location(cmd);
|
||||
|
||||
// decide if we will use terrain following
|
||||
int32_t curr_terr_alt_cm, target_terr_alt_cm;
|
||||
if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
|
||||
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
|
||||
curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
|
||||
// if using terrain, set target altitude to current altitude above terrain
|
||||
target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
|
||||
} else {
|
||||
// set target altitude to current altitude above home
|
||||
target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
}
|
||||
auto_wp_start(target_loc);
|
||||
}else{
|
||||
// set landing state
|
||||
@ -368,6 +383,26 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
}
|
||||
}
|
||||
|
||||
// do_payload_place - initiate placing procedure
|
||||
void Copter::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) {
|
||||
// set state to fly to location
|
||||
nav_payload_place.state = PayloadPlaceStateType_FlyToLocation;
|
||||
|
||||
Location_Class target_loc = terrain_adjusted_location(cmd);
|
||||
|
||||
auto_wp_start(target_loc);
|
||||
} else {
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
|
||||
// initialise placing controller
|
||||
auto_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)
|
||||
@ -650,6 +685,139 @@ bool Copter::verify_land()
|
||||
return retval;
|
||||
}
|
||||
|
||||
#define NAV_PAYLOAD_PLACE_DEBUGGING 0
|
||||
|
||||
#if NAV_PAYLOAD_PLACE_DEBUGGING
|
||||
#include <stdio.h>
|
||||
#define debug(fmt, args ...) do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
||||
#else
|
||||
#define debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
// verify_payload_place - returns true if placing has been completed
|
||||
bool Copter::verify_payload_place()
|
||||
{
|
||||
const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
|
||||
const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
|
||||
const float hover_throttle_placed_fraction = 0.8; // i.e. if throttle is less than 80% of hover we have placed
|
||||
const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
|
||||
const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed
|
||||
|
||||
const float current_throttle_level = motors.get_throttle();
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
switch (nav_payload_place.state) {
|
||||
case PayloadPlaceStateType_FlyToLocation:
|
||||
if (!wp_nav.reached_wp_destination()) {
|
||||
return false;
|
||||
}
|
||||
// we're there; set loiter target
|
||||
auto_payload_place_start(wp_nav.get_wp_destination());
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Calibrating_Hover_Start:
|
||||
// hover for 1 second to get an idea of what our hover
|
||||
// throttle looks like
|
||||
debug("Calibrate start");
|
||||
nav_payload_place.hover_start_timestamp = now;
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Calibrating_Hover: {
|
||||
if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) {
|
||||
// still calibrating...
|
||||
debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp);
|
||||
return false;
|
||||
}
|
||||
// we have a valid calibration. Hopefully.
|
||||
nav_payload_place.hover_throttle_level = current_throttle_level;
|
||||
const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover());
|
||||
gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", hover_throttle_delta);
|
||||
nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
|
||||
// fall through
|
||||
};
|
||||
case PayloadPlaceStateType_Descending_Start:
|
||||
nav_payload_place.descend_start_timestamp = now;
|
||||
nav_payload_place.descend_start_altitude = inertial_nav.get_altitude();
|
||||
nav_payload_place.descend_throttle_level = 0;
|
||||
nav_payload_place.state = PayloadPlaceStateType_Descending;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Descending:
|
||||
// make sure we don't descend too far:
|
||||
debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max);
|
||||
if (!is_zero(nav_payload_place.descend_max) &&
|
||||
nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) {
|
||||
nav_payload_place.state = PayloadPlaceStateType_Ascending;
|
||||
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Reached maximum descent");
|
||||
return false; // we'll do any cleanups required next time through the loop
|
||||
}
|
||||
// see if we've been descending long enough to calibrate a descend-throttle-level:
|
||||
if (nav_payload_place.descend_throttle_level == 0 &&
|
||||
now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) {
|
||||
nav_payload_place.descend_throttle_level = current_throttle_level;
|
||||
}
|
||||
// watch the throttle to determine whether the load has been placed
|
||||
// debug("hover ratio: %f descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level));
|
||||
if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction &&
|
||||
(nav_payload_place.descend_throttle_level == 0 ||
|
||||
current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) {
|
||||
// throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid)
|
||||
nav_payload_place.place_start_timestamp = 0;
|
||||
return false;
|
||||
}
|
||||
if (nav_payload_place.place_start_timestamp == 0) {
|
||||
// we've only just now hit the correct throttle level
|
||||
nav_payload_place.place_start_timestamp = now;
|
||||
return false;
|
||||
} else if (now - nav_payload_place.place_start_timestamp < placed_time) {
|
||||
// keep going down....
|
||||
debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp);
|
||||
return false;
|
||||
}
|
||||
nav_payload_place.state = PayloadPlaceStateType_Releasing_Start;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Releasing_Start:
|
||||
if (g2.gripper.valid()) {
|
||||
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Releasing the gripper");
|
||||
g2.gripper.release();
|
||||
} else {
|
||||
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Gripper not valid");
|
||||
}
|
||||
nav_payload_place.state = PayloadPlaceStateType_Releasing;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Releasing:
|
||||
if (g2.gripper.valid() && !g2.gripper.released()) {
|
||||
return false;
|
||||
}
|
||||
nav_payload_place.state = PayloadPlaceStateType_Released;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Released: {
|
||||
nav_payload_place.state = PayloadPlaceStateType_Ascending_Start;
|
||||
}
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Ascending_Start: {
|
||||
Location_Class target_loc = inertial_nav.get_position();
|
||||
target_loc.alt = nav_payload_place.descend_start_altitude;
|
||||
auto_wp_start(target_loc);
|
||||
nav_payload_place.state = PayloadPlaceStateType_Ascending;
|
||||
}
|
||||
//fall through
|
||||
case PayloadPlaceStateType_Ascending:
|
||||
if (!wp_nav.reached_wp_destination()) {
|
||||
return false;
|
||||
}
|
||||
nav_payload_place.state = PayloadPlaceStateType_Done;
|
||||
// fall through
|
||||
case PayloadPlaceStateType_Done:
|
||||
return true;
|
||||
default:
|
||||
// this should never happen
|
||||
// TO-DO: log an error
|
||||
return true;
|
||||
}
|
||||
// should never get here
|
||||
return true;
|
||||
}
|
||||
#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)
|
||||
{
|
||||
|
||||
@ -91,6 +91,10 @@ void Copter::auto_run()
|
||||
case Auto_Loiter:
|
||||
auto_loiter_run();
|
||||
break;
|
||||
|
||||
case Auto_NavPayloadPlace:
|
||||
auto_payload_place_run();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -746,3 +750,119 @@ float Copter::get_auto_heading(void)
|
||||
}
|
||||
}
|
||||
|
||||
// auto_payload_place_start - initialises controller to implement a placing
|
||||
void Copter::auto_payload_place_start()
|
||||
{
|
||||
// set target to stopping point
|
||||
Vector3f stopping_point;
|
||||
wp_nav.get_loiter_stopping_point_xy(stopping_point);
|
||||
|
||||
// call location specific place start function
|
||||
auto_payload_place_start(stopping_point);
|
||||
|
||||
}
|
||||
|
||||
// auto_payload_place_start - initialises controller to implement placement of a load
|
||||
void Copter::auto_payload_place_start(const Vector3f& destination)
|
||||
{
|
||||
auto_mode = Auto_NavPayloadPlace;
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
|
||||
// initialise loiter target destination
|
||||
wp_nav.init_loiter_target(destination);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target(inertial_nav.get_altitude());
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
|
||||
bool Copter::auto_payload_place_run_should_run()
|
||||
{
|
||||
// muts be armed
|
||||
if (!motors.armed()) {
|
||||
return false;
|
||||
}
|
||||
// muts be auto-armed
|
||||
if (!ap.auto_armed) {
|
||||
return false;
|
||||
}
|
||||
// must not be landed
|
||||
if (ap.land_complete) {
|
||||
return false;
|
||||
}
|
||||
// interlock must be enabled (i.e. unsafe)
|
||||
if (!motors.get_interlock()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// auto_payload_place_run - places an object in auto mode
|
||||
// called by auto_run at 100hz or more
|
||||
void Copter::auto_payload_place_run()
|
||||
{
|
||||
if (!auto_payload_place_run_should_run()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
return;
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
switch (nav_payload_place.state) {
|
||||
case PayloadPlaceStateType_FlyToLocation:
|
||||
case PayloadPlaceStateType_Calibrating_Hover_Start:
|
||||
case PayloadPlaceStateType_Calibrating_Hover:
|
||||
return auto_payload_place_run_loiter();
|
||||
case PayloadPlaceStateType_Descending_Start:
|
||||
case PayloadPlaceStateType_Descending:
|
||||
return auto_payload_place_run_descend();
|
||||
case PayloadPlaceStateType_Releasing_Start:
|
||||
case PayloadPlaceStateType_Releasing:
|
||||
case PayloadPlaceStateType_Released:
|
||||
case PayloadPlaceStateType_Ascending_Start:
|
||||
case PayloadPlaceStateType_Ascending:
|
||||
case PayloadPlaceStateType_Done:
|
||||
return auto_payload_place_run_loiter();
|
||||
}
|
||||
}
|
||||
|
||||
void Copter::auto_payload_place_run_loiter()
|
||||
{
|
||||
// loiter...
|
||||
land_run_horizontal_control();
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
const float target_yaw_rate = 0;
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// update altitude target and call position controller
|
||||
// const float target_climb_rate = 0;
|
||||
// pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
}
|
||||
|
||||
void Copter::auto_payload_place_run_descend()
|
||||
{
|
||||
land_run_horizontal_control();
|
||||
land_run_vertical_control();
|
||||
}
|
||||
|
||||
@ -202,7 +202,8 @@ enum AutoMode {
|
||||
Auto_Circle,
|
||||
Auto_Spline,
|
||||
Auto_NavGuided,
|
||||
Auto_Loiter
|
||||
Auto_Loiter,
|
||||
Auto_NavPayloadPlace,
|
||||
};
|
||||
|
||||
// Guided modes
|
||||
@ -277,6 +278,20 @@ enum LandStateType {
|
||||
LandStateType_Descending = 1
|
||||
};
|
||||
|
||||
enum PayloadPlaceStateType {
|
||||
PayloadPlaceStateType_FlyToLocation,
|
||||
PayloadPlaceStateType_Calibrating_Hover_Start,
|
||||
PayloadPlaceStateType_Calibrating_Hover,
|
||||
PayloadPlaceStateType_Descending_Start,
|
||||
PayloadPlaceStateType_Descending,
|
||||
PayloadPlaceStateType_Releasing_Start,
|
||||
PayloadPlaceStateType_Releasing,
|
||||
PayloadPlaceStateType_Released,
|
||||
PayloadPlaceStateType_Ascending_Start,
|
||||
PayloadPlaceStateType_Ascending,
|
||||
PayloadPlaceStateType_Done,
|
||||
};
|
||||
|
||||
// bit options for DEV_OPTIONS parameter
|
||||
enum DevOptions {
|
||||
DevOptionADSBMAVLink = 1,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user