mirror of https://github.com/ArduPilot/ardupilot
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:
parent
821cfcd6ac
commit
d462756cbc
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue