mirror of https://github.com/ArduPilot/ardupilot
Sub: Bug fix for camera_tilt_smooth() conflict with RC_CHANNELS_OVERRIDE
This commit is contained in:
parent
0ce2896e22
commit
c1959952b3
|
@ -50,7 +50,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
|
||||||
SCHED_TASK(gcs_send_deferred, 50, 550),
|
SCHED_TASK(gcs_send_deferred, 50, 550),
|
||||||
SCHED_TASK(gcs_data_stream_send, 50, 550),
|
SCHED_TASK(gcs_data_stream_send, 50, 550),
|
||||||
SCHED_TASK(update_mount, 50, 75),
|
SCHED_TASK(update_mount, 50, 75),
|
||||||
SCHED_TASK(camera_tilt_smooth, 50, 50),
|
|
||||||
SCHED_TASK(update_trigger, 50, 75),
|
SCHED_TASK(update_trigger, 50, 75),
|
||||||
SCHED_TASK(ten_hz_logging_loop, 10, 350),
|
SCHED_TASK(ten_hz_logging_loop, 10, 350),
|
||||||
SCHED_TASK(twentyfive_hz_logging, 25, 110),
|
SCHED_TASK(twentyfive_hz_logging, 25, 110),
|
||||||
|
@ -224,6 +223,7 @@ void Sub::fifty_hz_loop()
|
||||||
if (hal.rcin->new_input()) {
|
if (hal.rcin->new_input()) {
|
||||||
// Update servo output
|
// Update servo output
|
||||||
RC_Channels::set_pwm_all();
|
RC_Channels::set_pwm_all();
|
||||||
|
SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, 20.0f, 0.02f);
|
||||||
SRV_Channels::output_ch_all();
|
SRV_Channels::output_ch_all();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -713,7 +713,6 @@ private:
|
||||||
void init_joystick();
|
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 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 handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false);
|
||||||
void camera_tilt_smooth();
|
|
||||||
JSButton* get_button(uint8_t index);
|
JSButton* get_button(uint8_t index);
|
||||||
void default_js_buttons(void);
|
void default_js_buttons(void);
|
||||||
void set_throttle_zero_flag(int16_t throttle_control);
|
void set_throttle_zero_flag(int16_t throttle_control);
|
||||||
|
|
|
@ -7,8 +7,6 @@
|
||||||
namespace {
|
namespace {
|
||||||
int16_t mode_switch_pwm = 1100;
|
int16_t mode_switch_pwm = 1100;
|
||||||
float cam_tilt = 1500.0;
|
float cam_tilt = 1500.0;
|
||||||
float cam_tilt_goal = 1500.0;
|
|
||||||
float cam_tilt_alpha = 0.97;
|
|
||||||
int16_t lights1 = 1100;
|
int16_t lights1 = 1100;
|
||||||
int16_t lights2 = 1100;
|
int16_t lights2 = 1100;
|
||||||
int16_t rollTrim = 0;
|
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[4] = mode_switch_pwm; // for testing only
|
||||||
channels[5] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900); // forward for ROV
|
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[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[8] = lights1; // lights 1
|
||||||
channels[9] = lights2; // lights 2
|
channels[9] = lights2; // lights 2
|
||||||
channels[10] = video_switch; // video switch
|
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;
|
toggle_mode = false;
|
||||||
break;
|
break;
|
||||||
case JSButton::button_function_t::k_mount_center:
|
case JSButton::button_function_t::k_mount_center:
|
||||||
cam_tilt_goal = g.cam_tilt_center;
|
cam_tilt = g.cam_tilt_center;
|
||||||
break;
|
break;
|
||||||
case JSButton::button_function_t::k_mount_tilt_up: {
|
case JSButton::button_function_t::k_mount_tilt_up: {
|
||||||
uint8_t i;
|
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 min = ch->get_output_min();
|
||||||
uint16_t max = ch->get_output_max();
|
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;
|
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 min = ch->get_output_min();
|
||||||
uint16_t max = ch->get_output_max();
|
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;
|
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()
|
void Sub::default_js_buttons()
|
||||||
{
|
{
|
||||||
JSButton::button_function_t defaults[16][2] = {
|
JSButton::button_function_t defaults[16][2] = {
|
||||||
|
|
Loading…
Reference in New Issue