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

Copter: enable TMODE (ToyMode) support

Browse Source 
this is for the skyviper button based transmitter control
This commit is contained in:
Andrew Tridgell 2018-01-18 17:49:20 +11:00
parent 8f1e8e787a
commit af4f0c29ba
13 changed files with 1283 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/AP_Arming.h
View File

@ -5,6 +5,8 @@
class AP_Arming_Copter : public AP_Arming
{
public:
friend class Copter;
friend class ToyMode;
AP_Arming_Copter(const AP_AHRS_NavEKF &ahrs_ref, const AP_Baro &baro, Compass &compass,
const AP_BattMonitor &battery, const AP_InertialNav_NavEKF &inav,
const AP_InertialSensor &ins)

3
ArduCopter/ArduCopter.cpp
View File

@ -92,6 +92,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
#if TOY_MODE_ENABLED == ENABLED
SCHED_TASK(toy_mode_update, 10, 50),
#endif
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(read_rangefinder, 20, 100),

12
ArduCopter/Attitude.cpp
View File

@ -149,7 +149,7 @@ float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid
thr_mid = motors->get_throttle_hover();
}
int16_t mid_stick = channel_throttle->get_control_mid();
int16_t mid_stick = get_throttle_mid();
// protect against unlikely divide by zero
if (mid_stick <= 0) {
mid_stick = 500;
@ -186,8 +186,16 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
return 0.0f;
}
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
// allow throttle to be reduced after throttle arming and for
// slower descent close to the ground
g2.toy_mode.throttle_adjust(throttle_control);
}
#endif
float desired_rate = 0.0f;
float mid_stick = channel_throttle->get_control_mid();
float mid_stick = get_throttle_mid();
float deadband_top = mid_stick + g.throttle_deadzone;
float deadband_bottom = mid_stick - g.throttle_deadzone;

12
ArduCopter/Copter.h
View File

@ -130,6 +130,10 @@
#include "afs_copter.h"
#endif
#if TOY_MODE_ENABLED == ENABLED
#include "toy_mode.h"
#endif
// Local modules
#include "Parameters.h"
#include "avoidance_adsb.h"
@ -151,6 +155,7 @@ public:
friend class AP_AdvancedFailsafe_Copter;
#endif
friend class AP_Arming_Copter;
friend class ToyMode;
Copter(void);
@ -324,6 +329,9 @@ private:
RCMapper rcmap;
// intertial nav alt when we armed
float arming_altitude_m;
// board specific config
AP_BoardConfig BoardConfig;
@ -820,6 +828,9 @@ private:
void motors_output();
void lost_vehicle_check();
// toy_mode.cpp
void toy_mode_update(void);
// navigation.cpp
void run_nav_updates(void);
int32_t home_bearing();
@ -848,6 +859,7 @@ private:
void set_throttle_and_failsafe(uint16_t throttle_pwm);
void set_throttle_zero_flag(int16_t throttle_control);
void radio_passthrough_to_motors();
int16_t get_throttle_mid(void);
// sensors.cpp
void init_barometer(bool full_calibration);

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -1715,6 +1715,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
#endif
break;
#if TOY_MODE_ENABLED == ENABLED
case MAVLINK_MSG_ID_NAMED_VALUE_INT:
copter.g2.toy_mode.handle_message(msg);
break;
#endif
default:
handle_common_message(msg);
break;

6
ArduCopter/Parameters.cpp
View File

@ -934,7 +934,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_TempCalibration/AP_TempCalibration.cpp
AP_SUBGROUPINFO(temp_calibration, "TCAL", 19, ParametersG2, AP_TempCalibration),
// ID 20 reserved for TX_TYPE (PR pending)
#if TOY_MODE_ENABLED == ENABLED
// @Group: TMODE
// @Path: toy_mode.cpp
AP_SUBGROUPINFO(toy_mode, "TMODE", 20, ParametersG2, ToyMode),
#endif
// @Group: SRTL_
// @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp

4
ArduCopter/Parameters.h
View File

@ -564,6 +564,10 @@ public:
// temperature calibration handling
AP_TempCalibration temp_calibration;
#if TOY_MODE_ENABLED == ENABLED
ToyMode toy_mode;
#endif
};
extern const AP_Param::Info var_info[];

1
ArduCopter/defines.h
View File

@ -130,6 +130,7 @@ enum mode_reason_t {
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_THROW_COMPLETE,
MODE_REASON_TERMINATE,
MODE_REASON_TMODE,
};
// Tuning enumeration

1
ArduCopter/mode.h
View File

@ -5,6 +5,7 @@
class Mode {
friend class Copter;
friend class AP_Arming_Copter;
friend class ToyMode;
public:

15
ArduCopter/motors.cpp
View File

@ -13,6 +13,13 @@ void Copter::arm_motors_check()
{
static int16_t arming_counter;
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
// not armed with sticks in toy mode
return;
}
#endif
// ensure throttle is down
if (channel_throttle->get_control_in() > 0) {
arming_counter = 0;
@ -101,7 +108,7 @@ void Copter::auto_disarm_check()
if (flightmode->has_manual_throttle() || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = channel_throttle->get_control_mid() + g.throttle_deadzone;
float deadband_top = get_throttle_mid() + g.throttle_deadzone;
thr_low = channel_throttle->get_control_in() <= deadband_top;
}
@ -173,9 +180,15 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET);
// we have reset height, so arming height is zero
arming_altitude_m = 0;
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location(false);
// remember the height when we armed
arming_altitude_m = inertial_nav.get_altitude() * 0.01;
}
update_super_simple_bearing(false);

