3 #if TOY_MODE_ENABLED == ENABLED 6 #define TOY_COMMAND_DELAY 15 7 #define TOY_LONG_PRESS_COUNT 15 8 #define TOY_LAND_MANUAL_DISARM_COUNT 40 9 #define TOY_LAND_DISARM_COUNT 1 10 #define TOY_LAND_ARM_COUNT 1 11 #define TOY_RIGHT_PRESS_COUNT 1 12 #define TOY_ACTION_DELAY_MS 200 13 #define TOY_DESCENT_SLOW_HEIGHT 5 14 #define TOY_DESCENT_SLOW_RAMP 3 15 #define TOY_DESCENT_SLOW_MIN 300 16 #define TOY_RESET_TURTLE_TIME 5000 18 #define ENABLE_LOAD_TEST 0 195 if (!
copter.motors->armed()) {
222 bool left_button =
false;
223 bool right_button =
false;
224 bool left_action_button =
false;
225 bool right_action_button =
false;
226 bool power_button =
false;
227 bool left_change =
false;
233 if (
copter.failsafe.radio || ch5_in < 900) {
248 left_change = ((ch5_in > 1700 && last_ch5 <= 1700) || (ch5_in <= 1700 && last_ch5 > 1700));
253 left_button = (ch5_in > 2050 || (ch5_in > 1050 && ch5_in < 1150));
254 right_button = (ch6_in > 1500);
255 uint8_t ch7_bits = (ch7_in>1000)?uint8_t((ch7_in-1000)/100):0;
256 left_action_button = (ch7_bits&1) != 0;
257 right_action_button = (ch7_bits&2) != 0;
258 power_button = (ch7_bits&4) != 0;
262 uint8_t ch5_bits = (ch5_in>1000)?uint8_t((ch5_in-1000)/100):0;
263 uint8_t ch6_bits = (ch6_in>1000)?uint8_t((ch6_in-1000)/100):0;
264 left_button = (ch5_bits & 4) != 0;
265 right_button = (ch5_bits & 2) != 0;
266 right_action_button = (ch6_bits & 1) != 0;
267 left_action_button = (ch6_bits & 2) != 0;
268 power_button = (ch6_bits & 4) != 0;
274 uint8_t action_input = 0;
275 if (left_action_button) {
277 }
else if (right_action_button) {
279 }
else if (power_button) {
283 if (action_input != 0 && left_button) {
287 }
else if (left_button) {
293 bool reset_combination = left_action_button && right_action_button;
294 if (reset_combination && abs(
copter.ahrs.roll_sensor) > 160) {
304 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: WiFi reset");
311 if (reset_combination) {
341 if (action ==
ACTION_NONE && left_change && !left_button) {
393 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: action %u", action);
398 bool throttle_at_min =
399 copter.channel_throttle->get_control_in() < 150;
402 bool throttle_near_max =
403 copter.channel_throttle->get_control_in() > 700;
410 const uint8_t disarm_limit =
copter.flightmode->has_manual_throttle()?TOY_LAND_MANUAL_DISARM_COUNT:TOY_LAND_DISARM_COUNT;
412 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: throttle disarm");
413 copter.init_disarm_motors();
425 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: throttle arm");
432 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: ALT_HOLD update arm");
433 #if AC_FENCE == ENABLED 434 copter.fence.enable(
false);
436 if (!
copter.init_arm_motors(
true)) {
438 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: ALT_HOLD arm failed");
462 #if AC_FENCE == ENABLED 463 copter.fence.enable(
true);
465 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: LOITER update");
470 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: RTL cancel");
493 #if MODE_ACRO_ENABLED == ENABLED 496 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: ACRO is disabled");
545 #if MODE_THROW_ENABLED == ENABLED 548 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: THROW is disabled");
570 if (
copter.motors->armed()) {
571 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: Force disarm");
572 copter.init_disarm_motors();
590 if (!
copter.motors->armed()) {
592 }
else if (old_mode ==
RTL) {
595 }
else if (old_mode ==
LAND) {
599 }
else if (
copter.landing_with_GPS()) {
604 }
else if (
copter.flightmode->requires_GPS()) {
620 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: load_test off");
621 copter.init_disarm_motors();
625 #if AC_FENCE == ENABLED 626 copter.fence.enable(
false);
628 if (
copter.init_arm_motors(
true)) {
630 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: load_test on");
632 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: load_test failed");
644 if (new_mode !=
copter.control_mode) {
646 #if AC_FENCE == ENABLED 647 copter.fence.enable(
false);
652 #if AC_FENCE == ENABLED 653 if (
copter.flightmode->requires_GPS()) {
654 copter.fence.enable(
true);
658 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: %u FAILED", (
unsigned)new_mode);
659 if (new_mode ==
RTL) {
663 #if AC_FENCE == ENABLED 664 if (
copter.landing_with_GPS()) {
665 copter.fence.enable(
true);
679 if (
copter.control_mode == mode) {
682 if (!
copter.set_mode(mode, reason)) {
703 uint16_t throttle_trim =
copter.channel_throttle->get_radio_trim();
704 if (abs(throttle_trim - 1500) <=
trim_auto) {
709 int16_t new_value = 1000UL * (throttle_trim - ch_min) / (ch_max - ch_min);
712 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: thr mid %d\n",
723 const uint16_t noise_limit = 2;
724 for (uint8_t i=0; i<4; i++) {
734 if (
trim.start_ms == 0) {
736 memcpy(
trim.chan, chan, 4*
sizeof(uint16_t));
742 for (uint8_t i=0; i<4; i++) {
743 if (abs(
trim.chan[i] - chan[i]) > noise_limit) {
745 memcpy(
trim.chan, chan, 4*
sizeof(uint16_t));
751 if (now -
trim.start_ms < 4000) {
759 uint8_t need_trim = 0;
760 for (uint8_t i=0; i<4; i++) {
766 if (need_trim == 0) {
769 for (uint8_t i=0; i<4; i++) {
770 if (need_trim & (1U<<i)) {
776 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: trim %u %u %u %u\n",
777 chan[0], chan[1], chan[2], chan[3]);
785 bool needs_gps =
copter.flightmode->requires_GPS();
789 bool sticks_centered =
790 copter.channel_roll->get_control_in() == 0 &&
791 copter.channel_pitch->get_control_in() == 0 &&
792 copter.channel_yaw->get_control_in() == 0;
794 if (!sticks_centered) {
795 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: sticks not centered\n");
801 if (needs_gps &&
copter.arming.gps_checks(
false)) {
802 #if AC_FENCE == ENABLED 804 copter.fence.enable(
true);
806 copter.init_arm_motors(
false);
807 if (!
copter.motors->armed()) {
809 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: GPS arming failed");
811 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: GPS armed motors");
813 }
else if (needs_gps) {
816 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: GPS arming failed");
818 #if AC_FENCE == ENABLED 820 copter.fence.enable(
false);
822 copter.init_arm_motors(
false);
823 if (!
copter.motors->armed()) {
825 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: non-GPS arming failed");
827 gcs().
send_text(MAV_SEVERITY_ERROR,
"Tmode: non-GPS armed motors");
839 const uint32_t soft_start_ms = 5000;
840 const uint16_t throttle_start = 600 +
copter.g.throttle_deadzone;
842 throttle_control =
MIN(throttle_control, 500);
845 throttle_control =
MIN(throttle_control, throttle_start + p * (1000 - throttle_start));
849 float height =
copter.inertial_nav.get_altitude() * 0.01 -
copter.arming_altitude_m;
850 if (throttle_control < 500 &&
851 height < TOY_DESCENT_SLOW_HEIGHT + TOY_DESCENT_SLOW_RAMP &&
854 TOY_DESCENT_SLOW_HEIGHT, TOY_DESCENT_SLOW_HEIGHT+TOY_DESCENT_SLOW_RAMP);
855 if (throttle_control < limit) {
857 throttle_control = limit;
903 uint16_t pattern = 0;
916 if (
copter.motors->armed()) {
935 if (msg->msgid != MAVLINK_MSG_ID_NAMED_VALUE_INT) {
938 mavlink_named_value_int_t m;
939 mavlink_msg_named_value_int_decode(msg, &m);
940 if (strncmp(m.name,
"BLINKR", 10) == 0) {
944 }
else if (strncmp(m.name,
"BLINKG", 10) == 0) {
948 }
else if (strncmp(m.name,
"VNOTIFY", 10) == 0) {
958 }
else if (strncmp(m.name,
"WIFICHAN", 10) == 0) {
959 #if HAL_RCINPUT_WITH_AP_RADIO 965 }
else if (strncmp(m.name,
"LOGDISARM", 10) == 0) {
967 AP_Int8 *log_disarmed = (AP_Int8 *)
AP_Param::find(
"LOG_DISARMED", &vtype);
969 log_disarmed->set(int8_t(m.value));
979 mavlink_msg_named_value_int_send(MAVLINK_COMM_1,
AP_HAL::millis(), name, value);
988 for (uint8_t i=0; i<num_motors; i++) {
989 thrust[i] *= thrust_mul;
998 pwm[0], pwm[1], pwm[2], pwm[3]);
1003 #if ENABLE_LOAD_TEST 1012 for (uint8_t i=0; i<4; i++) {
1024 for (uint8_t i=0; i<4; i++) {
1035 for (uint8_t i=0; i<4; i++) {
1042 for (uint8_t i=0; i<4; i++) {
1048 if (
copter.failsafe.battery) {
1049 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: load_test off (battery)");
1050 copter.init_disarm_motors();
1054 #endif // ENABLE_LOAD_TEST 1064 float field =
copter.compass.get_field().length();
1066 if (offsets.
length() >
copter.compass.get_offsets_max() ||
1067 field < 200 || field > 800 ||
1068 !
copter.compass.configured()) {
1070 gcs().
send_text(MAV_SEVERITY_INFO,
"Tmode: enable compass learning");
1076 #endif // TOY_MODE_ENABLED
bool get_soft_armed() const
int32_t right_press_counter
int16_t get_radio_trim() const
virtual uint16_t read(uint8_t ch)=0
static RC_Channel * rc_channel(uint8_t chan)
int32_t left_press_counter
uint32_t throttle_low_counter
void thrust_limiting(float *thrust, uint8_t num_motors)
#define AP_GROUPINFO(name, idx, clazz, element, def)
uint32_t throttle_high_counter
static AP_Param * find(const char *name, enum ap_var_type *ptype)
uint16_t green_blink_pattern
void handle_message(mavlink_message_t *msg)
void arm_check_compass(void)
control_mode_t last_set_mode
struct ToyMode::@19 filter
uint32_t reset_turtle_start_ms
void set_and_save_radio_trim(int16_t val)
static AP_Radio * instance(void)
#define AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags)
void send_named_int(const char *name, int32_t value)
float linear_interpolate(float low_output, float high_output, float var_value, float var_low, float var_high)
void Log_Write(const char *name, const char *labels, const char *fmt,...)
uint16_t green_blink_count
uint8_t green_blink_index
static const struct AP_Param::GroupInfo var_info[]
static DataFlash_Class * instance(void)
void set_wifi_channel(uint8_t channel)
uint32_t failsafe_battery
virtual void write(uint8_t ch, uint16_t period_us)=0
void send_text(MAV_SEVERITY severity, const char *fmt,...)
uint8_t motor_log_counter
struct ToyMode::@20 load_test
void throttle_adjust(float &throttle_control)
bool is_v2450_buttons(void) const
void NOINLINE filter(float &dst, float val, const byte k)
static struct notify_flags_and_values_type flags
AP_HAL_MAIN_CALLBACKS & copter
int16_t get_radio_min() const
int16_t get_radio_max() const
static uint16_t get_radio_in(const uint8_t chan)
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
static const struct load_data load_data1[]
static struct notify_events_type events
bool is_f412_buttons(void) const
enum toy_action last_action
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
bool set_and_remember_mode(control_mode_t mode, mode_reason_t reason)
uint16_t red_blink_pattern