mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Rover: Implementation of the ARMING functionality
This commit is contained in:
parent
7edf8c0e0a
commit
3636b53313
@ -96,10 +96,6 @@ void Rover::setup()
|
||||
|
||||
notify.init(false);
|
||||
|
||||
// rover does not use arming nor pre-arm checks
|
||||
AP_Notify::flags.armed = true;
|
||||
AP_Notify::flags.pre_arm_check = true;
|
||||
AP_Notify::flags.pre_arm_gps_check = true;
|
||||
AP_Notify::flags.failsafe_battery = false;
|
||||
|
||||
rssi.init();
|
||||
@ -147,7 +143,8 @@ void Rover::loop()
|
||||
// update AHRS system
|
||||
void Rover::ahrs_update()
|
||||
{
|
||||
hal.util->set_soft_armed(hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
// update hil before AHRS update
|
||||
@ -292,6 +289,11 @@ void Rover::one_second_loop(void)
|
||||
// cope with changes to aux functions
|
||||
update_aux();
|
||||
|
||||
// update notify flags
|
||||
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
|
||||
AP_Notify::flags.pre_arm_gps_check = true;
|
||||
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
|
||||
|
||||
// cope with changes to mavlink system ID
|
||||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
|
||||
|
@ -1075,6 +1075,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
}
|
||||
break;
|
||||
|
||||
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::MAVLINK)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
} else if (is_zero(packet.param1)) {
|
||||
if (rover.disarm_motors()) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
} else {
|
||||
result = MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_GET_HOME_POSITION:
|
||||
send_home(rover.ahrs.get_home());
|
||||
break;
|
||||
|
@ -337,6 +337,23 @@ void Rover::Log_Write_Current()
|
||||
DataFlash.Log_Write_Power();
|
||||
}
|
||||
|
||||
struct PACKED log_Arm_Disarm {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t arm_state;
|
||||
uint16_t arm_checks;
|
||||
};
|
||||
|
||||
void Rover::Log_Arm_Disarm() {
|
||||
struct log_Arm_Disarm pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
|
||||
time_us : hal.scheduler->micros64(),
|
||||
arm_state : arming.is_armed(),
|
||||
arm_checks : arming.get_enabled_checks()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void Rover::Log_Write_RC(void)
|
||||
{
|
||||
DataFlash.Log_Write_RCIN();
|
||||
@ -380,6 +397,8 @@ const LogStructure Rover::log_structure[] = {
|
||||
"NTUN", "QHfHHb", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
|
||||
{ LOG_SONAR_MSG, sizeof(log_Sonar),
|
||||
"SONR", "QfHHHbHCb", "TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
|
||||
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
|
||||
"ARM", "QBH", "TimeUS,ArmState,ArmChecks" },
|
||||
{ LOG_STEERING_MSG, sizeof(log_Steering),
|
||||
"STER", "Qff", "TimeUS,Demanded,Achieved" },
|
||||
};
|
||||
@ -402,6 +421,8 @@ void Rover::log_init(void)
|
||||
if (g.log_bitmask != 0) {
|
||||
start_logging();
|
||||
}
|
||||
|
||||
arming.set_logging_available(DataFlash.CardInserted());
|
||||
}
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
|
@ -496,6 +496,10 @@ const AP_Param::Info Rover::var_info[] = {
|
||||
GOBJECT(camera_mount, "MNT", AP_Mount),
|
||||
#endif
|
||||
|
||||
// @Group: ARMING_
|
||||
// @Path: ../libraries/AP_Arming/AP_Arming.cpp
|
||||
GOBJECT(arming, "ARMING_", AP_Arming),
|
||||
|
||||
// @Group: BATT
|
||||
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
|
||||
GOBJECT(battery, "BATT", AP_BattMonitor),
|
||||
|
@ -58,6 +58,9 @@ public:
|
||||
// 97: RSSI
|
||||
k_param_rssi = 97,
|
||||
|
||||
// 100: Arming parameters
|
||||
k_param_arming = 100,
|
||||
|
||||
// 110: Telemetry control
|
||||
//
|
||||
k_param_gcs0 = 110, // stream rates for uartA
|
||||
|
@ -71,6 +71,7 @@
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
|
||||
|
||||
#include <AP_Arming/AP_Arming.h>
|
||||
#include "compat.h"
|
||||
|
||||
#include <AP_Notify/AP_Notify.h> // Notify library
|
||||
@ -96,6 +97,7 @@ class Rover : public AP_HAL::HAL::Callbacks {
|
||||
public:
|
||||
friend class GCS_MAVLINK;
|
||||
friend class Parameters;
|
||||
friend class AP_Arming;
|
||||
|
||||
Rover(void);
|
||||
|
||||
@ -154,6 +156,9 @@ private:
|
||||
AP_AHRS_DCM ahrs {ins, barometer, gps};
|
||||
#endif
|
||||
|
||||
// Arming/Disarming mangement class
|
||||
AP_Arming arming {ahrs, barometer, compass, home_is_set};
|
||||
|
||||
AP_L1_Control L1_controller;
|
||||
|
||||
// selected navigation controller
|
||||
@ -363,6 +368,14 @@ private:
|
||||
|
||||
float distance_past_wp; // record the distance we have gone past the wp
|
||||
|
||||
// time that rudder/steering arming has been running
|
||||
uint32_t rudder_arm_timer;
|
||||
|
||||
// true if we are in an auto-throttle mode, which means
|
||||
// we need to run the speed/height controller
|
||||
bool auto_throttle_mode;
|
||||
|
||||
|
||||
private:
|
||||
// private member functions
|
||||
void ahrs_update();
|
||||
@ -418,6 +431,7 @@ private:
|
||||
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
|
||||
void log_init(void);
|
||||
void start_logging() ;
|
||||
void Log_Arm_Disarm();
|
||||
|
||||
void load_parameters(void);
|
||||
void throttle_slew_limit(int16_t last_throttle);
|
||||
@ -504,6 +518,12 @@ private:
|
||||
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||
void init_capabilities(void);
|
||||
void rudder_arm_disarm_check();
|
||||
void change_arm_state(void);
|
||||
bool disarm_motors(void);
|
||||
bool arm_motors(AP_Arming::ArmingMethod method);
|
||||
bool is_moving();
|
||||
void update_home();
|
||||
|
||||
public:
|
||||
bool print_log_menu(void);
|
||||
|
@ -248,6 +248,10 @@ void Rover::set_servos(void)
|
||||
channel_throttle->servo_out = 0;
|
||||
}
|
||||
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
channel_throttle->servo_out = 0;
|
||||
}
|
||||
|
||||
// convert 0 to 100% into PWM
|
||||
channel_throttle->calc_pwm();
|
||||
|
||||
@ -277,6 +281,25 @@ void Rover::set_servos(void)
|
||||
channel_throttle->calc_pwm();
|
||||
}
|
||||
|
||||
if (!arming.is_armed()) {
|
||||
//Some ESCs get noisy (beep error msgs) if PWM == 0.
|
||||
//This little segment aims to avoid this.
|
||||
switch (arming.arming_required()) {
|
||||
case AP_Arming::NO:
|
||||
//keep existing behavior: do nothing to radio_out
|
||||
//(don't disarm throttle channel even if AP_Arming class is)
|
||||
break;
|
||||
|
||||
case AP_Arming::YES_ZERO_PWM:
|
||||
channel_throttle->radio_out = 0;
|
||||
break;
|
||||
|
||||
case AP_Arming::YES_MIN_PWM:
|
||||
default:
|
||||
channel_throttle->radio_out = channel_throttle->radio_trim;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
||||
// send values to the PWM timers for output
|
||||
|
@ -67,6 +67,7 @@ enum mode {
|
||||
#define LOG_PERFORMANCE_MSG 0x03
|
||||
#define LOG_STARTUP_MSG 0x06
|
||||
#define LOG_SONAR_MSG 0x07
|
||||
#define LOG_ARM_DISARM_MSG 0x08
|
||||
#define LOG_STEERING_MSG 0x0D
|
||||
|
||||
#define TYPE_AIRSTART_MSG 0x00
|
||||
@ -88,6 +89,7 @@ enum mode {
|
||||
#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_WHEN_DISARMED (1UL<<16)
|
||||
#define MASK_LOG_IMU_RAW (1UL<<19)
|
||||
|
||||
|
@ -46,4 +46,5 @@ LIBRARIES += AP_OpticalFlow
|
||||
LIBRARIES += AP_RSSI
|
||||
LIBRARIES += AP_Declination
|
||||
LIBRARIES += AP_RPM
|
||||
LIBRARIES += AP_Arming
|
||||
|
||||
|
@ -13,7 +13,12 @@ void Rover::set_control_channels(void)
|
||||
|
||||
// set rc channel ranges
|
||||
channel_steer->set_angle(SERVO_MAX);
|
||||
channel_throttle->set_angle(100);
|
||||
channel_throttle->set_angle(100);
|
||||
|
||||
// For a rover safety is TRIM throttle
|
||||
if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
|
||||
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_trim);
|
||||
}
|
||||
|
||||
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
|
||||
// take a proportion of speed.
|
||||
@ -34,10 +39,81 @@ void Rover::init_rc_out()
|
||||
{
|
||||
RC_Channel::rc_channel(CH_1)->enable_out();
|
||||
RC_Channel::rc_channel(CH_3)->enable_out();
|
||||
|
||||
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
|
||||
channel_throttle->enable_out();
|
||||
}
|
||||
|
||||
RC_Channel::output_trim_all();
|
||||
|
||||
// setup PWM values to send if the FMU firmware dies
|
||||
RC_Channel::setup_failsafe_trim_all();
|
||||
|
||||
// setup PX4 to output the min throttle when safety off if arming
|
||||
// is setup for min on disarm
|
||||
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
|
||||
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_trim);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
check for driver input on rudder/steering stick for arming/disarming
|
||||
*/
|
||||
void Rover::rudder_arm_disarm_check()
|
||||
{
|
||||
// In Rover we need to check that its set to the throttle trim and within the DZ
|
||||
// if throttle is not within trim dz, then pilot cannot rudder arm/disarm
|
||||
if (!channel_throttle->in_trim_dz()) {
|
||||
rudder_arm_timer = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// if not in a manual throttle mode then disallow rudder arming/disarming
|
||||
if (auto_throttle_mode) {
|
||||
rudder_arm_timer = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!arming.is_armed()) {
|
||||
// when not armed, full right rudder starts arming counter
|
||||
if (channel_steer->control_in > 4000) {
|
||||
uint32_t now = millis();
|
||||
|
||||
if (rudder_arm_timer == 0 ||
|
||||
now - rudder_arm_timer < 3000) {
|
||||
|
||||
if (rudder_arm_timer == 0) {
|
||||
rudder_arm_timer = now;
|
||||
}
|
||||
} else {
|
||||
//time to arm!
|
||||
arm_motors(AP_Arming::RUDDER);
|
||||
rudder_arm_timer = 0;
|
||||
}
|
||||
} else {
|
||||
// not at full right rudder
|
||||
rudder_arm_timer = 0;
|
||||
}
|
||||
} else if (!is_moving()) {
|
||||
// when armed and not moving, full left rudder starts disarming counter
|
||||
if (channel_steer->control_in < -4000) {
|
||||
uint32_t now = millis();
|
||||
|
||||
if (rudder_arm_timer == 0 ||
|
||||
now - rudder_arm_timer < 3000) {
|
||||
if (rudder_arm_timer == 0) {
|
||||
rudder_arm_timer = now;
|
||||
}
|
||||
} else {
|
||||
//time to disarm!
|
||||
disarm_motors();
|
||||
rudder_arm_timer = 0;
|
||||
}
|
||||
} else {
|
||||
// not at full left rudder
|
||||
rudder_arm_timer = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Rover::read_radio()
|
||||
@ -95,6 +171,9 @@ void Rover::read_radio()
|
||||
channel_steer->set_pwm(steer);
|
||||
channel_throttle->set_pwm(thr);
|
||||
}
|
||||
|
||||
rudder_arm_disarm_check();
|
||||
|
||||
}
|
||||
|
||||
void Rover::control_failsafe(uint16_t pwm)
|
||||
|
@ -285,18 +285,22 @@ void Rover::set_mode(enum mode mode)
|
||||
case HOLD:
|
||||
case LEARNING:
|
||||
case STEERING:
|
||||
auto_throttle_mode = false;
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
auto_throttle_mode = true;
|
||||
rtl_complete = false;
|
||||
restart_nav();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
auto_throttle_mode = true;
|
||||
do_RTL();
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
auto_throttle_mode = true;
|
||||
rtl_complete = false;
|
||||
/*
|
||||
when entering guided mode we set the target as the current
|
||||
@ -307,6 +311,7 @@ void Rover::set_mode(enum mode mode)
|
||||
break;
|
||||
|
||||
default:
|
||||
auto_throttle_mode = true;
|
||||
do_RTL();
|
||||
break;
|
||||
}
|
||||
@ -491,3 +496,51 @@ void Rover::frsky_telemetry_send(void)
|
||||
frsky_telemetry.send_frames((uint8_t)control_mode);
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
update AHRS soft arm state and log as needed
|
||||
*/
|
||||
void Rover::change_arm_state(void)
|
||||
{
|
||||
Log_Arm_Disarm();
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
}
|
||||
|
||||
/*
|
||||
arm motors
|
||||
*/
|
||||
bool Rover::arm_motors(AP_Arming::ArmingMethod method)
|
||||
{
|
||||
if (!arming.arm(method)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// only log if arming was successful
|
||||
channel_throttle->enable_out();
|
||||
|
||||
change_arm_state();
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
disarm motors
|
||||
*/
|
||||
bool Rover::disarm_motors(void)
|
||||
{
|
||||
if (!arming.disarm()) {
|
||||
return false;
|
||||
}
|
||||
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
|
||||
channel_throttle->disable_out();
|
||||
}
|
||||
if (control_mode != AUTO) {
|
||||
// reset the mission on disarm if we are not in auto
|
||||
mission.reset();
|
||||
}
|
||||
|
||||
//only log if disarming was successful
|
||||
change_arm_state();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user