2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-10-07 11:50:26 -03:00
|
|
|
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
|
2014-10-31 03:48:28 -03:00
|
|
|
|
2015-03-24 16:30:19 -03:00
|
|
|
//Documentation of Aux Switch Flags:
|
2016-10-13 22:28:56 -03:00
|
|
|
struct {
|
|
|
|
uint8_t CH6_flag : 2; // 0, 1 // ch6 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
|
|
uint8_t CH7_flag : 2; // 2, 3 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
|
|
uint8_t CH8_flag : 2; // 4, 5 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
|
|
uint8_t CH9_flag : 2; // 6, 7 // ch9 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
|
|
uint8_t CH10_flag : 2; // 8, 9 // ch10 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
|
|
uint8_t CH11_flag : 2; // 10,11 // ch11 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
|
|
uint8_t CH12_flag : 2; // 12,13 // ch12 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
2015-03-24 16:30:19 -03:00
|
|
|
} aux_con;
|
|
|
|
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::read_control_switch()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2014-10-07 11:50:26 -03:00
|
|
|
uint32_t tnow_ms = millis();
|
|
|
|
|
2014-10-31 03:48:28 -03:00
|
|
|
// calculate position of flight mode switch
|
2014-10-07 11:50:26 -03:00
|
|
|
int8_t switch_position;
|
2017-01-06 21:06:40 -04:00
|
|
|
uint16_t rc5_in = RC_Channels::rc_channel(CH_5)->get_radio_in();
|
|
|
|
if (rc5_in < 1231) switch_position = 0;
|
|
|
|
else if (rc5_in < 1361) switch_position = 1;
|
|
|
|
else if (rc5_in < 1491) switch_position = 2;
|
|
|
|
else if (rc5_in < 1621) switch_position = 3;
|
|
|
|
else if (rc5_in < 1750) switch_position = 4;
|
2014-10-07 11:50:26 -03:00
|
|
|
else switch_position = 5;
|
|
|
|
|
2014-10-31 03:48:28 -03:00
|
|
|
// store time that switch last moved
|
2016-12-18 22:19:03 -04:00
|
|
|
if (control_switch_state.last_switch_position != switch_position) {
|
2014-10-07 11:50:26 -03:00
|
|
|
control_switch_state.last_edge_time_ms = tnow_ms;
|
|
|
|
}
|
|
|
|
|
2014-10-31 03:48:28 -03:00
|
|
|
// debounce switch
|
2014-10-07 11:50:26 -03:00
|
|
|
bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
|
|
|
|
bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
|
|
|
|
bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;
|
|
|
|
|
|
|
|
if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
|
|
|
|
// set flight mode and simple mode setting
|
2016-01-25 19:40:41 -04:00
|
|
|
if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) {
|
2014-10-23 15:47:00 -03:00
|
|
|
// play a tone
|
|
|
|
if (control_switch_state.debounced_switch_position != -1) {
|
2014-12-15 00:41:43 -04:00
|
|
|
// alert user to mode change failure (except if autopilot is just starting up)
|
|
|
|
if (ap.initialised) {
|
|
|
|
AP_Notify::events.user_mode_change = 1;
|
|
|
|
}
|
2014-10-23 15:47:00 -03:00
|
|
|
}
|
2014-10-07 11:50:26 -03:00
|
|
|
|
2016-12-18 22:19:03 -04:00
|
|
|
if (!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) {
|
2015-03-10 17:53:30 -03:00
|
|
|
// if none of the Aux Switches are set to Simple or Super Simple Mode then
|
|
|
|
// set Simple Mode using stored parameters from EEPROM
|
2014-10-07 11:50:26 -03:00
|
|
|
if (BIT_IS_SET(g.super_simple, switch_position)) {
|
|
|
|
set_simple_mode(2);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2014-10-07 11:50:26 -03:00
|
|
|
set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position));
|
2013-07-19 23:01:10 -03:00
|
|
|
}
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2014-10-23 15:47:00 -03:00
|
|
|
|
2014-12-14 23:29:47 -04:00
|
|
|
} else if (control_switch_state.last_switch_position != -1) {
|
|
|
|
// alert user to mode change failure
|
2014-10-23 15:47:00 -03:00
|
|
|
AP_Notify::events.user_mode_change_failed = 1;
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2015-03-13 10:15:27 -03:00
|
|
|
|
|
|
|
// set the debounced switch position
|
|
|
|
control_switch_state.debounced_switch_position = switch_position;
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2012-12-14 17:30:48 -04:00
|
|
|
|
2014-10-07 11:50:26 -03:00
|
|
|
control_switch_state.last_switch_position = switch_position;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2015-03-10 17:53:30 -03:00
|
|
|
// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode.
|
2015-05-29 23:12:49 -03:00
|
|
|
bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
|
2015-03-17 09:15:52 -03:00
|
|
|
{
|
2015-03-10 17:53:30 -03:00
|
|
|
bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check
|
|
|
|
|| g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check;
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2015-03-10 22:20:07 -03:00
|
|
|
// check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated
|
2015-05-29 23:12:49 -03:00
|
|
|
bool Copter::check_duplicate_auxsw(void)
|
2015-03-17 09:15:52 -03:00
|
|
|
{
|
2016-09-23 03:41:10 -03:00
|
|
|
uint8_t auxsw_option_counts[AUXSW_SWITCH_MAX] = {};
|
|
|
|
auxsw_option_counts[g.ch7_option]++;
|
|
|
|
auxsw_option_counts[g.ch8_option]++;
|
|
|
|
auxsw_option_counts[g.ch9_option]++;
|
|
|
|
auxsw_option_counts[g.ch10_option]++;
|
|
|
|
auxsw_option_counts[g.ch11_option]++;
|
|
|
|
auxsw_option_counts[g.ch12_option]++;
|
|
|
|
|
|
|
|
for (uint8_t i=0; i<sizeof(auxsw_option_counts); i++) {
|
|
|
|
if (i == AUXSW_DO_NOTHING) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (auxsw_option_counts[i] > 1) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
2015-03-10 22:20:07 -03:00
|
|
|
}
|
|
|
|
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::reset_control_switch()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2014-10-07 11:50:26 -03:00
|
|
|
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
|
2012-08-16 21:50:02 -03:00
|
|
|
read_control_switch();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2013-10-30 00:01:46 -03:00
|
|
|
// read_3pos_switch
|
2017-01-06 21:06:40 -04:00
|
|
|
uint8_t Copter::read_3pos_switch(uint8_t chan)
|
2015-03-17 09:15:52 -03:00
|
|
|
{
|
2017-01-06 21:06:40 -04:00
|
|
|
uint16_t radio_in = RC_Channels::rc_channel(chan)->get_radio_in();
|
2013-07-29 04:28:04 -03:00
|
|
|
if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position
|
|
|
|
if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position
|
|
|
|
return AUX_SWITCH_MIDDLE; // switch is in middle position
|
|
|
|
}
|
|
|
|
|
2016-11-16 21:23:50 -04:00
|
|
|
// can't take reference to a bitfield member, thus a #define:
|
2017-01-06 21:06:40 -04:00
|
|
|
#define read_aux_switch(chan, flag, option) \
|
2016-11-16 21:23:50 -04:00
|
|
|
do { \
|
2017-01-06 21:06:40 -04:00
|
|
|
switch_position = read_3pos_switch(chan); \
|
2016-11-16 21:23:50 -04:00
|
|
|
if (flag != switch_position) { \
|
|
|
|
flag = switch_position; \
|
|
|
|
do_aux_switch_function(option, flag); \
|
|
|
|
} \
|
|
|
|
} while (false)
|
|
|
|
|
2013-05-17 02:42:28 -03:00
|
|
|
// read_aux_switches - checks aux switch positions and invokes configured actions
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::read_aux_switches()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2013-07-29 04:28:04 -03:00
|
|
|
uint8_t switch_position;
|
2013-07-27 20:28:00 -03:00
|
|
|
|
2013-09-26 08:19:39 -03:00
|
|
|
// exit immediately during radio failsafe
|
|
|
|
if (failsafe.radio || failsafe.radio_counter != 0) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-01-06 21:06:40 -04:00
|
|
|
read_aux_switch(CH_7, aux_con.CH7_flag, g.ch7_option);
|
|
|
|
read_aux_switch(CH_8, aux_con.CH8_flag, g.ch8_option);
|
|
|
|
read_aux_switch(CH_9, aux_con.CH9_flag, g.ch9_option);
|
|
|
|
read_aux_switch(CH_10, aux_con.CH10_flag, g.ch10_option);
|
|
|
|
read_aux_switch(CH_11, aux_con.CH11_flag, g.ch11_option);
|
2015-03-10 15:21:31 -03:00
|
|
|
|
2015-03-17 10:16:48 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
2017-01-06 21:06:40 -04:00
|
|
|
read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option);
|
2015-03-17 10:16:48 -03:00
|
|
|
#endif
|
2013-05-17 02:42:28 -03:00
|
|
|
}
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2016-11-16 21:23:50 -04:00
|
|
|
#undef read_aux_switch
|
|
|
|
|
2013-07-16 10:05:59 -03:00
|
|
|
// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::init_aux_switches()
|
2013-07-16 10:05:59 -03:00
|
|
|
{
|
2015-03-17 10:16:48 -03:00
|
|
|
// set the CH7 ~ CH12 flags
|
2017-01-06 21:06:40 -04:00
|
|
|
aux_con.CH7_flag = read_3pos_switch(CH_7);
|
|
|
|
aux_con.CH8_flag = read_3pos_switch(CH_8);
|
|
|
|
aux_con.CH10_flag = read_3pos_switch(CH_10);
|
|
|
|
aux_con.CH11_flag = read_3pos_switch(CH_11);
|
2015-03-17 10:16:48 -03:00
|
|
|
|
|
|
|
// ch9, ch12 only supported on some boards
|
2017-01-06 21:06:40 -04:00
|
|
|
aux_con.CH9_flag = read_3pos_switch(CH_9);
|
|
|
|
aux_con.CH12_flag = read_3pos_switch(CH_12);
|
2015-03-10 15:21:31 -03:00
|
|
|
|
2015-03-17 10:16:48 -03:00
|
|
|
// initialise functions assigned to switches
|
2015-03-24 16:30:19 -03:00
|
|
|
init_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
|
|
|
|
init_aux_switch_function(g.ch8_option, aux_con.CH8_flag);
|
|
|
|
init_aux_switch_function(g.ch10_option, aux_con.CH10_flag);
|
|
|
|
init_aux_switch_function(g.ch11_option, aux_con.CH11_flag);
|
2015-03-17 10:16:48 -03:00
|
|
|
|
|
|
|
// ch9, ch12 only supported on some boards
|
2015-03-24 16:30:19 -03:00
|
|
|
init_aux_switch_function(g.ch9_option, aux_con.CH9_flag);
|
|
|
|
init_aux_switch_function(g.ch12_option, aux_con.CH12_flag);
|
2015-03-10 15:21:31 -03:00
|
|
|
}
|
2013-07-16 10:05:59 -03:00
|
|
|
|
2015-03-10 15:21:31 -03:00
|
|
|
// init_aux_switch_function - initialize aux functions
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
|
2015-03-10 15:21:31 -03:00
|
|
|
{
|
|
|
|
// init channel options
|
|
|
|
switch(ch_option) {
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SIMPLE_MODE:
|
2016-04-27 08:37:04 -03:00
|
|
|
case AUXSW_RANGEFINDER:
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_FENCE:
|
|
|
|
case AUXSW_SUPERSIMPLE_MODE:
|
|
|
|
case AUXSW_ACRO_TRAINER:
|
2016-10-29 08:01:29 -03:00
|
|
|
case AUXSW_GRIPPER:
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SPRAYER:
|
|
|
|
case AUXSW_PARACHUTE_ENABLE:
|
|
|
|
case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
|
|
|
|
case AUXSW_RETRACT_MOUNT:
|
|
|
|
case AUXSW_MISSION_RESET:
|
|
|
|
case AUXSW_ATTCON_FEEDFWD:
|
|
|
|
case AUXSW_ATTCON_ACCEL_LIM:
|
2015-03-17 18:53:40 -03:00
|
|
|
case AUXSW_MOTOR_ESTOP:
|
|
|
|
case AUXSW_MOTOR_INTERLOCK:
|
2016-12-18 23:12:34 -04:00
|
|
|
case AUXSW_AVOID_ADSB:
|
|
|
|
case AUXSW_PRECISION_LOITER:
|
2016-12-18 23:15:17 -04:00
|
|
|
case AUXSW_AVOID_PROXIMITY:
|
2015-03-10 15:21:31 -03:00
|
|
|
do_aux_switch_function(ch_option, ch_flag);
|
2013-07-16 10:05:59 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-05-17 02:42:28 -03:00
|
|
|
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
2013-05-17 02:42:28 -03:00
|
|
|
{
|
2012-12-19 12:08:59 -04:00
|
|
|
|
2015-03-25 14:24:24 -03:00
|
|
|
switch(ch_function) {
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_FLIP:
|
2012-12-06 04:47:01 -04:00
|
|
|
// flip if switch is on, positive throttle and we're actually flying
|
2016-12-18 22:19:03 -04:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(FLIP, MODE_REASON_TX_COMMAND);
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2012-12-06 04:47:01 -04:00
|
|
|
break;
|
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SIMPLE_MODE:
|
2013-10-05 06:25:03 -03:00
|
|
|
// low = simple mode off, middle or high position turns simple mode on
|
|
|
|
set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
|
|
|
|
break;
|
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SUPERSIMPLE_MODE:
|
2013-10-05 06:25:03 -03:00
|
|
|
// low = simple mode off, middle = simple mode, high = super simple mode
|
2013-05-17 02:42:28 -03:00
|
|
|
set_simple_mode(ch_flag);
|
2012-12-06 04:47:01 -04:00
|
|
|
break;
|
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_RTL:
|
2013-07-29 04:28:04 -03:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2013-07-19 23:01:10 -03:00
|
|
|
// engage RTL (if not possible we remain in current flight mode)
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(RTL, MODE_REASON_TX_COMMAND);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2013-10-22 01:13:36 -03:00
|
|
|
// return to flight mode switch's flight mode if we are currently in RTL
|
|
|
|
if (control_mode == RTL) {
|
2012-12-06 04:47:01 -04:00
|
|
|
reset_control_switch();
|
|
|
|
}
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2012-12-06 04:47:01 -04:00
|
|
|
break;
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SAVE_TRIM:
|
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
|
|
|
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) {
|
2012-12-19 11:06:20 -04:00
|
|
|
save_trim();
|
2012-12-06 04:47:01 -04:00
|
|
|
}
|
|
|
|
break;
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SAVE_WP:
|
2013-10-31 23:42:33 -03:00
|
|
|
// save waypoint when switch is brought high
|
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2014-02-27 21:25:51 -04:00
|
|
|
// do not allow saving new waypoints while we're in auto or disarmed
|
2017-01-09 03:31:26 -04:00
|
|
|
if (control_mode == AUTO || !motors->armed()) {
|
2012-08-16 21:50:02 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
// do not allow saving the first waypoint with zero throttle
|
2016-12-18 22:19:03 -04:00
|
|
|
if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) {
|
2015-03-24 16:33:13 -03:00
|
|
|
return;
|
|
|
|
}
|
2013-10-30 00:01:46 -03:00
|
|
|
|
2014-02-27 21:25:51 -04:00
|
|
|
// create new mission command
|
2014-03-10 05:36:18 -03:00
|
|
|
AP_Mission::Mission_Command cmd = {};
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2014-02-27 21:25:51 -04:00
|
|
|
// if the mission is empty save a takeoff command
|
2016-12-18 22:19:03 -04:00
|
|
|
if (mission.num_commands() == 0) {
|
2012-08-16 21:50:02 -03:00
|
|
|
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
|
2014-02-27 21:25:51 -04:00
|
|
|
cmd.id = MAV_CMD_NAV_TAKEOFF;
|
|
|
|
cmd.content.location.options = 0;
|
2014-03-03 01:38:32 -04:00
|
|
|
cmd.p1 = 0;
|
2014-02-27 21:25:51 -04:00
|
|
|
cmd.content.location.lat = 0;
|
|
|
|
cmd.content.location.lng = 0;
|
2015-11-27 13:11:58 -04:00
|
|
|
cmd.content.location.alt = MAX(current_loc.alt,100);
|
2014-02-27 21:25:51 -04:00
|
|
|
|
|
|
|
// use the current altitude for the target alt for takeoff.
|
2012-08-16 21:50:02 -03:00
|
|
|
// only altitude will matter to the AP mission script for takeoff.
|
2016-12-18 22:19:03 -04:00
|
|
|
if (mission.add_cmd(cmd)) {
|
2014-02-27 21:25:51 -04:00
|
|
|
// log event
|
|
|
|
Log_Write_Event(DATA_SAVEWP_ADD_WP);
|
|
|
|
}
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
|
|
|
|
2014-02-27 21:25:51 -04:00
|
|
|
// set new waypoint to current location
|
|
|
|
cmd.content.location = current_loc;
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2014-02-27 21:25:51 -04:00
|
|
|
// if throttle is above zero, create waypoint command
|
2016-12-18 22:19:03 -04:00
|
|
|
if (channel_throttle->get_control_in() > 0) {
|
2014-02-27 21:25:51 -04:00
|
|
|
cmd.id = MAV_CMD_NAV_WAYPOINT;
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2015-03-24 16:33:13 -03:00
|
|
|
// with zero throttle, create LAND command
|
2014-02-27 21:25:51 -04:00
|
|
|
cmd.id = MAV_CMD_NAV_LAND;
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// save command
|
2016-12-18 22:19:03 -04:00
|
|
|
if (mission.add_cmd(cmd)) {
|
2014-02-27 21:25:51 -04:00
|
|
|
// log event
|
|
|
|
Log_Write_Event(DATA_SAVEWP_ADD_WP);
|
|
|
|
}
|
2012-12-06 04:47:01 -04:00
|
|
|
}
|
|
|
|
break;
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_CAMERA_TRIGGER:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if CAMERA == ENABLED
|
2013-07-29 04:28:04 -03:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2017-07-26 00:33:45 -03:00
|
|
|
camera.take_picture();
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2012-12-06 04:47:01 -04:00
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
2013-01-14 00:58:53 -04:00
|
|
|
|
2016-04-27 08:37:04 -03:00
|
|
|
case AUXSW_RANGEFINDER:
|
|
|
|
// enable or disable the rangefinder
|
2016-04-27 07:55:35 -03:00
|
|
|
#if RANGEFINDER_ENABLED == ENABLED
|
2017-02-09 07:30:59 -04:00
|
|
|
if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) {
|
2016-05-07 04:55:40 -03:00
|
|
|
rangefinder_state.enabled = true;
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2016-05-07 04:55:40 -03:00
|
|
|
rangefinder_state.enabled = false;
|
2013-07-29 04:28:04 -03:00
|
|
|
}
|
2014-07-08 02:41:39 -03:00
|
|
|
#endif
|
2013-01-14 00:58:53 -04:00
|
|
|
break;
|
2013-04-26 06:51:07 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_FENCE:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if AC_FENCE == ENABLED
|
2013-07-20 03:00:08 -03:00
|
|
|
// enable or disable the fence
|
2013-07-29 04:28:04 -03:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
|
|
fence.enable(true);
|
2013-10-31 23:53:16 -03:00
|
|
|
Log_Write_Event(DATA_FENCE_ENABLE);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2013-07-29 04:28:04 -03:00
|
|
|
fence.enable(false);
|
2013-10-31 23:53:16 -03:00
|
|
|
Log_Write_Event(DATA_FENCE_DISABLE);
|
2013-07-29 04:28:04 -03:00
|
|
|
}
|
2013-07-20 03:00:08 -03:00
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
2013-08-04 06:33:23 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_ACRO_TRAINER:
|
2013-08-04 06:33:23 -03:00
|
|
|
switch(ch_flag) {
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
g.acro_trainer = ACRO_TRAINER_DISABLED;
|
2013-10-31 23:53:16 -03:00
|
|
|
Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
|
2013-08-04 06:33:23 -03:00
|
|
|
break;
|
|
|
|
case AUX_SWITCH_MIDDLE:
|
|
|
|
g.acro_trainer = ACRO_TRAINER_LEVELING;
|
2013-10-31 23:53:16 -03:00
|
|
|
Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
|
2013-08-04 06:33:23 -03:00
|
|
|
break;
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
g.acro_trainer = ACRO_TRAINER_LIMITED;
|
2013-10-31 23:53:16 -03:00
|
|
|
Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
|
2013-08-04 06:33:23 -03:00
|
|
|
break;
|
|
|
|
}
|
2013-08-19 06:09:23 -03:00
|
|
|
break;
|
2016-12-19 03:08:43 -04:00
|
|
|
|
2016-10-29 08:01:29 -03:00
|
|
|
case AUXSW_GRIPPER:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if GRIPPER_ENABLED == ENABLED
|
2013-12-17 00:58:11 -04:00
|
|
|
switch(ch_flag) {
|
|
|
|
case AUX_SWITCH_LOW:
|
2016-10-28 19:08:22 -03:00
|
|
|
g2.gripper.release();
|
2016-10-29 08:01:29 -03:00
|
|
|
Log_Write_Event(DATA_GRIPPER_RELEASE);
|
2013-12-17 00:58:11 -04:00
|
|
|
break;
|
|
|
|
case AUX_SWITCH_HIGH:
|
2016-10-28 19:08:22 -03:00
|
|
|
g2.gripper.grab();
|
2016-10-29 08:01:29 -03:00
|
|
|
Log_Write_Event(DATA_GRIPPER_GRAB);
|
2013-12-17 00:58:11 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_SPRAYER:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if SPRAYER == ENABLED
|
2016-10-27 02:29:13 -03:00
|
|
|
sprayer.run(ch_flag == AUX_SWITCH_HIGH);
|
2013-09-11 05:05:25 -03:00
|
|
|
// if we are disarmed the pilot must want to test the pump
|
2017-01-09 03:31:26 -04:00
|
|
|
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors->armed());
|
2013-08-04 11:38:38 -03:00
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
2013-08-15 04:08:33 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_AUTO:
|
2013-08-19 06:09:23 -03:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(AUTO, MODE_REASON_TX_COMMAND);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2013-10-22 01:13:36 -03:00
|
|
|
// return to flight mode switch's flight mode if we are currently in AUTO
|
|
|
|
if (control_mode == AUTO) {
|
|
|
|
reset_control_switch();
|
|
|
|
}
|
2013-08-15 04:08:33 -03:00
|
|
|
}
|
2013-08-19 06:09:23 -03:00
|
|
|
break;
|
2013-09-03 06:31:06 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_AUTOTUNE:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if AUTOTUNE_ENABLED == ENABLED
|
2013-09-03 06:31:06 -03:00
|
|
|
// turn on auto tuner
|
|
|
|
switch(ch_flag) {
|
|
|
|
case AUX_SWITCH_LOW:
|
2013-10-18 01:57:50 -03:00
|
|
|
case AUX_SWITCH_MIDDLE:
|
2014-02-02 03:58:36 -04:00
|
|
|
// restore flight mode based on flight mode switch position
|
|
|
|
if (control_mode == AUTOTUNE) {
|
|
|
|
reset_control_switch();
|
2013-10-19 09:53:51 -03:00
|
|
|
}
|
2013-09-03 06:31:06 -03:00
|
|
|
break;
|
|
|
|
case AUX_SWITCH_HIGH:
|
2014-02-02 03:58:36 -04:00
|
|
|
// start an autotuning session
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
|
2013-09-03 06:31:06 -03:00
|
|
|
break;
|
|
|
|
}
|
2013-10-04 03:49:19 -03:00
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
2013-10-22 01:13:36 -03:00
|
|
|
|
2015-03-10 17:32:25 -03:00
|
|
|
case AUXSW_LAND:
|
2013-10-22 01:13:36 -03:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(LAND, MODE_REASON_TX_COMMAND);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2013-10-22 01:13:36 -03:00
|
|
|
// return to flight mode switch's flight mode if we are currently in LAND
|
|
|
|
if (control_mode == LAND) {
|
|
|
|
reset_control_switch();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
2014-01-03 21:39:08 -04:00
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
case AUXSW_PARACHUTE_ENABLE:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
2015-03-24 16:33:13 -03:00
|
|
|
// Parachute enable/disable
|
|
|
|
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
|
2016-12-19 03:08:43 -04:00
|
|
|
#endif
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AUXSW_PARACHUTE_RELEASE:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
2015-03-24 16:33:13 -03:00
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2014-04-02 21:56:30 -03:00
|
|
|
parachute_manual_release();
|
2015-03-24 16:33:13 -03:00
|
|
|
}
|
2016-12-19 03:08:43 -04:00
|
|
|
#endif
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AUXSW_PARACHUTE_3POS:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
2015-03-24 16:33:13 -03:00
|
|
|
// Parachute disable, enable, release with 3 position switch
|
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
parachute.enabled(false);
|
|
|
|
Log_Write_Event(DATA_PARACHUTE_DISABLED);
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_MIDDLE:
|
|
|
|
parachute.enabled(true);
|
|
|
|
Log_Write_Event(DATA_PARACHUTE_ENABLED);
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
parachute.enabled(true);
|
|
|
|
parachute_manual_release();
|
|
|
|
break;
|
|
|
|
}
|
2014-04-02 12:20:45 -03:00
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
2014-05-03 15:26:58 -03:00
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
case AUXSW_MISSION_RESET:
|
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
|
|
mission.reset();
|
|
|
|
}
|
|
|
|
break;
|
2014-06-10 03:56:15 -03:00
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
case AUXSW_ATTCON_FEEDFWD:
|
|
|
|
// enable or disable feed forward
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
2014-06-10 03:56:15 -03:00
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
case AUXSW_ATTCON_ACCEL_LIM:
|
|
|
|
// enable or disable accel limiting by restoring defaults
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->accel_limiting(ch_flag == AUX_SWITCH_HIGH);
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
2016-12-19 03:08:43 -04:00
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
case AUXSW_RETRACT_MOUNT:
|
2016-12-19 03:08:43 -04:00
|
|
|
#if MOUNT == ENABLE
|
2015-03-24 16:33:13 -03:00
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
camera_mount.set_mode_to_default();
|
|
|
|
break;
|
|
|
|
}
|
2014-07-10 01:00:28 -03:00
|
|
|
#endif
|
2016-12-19 03:08:43 -04:00
|
|
|
break;
|
2014-08-09 01:36:35 -03:00
|
|
|
|
2015-03-24 16:33:13 -03:00
|
|
|
case AUXSW_RELAY:
|
|
|
|
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
|
|
|
|
break;
|
2015-03-10 23:10:02 -03:00
|
|
|
|
2016-01-04 22:54:05 -04:00
|
|
|
case AUXSW_RELAY2:
|
2016-01-07 23:12:29 -04:00
|
|
|
ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AUXSW_RELAY3:
|
|
|
|
ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
|
|
|
|
break;
|
2016-01-04 22:54:05 -04:00
|
|
|
|
|
|
|
case AUXSW_RELAY4:
|
2016-01-07 23:12:29 -04:00
|
|
|
ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
|
|
|
|
break;
|
|
|
|
|
2016-12-19 03:08:43 -04:00
|
|
|
case AUXSW_LANDING_GEAR:
|
2015-03-24 16:33:13 -03:00
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_LOW:
|
2017-06-10 01:11:00 -03:00
|
|
|
landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
|
|
|
case AUX_SWITCH_HIGH:
|
2017-06-10 01:11:00 -03:00
|
|
|
landinggear.set_position(AP_LandingGear::LandingGear_Retract);
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AUXSW_LOST_COPTER_SOUND:
|
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
AP_Notify::flags.vehicle_lost = true;
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
AP_Notify::flags.vehicle_lost = false;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AUXSW_MOTOR_ESTOP:
|
2015-04-22 14:56:05 -03:00
|
|
|
// Turn on Emergency Stop logic when channel is high
|
|
|
|
set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AUXSW_MOTOR_INTERLOCK:
|
|
|
|
// Turn on when above LOW, because channel will also be used for speed
|
|
|
|
// control signal in tradheli
|
2016-01-21 22:59:47 -04:00
|
|
|
ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
|
2015-03-24 16:33:13 -03:00
|
|
|
break;
|
2015-05-17 00:22:47 -03:00
|
|
|
|
|
|
|
case AUXSW_BRAKE:
|
|
|
|
// brake flight mode
|
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(BRAKE, MODE_REASON_TX_COMMAND);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2015-05-17 00:22:47 -03:00
|
|
|
// return to flight mode switch's flight mode if we are currently in BRAKE
|
|
|
|
if (control_mode == BRAKE) {
|
|
|
|
reset_control_switch();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
2016-03-03 02:27:55 -04:00
|
|
|
|
|
|
|
case AUXSW_THROW:
|
|
|
|
// throw flight mode
|
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
2016-01-25 19:40:41 -04:00
|
|
|
set_mode(THROW, MODE_REASON_TX_COMMAND);
|
2016-03-03 02:27:55 -04:00
|
|
|
} else {
|
|
|
|
// return to flight mode switch's flight mode if we are currently in throw mode
|
|
|
|
if (control_mode == THROW) {
|
|
|
|
reset_control_switch();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
2016-07-21 09:44:09 -03:00
|
|
|
|
|
|
|
case AUXSW_AVOID_ADSB:
|
|
|
|
// enable or disable AP_Avoidance
|
|
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
|
|
avoidance_adsb.enable();
|
|
|
|
Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
|
2016-12-18 22:19:03 -04:00
|
|
|
} else {
|
2016-07-21 09:44:09 -03:00
|
|
|
avoidance_adsb.disable();
|
|
|
|
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
|
|
|
|
}
|
|
|
|
break;
|
2016-11-16 21:40:49 -04:00
|
|
|
|
|
|
|
case AUXSW_PRECISION_LOITER:
|
2016-12-05 06:06:08 -04:00
|
|
|
#if PRECISION_LANDING == ENABLED
|
2016-11-16 21:40:49 -04:00
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
set_precision_loiter_enabled(true);
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
set_precision_loiter_enabled(false);
|
|
|
|
break;
|
|
|
|
}
|
2016-12-05 06:06:08 -04:00
|
|
|
#endif
|
2016-11-16 21:40:49 -04:00
|
|
|
break;
|
|
|
|
|
2016-12-18 23:15:17 -04:00
|
|
|
case AUXSW_AVOID_PROXIMITY:
|
2017-01-02 20:36:26 -04:00
|
|
|
#if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED
|
2016-12-18 23:15:17 -04:00
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
avoid.proximity_avoidance_enable(true);
|
|
|
|
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE);
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
avoid.proximity_avoidance_enable(false);
|
|
|
|
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
break;
|
2017-02-06 02:28:52 -04:00
|
|
|
case AUXSW_ARMDISARM:
|
|
|
|
// arm or disarm the vehicle
|
|
|
|
switch (ch_flag) {
|
|
|
|
case AUX_SWITCH_HIGH:
|
|
|
|
init_arm_motors(false);
|
|
|
|
break;
|
|
|
|
case AUX_SWITCH_LOW:
|
|
|
|
init_disarm_motors();
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
break;
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
2011-01-15 22:37:35 -04:00
|
|
|
}
|
|
|
|
|
2012-11-05 00:32:38 -04:00
|
|
|
// save_trim - adds roll and pitch trims from the radio to ahrs
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::save_trim()
|
2011-06-16 14:03:26 -03:00
|
|
|
{
|
2012-12-19 11:06:20 -04:00
|
|
|
// save roll and pitch trim
|
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
|
|
|
float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f);
|
|
|
|
float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f);
|
2012-12-19 11:06:20 -04:00
|
|
|
ahrs.add_trim(roll_trim, pitch_trim);
|
2013-10-31 23:53:16 -03:00
|
|
|
Log_Write_Event(DATA_SAVE_TRIM);
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
|
2012-12-19 11:06:20 -04:00
|
|
|
}
|
2012-09-17 21:03:29 -03:00
|
|
|
|
2012-12-19 11:06:20 -04:00
|
|
|
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
|
|
|
|
// meant to be called continuously while the pilot attempts to keep the copter level
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::auto_trim()
|
2012-12-19 11:06:20 -04:00
|
|
|
{
|
2016-12-18 22:19:03 -04:00
|
|
|
if (auto_trim_counter > 0) {
|
2012-12-19 11:06:20 -04:00
|
|
|
auto_trim_counter--;
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2012-12-19 11:06:20 -04:00
|
|
|
// flash the leds
|
2013-08-14 00:07:35 -03:00
|
|
|
AP_Notify::flags.save_trim = true;
|
2012-11-05 00:32:38 -04:00
|
|
|
|
2012-12-19 11:06:20 -04:00
|
|
|
// calculate roll trim adjustment
|
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
|
|
|
float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f);
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2012-12-19 11:06:20 -04:00
|
|
|
// calculate pitch trim adjustment
|
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
|
|
|
float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f);
|
2012-08-16 21:50:02 -03:00
|
|
|
|
2012-12-19 11:06:20 -04:00
|
|
|
// add trim to ahrs object
|
|
|
|
// save to eeprom on last iteration
|
|
|
|
ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
|
|
|
|
|
|
|
|
// on last iteration restore leds and accel gains to normal
|
2016-12-18 22:19:03 -04:00
|
|
|
if (auto_trim_counter == 0) {
|
2013-08-14 00:07:35 -03:00
|
|
|
AP_Notify::flags.save_trim = false;
|
2012-08-16 21:50:02 -03:00
|
|
|
}
|
|
|
|
}
|
2011-06-16 14:03:26 -03:00
|
|
|
}
|
|
|
|
|