diff --git a/software/sys_controller/ossc/av_controller.c b/software/sys_controller/ossc/av_controller.c
index 8cd8a24..6e58c67 100644
--- a/software/sys_controller/ossc/av_controller.c
+++ b/software/sys_controller/ossc/av_controller.c
@@ -564,7 +564,7 @@ void update_sc_config(mode_data_t *vm_in, mode_data_t *vm_out, vm_proc_config_t
 // Configure TVP7002 and scan converter logic based on the video mode
 void program_mode()
 {
-    int retval, fpga_pll_config_changed;
+    int retval, fpga_pll_config_changed, vmode_changed;
     alt_u8 h_syncinlen, v_syncinlen, macrovis, hdmitx_pclk_level, osd_x_size, osd_y_size;
     alt_u32 h_hz, h_synclen_px, pclk_i_hz, dotclk_hz, pll_h_total;
 
@@ -599,6 +599,7 @@ void program_mode()
         vm_conf.si_pclk_mult = 0;
         return;
     }
+    vmode_changed = !(cm.id == retval);
     cm.id = retval;
     vm_sel = cm.id;
 
@@ -644,11 +645,12 @@ void program_mode()
 
     tvp_source_setup(target_type,
                      pll_h_total,
-                     cm.cc.adc_pll_bw ? pll_h_total : vmode_in.timings.h_total,
+                     (cm.cc.adc_pll_bw == 0) ? vmode_in.timings.h_total : pll_h_total<<(cm.cc.adc_pll_bw-1),
                      cm.clkcnt,
                      cm.cc.tvp_hpll2x && (pclk_i_hz < 50000000UL),
                      (alt_u8)h_synclen_px,
-                     (alt_8)(cm.cc.clamp_offset-SIGNED_NUMVAL_ZERO));
+                     (alt_8)(cm.cc.clamp_offset-SIGNED_NUMVAL_ZERO),
+                     vmode_changed);
     set_lpf(cm.cc.video_lpf);
     set_csc(cm.cc.ypbpr_cs);
 
diff --git a/software/sys_controller/tvp7002/tvp7002.c b/software/sys_controller/tvp7002/tvp7002.c
index 6568f2e..96d2a08 100644
--- a/software/sys_controller/tvp7002/tvp7002.c
+++ b/software/sys_controller/tvp7002/tvp7002.c
@@ -306,6 +306,8 @@ void tvp_setup_hpll(alt_u16 h_samplerate, alt_u16 pixs_per_line, alt_u16 refclks
     cp_current = (40*Kvco[vco_range]+pixs_per_line/2) / pixs_per_line; //"+pixs_per_line/2" for fast rounding
     if (cp_current > 6)
         cp_current = 6;
+    else if (cp_current == 0)
+        cp_current = 1;
 
     printf("VCO range: %s\nCPC: %u\n", Kvco_str[vco_range], cp_current);
     tvp_writereg(TVP_HPLLCTRL, ((vco_range << 6) | (cp_current << 3)));
@@ -375,11 +377,15 @@ void tvp_set_alcfilt(alt_u8 nsv, alt_u8 nsh) {
     tvp_writereg(TVP_ALCFILT, (nsv<<3)|nsh);
 }
 
-void tvp_source_setup(video_type type, alt_u16 h_samplerate, alt_u16 pixs_per_line, alt_u16 refclks_per_line, alt_u8 plldivby2, alt_u8 h_synclen_px, alt_8 clamp_user_offset)
+void tvp_source_setup(video_type type, alt_u16 h_samplerate, alt_u16 pixs_per_line, alt_u16 refclks_per_line, alt_u8 plldivby2, alt_u8 h_synclen_px, alt_8 clamp_user_offset, alt_u8 vmode_changed)
 {
     // Due to short MVS width, clamp reference starts prematurely (at the end of MVS window). Adjust offset so that reference moves back to hsync trailing edge.
     alt_u8 clamp_ref_offset = h_synclen_px - (((30*h_samplerate)/refclks_per_line)+5)/10;
 
+    // Reset sync processing if mode changed to avoid occasional wobbling issue
+    if (vmode_changed)
+        tvp_writereg(TVP_MISCCTRL4, tvp_readreg(TVP_MISCCTRL4) | (1<<7));
+
     // Clamp and ALC
     tvp_set_clamp_alc(type, clamp_ref_offset, clamp_user_offset, 1);
 
diff --git a/software/sys_controller/tvp7002/tvp7002.h b/software/sys_controller/tvp7002/tvp7002.h
index ca64d29..3a92300 100644
--- a/software/sys_controller/tvp7002/tvp7002.h
+++ b/software/sys_controller/tvp7002/tvp7002.h
@@ -114,7 +114,7 @@ void tvp_set_sog_thold(alt_u8 val);
 
 void tvp_set_alcfilt(alt_u8 nsv, alt_u8 nsh);
 
-void tvp_source_setup(video_type type, alt_u16 h_samplerate, alt_u16 pixs_per_line, alt_u16 refclks_per_line, alt_u8 plldivby2, alt_u8 h_synclen_px, alt_8 clamp_user_offset);
+void tvp_source_setup(video_type type, alt_u16 h_samplerate, alt_u16 pixs_per_line, alt_u16 refclks_per_line, alt_u8 plldivby2, alt_u8 h_synclen_px, alt_8 clamp_user_offset, alt_u8 vmode_changed);
 
 void tvp_source_sel(tvp_input_t input, tvp_sync_input_t syncinput, video_format fmt);