ArduCopter: 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
This commit is contained in:
Peter Barker 2024-02-07 15:14:28 +11:00 committed by Peter Barker
parent 821cfcd6ac
commit d462756cbc
6 changed files with 8 additions and 21 deletions

View File

@ -60,7 +60,7 @@ void Copter::update_throttle_hover()
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if (failsafe.radio || !ap.rc_receiver_present) {
if (failsafe.radio || !rc().has_ever_seen_rc_input()) {
return 0.0f;
}

View File

@ -374,7 +374,7 @@ private:
uint8_t land_complete : 1; // 7 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio
uint8_t usb_connected_unused : 1; // 9 // UNUSED
uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update
uint8_t rc_receiver_present_unused : 1; // 10 // UNUSED
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test
uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes

View File

@ -42,16 +42,6 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION;
const Copter::ap_t &ap = copter.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 && !copter.failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
// update flightmode-specific flags:
control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

View File

@ -472,7 +472,7 @@ void Copter::notify_flight_mode() {
void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const
{
// throttle failsafe check
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
roll_out_cd = 0.0;
pitch_out_cd = 0.0;
return;
@ -494,7 +494,7 @@ Vector2f Mode::get_pilot_desired_velocity(float vel_max) const
Vector2f vel;
// throttle failsafe check
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
return vel;
}
// fetch roll and pitch inputs
@ -1013,7 +1013,7 @@ Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
float Mode::get_pilot_desired_yaw_rate(float yaw_in)
{
// throttle failsafe check
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
if (copter.failsafe.radio || !rc().has_ever_seen_rc_input()) {
return 0.0f;
}

View File

@ -113,7 +113,7 @@ void ModeTurtle::change_motor_direction(bool reverse)
void ModeTurtle::run()
{
const float flip_power_factor = 1.0f - CRASH_FLIP_EXPO * 0.01f;
const bool norc = copter.failsafe.radio || !copter.ap.rc_receiver_present;
const bool norc = copter.failsafe.radio || !rc().has_ever_seen_rc_input();
const float stick_deflection_pitch = norc ? 0.0f : channel_pitch->norm_input_dz();
const float stick_deflection_roll = norc ? 0.0f : channel_roll->norm_input_dz();
const float stick_deflection_yaw = norc ? 0.0f : channel_yaw->norm_input_dz();

View File

@ -87,9 +87,6 @@ void Copter::read_radio()
set_throttle_and_failsafe(channel_throttle->get_radio_in());
set_throttle_zero_flag(channel_throttle->get_control_in());
// RC receiver must be attached if we've just got input
ap.rc_receiver_present = true;
// pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
radio_passthrough_to_motors();
@ -115,7 +112,7 @@ void Copter::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;
}
@ -137,7 +134,7 @@ void Copter::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;
}