Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: enable parameters inside mode objects

Browse Source 
this adds FHLD_* parameters for FlowHold mode. It is a large patch as
it needs to disentagle the mode class to enable it to be used in
Parameters.h
This commit is contained in:
Andrew Tridgell 2018-02-08 13:21:09 +11:00
parent 8eff43a352
commit 0ed75052f8
28 changed files with 598 additions and 512 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/Copter.h
View File

@ -953,36 +953,36 @@ private:
Mode *flightmode;
#if FRAME_CONFIG == HELI_FRAME
ModeAcro_Heli mode_acro{*this};
ModeAcro_Heli mode_acro;
#else
ModeAcro mode_acro{*this};
ModeAcro mode_acro;
#endif
ModeAltHold mode_althold{*this};
ModeAuto mode_auto{*this, mission, circle_nav};
ModeAltHold mode_althold;
ModeAuto mode_auto;
#if AUTOTUNE_ENABLED == ENABLED
ModeAutoTune mode_autotune{*this};
ModeAutoTune mode_autotune;
#endif
ModeBrake mode_brake{*this};
ModeCircle mode_circle{*this, circle_nav};
ModeDrift mode_drift{*this};
ModeFlip mode_flip{*this};
ModeGuided mode_guided{*this};
ModeLand mode_land{*this};
ModeLoiter mode_loiter{*this};
ModePosHold mode_poshold{*this};
ModeRTL mode_rtl{*this};
ModeBrake mode_brake;
ModeCircle mode_circle;
ModeDrift mode_drift;
ModeFlip mode_flip;
ModeGuided mode_guided;
ModeLand mode_land;
ModeLoiter mode_loiter;
ModePosHold mode_poshold;
ModeRTL mode_rtl;
#if FRAME_CONFIG == HELI_FRAME
ModeStabilize_Heli mode_stabilize{*this};
ModeStabilize_Heli mode_stabilize;
#else
ModeStabilize mode_stabilize{*this};
ModeStabilize mode_stabilize;
#endif
ModeSport mode_sport{*this};
ModeAvoidADSB mode_avoid_adsb{*this};
ModeThrow mode_throw{*this};
ModeGuidedNoGPS mode_guided_nogps{*this};
ModeSmartRTL mode_smartrtl{*this};
ModeSport mode_sport;
ModeAvoidADSB mode_avoid_adsb;
ModeThrow mode_throw;
ModeGuidedNoGPS mode_guided_nogps;
ModeSmartRTL mode_smartrtl;
#if OPTFLOW == ENABLED
ModeFlowHold mode_flowhold{*this};
ModeFlowHold mode_flowhold;
#endif
// mode.cpp

4
ArduCopter/Log.cpp
View File

@ -36,7 +36,7 @@ void Copter::ModeAutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step,
new_gain_sp : new_gain_sp,
new_ddt : new_ddt
};
_copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_AutoTuneDetails {
@ -55,7 +55,7 @@ void Copter::ModeAutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_
angle_cd : angle_cd,
rate_cds : rate_cds
};
_copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif

9
ArduCopter/Parameters.cpp
View File

@ -970,6 +970,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000),
#if OPTFLOW == ENABLED
// @Group: FHLD
// @Path: mode_flowhold.cpp
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold),
#endif
AP_GROUPEND
};
@ -987,6 +993,9 @@ ParametersG2::ParametersG2(void)
#endif
,smart_rtl(copter.ahrs)
,temp_calibration(copter.barometer, copter.ins)
#if OPTFLOW == ENABLED
,mode_flowhold_ptr(&copter.mode_flowhold)
#endif
{
AP_Param::setup_object_defaults(this, var_info);
}

5
ArduCopter/Parameters.h
View File

@ -568,6 +568,11 @@ public:
#if TOY_MODE_ENABLED == ENABLED
ToyMode toy_mode;
#endif
#if OPTFLOW == ENABLED
// we need a pointer to the mode for the G2 table
void *mode_flowhold_ptr;
#endif
};
extern const AP_Param::Info var_info[];

102
ArduCopter/commands_logic.cpp
View File

