mirror of https://github.com/ArduPilot/ardupilot
Copter: add GUIDED_OPTIONS to allow arming from transmitter
This commit is contained in:
parent
b0363a01b0
commit
eb3aca7acf
|
@ -985,6 +985,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_GROUPINFO("AUTO_OPTIONS", 40, ParametersG2, auto_options, 0),
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
// @Param: GUIDED_OPTIONS
|
||||
// @DisplayName: Guided mode options
|
||||
// @Description: Options that can be applied to change guided mode behaviour
|
||||
// @Bitmask: 0:Allow Arming from Transmitter
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("GUIDED_OPTIONS", 41, ParametersG2, guided_options, 0),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
@ -623,6 +623,10 @@ public:
|
|||
AP_Int32 auto_options;
|
||||
#endif
|
||||
|
||||
#if MODE_GUIDED_ENABLED == ENABLED
|
||||
AP_Int32 guided_options;
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
@ -794,7 +794,7 @@ public:
|
|||
|
||||
bool requires_GPS() const override { return true; }
|
||||
bool has_manual_throttle() const override { return false; }
|
||||
bool allows_arming(bool from_gcs) const override { return from_gcs; }
|
||||
bool allows_arming(bool from_gcs) const override;
|
||||
bool is_autopilot() const override { return true; }
|
||||
bool has_user_takeoff(bool must_navigate) const override { return true; }
|
||||
bool in_guided_mode() const override { return true; }
|
||||
|
@ -833,6 +833,11 @@ protected:
|
|||
|
||||
private:
|
||||
|
||||
// enum for GUIDED_OPTIONS parameter
|
||||
enum class Options : int32_t {
|
||||
AllowArmingFromTX = (1 << 0U),
|
||||
};
|
||||
|
||||
void pos_control_start();
|
||||
void vel_control_start();
|
||||
void posvel_control_start();
|
||||
|
|
|
@ -384,6 +384,17 @@ void ModeGuided::run()
|
|||
}
|
||||
}
|
||||
|
||||
bool ModeGuided::allows_arming(bool from_gcs) const
|
||||
{
|
||||
// always allow arming from the ground station
|
||||
if (from_gcs) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// optionally allow arming from the transmitter
|
||||
return (copter.g2.guided_options & (int32_t)Options::AllowArmingFromTX) != 0;
|
||||
};
|
||||
|
||||
// guided_takeoff_run - takeoff in guided mode
|
||||
// called by guided_run at 100hz or more
|
||||
void ModeGuided::takeoff_run()
|
||||
|
|
Loading…
Reference in New Issue