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

Rover: move arming-related functionality into parent classes 

Logging moves up
arming via mavlink moves up
arming via switch moves up
This commit is contained in:
Peter Barker 2019-05-03 22:32:42 +10:00 committed by Randy Mackay
parent c38e5ba4fc
commit 9851ec1ddb
13 changed files with 29 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/APMrover2.cpp
View File

@ -141,17 +141,17 @@ void Rover::loop()
G_Dt = scheduler.get_last_loop_time_s();
}
void Rover::update_soft_armed()
void AP_Arming_Rover::update_soft_armed()
{
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->set_soft_armed(is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
logger.set_vehicle_armed(hal.util->get_soft_armed());
AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
}
// update AHRS system
void Rover::ahrs_update()
{
update_soft_armed();
arming.update_soft_armed();
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before AHRS update

6
APMrover2/AP_Arming.h
View File

@ -20,9 +20,15 @@ public:
bool pre_arm_rc_checks(const bool display_failure);
bool gps_checks(bool display_failure) override;
bool disarm() override;
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
void update_soft_armed();
protected:
bool proximity_check(bool report);
private:
void change_arm_state(void);
};

17
APMrover2/GCS_Mavlink.cpp
View File

@ -614,23 +614,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_FAILED;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1, 1.0f)) {
// run pre_arm_checks and arm_checks and display failures
if (rover.arm_motors(AP_Arming::Method::MAVLINK)) {
return MAV_RESULT_ACCEPTED;
} else {
return MAV_RESULT_FAILED;
}
} else if (is_zero(packet.param1)) {
if (rover.disarm_motors()) {
return MAV_RESULT_ACCEPTED;
} else {
return MAV_RESULT_FAILED;
}
}
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_DO_CHANGE_SPEED:
// param1 : unused
// param2 : new speed in m/s

21
APMrover2/Log.cpp
View File

@ -4,24 +4,6 @@
#if LOGGING_ENABLED == ENABLED
struct PACKED log_Arm_Disarm {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t arm_state;
uint16_t arm_checks;
};
void Rover::Log_Write_Arm_Disarm()
{
struct log_Arm_Disarm pkt = {
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
time_us : AP_HAL::micros64(),
arm_state : arming.is_armed(),
arm_checks : arming.get_enabled_checks()
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
// Write an attitude packet
void Rover::Log_Write_Attitude()
{
@ -310,8 +292,6 @@ const LogStructure Rover::log_structure[] = {
"NTUN", "QfHHHf", "TimeUS,WpDist,WpBrg,DesYaw,Yaw,XTrack", "smdddm", "F0BBB0" },
{ LOG_RANGEFINDER_MSG, sizeof(log_Rangefinder),
"RGFD", "QfHHHbHCb", "TimeUS,LatAcc,R1Dist,R2Dist,DCnt,TAng,TTim,Spd,Thr", "somm-hsm-", "F0BB-0CB-" },
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
"ARM", "QBH", "TimeUS,ArmState,ArmChecks", "s--", "F--" },
{ LOG_STEERING_MSG, sizeof(log_Steering),
"STER", "Qhfffff", "TimeUS,SteerIn,SteerOut,DesLatAcc,LatAcc,DesTurnRate,TurnRate", "s--ookk", "F--0000" },
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
@ -326,7 +306,6 @@ void Rover::log_init(void)
#else // LOGGING_ENABLED
// dummy functions
void Rover::Log_Write_Arm_Disarm() {}
void Rover::Log_Write_Attitude() {}
void Rover::Log_Write_Depth() {}
void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}

9
APMrover2/RC_Channel.cpp
View File

@ -134,15 +134,6 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
}
break;
// arm or disarm the motors
case AUX_FUNC::ARMDISARM:
if (ch_flag == HIGH) {
rover.arm_motors(AP_Arming::Method::RUDDER);
} else if (ch_flag == LOW) {
rover.disarm_motors();
}
break;
// set mode to Manual
case AUX_FUNC::MANUAL:
do_aux_function_change_mode(rover.mode_manual, ch_flag);

5
APMrover2/Rover.h
View File

@ -406,7 +406,6 @@ private:
void send_wheel_encoder_distance(mavlink_channel_t chan);
// Log.cpp
void Log_Write_Arm_Disarm();
void Log_Write_Attitude();
void Log_Write_Depth();
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
@ -462,9 +461,6 @@ private:
void notify_mode(const Mode *new_mode);
uint8_t check_digital_pin(uint8_t pin);
bool should_log(uint32_t mask);
void change_arm_state(void);
bool arm_motors(AP_Arming::Method method);
bool disarm_motors(void);
bool is_boat() const;
enum Failsafe_Action {
@ -492,7 +488,6 @@ private:
public:
void mavlink_delay_cb();
void failsafe_check();
void update_soft_armed();
// Motor test
void motor_test_output();
bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value);

2
APMrover2/afs_rover.cpp
View File

