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
*/
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) {
case 0:
case SimpleMode::NONE:
AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off");
break;
case 1:
case SimpleMode::SIMPLE:
AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on");
break;
case 2:
default:
case SimpleMode::SUPERSIMPLE:
// initialise super simple heading
update_super_simple_bearing(true);
AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
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;
// 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;
}
// mark radio frame as consumed
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)
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;
@ -572,7 +572,7 @@ void Copter::update_simple_mode(void)
void Copter::update_super_simple_bearing(bool force_update)
{
if (!force_update) {
if (ap.simple_mode != 2) {
if (simple_mode != SimpleMode::SUPERSIMPLE) {
return;
}
if (home_distance() < SUPER_SIMPLE_RADIUS) {

10
ArduCopter/Copter.h
View File

@ -344,7 +344,7 @@ private:
typedef union {
struct {
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_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
@ -423,6 +423,12 @@ private:
// SIMPLE Mode
// 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.
enum class SimpleMode {
NONE = 0,
SIMPLE = 1,
SUPERSIMPLE = 2,
} simple_mode;
float simple_cos_yaw;
float simple_sin_yaw;
int32_t super_simple_last_bearing;
@ -622,7 +628,7 @@ private:
// AP_State.cpp
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_gcs(bool b);
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
{
return copter.ap.simple_mode == 1;
return copter.simple_mode == Copter::SimpleMode::SIMPLE;
}
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)

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
// set Simple Mode using stored parameters from EEPROM
if (BIT_IS_SET(copter.g.super_simple, new_pos)) {
copter.set_simple_mode(2);
copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE);
} 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:
// low = simple mode off, middle or high position turns simple mode on
copter.set_simple_mode(ch_flag == AuxSwitchPos::HIGH ||
ch_flag == AuxSwitchPos::MIDDLE);
copter.set_simple_mode((ch_flag == AuxSwitchPos::LOW) ? Copter::SimpleMode::NONE : Copter::SimpleMode::SIMPLE);
break;
case AUX_FUNC::SUPERSIMPLE_MODE:
// low = simple mode off, middle = simple mode, high = super simple mode
// there is an assumption here that the ch_flag
// enumeration's values match the different sorts of
// simple mode used in set_simple_mode
copter.set_simple_mode((uint8_t)ch_flag);
case AUX_FUNC::SUPERSIMPLE_MODE: {
Copter::SimpleMode newmode = Copter::SimpleMode::NONE;
switch (ch_flag) {
case AuxSwitchPos::LOW:
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:
#if MODE_RTL_ENABLED == ENABLED

4
ArduCopter/toy_mode.cpp
View File

@ -579,11 +579,11 @@ void ToyMode::update()
break;
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;
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;
case ACTION_ARM_LAND_RTL: