mirror of https://github.com/ArduPilot/ardupilot
APMrover2: move try_send_message handling of RC_CHANNELS_RAW up
This commit is contained in:
parent
b76fa57108
commit
9e1811027d
|
@ -66,7 +66,6 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
|
|||
SCHED_TASK(read_control_switch, 7, 200),
|
||||
SCHED_TASK(read_aux_switch, 10, 200),
|
||||
SCHED_TASK_CLASS(AP_BattMonitor, &rover.battery, read, 10, 300),
|
||||
SCHED_TASK(read_receiver_rssi, 10, 200),
|
||||
SCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events, 50, 200),
|
||||
SCHED_TASK(check_usb_mux, 3, 200),
|
||||
#if MOUNT == ENABLED
|
||||
|
|
|
@ -169,7 +169,7 @@ void Rover::send_servo_out(mavlink_channel_t chan)
|
|||
0,
|
||||
0,
|
||||
0,
|
||||
receiver_rssi);
|
||||
rssi.read_receiver_rssi_uint8());
|
||||
}
|
||||
|
||||
void Rover::send_vfr_hud(mavlink_channel_t chan)
|
||||
|
@ -333,11 +333,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|||
rover.send_servo_out(chan);
|
||||
break;
|
||||
|
||||
case MSG_RADIO_IN:
|
||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS);
|
||||
send_radio_in(rover.receiver_rssi);
|
||||
break;
|
||||
|
||||
case MSG_SERVO_OUTPUT_RAW:
|
||||
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
||||
send_servo_output_raw(false);
|
||||
|
|
|
@ -264,9 +264,6 @@ private:
|
|||
// true if we have a position estimate from AHRS
|
||||
bool have_position;
|
||||
|
||||
// receiver RSSI
|
||||
uint8_t receiver_rssi;
|
||||
|
||||
// the time when the last HEARTBEAT message arrived from a GCS
|
||||
uint32_t last_heartbeat_ms;
|
||||
|
||||
|
@ -525,7 +522,6 @@ private:
|
|||
void init_visual_odom();
|
||||
void update_visual_odom();
|
||||
void update_wheel_encoder();
|
||||
void read_receiver_rssi(void);
|
||||
void compass_cal_update(void);
|
||||
void accel_cal_update(void);
|
||||
void read_rangefinders(void);
|
||||
|
|
|
@ -145,13 +145,6 @@ void Rover::update_wheel_encoder()
|
|||
wheel_encoder_last_ekf_update_ms = now;
|
||||
}
|
||||
|
||||
// read the receiver RSSI as an 8 bit number for MAVLink
|
||||
// RC_CHANNELS_SCALED message
|
||||
void Rover::read_receiver_rssi(void)
|
||||
{
|
||||
receiver_rssi = rssi.read_receiver_rssi_uint8();
|
||||
}
|
||||
|
||||
// Calibrate compass
|
||||
void Rover::compass_cal_update() {
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
|
|
Loading…
Reference in New Issue