Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Implementation of the ARMING functionality

Browse Source
This commit is contained in:
Grant Morphett 2015-10-30 16:56:41 +11:00 committed by Andrew Tridgell
parent 7edf8c0e0a
commit 3636b53313
11 changed files with 233 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
APMrover2/APMrover2.cpp
View File

@ -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;

19
APMrover2/GCS_Mavlink.cpp
View File

@ -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;

21
APMrover2/Log.cpp
View File

@ -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

4
APMrover2/Parameters.cpp
View File

@ -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),

3
APMrover2/Parameters.h
View File

@ -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

20
APMrover2/Rover.h
View File

@ -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);

23
APMrover2/Steering.cpp
View File

@ -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

2
APMrover2/defines.h
View File

@ -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)

1
APMrover2/make.inc
View File

@ -46,4 +46,5 @@ LIBRARIES += AP_OpticalFlow
LIBRARIES += AP_RSSI
LIBRARIES += AP_Declination
LIBRARIES += AP_RPM
LIBRARIES += AP_Arming

79
APMrover2/radio.cpp
View File

@ -15,6 +15,11 @@ void Rover::set_control_channels(void)
channel_steer->set_angle(SERVO_MAX);
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.
hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max);
@ -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)

53
APMrover2/system.cpp
View File

@ -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;
}