13
ArduCopter/radio.cpp
View File

@ -192,3 +192,16 @@ void Copter::radio_passthrough_to_motors()
channel_throttle->get_control_in_zero_dz()*0.001,
channel_yaw->norm_input());
}
/*
return the throttle input for mid-stick as a control-in value
*/
int16_t Copter::get_throttle_mid(void)
{
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
return g2.toy_mode.get_throttle_mid();
}
#endif
return channel_throttle->get_control_mid();
}

1040
ArduCopter/toy_mode.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

172
ArduCopter/toy_mode.h Normal file
View File

@ -0,0 +1,172 @@
#pragma once
/*
class to support "toy" mode for simplified user interaction for
large volume consumer vehicles
*/
class ToyMode
{
public:
friend class Copter;
ToyMode();
bool enabled(void) const {
return enable.get() != 0;
}
void update(void);
// get throttle mid-point
int16_t get_throttle_mid(void) {
return throttle_mid;
}
// adjust throttle for throttle takeoff
void throttle_adjust(float &throttle_control);
// handle mavlink message
void handle_message(mavlink_message_t *msg);
void load_test_run(void);
static const struct AP_Param::GroupInfo var_info[];
private:
void trim_update(void);
void action_arm(void);
void blink_update(void);
void send_named_int(const char *name, int32_t value);
bool set_and_remember_mode(control_mode_t mode, mode_reason_t reason);
void thrust_limiting(float *thrust, uint8_t num_motors);
void arm_check_compass(void);
enum toy_action {
ACTION_NONE = 0,
ACTION_TAKE_PHOTO = 1,
ACTION_TOGGLE_VIDEO = 2,
ACTION_MODE_ACRO = 3,
ACTION_MODE_ALTHOLD = 4,
ACTION_MODE_AUTO = 5,
ACTION_MODE_LOITER = 6,
ACTION_MODE_RTL = 7,
ACTION_MODE_CIRCLE = 8,
ACTION_MODE_LAND = 9,
ACTION_MODE_DRIFT = 10,
ACTION_MODE_SPORT = 11,
ACTION_MODE_AUTOTUNE= 12,
ACTION_MODE_POSHOLD = 13,
ACTION_MODE_BRAKE = 14,
ACTION_MODE_THROW = 15,
ACTION_MODE_FLIP = 16,
ACTION_MODE_STAB = 17,
ACTION_DISARM = 18,
ACTION_TOGGLE_MODE = 19,
ACTION_ARM_LAND_RTL = 20,
ACTION_TOGGLE_SIMPLE = 21,
ACTION_TOGGLE_SSIMPLE = 22,
ACTION_LOAD_TEST = 23,
ACTION_MODE_FLOW = 24,
};
enum toy_action last_action;
// these are bitmask indexes for TMODE_FLAGS
enum toy_flags {
FLAG_THR_DISARM = 1<<0, // disarm on low throttle
FLAG_THR_ARM = 1<<1, // arm on high throttle
FLAG_UPGRADE_LOITER = 1<<2, // auto upgrade from ALT_HOLD to LOITER
FLAG_RTL_CANCEL = 1<<3, // cancel RTL on large stick input
};
enum blink_patterns {
BLINK_FULL = 0xFFFF,
BLINK_OFF = 0x0000,
BLINK_1 = 0xBFFF,
BLINK_2 = 0xAFFF,
BLINK_3 = 0xABFF,
BLINK_4 = 0xAAFF,
BLINK_6 = 0xAAAF,
BLINK_8 = 0xAAAA,
BLINK_NO_RX = 0x1111,
BLINK_SLOW_1 = 0xF0FF,
BLINK_VSLOW = 0xF000,
BLINK_MED_1 = 0xF0F0,
};
bool done_first_update;
AP_Int8 enable;
AP_Int8 primary_mode[2];
AP_Int8 actions[9];
AP_Int8 trim_auto;
AP_Int16 flags;
struct {
uint32_t start_ms;
uint16_t chan[4];
} trim;
uint32_t power_counter;
uint32_t throttle_low_counter;
uint32_t throttle_high_counter;
uint16_t last_ch5;
uint8_t last_mode_choice;
int32_t left_press_counter;
int32_t right_press_counter;
bool ignore_left_change;
int16_t throttle_mid = 500;
uint32_t throttle_arm_ms;
bool upgrade_to_loiter;
uint32_t last_action_ms;
uint32_t reset_turtle_start_ms;
// time when we were last told we are recording video
uint32_t last_video_ms;
// current blink indexes
uint16_t red_blink_pattern;
uint16_t green_blink_pattern;
uint8_t red_blink_index;
uint8_t green_blink_index;
uint16_t red_blink_count;
uint16_t green_blink_count;
uint8_t blink_disarm;
struct {
AP_Float volt_min;
AP_Float volt_max;
AP_Float thrust_min;
AP_Float thrust_max;
} filter;
// low-pass voltage
float filtered_voltage = 4.0;
uint8_t motor_log_counter;
// remember the last mode we set
control_mode_t last_set_mode = LOITER;
struct load_data {
uint16_t m[4];
};
enum load_type {
LOAD_TYPE_CONSTANT=0,
LOAD_TYPE_LOG1=1,
LOAD_TYPE_LOG2=2,
};
struct {
bool running;
uint32_t row;
uint8_t filter_counter;
AP_Float load_mul;
AP_Int8 load_filter;
AP_Int8 load_type;
} load_test;
static const struct load_data load_data1[];
};