mirror of https://github.com/ArduPilot/ardupilot
Rover: add FRAME_CLASS to differentiate boats from rovers
This commit is contained in:
parent
4289190654
commit
aa38239629
|
@ -535,6 +535,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("RTL_SPEED", 15, ParametersG2, rtl_speed, 0.0f),
|
||||
|
||||
// @Param: FRAME_CLASS
|
||||
// @DisplayName: Frame Class
|
||||
// @Description: Frame Class
|
||||
// @Values: 0:Undefined,1:Rover,2:Boat
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("FRAME_CLASS", 16, ParametersG2, frame_class, 1),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
@ -326,6 +326,9 @@ public:
|
|||
// default speeds for auto, rtl
|
||||
AP_Float wp_speed;
|
||||
AP_Float rtl_speed;
|
||||
|
||||
// frame class for vehicle
|
||||
AP_Int8 frame_class;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
@ -572,6 +572,7 @@ private:
|
|||
bool arm_motors(AP_Arming::ArmingMethod method);
|
||||
bool disarm_motors(void);
|
||||
void smart_rtl_update();
|
||||
bool is_boat() const;
|
||||
|
||||
// test.cpp
|
||||
void print_hit_enter();
|
||||
|
|
|
@ -139,5 +139,12 @@ enum pilot_steer_type_t {
|
|||
PILOT_STEER_TYPE_DIR_UNCHANGED_WHEN_REVERSING = 3,
|
||||
};
|
||||
|
||||
// frame class enum used for FRAME_CLASS parameter
|
||||
enum frame_class {
|
||||
FRAME_UNDEFINED = 0,
|
||||
FRAME_ROVER = 1,
|
||||
FRAME_BOAT = 2
|
||||
};
|
||||
|
||||
#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked
|
||||
#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled
|
||||
|
|
|
@ -367,3 +367,10 @@ void Rover::smart_rtl_update()
|
|||
const bool save_position = hal.util->get_soft_armed() && (control_mode != &mode_smartrtl);
|
||||
mode_smartrtl.save_position(save_position);
|
||||
}
|
||||
|
||||
// returns true if vehicle is a boat
|
||||
// this affects whether the vehicle tries to maintain position after reaching waypoints
|
||||
bool Rover::is_boat() const
|
||||
{
|
||||
return ((enum frame_class)g2.frame_class.get() == FRAME_BOAT);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue