Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Bug fix for camera_tilt_smooth() conflict with RC_CHANNELS_OVERRIDE

This commit is contained in:
Jacob Walser 2017-03-09 14:29:01 -05:00 committed by Randy Mackay
parent 0ce2896e22
commit c1959952b3
3 changed files with 5 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -50,7 +50,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(camera_tilt_smooth, 50, 50),
SCHED_TASK(update_trigger, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
@ -224,6 +223,7 @@ void Sub::fifty_hz_loop()
if (hal.rcin->new_input()) {
// Update servo output
RC_Channels::set_pwm_all();
SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, 20.0f, 0.02f);
SRV_Channels::output_ch_all();
}
}

1
ArduSub/Sub.h
View File

@ -713,7 +713,6 @@ private:
void init_joystick();
void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons);
void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false);
void camera_tilt_smooth();
JSButton* get_button(uint8_t index);
void default_js_buttons(void);
void set_throttle_zero_flag(int16_t throttle_control);

27
ArduSub/joystick.cpp
View File

@ -7,8 +7,6 @@
namespace {
int16_t mode_switch_pwm = 1100;
float cam_tilt = 1500.0;
float cam_tilt_goal = 1500.0;
float cam_tilt_alpha = 0.97;
int16_t lights1 = 1100;
int16_t lights2 = 1100;
int16_t rollTrim = 0;
@ -87,7 +85,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
channels[4] = mode_switch_pwm; // for testing only
channels[5] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900); // forward for ROV
channels[6] = constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900); // lateral for ROV
channels[7] = 0xffff; // camera tilt (sent in camera_tilt_smooth)
channels[7] = cam_tilt; // camera tilt
channels[8] = lights1; // lights 1
channels[9] = lights2; // lights 2
channels[10] = video_switch; // video switch
@ -160,7 +158,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
toggle_mode = false;
break;
case JSButton::button_function_t::k_mount_center:
cam_tilt_goal = g.cam_tilt_center;
cam_tilt = g.cam_tilt_center;
break;
case JSButton::button_function_t::k_mount_tilt_up: {
uint8_t i;
@ -173,7 +171,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
uint16_t min = ch->get_output_min();
uint16_t max = ch->get_output_max();
cam_tilt_goal = constrain_int16(cam_tilt_goal-g.cam_tilt_step,min,max);
cam_tilt = constrain_int16(cam_tilt-g.cam_tilt_step,min,max);
}
}
break;
@ -188,7 +186,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
uint16_t min = ch->get_output_min();
uint16_t max = ch->get_output_max();
cam_tilt_goal = constrain_int16(cam_tilt_goal+g.cam_tilt_step,min,max);
cam_tilt = constrain_int16(cam_tilt+g.cam_tilt_step,min,max);
}
}
break;
@ -424,23 +422,6 @@ JSButton* Sub::get_button(uint8_t index)
}
}
void Sub::camera_tilt_smooth()
{
if (failsafe.manual_control) {
return;
}
int16_t channels[11];
for (uint8_t i = 0 ; i < 11 ; i++) {
channels[i] = 0xffff;
}
// Camera tilt low pass filter
cam_tilt = cam_tilt*cam_tilt_alpha+cam_tilt_goal*(1-cam_tilt_alpha);
channels[7] = cam_tilt;
failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10);
}
void Sub::default_js_buttons()
{
JSButton::button_function_t defaults[16][2] = {