diff --git a/driver/include/sensor.h b/driver/include/sensor.h
index 5584243..f50501d 100755
--- a/driver/include/sensor.h
+++ b/driver/include/sensor.h
@@ -53,14 +53,42 @@ typedef struct {
     uint8_t VER;
 } sensor_id_t;
 
+typedef struct {
+    framesize_t framesize;//0 - 10
+    uint8_t quality;//0 - 63
+    int8_t brightness;//-2 - 2
+    int8_t contrast;//-2 - 2
+    int8_t saturation;//-2 - 2
+    uint8_t special_effect;//0 - 6
+    uint8_t wb_mode;//0 - 4
+    uint8_t awb;
+    uint8_t awb_gain;
+    uint8_t aec;
+    uint8_t aec2;
+    int8_t ae_level;//-2 - 2
+    uint16_t aec_value;//0 - 1200
+    uint8_t agc;
+    uint8_t agc_gain;//0 - 30
+    uint8_t gainceiling;//0 - 6
+    uint8_t bpc;
+    uint8_t wpc;
+    uint8_t raw_gma;
+    uint8_t lenc;
+    uint8_t hmirror;
+    uint8_t vflip;
+    uint8_t dcw;
+    uint8_t colorbar;
+} camera_status_t;
+
 typedef struct _sensor sensor_t;
 typedef struct _sensor {
     sensor_id_t id;             // Sensor ID.
     uint8_t  slv_addr;          // Sensor I2C slave address.
     pixformat_t pixformat;
-    framesize_t framesize;
+    camera_status_t status;
 
     // Sensor function pointers
+    int  (*init_status)         (sensor_t *sensor);
     int  (*reset)               (sensor_t *sensor);
     int  (*set_pixformat)       (sensor_t *sensor, pixformat_t pixformat);
     int  (*set_framesize)       (sensor_t *sensor, framesize_t framesize);
@@ -91,7 +119,6 @@ typedef struct _sensor {
 
     int  (*set_raw_gma)         (sensor_t *sensor, int enable);
     int  (*set_lenc)            (sensor_t *sensor, int enable);
-    int  (*set_pre)             (sensor_t *sensor, int enable);
 } sensor_t;
 
 // Resolution table (in camera.c)
diff --git a/sensors/ov2640.c b/sensors/ov2640.c
index 2024e7f..3ef91b2 100755
--- a/sensors/ov2640.c
+++ b/sensors/ov2640.c
@@ -75,18 +75,18 @@ static int set_reg_bits(sensor_t *sensor, uint8_t bank, uint8_t reg, uint8_t off
     return ret;
 }
 
-//Function is not needed currently
-#if 0
+static int read_reg(sensor_t *sensor, ov2640_bank_t bank, uint8_t reg)
+{
+    if(set_bank(sensor, bank)){
+        return 0;
+    }
+    return SCCB_Read(sensor->slv_addr, reg);
+}
+
 static uint8_t get_reg_bits(sensor_t *sensor, uint8_t bank, uint8_t reg, uint8_t offset, uint8_t mask)
 {
-    uint8_t ret = 0;
-    if(set_bank(sensor, bank)){
-        return ret;
-    }
-    ret = (SCCB_Read(sensor->slv_addr, reg) >> offset) & mask;
-    return ret;
+    return (read_reg(sensor, bank, reg) >> offset) & mask;
 }
-#endif
 
 static int write_reg_bits(sensor_t *sensor, uint8_t bank, uint8_t reg, uint8_t mask, int enable)
 {
@@ -229,7 +229,7 @@ static int set_framesize(sensor_t *sensor, framesize_t framesize)
     uint16_t h = resolution[framesize][1];
     const uint8_t (*regs)[2];
 
-    sensor->framesize = framesize;
+    sensor->status.framesize = framesize;
 
     if (framesize <= FRAMESIZE_CIF) {
         regs = ov2640_settings_to_cif;
@@ -261,6 +261,7 @@ static int set_contrast(sensor_t *sensor, int level)
     if (level <= 0 || level > NUM_CONTRAST_LEVELS) {
         return -1;
     }
+    sensor->status.contrast = level-3;
     for (int i=0; i<7; i++) {
         WRITE_REG_OR_RETURN(BANK_DSP, contrast_regs[0][i], contrast_regs[level][i]);
     }
@@ -274,6 +275,7 @@ static int set_brightness(sensor_t *sensor, int level)
     if (level <= 0 || level > NUM_BRIGHTNESS_LEVELS) {
         return -1;
     }
+    sensor->status.brightness = level-3;
     for (int i=0; i<5; i++) {
         WRITE_REG_OR_RETURN(BANK_DSP, brightness_regs[0][i], brightness_regs[level][i]);
     }
@@ -287,6 +289,7 @@ static int set_saturation(sensor_t *sensor, int level)
     if (level <= 0 || level > NUM_SATURATION_LEVELS) {
         return -1;
     }
+    sensor->status.saturation = level-3;
     for (int i=0; i<5; i++) {
         WRITE_REG_OR_RETURN(BANK_DSP, saturation_regs[0][i], saturation_regs[level][i]);
     }
@@ -300,6 +303,7 @@ static int set_special_effect(sensor_t *sensor, int effect)
     if (effect <= 0 || effect > NUM_SPECIAL_EFFECTS) {
         return -1;
     }
+    sensor->status.special_effect = effect-1;
     for (int i=0; i<5; i++) {
         WRITE_REG_OR_RETURN(BANK_DSP, special_effects_regs[0][i], special_effects_regs[effect][i]);
     }
@@ -312,6 +316,7 @@ static int set_wb_mode(sensor_t *sensor, int mode)
     if (mode < 0 || mode > NUM_WB_MODES) {
         return -1;
     }
+    sensor->status.wb_mode = mode;
     SET_REG_BITS_OR_RETURN(BANK_DSP, 0XC7, 6, 1, mode?1:0);
     if(mode) {
         for (int i=0; i<3; i++) {
@@ -328,121 +333,176 @@ static int set_ae_level(sensor_t *sensor, int level)
     if (level <= 0 || level > NUM_AE_LEVELS) {
         return -1;
     }
+    sensor->status.ae_level = level-3;
     for (int i=0; i<3; i++) {
         WRITE_REG_OR_RETURN(BANK_SENSOR, ae_levels_regs[0][i], ae_levels_regs[level][i]);
     }
     return ret;
 }
 
+static int set_quality(sensor_t *sensor, int quality)
+{
+    if(quality < 0) {
+        quality = 0;
+    } else if(quality > 63) {
+        quality = 63;
+    }
+    sensor->status.quality = quality;
+    return write_reg(sensor, BANK_DSP, QS, quality);
+}
+
 static int set_agc_gain(sensor_t *sensor, int gain)
 {
-    const uint8_t gain_tbl[31] = {
-        0x00, 0x10, 0x18, 0x30, 0x34, 0x38, 0x3C, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7A, 0x7C, 0x7E, 0xF0,
-        0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, 0xFB, 0xFC, 0xFD, 0xFE, 0xFF
-    };
     if(gain < 0) {
         gain = 0;
     } else if(gain > 30) {
         gain = 30;
     }
-    return write_reg(sensor, BANK_SENSOR, GAIN, gain_tbl[gain]);
-}
-
-static int set_quality(sensor_t *sensor, int qs)
-{
-    return write_reg(sensor, BANK_DSP, QS, qs);
-}
-
-static int set_colorbar(sensor_t *sensor, int enable)
-{
-    return write_reg_bits(sensor, BANK_SENSOR, COM7, COM7_COLOR_BAR, enable);
+    sensor->status.agc_gain = gain;
+    return write_reg(sensor, BANK_SENSOR, GAIN, agc_gain_tbl[gain]);
 }
 
 static int set_gainceiling_sensor(sensor_t *sensor, gainceiling_t gainceiling)
 {
-    return write_reg(sensor, BANK_SENSOR, COM9, COM9_AGC_SET(gainceiling));
-}
-
-static int set_agc_sensor(sensor_t *sensor, int enable)
-{
-    return write_reg_bits(sensor, BANK_SENSOR, COM8, COM8_AGC_EN, enable);
-}
-
-static int set_aec_sensor(sensor_t *sensor, int enable)
-{
-    return write_reg_bits(sensor, BANK_SENSOR, COM8, COM8_AEC_EN, enable);
-}
-
-static int set_hmirror_sensor(sensor_t *sensor, int enable)
-{
-    return write_reg_bits(sensor, BANK_SENSOR, REG04, REG04_HFLIP_IMG, enable);
-}
-
-static int set_vflip_sensor(sensor_t *sensor, int enable)
-{
-    return write_reg_bits(sensor, BANK_SENSOR, REG04, REG04_VFLIP_IMG, enable);
-}
-
-static int set_aec_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL0, 6, 1, enable?0:1);
-}
-
-static int set_raw_gma_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL1, 5, 1, enable?1:0);
-}
-
-static int set_awb_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL1, 3, 1, enable?1:0);
-}
-
-static int set_awb_gain_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL1, 2, 1, enable?1:0);
-}
-
-static int set_lenc_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL1, 1, 1, enable?1:0);
-}
-
-static int set_pre_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL1, 0, 1, enable?1:0);
-}
-
-static int set_dcw_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL2, 5, 1, enable?1:0);
-}
-
-static int set_bpc_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL3, 7, 1, enable?1:0);
-}
-
-static int set_wpc_dsp(sensor_t *sensor, int enable)
-{
-    return set_reg_bits(sensor, BANK_DSP, CTRL3, 6, 1, enable?1:0);
+    sensor->status.gainceiling = gainceiling;
+    //return write_reg(sensor, BANK_SENSOR, COM9, COM9_AGC_SET(gainceiling));
+    return set_reg_bits(sensor, BANK_SENSOR, COM9, 5, 7, gainceiling);
 }
 
 static int set_aec_value(sensor_t *sensor, int value)
 {
     if(value < 0) {
         value = 0;
-    } else if(value > 0xFFFF) {
-        value = 0xFFFF;
+    } else if(value > 1200) {
+        value = 1200;
     }
+    sensor->status.aec_value = value;
     return set_reg_bits(sensor, BANK_SENSOR, REG04, 0, 3, value & 0x3)
            || write_reg(sensor, BANK_SENSOR, AEC, (value >> 2) & 0xFF)
            || set_reg_bits(sensor, BANK_SENSOR, REG45, 0, 0x3F, value >> 10);
 }
 