@ -4,8 +4,8 @@
bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (_copter.should_log(MASK_LOG_CMD)) {
_copter.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
if (copter.should_log(MASK_LOG_CMD)) {
copter.DataFlash.Log_Write_Mission_Cmd(copter.mission, cmd);
}
switch(cmd.id) {
@ -86,20 +86,20 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_DO_SET_SERVO:
_copter.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:
_copter.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:
_copter.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:
_copter.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::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
_copter.camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
copter.camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
break;
#endif
@ -181,7 +181,7 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
// 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::ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (_copter.flightmode == &_copter.mode_auto) {
if (copter.flightmode == &copter.mode_auto) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
@ -297,7 +297,7 @@ void Copter::ModeAuto::exit_mission()
}
}else{
// if we've landed it's safe to disarm
_copter.init_disarm_motors();
copter.init_disarm_motors();
}
}
@ -327,7 +327,7 @@ void Copter::ModeAuto::do_takeoff(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 &current_loc = _copter.current_loc;
const Location_Class &current_loc = copter.current_loc;
// use current lat, lon if zero
if (target_loc.lat == 0 && target_loc.lng == 0) {
@ -356,7 +356,7 @@ void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// if no delay set the waypoint as "fast"
if (loiter_time_max == 0 ) {
wp_nav->set_fast_waypoint(true);
copter.wp_nav->set_fast_waypoint(true);
}
}
@ -366,7 +366,7 @@ Location_Class Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mis
{
// convert to location class
Location_Class target_loc(cmd.content.location);
const Location_Class &current_loc = _copter.current_loc;
const Location_Class &current_loc = copter.current_loc;
// decide if we will use terrain following
int32_t curr_terr_alt_cm, target_terr_alt_cm;
@ -430,13 +430,13 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm
{
// convert back to location
Location_Class target_loc(cmd.content.location);
const Location_Class &current_loc = _copter.current_loc;
const Location_Class &current_loc = copter.current_loc;
// use current location if not provided
if (target_loc.lat == 0 && target_loc.lng == 0) {
// To-Do: make this simpler
Vector3f temp_pos;
wp_nav->get_wp_stopping_point_xy(temp_pos);
copter.wp_nav->get_wp_stopping_point_xy(temp_pos);
Location_Class temp_loc(temp_pos);
target_loc.lat = temp_loc.lat;
target_loc.lng = temp_loc.lng;
@ -463,7 +463,7 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm
void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
{
Location_Class circle_center(cmd.content.location);
const Location_Class &current_loc = _copter.current_loc;
const Location_Class &current_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?
@ -481,7 +481,7 @@ void Copter::ModeAuto::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());
_copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
@ -508,7 +508,7 @@ void Copter::ModeAuto::do_loiter_time(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 &current_loc = _copter.current_loc;
const Location_Class &current_loc = copter.current_loc;
// use current lat, lon if zero
if (target_loc.lat == 0 && target_loc.lng == 0) {
@ -539,9 +539,9 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// 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();
uint16_t prev_cmd_idx = copter.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 (copter.mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
stopped_at_start = false;
}
@ -550,7 +550,7 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// if there is no delay at the end of this segment get next nav command
Location_Class next_loc;
if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
if (cmd.p1 == 0 && copter.mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
next_loc = temp_cmd.content.location;
// default lat, lon to first waypoint's lat, lon
if (next_loc.lat == 0 && next_loc.lng == 0) {
@ -585,7 +585,7 @@ void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& c
{
if (cmd.p1 > 0) {
// initialise guided limits
_copter.mode_guided.limit_init_time_and_pos();
copter.mode_guided.limit_init_time_and_pos();
// set spline navigation target
nav_guided_start();
@ -614,15 +614,15 @@ void Copter::ModeAuto::do_parachute(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.p1) {
case PARACHUTE_DISABLE:
_copter.parachute.enabled(false);
copter.parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case PARACHUTE_ENABLE:
_copter.parachute.enabled(true);
copter.parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case PARACHUTE_RELEASE:
_copter.parachute_release();
copter.parachute_release();
break;
default:
// do nothing
@ -656,7 +656,7 @@ void Copter::ModeAuto::do_gripper(const AP_Mission::Mission_Command& cmd)
// do_guided_limits - pass guided limits to guided controller
void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
{
_copter.mode_guided.limit_set(
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
@ -695,7 +695,7 @@ void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
bool Copter::ModeAuto::verify_takeoff()
{
// have we reached our target altitude?
return wp_nav->reached_wp_destination();
return copter.wp_nav->reached_wp_destination();
}
// verify_land - returns true if landing has been completed
@ -706,9 +706,9 @@ bool Copter::ModeAuto::verify_land()
switch (land_state) {
case LandStateType_FlyToLocation:
// check if we've reached the location
if (wp_nav->reached_wp_destination()) {
if (copter.wp_nav->reached_wp_destination()) {
// get destination so we can use it for loiter target
Vector3f dest = wp_nav->get_wp_destination();
Vector3f dest = copter.wp_nav->get_wp_destination();
// initialise landing controller
land_start(dest);
@ -778,7 +778,7 @@ bool Copter::ModeAuto::verify_payload_place()
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
if (!wp_nav->reached_wp_destination()) {
if (!copter.wp_nav->reached_wp_destination()) {
return false;
}
// we're there; set loiter target
@ -879,7 +879,7 @@ bool Copter::ModeAuto::verify_payload_place()
}
FALLTHROUGH;
case PayloadPlaceStateType_Ascending:
if (!wp_nav->reached_wp_destination()) {
if (!copter.wp_nav->reached_wp_destination()) {
return false;
}
nav_payload_place.state = PayloadPlaceStateType_Done;
@ -900,7 +900,7 @@ bool Copter::ModeAuto::verify_payload_place()
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() ) {
if( !copter.wp_nav->reached_wp_destination() ) {
return false;
}
@ -930,7 +930,7 @@ bool Copter::ModeAuto::verify_loiter_unlimited()
bool Copter::ModeAuto::verify_loiter_time()
{
// return immediately if we haven't reached our destination
if (!wp_nav->reached_wp_destination()) {
if (!copter.wp_nav->reached_wp_destination()) {
return false;
}
@ -948,9 +948,9 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
{
// check if we've reached the edge
if (mode() == Auto_CircleMoveToEdge) {
if (wp_nav->reached_wp_destination()) {
const Vector3f curr_pos = _copter.inertial_nav.get_position();
Vector3f circle_center = _copter.pv_location_to_vector(cmd.content.location);
if (copter.wp_nav->reached_wp_destination()) {
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)) {
@ -970,7 +970,7 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
}
// check if we have completed circling
return fabsf(circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
// verify_RTL - handles any state changes required to implement RTL
@ -978,14 +978,14 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
// returns true with RTL has completed successfully
bool Copter::ModeAuto::verify_RTL()
{
return (_copter.mode_rtl.state_complete() && (_copter.mode_rtl.state() == RTL_FinalDescent || _copter.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::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if( !wp_nav->reached_wp_destination() ) {
if( !copter.wp_nav->reached_wp_destination() ) {
return false;
}
@ -1013,7 +1013,7 @@ bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Comman
}
// check time and position limits
return _copter.mode_guided.limit_check();
return copter.mode_guided.limit_check();
}
#endif // NAV_GUIDED
@ -1084,7 +1084,7 @@ bool Copter::ModeAuto::verify_yaw()
}
// check if we are within 2 degrees of the target heading
if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.yaw_look_at_heading)) <= 200) {
if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.yaw_look_at_heading)) <= 200) {
return true;
}else{
return false;
@ -1099,7 +1099,7 @@ bool Copter::ModeAuto::verify_yaw()
bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
{
// only process guided waypoint if we are in guided mode
if (_copter.control_mode != GUIDED && !(_copter.control_mode == AUTO && mode() == Auto_NavGuided)) {
if (copter.control_mode != GUIDED && !(copter.control_mode == AUTO && mode() == Auto_NavGuided)) {
return false;
}
@ -1110,7 +1110,7 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
{
// set wp_nav's destination
Location_Class dest(cmd.content.location);
return _copter.mode_guided.set_destination(dest);
return copter.mode_guided.set_destination(dest);
}
case MAV_CMD_CONDITION_YAW:
@ -1128,16 +1128,16 @@ bool Copter::ModeAuto::do_guided(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);
copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
}
}
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)) {
_copter.set_home_to_current_location(false);
copter.set_home_to_current_location(false);
} else {
_copter.set_home(cmd.content.location, false);
copter.set_home(cmd.content.location, false);
}
}
@ -1147,7 +1147,7 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
{
_copter.set_auto_yaw_roi(cmd.content.location);
copter.set_auto_yaw_roi(cmd.content.location);
}
#if CAMERA == ENABLED
@ -1155,7 +1155,7 @@ void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
// do_digicam_configure Send Digicam Configure message with the camera library
void Copter::ModeAuto::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
{
_copter.camera.configure(
copter.camera.configure(
cmd.content.digicam_configure.shooting_mode,
cmd.content.digicam_configure.shutter_speed,
cmd.content.digicam_configure.aperture,
@ -1168,7 +1168,7 @@ void Copter::ModeAuto::do_digicam_configure(const AP_Mission::Mission_Command& c
// do_digicam_control Send Digicam Control message with the camera library
void Copter::ModeAuto::do_digicam_control(const AP_Mission::Mission_Command& cmd)
{
_copter.camera.control(cmd.content.digicam_control.session,
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,
@ -1182,9 +1182,9 @@ void Copter::ModeAuto::do_digicam_control(const AP_Mission::Mission_Command& cmd
void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if MOUNT == ENABLED
if(!_copter.camera_mount.has_pan_control()) {
_copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0);
if(!copter.camera_mount.has_pan_control()) {
copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0);
}
_copter.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
}

150
ArduCopter/mode.cpp
View File

@ -5,6 +5,33 @@
* flight modes is in control_acro.cpp, control_stabilize.cpp, etc
*/
/*
constructor for Mode object
*/
Copter::Mode::Mode(void) :
g(copter.g),
g2(copter.g2),
wp_nav(copter.wp_nav),
pos_control(copter.pos_control),
inertial_nav(copter.inertial_nav),
ahrs(copter.ahrs),
attitude_control(copter.attitude_control),
motors(copter.motors),
channel_roll(copter.channel_roll),
channel_pitch(copter.channel_pitch),
channel_throttle(copter.channel_throttle),
channel_yaw(copter.channel_yaw),
G_Dt(copter.G_Dt),
ap(copter.ap),
takeoff_state(copter.takeoff_state),
ekfGndSpdLimit(copter.ekfGndSpdLimit),
ekfNavVelGainScaler(copter.ekfNavVelGainScaler),
#if FRAME_CONFIG == HELI_FRAME
heli_flags(copter.heli_flags),
#endif
auto_yaw_mode(copter.auto_yaw_mode)
{ };
// return the static controller object corresponding to supplied mode
Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
{
@ -91,7 +118,7 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
#if OPTFLOW == ENABLED
case FLOWHOLD:
ret = &mode_flowhold;
ret = (Copter::Mode *)g2.mode_flowhold_ptr;
break;
#endif
@ -274,10 +301,127 @@ void Copter::Mode::zero_throttle_and_relax_ac()
#if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control->set_throttle_out(0.0f, false, g.throttle_filt);
attitude_control->set_throttle_out(0.0f, false, copter.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.0f, true, g.throttle_filt);
attitude_control->set_throttle_out_unstabilized(0.0f, true, copter.g.throttle_filt);
#endif
}
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
}
float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
{
return copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt);
}
float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
return copter.get_pilot_desired_yaw_rate(stick_angle);
}
float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control)
{
return copter.get_pilot_desired_climb_rate(throttle_control);
}
float Copter::Mode::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
return copter.get_pilot_desired_throttle(throttle_control, thr_mid);
}
float Copter::Mode::get_non_takeoff_throttle()
{
return copter.get_non_takeoff_throttle();
}
void Copter::Mode::update_simple_mode(void) {
copter.update_simple_mode();
}
float Copter::Mode::get_smoothing_gain() {
return copter.get_smoothing_gain();
}
bool Copter::Mode::set_mode(control_mode_t mode, mode_reason_t reason)
{
return copter.set_mode(mode, reason);
}
void Copter::Mode::set_land_complete(bool b)
{
return copter.set_land_complete(b);
}
GCS_Copter &Copter::Mode::gcs()
{
return copter.gcs();
}
void Copter::Mode::Log_Write_Event(uint8_t id)
{
return copter.Log_Write_Event(id);
}
void Copter::Mode::set_throttle_takeoff()
{
return copter.set_throttle_takeoff();
}
void Copter::Mode::set_auto_yaw_mode(uint8_t yaw_mode)
{
return copter.set_auto_yaw_mode(yaw_mode);
}
void Copter::Mode::set_auto_yaw_rate(float turn_rate_cds)
{
return copter.set_auto_yaw_rate(turn_rate_cds);
}
void Copter::Mode::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
{
return copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle);
}
void Copter::Mode::takeoff_timer_start(float alt_cm)
{
return copter.takeoff_timer_start(alt_cm);
}
void Copter::Mode::takeoff_stop()
{
return copter.takeoff_stop();
}
void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
{
return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
}
float Copter::Mode::get_auto_heading()
{
return copter.get_auto_heading();
}
float Copter::Mode::get_auto_yaw_rate_cds()
{
return copter.get_auto_yaw_rate_cds();
}
float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate)
{
return copter.get_avoidance_adjusted_climbrate(target_rate);
}
uint16_t Copter::Mode::get_pilot_speed_dn()
{
return copter.get_pilot_speed_dn();
}

308
ArduCopter/mode.h
View File

@ -7,33 +7,9 @@ class Mode {
friend class AP_Arming_Copter;
friend class ToyMode;
public:
Mode(Copter &copter) :
_copter(copter),
g(copter.g),
g2(copter.g2),
wp_nav(_copter.wp_nav),
pos_control(_copter.pos_control),
inertial_nav(_copter.inertial_nav),
ahrs(_copter.ahrs),
attitude_control(_copter.attitude_control),
motors(_copter.motors),
channel_roll(_copter.channel_roll),
channel_pitch(_copter.channel_pitch),
channel_throttle(_copter.channel_throttle),
channel_yaw(_copter.channel_yaw),
G_Dt(_copter.G_Dt),
ap(_copter.ap),
takeoff_state(_copter.takeoff_state),
ekfGndSpdLimit(_copter.ekfGndSpdLimit),
ekfNavVelGainScaler(_copter.ekfNavVelGainScaler),
#if FRAME_CONFIG == HELI_FRAME
heli_flags(_copter.heli_flags),
#endif
auto_yaw_mode(_copter.auto_yaw_mode)
{ };
// constructor
Mode(void);
protected:
virtual bool init(bool ignore_checks) = 0;
@ -57,8 +33,6 @@ protected:
virtual uint32_t wp_distance() const { return 0; }
virtual int32_t wp_bearing() const { return 0; }
Copter &_copter;
// convenience references to avoid code churn in conversion:
Parameters &g;
ParametersG2 &g2;
@ -96,75 +70,29 @@ protected:
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max) {
_copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
}
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) {
return _copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt);
}
float get_pilot_desired_yaw_rate(int16_t stick_angle) {
return _copter.get_pilot_desired_yaw_rate(stick_angle);
}
float get_pilot_desired_climb_rate(float throttle_control) {
return _copter.get_pilot_desired_climb_rate(throttle_control);
}
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f) {
return _copter.get_pilot_desired_throttle(throttle_control, thr_mid);
}
float get_non_takeoff_throttle() {
return _copter.get_non_takeoff_throttle();
}
void update_simple_mode(void) {
_copter.update_simple_mode();
}
float get_smoothing_gain() {
return _copter.get_smoothing_gain();
}
bool set_mode(control_mode_t mode, mode_reason_t reason) {
return _copter.set_mode(mode, reason);
}
void set_land_complete(bool b) {
return _copter.set_land_complete(b);
}
GCS_Copter &gcs() {
return _copter.gcs();
}
void Log_Write_Event(uint8_t id) {
return _copter.Log_Write_Event(id);
}
void set_throttle_takeoff() {
return _copter.set_throttle_takeoff();
}
void set_auto_yaw_mode(uint8_t yaw_mode) {
return _copter.set_auto_yaw_mode(yaw_mode);
}
void set_auto_yaw_rate(float turn_rate_cds) {
return _copter.set_auto_yaw_rate(turn_rate_cds);
}
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) {
return _copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle);
}
void takeoff_timer_start(float alt_cm) {
return _copter.takeoff_timer_start(alt_cm);
}
void takeoff_stop() {
return _copter.takeoff_stop();
}
void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) {
return _copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
}
float get_auto_heading() {
return _copter.get_auto_heading();
}
float get_auto_yaw_rate_cds() {
return _copter.get_auto_yaw_rate_cds();
}
float get_avoidance_adjusted_climbrate(float target_rate) {
return _copter.get_avoidance_adjusted_climbrate(target_rate);
}
uint16_t get_pilot_speed_dn() {
return _copter.get_pilot_speed_dn();
}
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_pilot_desired_climb_rate(float throttle_control);
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
float get_non_takeoff_throttle(void);
void update_simple_mode(void);
float get_smoothing_gain(void);
bool set_mode(control_mode_t mode, mode_reason_t reason);
void set_land_complete(bool b);
GCS_Copter &gcs();
void Log_Write_Event(uint8_t id);
void set_throttle_takeoff(void);
void set_auto_yaw_mode(uint8_t yaw_mode);
void set_auto_yaw_rate(float turn_rate_cds);
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle);
void takeoff_timer_start(float alt_cm);
void takeoff_stop(void);
void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
float get_auto_heading(void);
float get_auto_yaw_rate_cds(void);
float get_avoidance_adjusted_climbrate(float target_rate);
uint16_t get_pilot_speed_dn(void);
// end pass-through functions
@ -175,10 +103,9 @@ protected:
class ModeAcro : public Mode {
public:
ModeAcro(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
@ -202,10 +129,8 @@ private:
class ModeAcro_Heli : public ModeAcro {
public:
ModeAcro_Heli(Copter &copter) :
Copter::ModeAcro(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -219,10 +144,8 @@ private:
class ModeAltHold : public Mode {
public:
ModeAltHold(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -245,12 +168,8 @@ private:
class ModeAuto : public Mode {
public:
ModeAuto(Copter &copter, AP_Mission &_mission, AC_Circle *& _circle_nav) :
Copter::Mode(copter),
mission(_mission),
circle_nav(_circle_nav)
{ }
// inherit constructor
using Copter::Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
@ -293,13 +212,9 @@ protected:
const char *name() const override { return "AUTO"; }
const char *name4() const override { return "AUTO"; }
uint32_t wp_distance() const override {
return wp_nav->get_wp_distance_to_destination();
}
int32_t wp_bearing() const override {
return wp_nav->get_wp_bearing_to_destination();
}
void run_autopilot() override { mission.update(); }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
void run_autopilot() override;
private:
@ -323,9 +238,6 @@ private:
AutoMode _mode = Auto_TakeOff; // controls which auto controller is run
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);
@ -407,15 +319,12 @@ private:
};
#if AUTOTUNE_ENABLED == ENABLED
class ModeAutoTune : public Mode {
public:
ModeAutoTune(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -564,10 +473,8 @@ private:
class ModeBrake : public Mode {
public:
ModeBrake(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -595,11 +502,8 @@ private:
class ModeCircle : public Mode {
public:
ModeCircle(Copter &copter, AC_Circle *& _circle_nav) :
Copter::Mode(copter),
circle_nav(_circle_nav)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -614,29 +518,21 @@ protected:
const char *name() const override { return "CIRCLE"; }
const char *name4() const override { return "CIRC"; }
uint32_t wp_distance() const override {
return wp_nav->get_loiter_distance_to_target();
}
int32_t wp_bearing() const override {
return wp_nav->get_loiter_bearing_to_target();
}
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
private:
// Circle
bool pilot_yaw_override = false; // true if pilot is overriding yaw
AC_Circle *&circle_nav;
};
class ModeDrift : public Mode {
public:
ModeDrift(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
@ -661,10 +557,8 @@ private:
class ModeFlip : public Mode {
public:
ModeFlip(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
@ -690,9 +584,8 @@ private:
class ModeGuided : public Mode {
public:
ModeGuided(Copter &copter) :
Copter::Mode(copter) { }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -749,9 +642,8 @@ private:
class ModeGuidedNoGPS : public ModeGuided {
public:
ModeGuidedNoGPS(Copter &copter) :
Copter::ModeGuided(copter) { }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -774,10 +666,8 @@ private:
class ModeLand : public Mode {
public:
ModeLand(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -809,10 +699,8 @@ private:
class ModeLoiter : public Mode {
public:
ModeLoiter(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -831,12 +719,8 @@ protected:
const char *name() const override { return "LOITER"; }
const char *name4() const override { return "LOIT"; }
uint32_t wp_distance() const override {
return wp_nav->get_loiter_distance_to_target();
}
int32_t wp_bearing() const override {
return wp_nav->get_loiter_bearing_to_target();
}
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
#if PRECISION_LANDING == ENABLED
bool do_precision_loiter();
@ -855,10 +739,8 @@ private:
class ModePosHold : public Mode {
public:
ModePosHold(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -889,10 +771,8 @@ private:
class ModeRTL : public Mode {
public:
ModeRTL(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override {
@ -919,12 +799,8 @@ protected:
const char *name() const override { return "RTL"; }
const char *name4() const override { return "RTL "; }
uint32_t wp_distance() const override {
return wp_nav->get_wp_distance_to_destination();
}
int32_t wp_bearing() const override {
return wp_nav->get_wp_bearing_to_destination();
}
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
void descent_start();
void descent_run();
@ -965,10 +841,8 @@ private:
class ModeSmartRTL : public ModeRTL {
public:
ModeSmartRTL(Copter &copter) :
ModeSmartRTL::ModeRTL(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -986,12 +860,8 @@ protected:
const char *name() const override { return "SMARTRTL"; }
const char *name4() const override { return "SRTL"; }
uint32_t wp_distance() const override {
return wp_nav->get_wp_distance_to_destination();
}
int32_t wp_bearing() const override {
return wp_nav->get_wp_bearing_to_destination();
}
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
private:
@ -1007,10 +877,8 @@ private:
class ModeSport : public Mode {
public:
ModeSport(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
@ -1033,10 +901,8 @@ private:
class ModeStabilize : public Mode {
public:
ModeStabilize(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
@ -1059,10 +925,8 @@ private:
class ModeStabilize_Heli : public ModeStabilize {
public:
ModeStabilize_Heli(Copter &copter) :
Copter::ModeStabilize(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -1078,10 +942,8 @@ private:
class ModeThrow : public Mode {
public:
ModeThrow(Copter &copter) :
Copter::Mode(copter)
{ }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -1117,9 +979,8 @@ private:
class ModeAvoidADSB : public ModeGuided {
public:
ModeAvoidADSB(Copter &copter) :
Copter::ModeGuided(copter) { }
// inherit constructor
using Copter::Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
@ -1141,6 +1002,7 @@ private:
};
#if OPTFLOW == ENABLED
/*
class to support FLOWHOLD mode, which is a position hold mode using
optical flow directly, avoiding the need for a rangefinder
@ -1148,10 +1010,8 @@ private:
class ModeFlowHold : public Mode {
public:
ModeFlowHold(Copter &copter) :
Copter::Mode(copter) {
AP_Param::setup_object_defaults(this, var_info);
}
// need a constructor for parameters
ModeFlowHold(void);
bool init(bool ignore_checks) override;
void run(void) override;
@ -1223,3 +1083,5 @@ private:
// last time there was significant stick input
uint32_t last_stick_input_ms;
};
#endif // OPTFLOW

8
ArduCopter/mode_acro.cpp
View File

@ -9,8 +9,8 @@
bool Copter::ModeAcro::init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), _copter.g2.acro_thr_mid) > _copter.get_non_takeoff_throttle())) {
if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), copter.g2.acro_thr_mid) > copter.get_non_takeoff_throttle())) {
return false;
}
// set target altitude to zero for reporting
@ -45,7 +45,7 @@ void Copter::ModeAcro::run()
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
attitude_control->set_throttle_out(pilot_throttle_scaled, false, copter.g.throttle_filt);
}
@ -56,7 +56,7 @@ void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pi
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
AP_Vehicle::MultiCopter &aparm = _copter.aparm;
AP_Vehicle::MultiCopter &aparm = copter.aparm;
// apply circular limit to pitch and roll inputs
float total_in = norm(pitch_in, roll_in);

10
ArduCopter/mode_acro_heli.cpp
View File

@ -14,7 +14,7 @@ bool Copter::ModeAcro_Heli::init(bool ignore_checks)
motors->set_acro_tail(true);
// set stab collective false to use full collective pitch range
_copter.input_manager.set_use_stab_col(false);
copter.input_manager.set_use_stab_col(false);
// always successfully enter acro
return true;
@ -34,16 +34,16 @@ void Copter::ModeAcro_Heli::run()
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if(!motors->armed()) {
_copter.heli_flags.init_targets_on_arming=true;
copter.heli_flags.init_targets_on_arming=true;
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
}
if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
if(motors->armed() && copter.heli_flags.init_targets_on_arming) {
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
if (motors->get_interlock()) {
_copter.heli_flags.init_targets_on_arming=false;
copter.heli_flags.init_targets_on_arming=false;
}
}
@ -91,7 +91,7 @@ void Copter::ModeAcro_Heli::run()
}
// get pilot's desired throttle
pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);

4
ArduCopter/mode_althold.cpp
View File

@ -142,14 +142,14 @@ void Copter::ModeAltHold::run()
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
_copter.avoid.adjust_roll_pitch(target_roll, target_pitch, _copter.aparm.angle_max);
copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
#endif
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (_copter.rangefinder_alt_ok()) {
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}

123
ArduCopter/mode_auto.cpp
View File

@ -17,21 +17,38 @@
* Code in this file implements the navigation commands
*/
// update mission
void Copter::ModeAuto::run_autopilot()
{
copter.mission.update();
}
uint32_t Copter::ModeAuto::wp_distance() const
{
return wp_nav->get_wp_distance_to_destination();
}
int32_t Copter::ModeAuto::wp_bearing() const
{
return wp_nav->get_wp_bearing_to_destination();
}
// auto_init - initialise auto controller
bool Copter::ModeAuto::init(bool ignore_checks)
{
if ((_copter.position_ok() && mission.num_commands() > 1) || ignore_checks) {
if ((copter.position_ok() && copter.mission.num_commands() > 1) || ignore_checks) {
_mode = Auto_Loiter;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
if (motors->armed() && ap.land_complete && !copter.mission.starts_with_takeoff_cmd()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (_copter.auto_yaw_mode == AUTO_YAW_ROI) {
if (copter.auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
@ -39,10 +56,10 @@ bool Copter::ModeAuto::init(bool ignore_checks)
wp_nav->wp_and_spline_init();
// clear guided limits
_copter.mode_guided.limit_clear();
copter.mode_guided.limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
copter.mission.start_or_resume();
return true;
}else{
return false;
@ -107,21 +124,21 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
Location_Class dest(dest_loc);
// set horizontal target
dest.lat = _copter.current_loc.lat;
dest.lng = _copter.current_loc.lng;
dest.lat = copter.current_loc.lat;
dest.lng = copter.current_loc.lng;
// get altitude target
int32_t alt_target;
if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) {
// this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
_copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
// fall back to altitude above current altitude
alt_target = _copter.current_loc.alt + dest.alt;
alt_target = copter.current_loc.alt + dest.alt;
}
// sanity check target
if (alt_target < _copter.current_loc.alt) {
dest.set_alt_cm(_copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
if (alt_target < copter.current_loc.alt) {
dest.set_alt_cm(copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
}
// Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
if (alt_target < 100) {
@ -131,7 +148,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
// set waypoint controller target
if (!wp_nav->set_wp_destination(dest)) {
// failure to set destination can only be because of missing terrain data
_copter.failsafe_terrain_on_event();
copter.failsafe_terrain_on_event();
return;
}
@ -142,7 +159,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
set_throttle_takeoff();
// get initial alt for WP_NAVALT_MIN
_copter.auto_takeoff_set_start_alt();
copter.auto_takeoff_set_start_alt();
}
// auto_takeoff_run - takeoff in auto mode
@ -161,7 +178,7 @@ void Copter::ModeAuto::takeoff_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
@ -182,13 +199,13 @@ void Copter::ModeAuto::takeoff_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
_copter.auto_takeoff_attitude_run(target_yaw_rate);
copter.auto_takeoff_attitude_run(target_yaw_rate);
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
@ -201,8 +218,8 @@ void Copter::ModeAuto::wp_start(const Vector3f& 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 (_copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
}
}
@ -214,14 +231,14 @@ void Copter::ModeAuto::wp_start(const Location_Class& dest_loc)
// send target to waypoint controller
if (!wp_nav->set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
_copter.failsafe_terrain_on_event();
copter.failsafe_terrain_on_event();
return;
}
// 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 (_copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
}
}
@ -241,7 +258,7 @@ void Copter::ModeAuto::wp_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -253,13 +270,13 @@ void Copter::ModeAuto::wp_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
@ -279,14 +296,14 @@ void Copter::ModeAuto::spline_start(const Location_Class& destination, bool stop
// initialise wpnav
if (!wp_nav->set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
// failure to set destination can only be because of missing terrain data
_copter.failsafe_terrain_on_event();
copter.failsafe_terrain_on_event();
return;
}
// 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 (_copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
}
}
@ -306,7 +323,7 @@ void Copter::ModeAuto::spline_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -324,7 +341,7 @@ void Copter::ModeAuto::spline_run()
pos_control->update_z_controller();
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
@ -377,8 +394,8 @@ void Copter::ModeAuto::land_run()
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
_copter.land_run_horizontal_control();
_copter.land_run_vertical_control();
copter.land_run_horizontal_control();
copter.land_run_vertical_control();
}
bool Copter::ModeAuto::landing_gear_should_be_deployed() const
@ -387,7 +404,7 @@ bool Copter::ModeAuto::landing_gear_should_be_deployed() const
case Auto_Land:
return true;
case Auto_RTL:
return _copter.mode_rtl.landing_gear_should_be_deployed();
return copter.mode_rtl.landing_gear_should_be_deployed();
default:
return false;
}
@ -400,7 +417,7 @@ void Copter::ModeAuto::rtl_start()
_mode = Auto_RTL;
// call regular rtl flight mode initialisation and ask it to ignore checks
_copter.mode_rtl.init(true);
copter.mode_rtl.init(true);
}
// auto_rtl_run - rtl in AUTO flight mode
@ -408,7 +425,7 @@ void Copter::ModeAuto::rtl_start()
void Copter::ModeAuto::rtl_run()
{
// call regular rtl flight mode run function
_copter.mode_rtl.run(false);
copter.mode_rtl.run(false);
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
@ -420,18 +437,18 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
}
circle_nav->set_center(circle_center_neu);
copter.circle_nav->set_center(circle_center_neu);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav->set_radius(radius_m * 100.0f);
copter.circle_nav->set_radius(radius_m * 100.0f);
}
// check our distance from edge of circle
Vector3f circle_edge_neu;
circle_nav->get_closest_point_on_circle(circle_edge_neu);
copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
// if more than 3m then fly to edge
@ -448,14 +465,14 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent
// initialise wpnav to move to edge of circle
if (!wp_nav->set_wp_destination(circle_edge)) {
// failure to set destination can only be because of missing terrain data
_copter.failsafe_terrain_on_event();
copter.failsafe_terrain_on_event();
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav->get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -472,7 +489,7 @@ void Copter::ModeAuto::circle_start()
_mode = Auto_Circle;
// initialise circle controller
circle_nav->init(circle_nav->get_center());
copter.circle_nav->init(copter.circle_nav->get_center());
}
// auto_circle_run - circle in AUTO flight mode
@ -480,13 +497,13 @@ void Copter::ModeAuto::circle_start()
void Copter::ModeAuto::circle_run()
{
// call circle controller
circle_nav->update();
copter.circle_nav->update();
// call z-axis position controller
pos_control->update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(),true, get_smoothing_gain());
}
#if NAV_GUIDED == ENABLED
@ -496,10 +513,10 @@ void Copter::ModeAuto::nav_guided_start()
_mode = Auto_NavGuided;
// call regular guided flight mode initialisation
_copter.mode_guided.init(true);
copter.mode_guided.init(true);
// initialise guided start time and position as reference for limit checking
_copter.mode_guided.limit_init_time_and_pos();
copter.mode_guided.limit_init_time_and_pos();
}
// auto_nav_guided_run - allows control by external navigation controller
@ -507,7 +524,7 @@ void Copter::ModeAuto::nav_guided_start()
void Copter::ModeAuto::nav_guided_run()
{
// call regular guided flight mode run function
_copter.mode_guided.run();
copter.mode_guided.run();
}
#endif // NAV_GUIDED
@ -516,7 +533,7 @@ void Copter::ModeAuto::nav_guided_run()
bool Copter::ModeAuto::loiter_start()
{
// return failure if GPS is bad
if (!_copter.position_ok()) {
if (!copter.position_ok()) {
return false;
}
_mode = Auto_Loiter;
@ -546,7 +563,7 @@ void Copter::ModeAuto::loiter_run()
// accept pilot input of yaw
float target_yaw_rate = 0;
if(!_copter.failsafe.radio) {
if(!copter.failsafe.radio) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
@ -554,7 +571,7 @@ void Copter::ModeAuto::loiter_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint and z-axis position controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
@ -833,7 +850,7 @@ void Copter::ModeAuto::payload_place_run()
void Copter::ModeAuto::payload_place_run_loiter()
{
// loiter...
_copter.land_run_horizontal_control();
copter.land_run_horizontal_control();
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
@ -848,6 +865,6 @@ void Copter::ModeAuto::payload_place_run_loiter()
void Copter::ModeAuto::payload_place_run_descend()
{
_copter.land_run_horizontal_control();
_copter.land_run_vertical_control();
copter.land_run_horizontal_control();
copter.land_run_vertical_control();
}

24
ArduCopter/mode_autotune.cpp
View File

@ -138,7 +138,7 @@ bool Copter::ModeAutoTune::init(bool ignore_checks)
}
// only do position hold if starting autotune from LOITER or POSHOLD
use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD);
use_poshold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD);
have_position = false;
return success;
@ -165,8 +165,8 @@ void Copter::ModeAutoTune::stop()
bool Copter::ModeAutoTune::start(bool ignore_checks)
{
// only allow flip from Stabilize, AltHold, PosHold or Loiter modes
if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD &&
_copter.control_mode != LOITER && _copter.control_mode != POSHOLD) {
if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD &&
copter.control_mode != LOITER && copter.control_mode != POSHOLD) {
return false;
}
@ -337,7 +337,7 @@ void Copter::ModeAutoTune::run()
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
@ -601,25 +601,25 @@ void Copter::ModeAutoTune::autotune_attitude_control()
switch (axis) {
case ROLL:
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), _copter.scheduler.get_loop_period_s());
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), copter.scheduler.get_loop_period_s());
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), _copter.scheduler.get_loop_period_s());
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
}
lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)start_angle);
break;
case PITCH:
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), _copter.scheduler.get_loop_period_s());
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), copter.scheduler.get_loop_period_s());
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), _copter.scheduler.get_loop_period_s());
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
}
lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)start_angle);
break;
case YAW:
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), _copter.scheduler.get_loop_period_s());
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), copter.scheduler.get_loop_period_s());
} else {
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), _copter.scheduler.get_loop_period_s());
rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
}
lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)start_angle);
break;
@ -650,7 +650,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
// log this iterations lean angle and rotation rate
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
_copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
break;
case UPDATE_GAINS:
@ -1456,7 +1456,7 @@ void Copter::ModeAutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch
}
// do we know where we are?
if (!_copter.position_ok()) {
if (!copter.position_ok()) {
return;
}

2
ArduCopter/mode_avoid_adsb.cpp
View File

@ -19,7 +19,7 @@ bool Copter::ModeAvoidADSB::init(const bool ignore_checks)
bool Copter::ModeAvoidADSB::set_velocity(const Vector3f& velocity_neu)
{
// check flight mode
if (_copter.control_mode != AVOID_ADSB) {
if (copter.control_mode != AVOID_ADSB) {
return false;
}

8
ArduCopter/mode_brake.cpp
View File

@ -7,7 +7,7 @@
// brake_init - initialise brake controller
bool Copter::ModeBrake::init(bool ignore_checks)
{
if (_copter.position_ok() || ignore_checks) {
if (copter.position_ok() || ignore_checks) {
// set desired acceleration to zero
wp_nav->clear_pilot_desired_acceleration();
@ -52,7 +52,7 @@ void Copter::ModeBrake::run()
// if landed immediately disarm
if (ap.land_complete) {
_copter.init_disarm_motors();
copter.init_disarm_motors();
}
// set motors to full range
@ -71,8 +71,8 @@ void Copter::ModeBrake::run()
pos_control->update_z_controller();
if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
if (!_copter.set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
_copter.set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
if (!copter.set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
copter.set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
}
}
}

28
ArduCopter/mode_circle.cpp
View File

@ -7,7 +7,7 @@
// circle_init - initialise circle controller flight mode
bool Copter::ModeCircle::init(bool ignore_checks)
{
if (_copter.position_ok() || ignore_checks) {
if (copter.position_ok() || ignore_checks) {
pilot_yaw_override = false;
// initialize speeds and accelerations
@ -18,7 +18,7 @@ bool Copter::ModeCircle::init(bool ignore_checks)
pos_control->set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed
circle_nav->init();
copter.circle_nav->init();
return true;
}else{
@ -48,7 +48,7 @@ void Copter::ModeCircle::run()
}
// process pilot inputs
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -71,17 +71,21 @@ void Copter::ModeCircle::run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run circle controller
circle_nav->update();
copter.circle_nav->update();
// call attitude controller
if (pilot_yaw_override) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(),
copter.circle_nav->get_pitch(),
target_yaw_rate, get_smoothing_gain());
}else{
attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(),
copter.circle_nav->get_pitch(),
copter.circle_nav->get_yaw(),true, get_smoothing_gain());
}
// adjust climb rate using rangefinder
if (_copter.rangefinder_alt_ok()) {
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
@ -89,3 +93,13 @@ void Copter::ModeCircle::run()
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
pos_control->update_z_controller();
}
uint32_t Copter::ModeCircle::wp_distance() const
{
return wp_nav->get_loiter_distance_to_target();
}
int32_t Copter::ModeCircle::wp_bearing() const
{
return wp_nav->get_loiter_bearing_to_target();
}

4
ArduCopter/mode_drift.cpp
View File

@ -29,7 +29,7 @@
// drift_init - initialise drift controller
bool Copter::ModeDrift::init(bool ignore_checks)
{
if (_copter.position_ok() || ignore_checks) {
if (copter.position_ok() || ignore_checks) {
return true;
}else{
return false;
@ -58,7 +58,7 @@ void Copter::ModeDrift::run()
}
// convert pilot input to lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());

24
ArduCopter/mode_flip.cpp
View File

@ -41,15 +41,15 @@ int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 =
bool Copter::ModeFlip::init(bool ignore_checks)
{
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (_copter.control_mode != ACRO &&
_copter.control_mode != STABILIZE &&
_copter.control_mode != ALT_HOLD &&
_copter.control_mode != FLOWHOLD) {
if (copter.control_mode != ACRO &&
copter.control_mode != STABILIZE &&
copter.control_mode != ALT_HOLD &&
copter.control_mode != FLOWHOLD) {
return false;
}
// if in acro or stabilize ensure throttle is above zero
if (ap.throttle_zero && (_copter.control_mode == ACRO || _copter.control_mode == STABILIZE)) {
if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
return false;
}
@ -64,7 +64,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
}
// capture original flight mode so that we can return to it after completion
flip_orig_control_mode = _copter.control_mode;
flip_orig_control_mode = copter.control_mode;
// initialise state
flip_state = Flip_Start;
@ -87,7 +87,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
Log_Write_Event(DATA_FLIP_START);
// capture current attitude which will be used during the Flip_Recovery stage
float angle_max = _copter.aparm.angle_max;
float angle_max = copter.aparm.angle_max;
flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
flip_orig_attitude.z = ahrs.yaw_sensor;
@ -196,9 +196,9 @@ void Copter::ModeFlip::run()
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
if (!copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
_copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log successful completion
Log_Write_Event(DATA_FLIP_END);
@ -207,12 +207,12 @@ void Copter::ModeFlip::run()
case Flip_Abandon:
// restore original flight mode
if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
if (!copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
_copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
// log abandoning flip
_copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
break;
}

13
ArduCopter/mode_flowhold.cpp
View File

@ -46,12 +46,12 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @User: Standard
AP_GROUPINFO("_FILT_HZ", 3, Copter::ModeFlowHold, flow_filter_hz, 5),
// @Param: _MIN_QUAL
// @Param: _QUAL_MIN
// @DisplayName: Minimum flow quality
// @Description: Minimum flow quality to use flow position hold
// @Range: 0 255
// @User: Standard
AP_GROUPINFO("_MIN_QUAL", 4, Copter::ModeFlowHold, flow_min_quality, 10),
AP_GROUPINFO("_QUAL_MIN", 4, Copter::ModeFlowHold, flow_min_quality, 10),
// 5 was FLOW_SPEED
@ -66,13 +66,18 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
AP_GROUPEND
};
Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
{
AP_Param::setup_object_defaults(this, var_info);
}
#define CONTROL_FLOWHOLD_EARTH_FRAME 0
// flowhold_init - initialise flowhold controller
bool Copter::ModeFlowHold::init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
// do not allow helis to enter Flow Hold if the Rotor Runup is not complete
if (!ignore_checks && !motors->rotor_runup_complete()){
return false;
}
@ -279,7 +284,7 @@ void Copter::ModeFlowHold::run()
bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
// Alt Hold State Machine
// Flow Hold State Machine
switch (flowhold_state) {
case FlowHold_MotorStopped:

68
ArduCopter/mode_guided.cpp
View File

@ -38,9 +38,9 @@ struct Guided_Limit {
// guided_init - initialise guided controller
bool Copter::ModeGuided::init(bool ignore_checks)
{
if (_copter.position_ok() || ignore_checks) {
if (copter.position_ok() || ignore_checks) {
// initialise yaw
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
// start in position control mode
pos_control_start();
return true;
@ -56,12 +56,12 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
guided_mode = Guided_TakeOff;
// initialise wpnav destination
Location_Class target_loc = _copter.current_loc;
Location_Class target_loc = copter.current_loc;
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
if (!wp_nav->set_wp_destination(target_loc)) {
// failure to set destination can only be because of missing terrain data
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
@ -73,7 +73,7 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
set_throttle_takeoff();
// get initial alt for WP_NAVALT_MIN
_copter.auto_takeoff_set_start_alt();
copter.auto_takeoff_set_start_alt();
return true;
}
@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start()
wp_nav->set_wp_destination(stopping_point, false);
// initialise yaw
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
}
// initialise guided mode's velocity controller
@ -163,7 +163,7 @@ void Copter::ModeGuided::angle_control_start()
// initialise targets
guided_angle_state.update_time_ms = millis();
guided_angle_state.roll_cd = _copter.ahrs.roll_sensor;
guided_angle_state.roll_cd = copter.ahrs.roll_sensor;
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
guided_angle_state.climb_rate_cms = 0.0f;
@ -187,8 +187,8 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
if (!_copter.fence.check_destination_within_fence(dest_loc)) {
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
@ -201,7 +201,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
wp_nav->set_wp_destination(destination, false);
// log target
_copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
return true;
}
@ -218,8 +218,8 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
if (!_copter.fence.check_destination_within_fence(dest_loc)) {
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
@ -227,7 +227,7 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
if (!wp_nav->set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
@ -236,7 +236,7 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// log target
_copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
return true;
}
@ -256,7 +256,7 @@ void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, fl
vel_update_time_ms = millis();
// log target
_copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
// set guided mode posvel target
@ -270,8 +270,8 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
if (!_copter.fence.check_destination_within_fence(dest_loc)) {
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
@ -284,10 +284,10 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
guided_pos_target_cm = destination;
guided_vel_target_cms = velocity;
_copter.pos_control->set_pos_target(guided_pos_target_cm);
copter.pos_control->set_pos_target(guided_pos_target_cm);
// log target
_copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
return true;
}
@ -312,11 +312,11 @@ void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bo
// interpret positive climb rate as triggering take-off
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
_copter.set_auto_armed(true);
copter.set_auto_armed(true);
}
// log target
_copter.Log_Write_GuidedTarget(guided_mode,
copter.Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
}
@ -371,7 +371,7 @@ void Copter::ModeGuided::takeoff_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
@ -392,13 +392,13 @@ void Copter::ModeGuided::takeoff_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
_copter.pos_control->update_z_controller();
copter.pos_control->update_z_controller();
// call attitude controller
_copter.auto_takeoff_attitude_run(target_yaw_rate);
copter.auto_takeoff_attitude_run(target_yaw_rate);
}
// guided_pos_control_run - runs the guided position controller
@ -413,7 +413,7 @@ void Copter::ModeGuided::pos_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -425,13 +425,13 @@ void Copter::ModeGuided::pos_control_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
@ -457,7 +457,7 @@ void Copter::ModeGuided::vel_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -485,7 +485,7 @@ void Copter::ModeGuided::vel_control_run()
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
@ -513,7 +513,7 @@ void Copter::ModeGuided::posvel_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -557,7 +557,7 @@ void Copter::ModeGuided::posvel_control_run()
pos_control->update_z_controller();
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
@ -587,7 +587,7 @@ void Copter::ModeGuided::angle_control_run()
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), _copter.aparm.angle_max);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
@ -653,7 +653,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
#if AC_AVOID_ENABLED
// limit the velocity to prevent fence violations
_copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
// get avoidance adjusted climb rate
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
#endif

28
ArduCopter/mode_land.cpp
View File

@ -10,7 +10,7 @@ static bool land_pause;
bool Copter::ModeLand::init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = _copter.position_ok();
land_with_gps = copter.position_ok();
if (land_with_gps) {
// set target to stopping point
Vector3f stopping_point;
@ -61,7 +61,7 @@ void Copter::ModeLand::gps_run()
// disarm when the landing detector says we've landed
if (ap.land_complete) {
_copter.init_disarm_motors();
copter.init_disarm_motors();
}
return;
}
@ -74,8 +74,8 @@ void Copter::ModeLand::gps_run()
land_pause = false;
}
_copter.land_run_horizontal_control();
_copter.land_run_vertical_control(land_pause);
copter.land_run_horizontal_control();
copter.land_run_vertical_control(land_pause);
}
// land_nogps_run - runs the land controller
@ -87,11 +87,11 @@ void Copter::ModeLand::nogps_run()
float target_yaw_rate = 0;
// process pilot inputs
if (!_copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
_copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
@ -99,7 +99,7 @@ void Copter::ModeLand::nogps_run()
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
}
// get pilot's desired yaw rate
@ -120,7 +120,7 @@ void Copter::ModeLand::nogps_run()
// disarm when the landing detector says we've landed
if (ap.land_complete) {
_copter.init_disarm_motors();
copter.init_disarm_motors();
}
return;
}
@ -136,7 +136,7 @@ void Copter::ModeLand::nogps_run()
land_pause = false;
}
_copter.land_run_vertical_control(land_pause);
copter.land_run_vertical_control(land_pause);
}
/*
@ -145,12 +145,12 @@ void Copter::ModeLand::nogps_run()
int32_t Copter::ModeLand::get_alt_above_ground(void)
{
int32_t alt_above_ground;
if (_copter.rangefinder_alt_ok()) {
alt_above_ground = _copter.rangefinder_state.alt_cm_filt.get();
if (copter.rangefinder_alt_ok()) {
alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
} else {
bool navigating = pos_control->is_active_xy();
if (!navigating || !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
alt_above_ground = _copter.current_loc.alt;
if (!navigating || !copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
alt_above_ground = copter.current_loc.alt;
}
}
return alt_above_ground;

22
ArduCopter/mode_loiter.cpp
View File

@ -7,7 +7,7 @@
// loiter_init - initialise loiter controller
bool Copter::ModeLoiter::init(bool ignore_checks)
{
if (_copter.position_ok() || ignore_checks) {
if (copter.position_ok() || ignore_checks) {
// set target to current position
wp_nav->init_loiter_target();
@ -41,7 +41,7 @@ bool Copter::ModeLoiter::do_precision_loiter()
if (wp_nav->get_pilot_desired_acceleration().length() > 50.0f) {
return false;
}
if (!_copter.precland.target_acquired()) {
if (!copter.precland.target_acquired()) {
return false; // we don't have a good vector
}
return true;
@ -51,11 +51,11 @@ void Copter::ModeLoiter::precision_loiter_xy()
{
wp_nav->clear_pilot_desired_acceleration();
Vector2f target_pos, target_vel_rel;
if (!_copter.precland.get_target_position_cm(target_pos)) {
if (!copter.precland.get_target_position_cm(target_pos)) {
target_pos.x = inertial_nav.get_position().x;
target_pos.y = inertial_nav.get_position().y;
}
if (!_copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
target_vel_rel.x = -inertial_nav.get_velocity().x;
target_vel_rel.y = -inertial_nav.get_velocity().y;
}
@ -78,7 +78,7 @@ void Copter::ModeLoiter::run()
pos_control->set_accel_z(g.pilot_accel_z);
// process pilot inputs unless we are in radio failsafe
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
@ -196,7 +196,7 @@ void Copter::ModeLoiter::run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (_copter.rangefinder_alt_ok()) {
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
@ -210,3 +210,13 @@ void Copter::ModeLoiter::run()
break;
}
}
uint32_t Copter::ModeLoiter::wp_distance() const
{
return wp_nav->get_loiter_distance_to_target();
}
int32_t Copter::ModeLoiter::wp_bearing() const
{
return wp_nav->get_loiter_bearing_to_target();
}

10
ArduCopter/mode_poshold.cpp
View File

@ -74,7 +74,7 @@ static struct {
bool Copter::ModePosHold::init(bool ignore_checks)
{
// fail to initialise PosHold mode if no GPS lock
if (!_copter.position_ok() && !ignore_checks) {
if (!copter.position_ok() && !ignore_checks) {
return false;
}
@ -147,7 +147,7 @@ void Copter::ModePosHold::run()
}
// process pilot inputs
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
@ -201,7 +201,7 @@ void Copter::ModePosHold::run()
return;
}else{
// convert pilot input to lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
@ -517,7 +517,7 @@ void Copter::ModePosHold::run()
}
// constrain target pitch/roll angles
float angle_max = _copter.aparm.angle_max;
float angle_max = copter.aparm.angle_max;
poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max);
poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max);
@ -525,7 +525,7 @@ void Copter::ModePosHold::run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (_copter.rangefinder_alt_ok()) {
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}

72
ArduCopter/mode_rtl.cpp
View File

@ -10,10 +10,10 @@
// rtl_init - initialise rtl controller
bool Copter::ModeRTL::init(bool ignore_checks)
{
if (_copter.position_ok() || ignore_checks) {
if (copter.position_ok() || ignore_checks) {
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
build_path(!_copter.failsafe.terrain);
build_path(!copter.failsafe.terrain);
climb_start();
return true;
}else{
@ -25,7 +25,7 @@ bool Copter::ModeRTL::init(bool ignore_checks)
void Copter::ModeRTL::restart_without_terrain()
{
// log an error
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
if (rtl_path.terrain_used) {
build_path(false);
climb_start();
@ -47,7 +47,7 @@ void Copter::ModeRTL::run(bool disarm_on_land)
loiterathome_start();
break;
case RTL_LoiterAtHome:
if (rtl_path.land || _copter.failsafe.radio) {
if (rtl_path.land || copter.failsafe.radio) {
land_start();
}else{
descent_start();
@ -101,8 +101,8 @@ void Copter::ModeRTL::climb_start()
// set the destination
if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
// this should not happen because rtl_build_path will have checked terrain data was available
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
_copter.set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
copter.set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
return;
}
wp_nav->set_fast_waypoint(true);
@ -123,7 +123,7 @@ void Copter::ModeRTL::return_start()
}
// initialise yaw to point home (maybe)
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(true));
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
}
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
@ -139,7 +139,7 @@ void Copter::ModeRTL::climb_return_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -151,13 +151,13 @@ void Copter::ModeRTL::climb_return_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
@ -177,7 +177,7 @@ void Copter::ModeRTL::loiterathome_start()
_loiter_start_time = millis();
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw
if(_copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
if(copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
} else {
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -197,7 +197,7 @@ void Copter::ModeRTL::loiterathome_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!_copter.failsafe.radio) {
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -209,13 +209,13 @@ void Copter::ModeRTL::loiterathome_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
@ -225,9 +225,9 @@ void Copter::ModeRTL::loiterathome_run()
// check if we've completed this stage of RTL
if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
if (_copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
if (copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
// check if heading is within 2 degrees of heading when vehicle was armed
if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.initial_armed_bearing)) <= 200) {
if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
_state_complete = true;
}
} else {
@ -269,12 +269,12 @@ void Copter::ModeRTL::descent_run()
}
// process pilot's input
if (!_copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!_copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
_copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
if (!copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}
@ -308,7 +308,7 @@ void Copter::ModeRTL::descent_run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
// check if we've reached within 20cm of final altitude
_state_complete = labs(rtl_path.descent_target.alt - _copter.current_loc.alt) < 20;
_state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
}
// rtl_loiterathome_start - initialise controllers to loiter over home
@ -355,7 +355,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
// disarm when the landing detector says we've landed
if (ap.land_complete && disarm_on_land) {
_copter.init_disarm_motors();
copter.init_disarm_motors();
}
// check if we've completed this stage of RTL
@ -366,8 +366,8 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
_copter.land_run_horizontal_control();
_copter.land_run_vertical_control();
copter.land_run_horizontal_control();
copter.land_run_vertical_control();
// check if we've completed this stage of RTL
_state_complete = ap.land_complete;
@ -402,24 +402,24 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed)
{
// set return target to nearest rally point or home position (Note: alt is absolute)
#if AC_RALLY == ENABLED
rtl_path.return_target = _copter.rally.calc_best_rally_or_home_location(_copter.current_loc, ahrs.get_home().alt);
rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt);
#else
rtl_path.return_target = ahrs.get_home();
#endif
// curr_alt is current altitude above home or above terrain depending upon use_terrain
int32_t curr_alt = _copter.current_loc.alt;
int32_t curr_alt = copter.current_loc.alt;
// decide if we should use terrain altitudes
rtl_path.terrain_used = _copter.terrain_use() && terrain_following_allowed;
rtl_path.terrain_used = copter.terrain_use() && terrain_following_allowed;
if (rtl_path.terrain_used) {
// attempt to retrieve terrain alt for current location, stopping point and origin
int32_t origin_terr_alt, return_target_terr_alt;
if (!rtl_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
!rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) ||
!_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
!copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
rtl_path.terrain_used = false;
_copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
@ -457,10 +457,10 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed)
// if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to
// the vehicle not climbing at all as RTL begins. This can be overly conservative and it might be better
// to apply the fence alt limit independently on the origin_point and return_target
if ((_copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
if ((copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
// get return target as alt-above-home so it can be compared to fence's alt
if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) {
float fence_alt = _copter.fence.get_safe_alt_max()*100.0f;
float fence_alt = copter.fence.get_safe_alt_max()*100.0f;
if (target_alt > fence_alt) {
// reduce target alt to the fence alt
rtl_path.return_target.alt -= (target_alt - fence_alt);
@ -472,3 +472,13 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed)
// ensure we do not descend
rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
}
uint32_t Copter::ModeRTL::wp_distance() const
{
return wp_nav->get_wp_distance_to_destination();
}
int32_t Copter::ModeRTL::wp_bearing() const
{
return wp_nav->get_wp_bearing_to_destination();
}

10
ArduCopter/mode_smart_rtl.cpp
View File

@ -137,3 +137,13 @@ void Copter::ModeSmartRTL::save_position()
copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
}
uint32_t Copter::ModeSmartRTL::wp_distance() const
{
return wp_nav->get_wp_distance_to_destination();
}
int32_t Copter::ModeSmartRTL::wp_bearing() const
{
return wp_nav->get_wp_bearing_to_destination();
}

4
ArduCopter/mode_sport.cpp
View File

@ -51,7 +51,7 @@ void Copter::ModeSport::run()
int32_t pitch_angle = wrap_180_cd(att_target.y);
target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
AP_Vehicle::MultiCopter &aparm = _copter.aparm;
AP_Vehicle::MultiCopter &aparm = copter.aparm;
if (roll_angle > aparm.angle_max){
target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
@ -150,7 +150,7 @@ void Copter::ModeSport::run()
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// adjust climb rate using rangefinder
if (_copter.rangefinder_alt_ok()) {
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}

4
ArduCopter/mode_stabilize.cpp
View File

@ -8,7 +8,7 @@
bool Copter::ModeStabilize::init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
return false;
}
@ -40,7 +40,7 @@ void Copter::ModeStabilize::run()
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
AP_Vehicle::MultiCopter &aparm = _copter.aparm;
AP_Vehicle::MultiCopter &aparm = copter.aparm;
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats

12
ArduCopter/mode_stabilize_heli.cpp
View File

@ -13,7 +13,7 @@ bool Copter::ModeStabilize_Heli::init(bool ignore_checks)
pos_control->set_alt_target(0);
// set stab collective true to use stabilize scaled collective pitch range
_copter.input_manager.set_use_stab_col(true);
copter.input_manager.set_use_stab_col(true);
return true;
}
@ -33,16 +33,16 @@ void Copter::ModeStabilize_Heli::run()
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if(!motors->armed()) {
_copter.heli_flags.init_targets_on_arming = true;
copter.heli_flags.init_targets_on_arming = true;
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
}
if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
if(motors->armed() && copter.heli_flags.init_targets_on_arming) {
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
if (motors->get_interlock()) {
_copter.heli_flags.init_targets_on_arming=false;
copter.heli_flags.init_targets_on_arming=false;
}
}
@ -56,13 +56,13 @@ void Copter::ModeStabilize_Heli::run()
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot's desired throttle
pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

10
ArduCopter/mode_throw.cpp
View File

@ -71,7 +71,7 @@ void Copter::ModeThrow::run()
pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
_copter.set_auto_armed(true);
copter.set_auto_armed(true);
} else if (stage == Throw_HgtStabilise && throw_height_good()) {
gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
@ -81,7 +81,7 @@ void Copter::ModeThrow::run()
wp_nav->init_loiter_target();
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
_copter.set_auto_armed(true);
copter.set_auto_armed(true);
} else if (stage == Throw_PosHold && throw_position_good()) {
if (!nextmode_attempted) {
switch (g2.throw_nextmode) {
@ -186,13 +186,13 @@ void Copter::ModeThrow::run()
last_log_ms = now;
float velocity = inertial_nav.get_velocity().length();
float velocity_z = inertial_nav.get_velocity().z;
float accel = _copter.ins.get_accel().length();
float accel = copter.ins.get_accel().length();
float ef_accel_z = ahrs.get_accel_ef().z;
bool throw_detect = (stage > Throw_Detecting) || throw_detected();
bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
_copter.Log_Write_Throw(stage,
copter.Log_Write_Throw(stage,
velocity,
velocity_z,
accel,
@ -227,7 +227,7 @@ bool Copter::ModeThrow::throw_detected()
bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
// Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
bool no_throw_action = _copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
// High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;