From 0ce2896e22d849c588d8265d42568acd008f029b Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 9 Mar 2017 12:50:58 -0500 Subject: [PATCH] Sub: Remove rc receiver code --- ArduSub/ArduSub.cpp | 32 +++++++++++++------------------ ArduSub/GCS_Mavlink.cpp | 7 +------ ArduSub/Sub.cpp | 1 - ArduSub/Sub.h | 9 +-------- ArduSub/esc_calibration.cpp | 4 ---- ArduSub/radio.cpp | 38 ------------------------------------- 6 files changed, 15 insertions(+), 76 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index bfdbebd6f4..bd257bf331 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -25,8 +25,7 @@ and the maximum time they are expected to take (in microseconds) */ const AP_Scheduler::Task Sub::scheduler_tasks[] = { - SCHED_TASK(rc_loop, 100, 130), - SCHED_TASK(throttle_loop, 50, 75), + SCHED_TASK(fifty_hz_loop, 50, 75), SCHED_TASK(update_GPS, 50, 200), #if OPTFLOW == ENABLED SCHED_TASK(update_optical_flow, 200, 160), @@ -213,22 +212,20 @@ void Sub::fast_loop() } } -// rc_loops - reads user input from transmitter/receiver -// called at 100hz -void Sub::rc_loop() -{ - // Read radio - // ----------------------------------------- - read_radio(); - failsafe_manual_control_check(); -} - -// throttle_loop - should be run at 50 hz -// --------------------------- -void Sub::throttle_loop() +// 50 Hz tasks +void Sub::fifty_hz_loop() { // check auto_armed status update_auto_armed(); + + // check pilot input failsafe + failsafe_manual_control_check(); + + if (hal.rcin->new_input()) { + // Update servo output + RC_Channels::set_pwm_all(); + SRV_Channels::output_ch_all(); + } } // update_mount - update camera mount position @@ -466,13 +463,10 @@ void Sub::update_simple_mode(void) float rollx, pitchx; // exit immediately if no new radio frame or not in simple mode - if (ap.simple_mode == 0 || !ap.new_radio_frame) { + if (ap.simple_mode == 0) { return; } - // mark radio frame as consumed - ap.new_radio_frame = false; - if (ap.simple_mode == 1) { // rotate roll, pitch input by -initial simple heading (i.e. north facing) rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw; diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 5908075a9a..1b003459d5 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -130,9 +130,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } #endif - if (ap.rc_receiver_present) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } // all present sensors enabled by default except altitude and position control and motors which we will set individually control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & @@ -181,9 +178,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan) control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; } #endif - if (ap.rc_receiver_present) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } + if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; } diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index fde7c842e9..d9d18ea1bf 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -50,7 +50,6 @@ Sub::Sub(void) : target_rangefinder_alt(0.0f), baro_alt(0), baro_climbrate(0.0f), - rc_throttle_control_in_filter(1.0f), auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), yaw_look_at_WP_bearing(0.0f), yaw_look_at_heading(0), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 05d7609e3e..e4e1db986f 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -237,9 +237,7 @@ private: uint8_t pre_arm_check : 1; // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t auto_armed : 1; // stops auto missions from beginning until throttle is raised uint8_t logging_started : 1; // true if dataflash logging has started - uint8_t new_radio_frame : 1; // Set true if we have new PWM data to act on from the Radio uint8_t usb_connected : 1; // true if APM is powered from USB connection - uint8_t rc_receiver_present : 1; // true if we have an rc receiver present (i.e. if we've ever received an update uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // true if we are currently performing the motors test uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes @@ -360,9 +358,6 @@ private: int32_t quarter_turn_count; uint8_t last_turn_state; - // filtered pilot's throttle input used to cancel landing if throttle held high - LowPassFilterFloat rc_throttle_control_in_filter; - // 3D Location vectors // Current location of the Sub (altitude is relative to home) Location_Class current_loc; @@ -484,7 +479,7 @@ private: void perf_update(void); void fast_loop(); void rc_loop(); - void throttle_loop(); + void fifty_hz_loop(); void update_mount(); void update_trigger(); void update_batt_compass(void); @@ -715,7 +710,6 @@ private: void init_rc_in(); void init_rc_out(); void enable_motor_output(); - void read_radio(); void init_joystick(); void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false); @@ -723,7 +717,6 @@ private: JSButton* get_button(uint8_t index); void default_js_buttons(void); void set_throttle_zero_flag(int16_t throttle_control); - void radio_passthrough_to_motors(); void init_barometer(bool save); void read_barometer(void); void init_rangefinder(void); diff --git a/ArduSub/esc_calibration.cpp b/ArduSub/esc_calibration.cpp index bb378c88f2..294e30915e 100644 --- a/ArduSub/esc_calibration.cpp +++ b/ArduSub/esc_calibration.cpp @@ -91,10 +91,6 @@ void Sub::esc_calibration_passthrough() // flash LEDS AP_Notify::flags.esc_calibration = true; - // read pilot input - read_radio(); - hal.scheduler->delay(10); - // pass through to motors motors.set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); } diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index e871d78af8..5fed248700 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -67,11 +67,6 @@ void Sub::init_rc_out() motors.init((AP_Motors::motor_frame_class)g.frame_configuration.get(), (AP_Motors::motor_frame_type)0); motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); - for (uint8_t i = 0; i < 5; i++) { - hal.scheduler->delay(20); - read_radio(); - } - // check if we should enter esc calibration mode esc_calibration_startup_check(); @@ -92,34 +87,6 @@ void Sub::enable_motor_output() motors.output_min(); } -void Sub::read_radio() -{ - static uint32_t last_update_ms = 0; - uint32_t tnow_ms = millis(); - - if (hal.rcin->new_input()) { - ap.new_radio_frame = true; - RC_Channels::set_pwm_all(); - - set_throttle_zero_flag(channel_throttle->get_control_in()); - - // flag we must have an rc receiver attached - if (!failsafe.rc_override_active) { - ap.rc_receiver_present = true; - } - - // update output on any aux channels, for manual passthru - SRV_Channels::output_ch_all(); - - // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters) - radio_passthrough_to_motors(); - - float dt = (tnow_ms - last_update_ms)*1.0e-3f; - rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt); - last_update_ms = tnow_ms; - } -} - #define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400 // set_throttle_zero_flag - set throttle_zero flag from debounced throttle control // throttle_zero is used to determine if the pilot intends to shut down the motors @@ -141,8 +108,3 @@ void Sub::set_throttle_zero_flag(int16_t throttle_control) } } -// pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax copters) -void Sub::radio_passthrough_to_motors() -{ - motors.set_radio_passthrough(channel_roll->get_control_in()/1000.0f, channel_pitch->get_control_in()/1000.0f, channel_throttle->get_control_in()/1000.0f, channel_yaw->get_control_in()/1000.0f); -}