zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sub: rename modes loiter->velhold, sport->transect

master
Jacob Walser 9 years ago committed by Andrew Tridgell
parent
e844e28a77
commit
0085cc8a27
10 changed files with 32 additions and 134 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 83
      ArduSub/GCS_Mavlink.cpp
  2. 8
      ArduSub/Sub.h
  3. 2
      ArduSub/control_brake.cpp
  4. 2
      ArduSub/control_rtl.cpp
  5. 0
      ArduSub/control_transect.cpp
  6. 0
      ArduSub/control_velhold.cpp
  7. 4
      ArduSub/defines.h
  8. 29
      ArduSub/flight_mode.cpp
  9. 4
      ArduSub/navigation.cpp
  10. 34
      ArduSub/takeoff.cpp

83
ArduSub/GCS_Mavlink.cpp
Unescape View File

@ -49,11 +49,12 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) @@ -49,11 +49,12 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
switch (control_mode) {
case AUTO:
case RTL:
case LOITER:
case VELHOLD:
case GUIDED:
case CIRCLE:
case POSHOLD:
case BRAKE:
case TRANSECT:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
@ -167,19 +168,17 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) @@ -167,19 +168,17 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
case ALT_HOLD:
case AUTO:
case GUIDED:
case LOITER:
case VELHOLD:
case RTL:
case CIRCLE:
case SURFACE:
case OF_LOITER:
case POSHOLD:
case BRAKE:
case TRANSECT:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
case SPORT:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
break;
default:
break;
}
@ -1267,7 +1266,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1267,7 +1266,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_LOITER_UNLIM:
if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
if (sub.set_mode(POSHOLD, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
@ -1638,78 +1637,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1638,78 +1637,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
/* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK: {
result = MAV_RESULT_ACCEPTED;
if (sub.failsafe.radio) {
break;
}
// set mode to Loiter or fall back to AltHold
if (!sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
sub.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
}
break;
}
// Not supported in Sub
// /* Solo user holds down Fly button for a couple of seconds */
// case MAV_CMD_SOLO_BTN_FLY_HOLD: {
// result = MAV_RESULT_ACCEPTED;
//
// if (sub.failsafe.radio) {
// break;
// }
//
// if (!sub.motors.armed()) {
// // if disarmed, arm motors
// sub.init_arm_motors(true);
// } else if (sub.ap.land_complete) {
// // if armed and landed, takeoff
// if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
// sub.do_user_takeoff(packet.param1*100, true);
// }
// } else {
// // if flying, land
// sub.set_mode(LAND, MODE_REASON_GCS_COMMAND);
// }
// break;
// }
/* Solo user presses pause button */
case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
result = MAV_RESULT_ACCEPTED;
if (sub.failsafe.radio) {
break;
}
if (sub.motors.armed()) {
if (sub.ap.land_complete) {
// if landed, disarm motors
sub.init_disarm_motors();
} else {
// assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode
bool shot_mode = (!is_zero(packet.param1) && sub.control_mode == GUIDED);
if (!shot_mode) {
if (sub.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) {
sub.brake_timeout_to_loiter_ms(2500);
} else {
sub.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
}
} else {
// SoloLink is expected to handle pause in shots
}
}
}
break;
}
default:
result = MAV_RESULT_UNSUPPORTED;
break;

8
ArduSub/Sub.h
Unescape View File

@ -792,8 +792,8 @@ private: @@ -792,8 +792,8 @@ private:
void guided_limit_init_time_and_pos();
bool guided_limit_check();
float get_land_descent_speed();
bool loiter_init(bool ignore_checks);
void loiter_run();
bool velhold_init(bool ignore_checks);
void velhold_run();
bool poshold_init(bool ignore_checks);
void poshold_run();
void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
@ -826,8 +826,8 @@ private: @@ -826,8 +826,8 @@ private:
void rtl_land_run();
void rtl_build_path(bool terrain_following_allowed);
void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed);
bool sport_init(bool ignore_checks);
void sport_run();
bool transect_init(bool ignore_checks);
void transect_run();
bool stabilize_init(bool ignore_checks);
void stabilize_run();
bool manual_init(bool ignore_checks);

2
ArduSub/control_brake.cpp
Unescape View File

