From 1f46bd3a6f7891bba240124c01bceeec653a0a0f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 7 Feb 2024 15:14:28 +1100 Subject: [PATCH] Blimp: move RC bits in mavlink to common code Plane's semantics change to be like Copter. Rover, Sub and Tracker will start reporting the bits --- Blimp/Blimp.h | 2 +- Blimp/GCS_Blimp.cpp | 10 ---------- Blimp/mode.cpp | 2 +- Blimp/radio.cpp | 7 ++----- 4 files changed, 4 insertions(+), 17 deletions(-) diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 63f5eb888f..9566f9294e 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -151,7 +151,7 @@ private: uint8_t logging_started : 1; // 4 // true if logging has started uint8_t land_complete : 1; // 5 // true if we have detected a landing uint8_t new_radio_frame : 1; // 6 // Set true if we have new PWM data to act on from the Radio - uint8_t rc_receiver_present : 1; // 7 // true if we have an rc receiver present (i.e. if we've ever received an update + uint8_t rc_receiver_present_unused : 1; // 7 // UNUSED uint8_t compass_mot : 1; // 8 // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // 9 // true if we are currently performing the motors test uint8_t initialised : 1; // 10 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes diff --git a/Blimp/GCS_Blimp.cpp b/Blimp/GCS_Blimp.cpp index ed7c212014..cc2ca07232 100644 --- a/Blimp/GCS_Blimp.cpp +++ b/Blimp/GCS_Blimp.cpp @@ -32,14 +32,4 @@ void GCS_Blimp::update_vehicle_sensor_status_flags(void) control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; - - const Blimp::ap_t &ap = blimp.ap; - - if (ap.rc_receiver_present) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - if (ap.rc_receiver_present && !blimp.failsafe.radio) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } } diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index 9165036cb7..849b4af275 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -162,7 +162,7 @@ void Mode::update_navigation() void Mode::get_pilot_input(Vector3f &pilot, float &yaw) { // throttle failsafe check - if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) { + if (blimp.failsafe.radio || !rc().has_ever_seen_rc_input()) { pilot.y = 0; pilot.x = 0; pilot.z = 0; diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index 02a42672e1..fddce3e914 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -61,9 +61,6 @@ void Blimp::read_radio() set_throttle_and_failsafe(channel_up->get_radio_in()); set_throttle_zero_flag(channel_up->get_control_in()); - // RC receiver must be attached if we've just got input - ap.rc_receiver_present = true; - const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f; rc_throttle_control_in_filter.apply(channel_up->get_control_in(), dt); last_radio_update_ms = tnow_ms; @@ -87,7 +84,7 @@ void Blimp::read_radio() // throttle failsafe not enabled return; } - if (!ap.rc_receiver_present && !motors->armed()) { + if (!rc().has_ever_seen_rc_input() && !motors->armed()) { // we only failsafe if we are armed OR we have ever seen an RC receiver return; } @@ -109,7 +106,7 @@ void Blimp::set_throttle_and_failsafe(uint16_t throttle_pwm) if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) { // if we are already in failsafe or motors not armed pass through throttle and exit - if (failsafe.radio || !(ap.rc_receiver_present || motors->armed())) { + if (failsafe.radio || !(rc().has_ever_seen_rc_input() || motors->armed())) { return; }