Skip to content

Commit 3d79f23

Browse files
committed
Rover: remove sending of scaled_outputs in Rover
1 parent 6cec184 commit 3d79f23

File tree

2 files changed

+0
-37
lines changed

2 files changed

+0
-37
lines changed

Rover/GCS_MAVLink_Rover.cpp

Lines changed: 0 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -120,36 +120,6 @@ void GCS_MAVLINK_Rover::send_nav_controller_output() const
120120
control_mode->crosstrack_error());
121121
}
122122

123-
void GCS_MAVLINK_Rover::send_servo_out()
124-
{
125-
float motor1, motor3;
126-
if (rover.g2.motors.have_skid_steering()) {
127-
motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleLeft) * 0.001f);
128-
motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleRight) * 0.001f);
129-
} else {
130-
motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f);
131-
motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
132-
}
133-
mavlink_msg_rc_channels_scaled_send(
134-
chan,
135-
millis(),
136-
0, // port 0
137-
motor1,
138-
0,
139-
motor3,
140-
0,
141-
0,
142-
0,
143-
0,
144-
0,
145-
#if AP_RSSI_ENABLED
146-
receiver_rssi()
147-
#else
148-
255
149-
#endif
150-
);
151-
}
152-
153123
int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const
154124
{
155125
return rover.g2.motors.get_throttle();
@@ -423,11 +393,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
423393
{
424394
switch (id) {
425395

426-
case MSG_SERVO_OUT:
427-
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
428-
send_servo_out();
429-
break;
430-
431396
case MSG_WHEEL_DISTANCE:
432397
CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
433398
rover.send_wheel_encoder_distance(chan);

Rover/GCS_MAVLink_Rover.h

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,6 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK
5050
void handle_radio(const mavlink_message_t &msg);
5151
void handle_landing_target(const mavlink_landing_target_t &msg, uint32_t timestamp_ms) override;
5252

53-
void send_servo_out();
54-
5553
uint8_t base_mode() const override;
5654
MAV_STATE vehicle_system_status() const override;
5755

0 commit comments

Comments
 (0)