@ -74,7 +74,7 @@ void Sub::brake_run() @@ -74,7 +74,7 @@ void Sub::brake_run()
pos_control.update_z_controller();
if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) {
if (!set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
if (!set_mode(POSHOLD, MODE_REASON_BRAKE_TIMEOUT)) {
set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
}
}

2
ArduSub/control_rtl.cpp
Unescape View File

@ -287,7 +287,7 @@ void Sub::rtl_descent_run() @@ -287,7 +287,7 @@ void Sub::rtl_descent_run()
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && 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 (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
if (!set_mode(POSHOLD, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}

0
ArduSub/control_sport.cpp → ArduSub/control_transect.cpp
Unescape View File

0
ArduSub/control_loiter.cpp → ArduSub/control_velhold.cpp
Unescape View File

4
ArduSub/defines.h
Unescape View File

@ -102,13 +102,13 @@ enum control_mode_t { @@ -102,13 +102,13 @@ enum control_mode_t {
ALT_HOLD = 2, // manual airframe angle with automatic throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5, // automatic horizontal acceleration with automatic throttle
VELHOLD = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
SURFACE = 9, // automatic landing with horizontal position control
OF_LOITER = 10, // deprecated
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
TRANSECT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle

29
ArduSub/flight_mode.cpp
Unescape View File

@ -47,8 +47,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) @@ -47,8 +47,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
success = circle_init(ignore_checks);
break;
case LOITER:
success = loiter_init(ignore_checks);
case VELHOLD:
success = velhold_init(ignore_checks);
break;
case GUIDED:
@ -67,8 +67,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) @@ -67,8 +67,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
success = drift_init(ignore_checks);
break;
case SPORT:
success = sport_init(ignore_checks);
case TRANSECT:
success = transect_init(ignore_checks);
break;
case FLIP:
@ -164,8 +164,8 @@ void Sub::update_flight_mode() @@ -164,8 +164,8 @@ void Sub::update_flight_mode()
circle_run();
break;
case LOITER:
loiter_run();
case VELHOLD:
velhold_run();
break;
case GUIDED:
@ -184,8 +184,8 @@ void Sub::update_flight_mode() @@ -184,8 +184,8 @@ void Sub::update_flight_mode()
drift_run();
break;
case SPORT:
sport_run();
case TRANSECT:
transect_run();
break;
case FLIP:
@ -259,13 +259,14 @@ bool Sub::mode_requires_GPS(control_mode_t mode) { @@ -259,13 +259,14 @@ bool Sub::mode_requires_GPS(control_mode_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case LOITER:
case VELHOLD:
case RTL:
case CIRCLE:
case DRIFT:
case POSHOLD:
case BRAKE:
case THROW:
case TRANSECT:
return true;
default:
return false;
@ -291,7 +292,7 @@ bool Sub::mode_has_manual_throttle(control_mode_t mode) { @@ -291,7 +292,7 @@ bool Sub::mode_has_manual_throttle(control_mode_t mode) {
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) {
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
if (mode_has_manual_throttle(mode) || mode == VELHOLD || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == TRANSECT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
@ -336,8 +337,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) @@ -336,8 +337,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case GUIDED:
port->print("GUIDED");
break;
case LOITER:
port->print("LOITER");
case VELHOLD:
port->print("VELHOLD");
break;
case RTL:
port->print("RTL");
@ -354,8 +355,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) @@ -354,8 +355,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case DRIFT:
port->print("DRIFT");
break;
case SPORT:
port->print("SPORT");
case TRANSECT:
port->print("TRANSECT");
break;
case FLIP:
port->print("FLIP");

4
ArduSub/navigation.cpp
Unescape View File

@ -26,7 +26,7 @@ void Sub::calc_distance_and_bearing() @@ -26,7 +26,7 @@ void Sub::calc_distance_and_bearing()
void Sub::calc_wp_distance()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
if (control_mode == CIRCLE) {
wp_distance = wp_nav.get_loiter_distance_to_target();
}else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_distance = wp_nav.get_wp_distance_to_destination();
@ -39,7 +39,7 @@ void Sub::calc_wp_distance() @@ -39,7 +39,7 @@ void Sub::calc_wp_distance()
void Sub::calc_wp_bearing()
{
// get target from loiter or wpinav controller
if (control_mode == LOITER || control_mode == CIRCLE) {
if (control_mode == CIRCLE) {
wp_bearing = wp_nav.get_loiter_bearing_to_target();
} else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
wp_bearing = wp_nav.get_wp_bearing_to_destination();

34
ArduSub/takeoff.cpp
Unescape View File

@ -11,43 +11,13 @@ @@ -11,43 +11,13 @@
// must_nagivate is true if mode must also control horizontal position
bool Sub::current_mode_has_user_takeoff(bool must_navigate)
{
switch (control_mode) {
case GUIDED:
case LOITER:
case POSHOLD:
return true;
case ALT_HOLD:
case SPORT:
return !must_navigate;
default:
return false;
}
return false; // not supported in Sub
}
// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
switch(control_mode) {
case GUIDED:
if (guided_takeoff_start(takeoff_alt_cm)) {
set_auto_armed(true);
return true;
}
return false;
case LOITER:
case POSHOLD:
case ALT_HOLD:
case SPORT:
set_auto_armed(true);
takeoff_timer_start(takeoff_alt_cm);
return true;
default:
return false;
}
}
return false;
return false; // not supported in Sub
}
// start takeoff to specified altitude above home in centimeters

Write Preview
Loading…
Cancel
Save