Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: make SuperSimple type-safe

This commit is contained in:
Peter Barker 2020-06-15 23:05:09 +10:00 committed by Peter Barker
parent d35643a372
commit 0eb03ba7d5
6 changed files with 38 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/AP_State.cpp
View File

@ -19,27 +19,26 @@ void Copter::set_auto_armed(bool b)
* *
* @param [in] b 0:false or disabled, 1:true or SIMPLE, 2:SUPERSIMPLE * @param [in] b 0:false or disabled, 1:true or SIMPLE, 2:SUPERSIMPLE
*/ */
void Copter::set_simple_mode(uint8_t b) void Copter::set_simple_mode(SimpleMode b)
{ {
if (ap.simple_mode != b) { if (simple_mode != b) {
switch (b) { switch (b) {
case 0: case SimpleMode::NONE:
AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF); AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off"); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off");
break; break;
case 1: case SimpleMode::SIMPLE:
AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON); AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on"); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on");
break; break;
case 2: case SimpleMode::SUPERSIMPLE:
default:
// initialise super simple heading // initialise super simple heading
update_super_simple_bearing(true); update_super_simple_bearing(true);
AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON); AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on"); gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
break; break;
} }
ap.simple_mode = b; simple_mode = b;
} }
} }

6
ArduCopter/Copter.cpp
View File

@ -545,14 +545,14 @@ void Copter::update_simple_mode(void)
float rollx, pitchx; float rollx, pitchx;
// exit immediately if no new radio frame or not in simple mode // exit immediately if no new radio frame or not in simple mode
if (ap.simple_mode == 0 || !ap.new_radio_frame) { if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
return; return;
} }
// mark radio frame as consumed // mark radio frame as consumed
ap.new_radio_frame = false; ap.new_radio_frame = false;
if (ap.simple_mode == 1) { if (simple_mode == SimpleMode::SIMPLE) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing) // 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; 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; pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
@ -572,7 +572,7 @@ void Copter::update_simple_mode(void)
void Copter::update_super_simple_bearing(bool force_update) void Copter::update_super_simple_bearing(bool force_update)
{ {
if (!force_update) { if (!force_update) {
if (ap.simple_mode != 2) { if (simple_mode != SimpleMode::SUPERSIMPLE) {
return; return;
} }
if (home_distance() < SUPER_SIMPLE_RADIUS) { if (home_distance() < SUPER_SIMPLE_RADIUS) {

10
ArduCopter/Copter.h
View File

@ -344,7 +344,7 @@ private:
typedef union { typedef union {
struct { struct {
uint8_t unused1 : 1; // 0 uint8_t unused1 : 1; // 0
uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE uint8_t unused_was_simple_mode : 2; // 1,2
uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
@ -423,6 +423,12 @@ private:
// SIMPLE Mode // SIMPLE Mode
// Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming
// or in SuperSimple mode when the vehicle leaves a 20m radius from home. // or in SuperSimple mode when the vehicle leaves a 20m radius from home.
enum class SimpleMode {
NONE = 0,
SIMPLE = 1,
SUPERSIMPLE = 2,
} simple_mode;
float simple_cos_yaw; float simple_cos_yaw;
float simple_sin_yaw; float simple_sin_yaw;
int32_t super_simple_last_bearing; int32_t super_simple_last_bearing;
@ -622,7 +628,7 @@ private:
// AP_State.cpp // AP_State.cpp
void set_auto_armed(bool b); void set_auto_armed(bool b);
void set_simple_mode(uint8_t b); void set_simple_mode(SimpleMode b);
void set_failsafe_radio(bool b); void set_failsafe_radio(bool b);
void set_failsafe_gcs(bool b); void set_failsafe_gcs(bool b);
void update_using_interlock(); void update_using_interlock();

4
ArduCopter/GCS_Copter.cpp
View File

@ -14,12 +14,12 @@ const char* GCS_Copter::frame_string() const
bool GCS_Copter::simple_input_active() const bool GCS_Copter::simple_input_active() const
{ {
return copter.ap.simple_mode == 1; return copter.simple_mode == Copter::SimpleMode::SIMPLE;
} }
bool GCS_Copter::supersimple_input_active() const bool GCS_Copter::supersimple_input_active() const
{ {
return copter.ap.simple_mode == 2; return copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE;
} }
void GCS_Copter::update_vehicle_sensor_status_flags(void) void GCS_Copter::update_vehicle_sensor_status_flags(void)

27
ArduCopter/RC_Channel.cpp
View File

@ -39,9 +39,9 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
// if none of the Aux Switches are set to Simple or Super Simple Mode then // if none of the Aux Switches are set to Simple or Super Simple Mode then
// set Simple Mode using stored parameters from EEPROM // set Simple Mode using stored parameters from EEPROM
if (BIT_IS_SET(copter.g.super_simple, new_pos)) { if (BIT_IS_SET(copter.g.super_simple, new_pos)) {
copter.set_simple_mode(2); copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE);
} else { } else {
copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos)); copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos) ? Copter::SimpleMode::SIMPLE : Copter::SimpleMode::NONE);
} }
} }
} }
@ -170,17 +170,24 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
case AUX_FUNC::SIMPLE_MODE: case AUX_FUNC::SIMPLE_MODE:
// low = simple mode off, middle or high position turns simple mode on // low = simple mode off, middle or high position turns simple mode on
copter.set_simple_mode(ch_flag == AuxSwitchPos::HIGH || copter.set_simple_mode((ch_flag == AuxSwitchPos::LOW) ? Copter::SimpleMode::NONE : Copter::SimpleMode::SIMPLE);
ch_flag == AuxSwitchPos::MIDDLE);
break; break;
case AUX_FUNC::SUPERSIMPLE_MODE: case AUX_FUNC::SUPERSIMPLE_MODE: {
// low = simple mode off, middle = simple mode, high = super simple mode Copter::SimpleMode newmode = Copter::SimpleMode::NONE;
// there is an assumption here that the ch_flag switch (ch_flag) {
// enumeration's values match the different sorts of case AuxSwitchPos::LOW:
// simple mode used in set_simple_mode
copter.set_simple_mode((uint8_t)ch_flag);
break; break;
case AuxSwitchPos::MIDDLE:
newmode = Copter::SimpleMode::SIMPLE;
break;
case AuxSwitchPos::HIGH:
newmode = Copter::SimpleMode::SUPERSIMPLE;
break;
}
copter.set_simple_mode(newmode);
break;
}
case AUX_FUNC::RTL: case AUX_FUNC::RTL:
#if MODE_RTL_ENABLED == ENABLED #if MODE_RTL_ENABLED == ENABLED

4
ArduCopter/toy_mode.cpp
View File

@ -579,11 +579,11 @@ void ToyMode::update()
break; break;
case ACTION_TOGGLE_SIMPLE: case ACTION_TOGGLE_SIMPLE:
copter.set_simple_mode(copter.ap.simple_mode?0:1); copter.set_simple_mode(bool(copter.simple_mode)?Copter::SimpleMode::NONE:Copter::SimpleMode::SIMPLE);
break; break;
case ACTION_TOGGLE_SSIMPLE: case ACTION_TOGGLE_SSIMPLE:
copter.set_simple_mode(copter.ap.simple_mode?0:2); copter.set_simple_mode(bool(copter.simple_mode)?Copter::SimpleMode::NONE:Copter::SimpleMode::SUPERSIMPLE);
break; break;
case ACTION_ARM_LAND_RTL: case ACTION_ARM_LAND_RTL: