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

Plane: Now using AP_Arming library.

Browse Source
This commit is contained in:
Michael Day 2013-11-27 18:19:34 -08:00 committed by Andrew Tridgell
parent ebf1d9c136
commit b0a0316dd6
8 changed files with 139 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduPlane/ArduPlane.pde
View File

@ -73,6 +73,8 @@
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_Arming.h>
// Pre-AP_HAL compatibility
#include "compat.h"
@ -697,6 +699,11 @@ AP_Mount camera_mount(&current_loc, g_gps, ahrs, 0);
AP_Mount camera_mount2(&current_loc, g_gps, ahrs, 1);
#endif
////////////////////////////////////////////////////////////////////////////////
// Arming/Disarming mangement class
////////////////////////////////////////////////////////////////////////////////
AP_Arming arming(ahrs, barometer, home_is_set, gcs_send_text_P);
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
@ -756,9 +763,6 @@ void setup() {
// load the default values of variables listed in var_info[]
AP_Param::setup_sketch_defaults();
// arduplane does not use arming nor pre-arm checks
AP_Notify::flags.armed = true;
AP_Notify::flags.pre_arm_check = true;
AP_Notify::flags.failsafe_battery = false;
notify.init(false);
@ -771,6 +775,12 @@ void setup() {
init_ardupilot();
//if no user intervention require to arm, then just arm already
//(maintain old behavior)
if (arming.arming_required() == AP_Arming::NO) {
arming.arm(AP_Arming::NONE);
}
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
}

18
ArduPlane/Attitude.pde
View File

@ -871,6 +871,24 @@ static void set_servos(void)
channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out);
}
//send throttle to 0 or MIN_PWM if not yet armed
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::YES_MIN_PWM:
channel_throttle->radio_out = channel_throttle->radio_min;
break;
case AP_Arming::YES_ZERO_PWM:
channel_throttle->radio_out = 0;
break;
default:
//keep existing behavior: do nothing to radio_out
//(don't disarm throttle channel even if AP_Arming class is)
break;
}
}
// send values to the PWM timers for output
// ----------------------------------------
channel_roll->output();

23
ArduPlane/GCS_Mavlink.pde
View File

@ -1280,6 +1280,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (packet.param1 == 1.0f) {
// run pre_arm_checks and arm_checks and display failures
if (arming.arm(AP_Arming::MAVLINK)) {
//only log if arming was successful
Log_Arm_Disarm();
}
result = MAV_RESULT_ACCEPTED;
} else if (packet.param1 == 0.0f) {
if (arming.disarm()) {
//only log if disarming was successful
Log_Arm_Disarm();
}
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:

18
ArduPlane/Log.pde
View File

@ -426,6 +426,13 @@ struct PACKED log_Current {
float current_total;
};
struct PACKED log_Arm_Disarm {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint8_t arm_state;
uint16_t arm_checks;
};
static void Log_Write_Current()
{
struct log_Current pkt = {
@ -440,6 +447,15 @@ static void Log_Write_Current()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
static void Log_Arm_Disarm() {
struct log_Arm_Disarm pkt = {
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
time_ms : hal.scheduler->millis(),
arm_state : arming.is_armed(),
arm_checks : arming.get_enabled_checks()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Compass {
LOG_PACKET_HEADER;
@ -529,6 +545,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
"MAG", "Ihhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
{ LOG_COMPASS2_MSG, sizeof(log_Compass),
"MAG2", "Ihhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" },
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),
"ARM", "IHB", "TimeMS,ArmState,ArmChecks" },
TECS_LOG_FORMAT(LOG_TECS_MSG),
};

3
ArduPlane/Parameters.h
View File

@ -100,6 +100,9 @@ public:
k_param_hil_err_limit,
k_param_sonar,
// 100: Arming parameters
k_param_arming = 100,
// 110: Telemetry control
//
k_param_gcs0 = 110, // stream rates for uartA

4
ArduPlane/Parameters.pde
View File

@ -772,6 +772,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(camera, "CAM_", AP_Camera),
#endif
// @Group: ARMING_
// @Path: ../libararies/AP_Arming/AP_Arming.cpp
GOBJECT(arming, "ARMING_", AP_Arming),
// @Group: RELAY_
// @Path: ../libraries/AP_Relay/AP_Relay.cpp
GOBJECT(relay, "RELAY_", AP_Relay),

2
ArduPlane/defines.h
View File

@ -165,6 +165,7 @@ enum log_messages {
LOG_RC_MSG,
LOG_SONAR_MSG,
LOG_COMPASS2_MSG,
LOG_ARM_DISARM_MSG,
MAX_NUM_LOGS // always at the end
};
@ -183,6 +184,7 @@ enum log_messages {
#define MASK_LOG_CAMERA (1<<12)
#define MASK_LOG_RC (1<<13)
#define MASK_LOG_SONAR (1<<14)
#define MASK_LOG_ARM_DISARM (1<<15)
// Waypoint Modes
// ----------------

58
ArduPlane/radio.pde
View File

@ -62,6 +62,62 @@ static void init_rc_out()
#endif
}
// check for pilot input on rudder stick for arming
static void rudder_arm_check() {
//TODO: ensure rudder arming disallowed during radio calibration
//TODO: waggle ailerons and rudder and beep after rudder arming
static uint32_t rudder_arm_timer = 0;
if (arming.is_armed()) {
//already armed, no need to run remainder of this function
rudder_arm_timer = 0;
return;
}
if (! arming.rudder_arming_enabled()) {
//parameter disallows rudder arming
return;
}
//if throttle is not down, then pilot cannot rudder arm
if(g.rc_3.control_in > 0 ) {
rudder_arm_timer = 0;
return;
}
//if not in a 'manual' mode then disallow rudder arming
if (auto_throttle_mode ) {
rudder_arm_timer = 0;
return;
}
// full left or right rudder starts arming counter
if (g.rc_4.control_in > 4000
|| g.rc_4.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!
if (arming.arm(AP_Arming::RUDDER)) {
//only log if arming was successful
Log_Arm_Disarm();
}
}
}
else { //not at full left or right rudder
rudder_arm_timer = 0;
return;
}
}
static void read_radio()
{
elevon.ch1_temp = channel_roll->read();
@ -110,6 +166,8 @@ static void read_radio()
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_check();
}
static void control_failsafe(uint16_t pwm)