@ -18,7 +18,7 @@ AP_AdvancedFailsafe_Rover::AP_AdvancedFailsafe_Rover(AP_Mission &_mission, const
void AP_AdvancedFailsafe_Rover::terminate_vehicle(void)
{
// disarm as well
rover.disarm_motors();
AP::arming().disarm();
// Set to HOLD mode
rover.set_mode(rover.mode_hold, MODE_REASON_CRASH_FAILSAFE);

4
APMrover2/crash_check.cpp
View File

@ -51,14 +51,14 @@ void Rover::crash_check()
if (is_balancebot()) {
// send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming");
disarm_motors();
arming.disarm();
} else {
// send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
// change mode to hold and disarm
set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
disarm_motors();
arming.disarm();
}
}
}

3
APMrover2/defines.h
View File

@ -26,7 +26,6 @@
#define LOG_NTUN_MSG 0x02
#define LOG_STARTUP_MSG 0x06
#define LOG_RANGEFINDER_MSG 0x07
#define LOG_ARM_DISARM_MSG 0x08
#define LOG_STEERING_MSG 0x0D
#define LOG_GUIDEDTARGET_MSG 0x0E
@ -48,7 +47,7 @@
#define MASK_LOG_CAMERA (1<<12)
#define MASK_LOG_STEERING (1<<13)
#define MASK_LOG_RC (1<<14)
#define MASK_LOG_ARM_DISARM (1<<15)
// #define MASK_LOG_ARM_DISARM (1<<15)
#define MASK_LOG_IMU_RAW (1UL<<19)
// for mavlink SET_POSITION_TARGET messages

4
APMrover2/failsafe.cpp
View File

@ -35,7 +35,7 @@ void Rover::failsafe_check()
// To-Do: log error
if (arming.is_armed()) {
// disarm motors
disarm_motors();
arming.disarm();
}
}
}
@ -135,7 +135,7 @@ void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
snprintf(battery_type_str, 17, "%s battery", type_str);
g2.afs.gcs_terminate(true, battery_type_str);
#else
disarm_motors();
arming.disarm();
#endif // ADVANCED_FAILSAFE == ENABLED
break;
}

4
APMrover2/motor_test.cpp
View File

@ -126,7 +126,7 @@ MAV_RESULT Rover::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor
// arm motors
if (!arming.is_armed()) {
arm_motors(AP_Arming::Method::MOTORTEST);
arming.arm(AP_Arming::Method::MOTORTEST);
}
// disable failsafes
@ -161,7 +161,7 @@ void Rover::motor_test_stop()
}
// disarm motors
disarm_motors();
AP::arming().disarm();
// reset timeout
motor_test_start_ms = 0;

4
APMrover2/radio.cpp
View File

@ -72,7 +72,7 @@ void Rover::rudder_arm_disarm_check()
}
} else {
// time to arm!
arm_motors(AP_Arming::Method::RUDDER);
arming.arm(AP_Arming::Method::RUDDER);
rudder_arm_timer = 0;
}
} else {
@ -91,7 +91,7 @@ void Rover::rudder_arm_disarm_check()
}
} else {
// time to disarm!
disarm_motors();
arming.disarm();
rudder_arm_timer = 0;
}
} else {

17
APMrover2/system.cpp
View File

@ -317,7 +317,7 @@ bool Rover::should_log(uint32_t mask)
/*
update AHRS soft arm state and log as needed
*/
void Rover::change_arm_state(void)
void AP_Arming_Rover::change_arm_state(void)
{
Log_Write_Arm_Disarm();
update_soft_armed();
@ -326,15 +326,15 @@ void Rover::change_arm_state(void)
/*
arm motors
*/
bool Rover::arm_motors(AP_Arming::Method method)
bool AP_Arming_Rover::arm(AP_Arming::Method method, const bool do_arming_checks)
{
if (!arming.arm(method)) {
if (!AP_Arming::arm(method, do_arming_checks)) {
AP_Notify::events.arming_failed = true;
return false;
}
// Set the SmartRTL home location. If activated, SmartRTL will ultimately try to land at this point
g2.smart_rtl.set_home(true);
rover.g2.smart_rtl.set_home(true);
// initialize simple mode heading
rover.mode_simple.init_heading();
@ -343,20 +343,21 @@ bool Rover::arm_motors(AP_Arming::Method method)
rover.g2.windvane.record_home_heading();
change_arm_state();
return true;
}
/*
disarm motors
*/
bool Rover::disarm_motors(void)
bool AP_Arming_Rover::disarm(void)
{
if (!arming.disarm()) {
if (!AP_Arming::disarm()) {
return false;
}
if (control_mode != &mode_auto) {
if (rover.control_mode != &rover.mode_auto) {
// reset the mission on disarm if we are not in auto
mode_auto.mission.reset();
rover.mode_auto.mission.reset();
}
// only log if disarming was successful