+static int set_aec2(sensor_t *sensor, int enable)
+{
+    sensor->status.aec2 = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL0, 6, 1, enable?0:1);
+}
+
+static int set_colorbar(sensor_t *sensor, int enable)
+{
+    sensor->status.colorbar = enable;
+    return write_reg_bits(sensor, BANK_SENSOR, COM7, COM7_COLOR_BAR, enable?1:0);
+}
+
+static int set_agc_sensor(sensor_t *sensor, int enable)
+{
+    sensor->status.agc = enable;
+    return write_reg_bits(sensor, BANK_SENSOR, COM8, COM8_AGC_EN, enable?1:0);
+}
+
+static int set_aec_sensor(sensor_t *sensor, int enable)
+{
+    sensor->status.aec = enable;
+    return write_reg_bits(sensor, BANK_SENSOR, COM8, COM8_AEC_EN, enable?1:0);
+}
+
+static int set_hmirror_sensor(sensor_t *sensor, int enable)
+{
+    sensor->status.hmirror = enable;
+    return write_reg_bits(sensor, BANK_SENSOR, REG04, REG04_HFLIP_IMG, enable?1:0);
+}
+
+static int set_vflip_sensor(sensor_t *sensor, int enable)
+{
+    sensor->status.vflip = enable;
+    return write_reg_bits(sensor, BANK_SENSOR, REG04, REG04_VFLIP_IMG, enable?1:0);
+}
+
+static int set_raw_gma_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.raw_gma = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL1, 5, 1, enable?1:0);
+}
+
+static int set_awb_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.awb = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL1, 3, 1, enable?1:0);
+}
+
+static int set_awb_gain_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.awb_gain = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL1, 2, 1, enable?1:0);
+}
+
+static int set_lenc_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.lenc = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL1, 1, 1, enable?1:0);
+}
+
+static int set_dcw_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.dcw = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL2, 5, 1, enable?1:0);
+}
+
+static int set_bpc_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.bpc = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL3, 7, 1, enable?1:0);
+}
+
+static int set_wpc_dsp(sensor_t *sensor, int enable)
+{
+    sensor->status.wpc = enable;
+    return set_reg_bits(sensor, BANK_DSP, CTRL3, 6, 1, enable?1:0);
+}
+
+static int init_status(sensor_t *sensor){
+    sensor->status.brightness = 0;
+    sensor->status.contrast = 0;
+    sensor->status.saturation = 0;
+    sensor->status.ae_level = 0;
+    sensor->status.special_effect = 0;
+    sensor->status.wb_mode = 0;
+
+    sensor->status.agc_gain = 30;
+    int agc_gain = read_reg(sensor, BANK_SENSOR, GAIN);
+    for (int i=0; i<30; i++){
+        if(agc_gain >= agc_gain_tbl[i] && agc_gain < agc_gain_tbl[i+1]){
+            sensor->status.agc_gain = i;
+            break;
+        }
+    }
+
+    sensor->status.aec_value = ((uint16_t)get_reg_bits(sensor, BANK_SENSOR, REG45, 0, 0x3F) << 10)
+                             | ((uint16_t)read_reg(sensor, BANK_SENSOR, AEC) << 2)
+                             | get_reg_bits(sensor, BANK_SENSOR, REG04, 0, 3);//0 - 1200
+    sensor->status.quality = read_reg(sensor, BANK_DSP, QS);
+    sensor->status.gainceiling = get_reg_bits(sensor, BANK_SENSOR, COM9, 5, 7);
+
+    sensor->status.awb = get_reg_bits(sensor, BANK_DSP, CTRL1, 3, 1);
+    sensor->status.awb_gain = get_reg_bits(sensor, BANK_DSP, CTRL1, 2, 1);
+    sensor->status.aec = get_reg_bits(sensor, BANK_SENSOR, COM8, 0, 1);
+    sensor->status.aec2 = get_reg_bits(sensor, BANK_DSP, CTRL0, 6, 1);
+    sensor->status.agc = get_reg_bits(sensor, BANK_SENSOR, COM8, 2, 1);
+    sensor->status.bpc = get_reg_bits(sensor, BANK_DSP, CTRL3, 7, 1);
+    sensor->status.wpc = get_reg_bits(sensor, BANK_DSP, CTRL3, 6, 1);
+    sensor->status.raw_gma = get_reg_bits(sensor, BANK_DSP, CTRL1, 5, 1);
+    sensor->status.lenc = get_reg_bits(sensor, BANK_DSP, CTRL1, 1, 1);
+    sensor->status.hmirror = get_reg_bits(sensor, BANK_SENSOR, REG04, 7, 1);
+    sensor->status.vflip = get_reg_bits(sensor, BANK_SENSOR, REG04, 6, 1);
+    sensor->status.dcw = get_reg_bits(sensor, BANK_DSP, CTRL2, 5, 1);
+    sensor->status.colorbar = get_reg_bits(sensor, BANK_SENSOR, COM7, 1, 1);
+    return 0;
+}
+
 int ov2640_init(sensor_t *sensor)
 {
     sensor->reset = reset;
+    sensor->init_status = init_status;
     sensor->set_pixformat = set_pixformat;
     sensor->set_framesize = set_framesize;
     sensor->set_contrast  = set_contrast;
@@ -459,7 +519,7 @@ int ov2640_init(sensor_t *sensor)
     sensor->set_vflip = set_vflip_sensor;
 
     sensor->set_whitebal = set_awb_dsp;
-    sensor->set_aec2 = set_aec_dsp;
+    sensor->set_aec2 = set_aec2;
     sensor->set_aec_value = set_aec_value;
     sensor->set_special_effect = set_special_effect;
     sensor->set_wb_mode = set_wb_mode;
@@ -473,9 +533,7 @@ int ov2640_init(sensor_t *sensor)
 
     sensor->set_raw_gma = set_raw_gma_dsp;
     sensor->set_lenc = set_lenc_dsp;
-    sensor->set_pre = set_pre_dsp;
-    
-    ESP_LOGD(TAG, "OV2640 Attached");
 
+    ESP_LOGD(TAG, "OV2640 Attached");
     return 0;
 }
diff --git a/sensors/private_include/ov2640_settings.h b/sensors/private_include/ov2640_settings.h
index 1565d10..d415d20 100644
--- a/sensors/private_include/ov2640_settings.h
+++ b/sensors/private_include/ov2640_settings.h
@@ -436,4 +436,9 @@ static const uint8_t ae_levels_regs[NUM_AE_LEVELS + 1][3] = {
     {0x58, 0X50, 0x92 },
 };
 
+const uint8_t agc_gain_tbl[31] = {
+    0x00, 0x10, 0x18, 0x30, 0x34, 0x38, 0x3C, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7A, 0x7C, 0x7E, 0xF0,
+    0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, 0xFB, 0xFC, 0xFD, 0xFE, 0xFF
+};
+
 #endif /* _OV2640_SETTINGS_H_ */