Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove simple mode

Browse Source
This commit is contained in:
Jacob Walser 2017-03-22 23:39:37 -04:00
parent 2d603c1cba
commit 6886952438
12 changed files with 1 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduSub/AP_State.cpp
View File

@ -37,26 +37,6 @@ void Sub::set_auto_armed(bool b)
}
}
// ---------------------------------------------
void Sub::set_simple_mode(uint8_t b)
{
if (ap.simple_mode != b) {
if (b == 0) {
Log_Write_Event(DATA_SET_SIMPLE_OFF);
gcs().send_statustext(MAV_SEVERITY_INFO, 0XFF, "SIMPLE mode off");
} else if (b == 1) {
Log_Write_Event(DATA_SET_SIMPLE_ON);
gcs().send_statustext(MAV_SEVERITY_INFO, 0XFF, "SIMPLE mode on");
} else {
// initialise super simple heading
update_super_simple_bearing(true);
Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
gcs().send_statustext(MAV_SEVERITY_INFO, 0XFF, "SUPERSIMPLE mode on");
}
ap.simple_mode = b;
}
}
// ---------------------------------------------
void Sub::set_failsafe_battery(bool b)
{

58
ArduSub/ArduSub.cpp
View File

@ -435,64 +435,6 @@ void Sub::update_GPS(void)
}
}
void Sub::init_simple_bearing()
{
// capture current cos_yaw and sin_yaw values
simple_cos_yaw = ahrs.cos_yaw();
simple_sin_yaw = ahrs.sin_yaw();
// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);
super_simple_cos_yaw = simple_cos_yaw;
super_simple_sin_yaw = simple_sin_yaw;
// log the simple bearing to dataflash
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
}
}
// update_simple_mode - rotates pilot input if we are in simple mode
void Sub::update_simple_mode(void)
{
float rollx, pitchx;
// exit immediately if no new radio frame or not in simple mode
if (ap.simple_mode == 0) {
return;
}
if (ap.simple_mode == 1) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing)
rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
} else {
// rotate roll, pitch input by -super simple heading (reverse of heading to home)
rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
}
// rotate roll, pitch input from north facing to vehicle's perspective
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
}
// update_super_simple_bearing - adjusts simple bearing based on location
// should be called after home_bearing has been updated
void Sub::update_super_simple_bearing(bool force_update)
{
// check if we are in super simple mode and at least 10m from home
if (force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) {
// check the bearing to home has changed by at least 5 degrees
if (labs(super_simple_last_bearing - home_bearing) > 500) {
super_simple_last_bearing = home_bearing;
float angle_rad = radians((super_simple_last_bearing+18000)/100);
super_simple_cos_yaw = cosf(angle_rad);
super_simple_sin_yaw = sinf(angle_rad);
}
}
}
void Sub::read_AHRS(void)
{
// Perform IMU calculations and get attitude info

5
ArduSub/Sub.cpp
View File

@ -37,11 +37,6 @@ Sub::Sub(void) :
auto_mode(Auto_WP),
guided_mode(Guided_WP),
circle_pilot_yaw_override(false),
simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f),
super_simple_last_bearing(0),
super_simple_cos_yaw(1.0),
super_simple_sin_yaw(0.0f),
initial_armed_bearing(0),
desired_climb_rate(0),
loiter_time_max(0),

14
ArduSub/Sub.h
View File

@ -233,7 +233,6 @@ private:
// Documentation of Globals:
union {
struct {
uint8_t simple_mode : 2; // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
uint8_t pre_arm_check : 1; // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // true if dataflash logging has started
@ -319,15 +318,6 @@ private:
// Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
// SIMPLE Mode
// Used to track the orientation of the Sub for Simple mode. This value is reset at each arming
// or in SuperSimple mode when the Sub leaves a 20m radius from home.
float simple_cos_yaw;
float simple_sin_yaw;
int32_t super_simple_last_bearing;
float super_simple_cos_yaw;
float super_simple_sin_yaw;
// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable
int32_t initial_armed_bearing;
@ -489,15 +479,11 @@ private:
void one_hz_loop();
void update_GPS(void);
void update_turn_counter();
void init_simple_bearing();
void update_simple_mode(void);
void update_super_simple_bearing(bool force_update);
void read_AHRS(void);
void update_altitude();
void set_home_state(enum HomeState new_home_state);
bool home_is_set();
void set_auto_armed(bool b);
void set_simple_mode(uint8_t b);
void set_failsafe_battery(bool b);
void set_pre_arm_check(bool b);
void set_motor_emergency_stop(bool b);

3
ArduSub/control_althold.cpp
View File

@ -50,9 +50,6 @@ void Sub::althold_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());

1
ArduSub/control_auto.cpp
View File

@ -584,6 +584,7 @@ float Sub::get_auto_heading(void)
break;
case AUTO_YAW_CORRECT_XTRACK: {
// TODO return current yaw if not in appropriate mode
// Bearing of current track (centidegrees)
float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());

3
ArduSub/control_poshold.cpp
View File

@ -50,9 +50,6 @@ void Sub::poshold_run()
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

3
ArduSub/control_stabilize.cpp
View File

@ -28,9 +28,6 @@ void Sub::stabilize_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// 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, aparm.angle_max);

3
ArduSub/control_velhold.cpp
View File

@ -86,9 +86,6 @@ void Sub::velhold_run()
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

4
ArduSub/motors.cpp
View File

@ -70,10 +70,6 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
gcs_send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (ap.home_state == HOME_UNSET) {

3
ArduSub/navigation.cpp
View File

@ -55,9 +55,6 @@ void Sub::calc_home_distance_and_bearing()
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}
}

1
ArduSub/setup.cpp
View File

@ -363,7 +363,6 @@ void Sub::report_flight_modes()
print_divider();
for (int16_t i = 0; i < 6; i++) {
// print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(g.simple_modes, i));
print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(0, i));
}
print_blanks(2);