source: branches/reyalp-ptp-live/core/motion_detector.c @ 1711

/*

Motion detection module

Author: mx3 (Max Sagaydachny) . win1251 ( Ìàêñèì Ñàãàéäà÷íûé )
Email: win.drivers@gmail.com
Skype: max_dtc
ICQ#: 125-985-663
Country: Ukraine
Sity: Kharkiv

20070912 mx3: first version

20070918 mx3: speed optimization,

20101201 CHDKLover: speed optimization

*/

#ifdef OPT_MD_DEBUG
#define MD_REC_CALLS_CNT 2048
#define MD_INFO_BUF_SIZE 4096
#endif

#include "motion_detector.h"
#include "action_stack.h"
#include "console.h"
#include "keyboard.h"

#include "gui.h"
#include "gui_draw.h"


#define MD_XY2IDX(x,y) ((y)*motion_detector->columns+x)

enum {

    MD_MEASURE_MODE_U=0,
    MD_MEASURE_MODE_Y=1,
    MD_MEASURE_MODE_V=2,
    MD_MEASURE_MODE_R=3,
    MD_MEASURE_MODE_G=4,
    MD_MEASURE_MODE_B=5
};

enum {
    MD_DO_IMMEDIATE_SHOOT=1,
    MD_MAKE_DEBUG_LOG_FILE=2,
    MD_MAKE_RAM_DUMP_FILE=4,
    MD_NO_SHUTTER_RELEASE_ON_SHOOT=8
};

enum {
    MD_REGION_NONE=0,
    MD_REGION_INCLUDE=1,
    MD_REGION_EXCLUDE=2
};





struct motion_detector_s {
    int *curr; // points to buff1 or buff2
    int *prev; // points to buff2 or buff1
    int buff1[MOTION_DETECTOR_CELLS];
    int buff2[MOTION_DETECTOR_CELLS];
    int points[MOTION_DETECTOR_CELLS];

    int columns;
    int rows;
    int threshold;
    int pixel_measure_mode;
    int timeout;
    int measure_interval;

    int last_measure_time;
    int start_time;

    int running;
    int detected_cells;

    int draw_grid;
    int clipping_region_mode;
    int clipping_region_row1;
    int clipping_region_column1;
    int clipping_region_row2;
    int clipping_region_column2;

    int previous_picture_is_ready;

    int return_value;
    int parameters;
    int pixels_step;
    int msecs_before_trigger;

// debug
#ifdef OPT_MD_DEBUG
    int comp_calls_cnt;
    int comp_calls[MD_REC_CALLS_CNT];
#endif
};

static struct motion_detector_s *motion_detector=NULL;

static void md_kbd_sched_immediate_shoot(int no_release)
{
    action_pop();// REMOVE MD ITEM

    // stack operations are reversed!
    if (!no_release)  // only release shutter if allowed
    {
      action_push_release(KEY_SHOOT_FULL);
      action_push_delay(20);
    }
    action_push(AS_MOTION_DETECTOR); // it will removed right after exit from this function
    kbd_key_press(KEY_SHOOT_FULL); // not a stack operation... pressing right now
}

static int clip(int v) {
    if (v<0) v=0;
    if (v>255) v=255;
    return v;
}


// TODO add script interface, currently done when script ends
void md_close_motion_detector()
{
    if (motion_detector)
    {
            free(motion_detector);
        motion_detector=NULL;
    }
}


int md_init_motion_detector(
 int columns,
 int rows,
 int pixel_measure_mode,
 int detection_timeout,
 int measure_interval,
 int threshold,
 int draw_grid,
 int clipping_region_mode,
 int clipping_region_column1,
 int clipping_region_row1,
 int clipping_region_column2,
 int clipping_region_row2,
 int parameters,
 int pixels_step,
 int msecs_before_trigger
){

    if(!motion_detector) {
        motion_detector=malloc(sizeof(struct motion_detector_s));
        if(!motion_detector)
            return 0; // TODO make sure callers handle this
    }
#ifdef OPT_MD_DEBUG
    motion_detector->comp_calls_cnt=0;
#endif
    motion_detector->previous_picture_is_ready=0;
    motion_detector->curr=motion_detector->buff1;
    motion_detector->prev=motion_detector->buff2;

    if(     pixel_measure_mode != MD_MEASURE_MODE_Y
        &&  pixel_measure_mode != MD_MEASURE_MODE_U
        &&  pixel_measure_mode != MD_MEASURE_MODE_V
        &&  pixel_measure_mode != MD_MEASURE_MODE_R
        &&  pixel_measure_mode != MD_MEASURE_MODE_G
        &&  pixel_measure_mode != MD_MEASURE_MODE_B
        ){
        pixel_measure_mode = MD_MEASURE_MODE_Y;
    }


    if( columns<1 || rows<1 || columns * rows > MOTION_DETECTOR_CELLS ){
        columns=3;
        rows=3;
    }

    if(msecs_before_trigger<0){
        msecs_before_trigger=0;
    }

    if (pixels_step<1){
        pixels_step=1;
    }

    if(detection_timeout<0){
        detection_timeout=0;
    }

    if(measure_interval<0) {
        measure_interval=0;
    }

    if(threshold<0) {
        threshold=0;
    }


    motion_detector->msecs_before_trigger=msecs_before_trigger;
    motion_detector->parameters = parameters;
    motion_detector->pixels_step=pixels_step;
    motion_detector->columns=columns;
    motion_detector->rows=rows;
    motion_detector->return_value=0;


    motion_detector->pixel_measure_mode=pixel_measure_mode;
    motion_detector->timeout=detection_timeout;
    motion_detector->measure_interval=measure_interval;
    motion_detector->threshold=threshold;
    motion_detector->draw_grid=draw_grid;


    if (clipping_region_column1>clipping_region_column2){
        motion_detector->clipping_region_column2=clipping_region_column1;
        motion_detector->clipping_region_column1=clipping_region_column2;
    } else {
        motion_detector->clipping_region_column2=clipping_region_column2;
        motion_detector->clipping_region_column1=clipping_region_column1;
    }

    if (clipping_region_row1>clipping_region_row2){
        motion_detector->clipping_region_row2=clipping_region_row1;
        motion_detector->clipping_region_row1=clipping_region_row2;
    } else {
        motion_detector->clipping_region_row2=clipping_region_row2;
        motion_detector->clipping_region_row1=clipping_region_row1;
    }

    if (clipping_region_mode!=MD_REGION_NONE && clipping_region_mode!=MD_REGION_INCLUDE && clipping_region_mode!=MD_REGION_EXCLUDE){
        clipping_region_mode=MD_REGION_NONE;
    }
    motion_detector->clipping_region_mode=clipping_region_mode;

    motion_detector->detected_cells=0;
    motion_detector->previous_picture_is_ready=0;
    motion_detector->start_time=get_tick_count();

    motion_detector->last_measure_time = motion_detector->start_time - motion_detector->measure_interval;

    motion_detector->running=1;

    action_push(AS_MOTION_DETECTOR);
    draw_clear();

    return 1;
}

#ifdef OPT_MD_DEBUG
static void md_save_calls_history(){
    char buf[200], fn[30];
    char big[MD_INFO_BUF_SIZE];
    int big_ln;
    int calls,i, ln, fd;
    static struct utimbuf t;
    static struct tm *ttm;


    if( (motion_detector->parameters & MD_MAKE_DEBUG_LOG_FILE) == 0 ){
        return;
    }


    strcpy(fn,"A/MD_INFO.TXT");//,BUILD_NUMBER,motion_detector->pixels_step);
    fd = open(fn, O_WRONLY|O_CREAT, 0777);
    if( fd>=0) {
        console_add_line("Writing info file...");
        lseek(fd,0,SEEK_END);
            ttm = get_localtime();
        big_ln=sprintf(big, 
                                "\r\n--- %04u-%02u-%02u  %02u:%02u:%02u\r\n"
                                "CHDK Ver: %s [ #%s ]\r\nBuild Date: %s %s\r\nCamera:  %s [ %s ]\r\n"
                                "[%dx%d], threshold: %d, interval: %d, pixels step: %d\r\n"
                                "region: [%d,%d-%d,%d], region type: %d\r\n"
                                "wait interval: %d, parameters: %d, calls: %d, detected cells: %d\r\n", 
                                1900+ttm->tm_year, ttm->tm_mon+1, ttm->tm_mday, ttm->tm_hour, ttm->tm_min, ttm->tm_sec,
                                HDK_VERSION, BUILD_NUMBER, __DATE__, __TIME__, PLATFORM, PLATFORMSUB,
                                motion_detector->columns, motion_detector->rows, motion_detector->threshold, motion_detector->measure_interval, motion_detector->pixels_step,
                                motion_detector->clipping_region_column1, motion_detector->clipping_region_row1, motion_detector->clipping_region_column2, motion_detector->clipping_region_row2, motion_detector->clipping_region_mode,
                                motion_detector->msecs_before_trigger, motion_detector->parameters, motion_detector->comp_calls_cnt,
                                motion_detector->detected_cells
                );

        calls = ( motion_detector->comp_calls_cnt < MD_REC_CALLS_CNT) ?motion_detector->comp_calls_cnt: MD_REC_CALLS_CNT;

        for(i=0;i<calls;i++){
            ln=sprintf(buf,"[%d] - %d\r\n",i,motion_detector->comp_calls[i]);
            if(big_ln+ln>MD_INFO_BUF_SIZE){
          write(fd,big,big_ln);
                big_ln=0;
            }
            memcpy(big+big_ln,buf,ln+1);
            big_ln+=ln;
        }
    write(fd,big,big_ln);
        close(fd);
      t.actime = t.modtime = time(NULL);
    utime(fn, &t);
    }
}

static void mx_dump_memory(void *img){
    char fn[36];
    int fd, i;
    static int cnt=0;

    started();
    mkdir("A/MD");

        do {
            cnt++;
            sprintf(fn, "A/MD/%04d.FB", cnt);
            fd = open(fn, O_RDONLY, 0777);

            if(fd>=0){
                close(fd);
            }
        } while(fd>=0);


                sprintf(fn, "A/MD/%04d.FB", cnt );
                fd = open(fn, O_WRONLY|O_CREAT, 0777);
                if (fd) {
            write(fd, img, camera_screen.width*vid_get_viewport_height()*3);
            close(fd);
                }
  vid_bitmap_refresh();
  finished();

}
#else
#define md_save_calls_history()
#define mx_dump_memory(x)
#endif


static int md_running(){
        return motion_detector?motion_detector->running:0;
}


int md_detect_motion(void){
    int *tmp;
    unsigned char * img;
    int vp_w, vp_h, idx, tmp2, tick, in_clipping_region, x_step, y_step, x_end, y_end;
    int val;
    int cy,cv,cu;

    register int col, row, x, y;

    if(!md_running()){
        return 0;
    }

    tick=get_tick_count();
#ifdef OPT_MD_DEBUG
    if(motion_detector->comp_calls_cnt < MD_REC_CALLS_CNT) {
        motion_detector->comp_calls[motion_detector->comp_calls_cnt]=tick;
    }
    motion_detector->comp_calls_cnt++;
#endif

    if(motion_detector->start_time + motion_detector->timeout < tick ) {
        md_save_calls_history();
        motion_detector->running = 0;
        return 0;
    }

    if(motion_detector->last_measure_time + motion_detector->measure_interval > tick){
        // wait for the next time
        return 1;
    }

    motion_detector->last_measure_time=tick;

// swap pointers so we don't need to copy last data array into Previous one
    tmp=motion_detector->curr;
    motion_detector->curr=motion_detector->prev;
    motion_detector->prev=tmp;

     img = vid_get_viewport_live_fb();
        if(img==NULL){
            img = vid_get_viewport_fb();
        }

#ifdef OPT_MD_DEBUG
    if(motion_detector->comp_calls_cnt==50 && (motion_detector->parameters & MD_MAKE_RAM_DUMP_FILE) != 0 ){
        mx_dump_memory((char*)img);
    }
#endif

        vp_h = vid_get_viewport_height();
        vp_w = vid_get_viewport_byte_width() * vid_get_viewport_yscale();
        img += vid_get_viewport_image_offset();         // offset into viewport for when image size != viewport size (e.g. 16:9 image on 4:3 LCD)

        x_step=(vid_get_viewport_width()*3)/motion_detector->columns;
        y_step=vp_h/motion_detector->rows;

    for (idx=0, row=0; row < motion_detector->rows; row++)
    {
        for (col=0; col < motion_detector->columns; col++, idx++)
        {
            // clear cur and points, previously down in it's own loop
            // might be able to avoid clearing all, since some are overwritten below
            motion_detector->points[idx] = 0;
            motion_detector->curr[idx] = 0;

            in_clipping_region=0;

            if (col+1 >= motion_detector->clipping_region_column1 &&
                col+1 <= motion_detector->clipping_region_column2 &&
                row+1 >= motion_detector->clipping_region_row1 &&
                row+1 <= motion_detector->clipping_region_row2)
            {
                in_clipping_region=1;
            }

            if (
                (motion_detector->clipping_region_mode==MD_REGION_NONE) ||
                (motion_detector->clipping_region_mode==MD_REGION_EXCLUDE && in_clipping_region==0) ||
                (motion_detector->clipping_region_mode==MD_REGION_INCLUDE && in_clipping_region==1)
               )
            {
                x_end=(col+1)*x_step;
                                y_end=(row+1)*y_step*vp_w;
                                for(y=row*y_step*vp_w; y<y_end; y+=motion_detector->pixels_step*vp_w){
                                        for(x=col*x_step; x<x_end; x+=motion_detector->pixels_step*3){

                                                // ARRAY of UYVYYY values
                        // 6 bytes - 4 pixels

                        switch(motion_detector->pixel_measure_mode){
                                                default:
                                                case MD_MEASURE_MODE_Y:
                                                        val = img[y + x + 1];                           //Y
                                                        break;
                                                case MD_MEASURE_MODE_U:
                                                        val = img[y + (x&0xFFFFFFFE)];          //U
                                                        break;
                                                case MD_MEASURE_MODE_V:
                                                        val = img[y + (x&0xFFFFFFFE) + 2];      //V
                                                        break;

                                                case MD_MEASURE_MODE_R:
                                                        cy=img[y + x + 1];
                                                        cv=img[y + (x&0xFFFFFFFE) + 2];
                                                        val = clip(((cy<<12)           + cv*5743 + 2048)>>12); // R
                                                        break;

                                                case MD_MEASURE_MODE_G:
                                                        cy=img[y + x + 1];
                                                        cu=img[y + (x&0xFFFFFFFE)];
                                                        cv=img[y + (x&0xFFFFFFFE) + 2];
                                                        val = clip(((cy<<12) - cu*1411 - cv*2925 + 2048)>>12); // G
                                                        break;

                                                case MD_MEASURE_MODE_B:
                                                        cy=img[y + x + 1];
                                                        cu=img[y + (x&0xFFFFFFFE)];
                                                        val = clip(((cy<<12) + cu*7258           + 2048)>>12); // B
                                                        break;
                        }

                        motion_detector->curr[ idx ] += val;
                        motion_detector->points[ idx ]++;
                    }
                }
            }
        }
    }
    // << fill "curr" array


    if(motion_detector->previous_picture_is_ready==0){
        motion_detector->previous_picture_is_ready=1;
        motion_detector->start_time=get_tick_count();
        motion_detector->last_measure_time=motion_detector->start_time-motion_detector->measure_interval;
        return 1;
    }


    // >> compare arrays here
    for (idx=0; idx < motion_detector->rows*motion_detector->columns; idx++)
    {
        tmp2=0;
        if(motion_detector->points[idx]>0){
            motion_detector->prev[idx] = (motion_detector->curr[idx]-motion_detector->prev[idx])/motion_detector->points[idx];
            tmp2 = ( motion_detector->prev[idx] < 0 ) ? -motion_detector->prev[idx] : motion_detector->prev[idx] ;
        }

        if( tmp2 > motion_detector->threshold ){
            if (motion_detector->start_time+motion_detector->msecs_before_trigger < tick){
                motion_detector->detected_cells++;
            }
        }
    }
    // << compare arrays here


    if( motion_detector->detected_cells > 0 ){
//      sprintf(buf,"-cells: %d", motion_detector->detected_cells);
//      script_console_add_line(buf);

        if (motion_detector->start_time+motion_detector->msecs_before_trigger < tick){
            motion_detector->running=0;
            motion_detector->return_value = motion_detector->detected_cells;

//          md_save_calls_history();
            if( ( motion_detector->parameters&MD_DO_IMMEDIATE_SHOOT ) !=0){
                //make shoot
                md_kbd_sched_immediate_shoot(motion_detector->parameters&MD_NO_SHUTTER_RELEASE_ON_SHOOT);
            }
            return 0;
        }
    }


    return 1;
}


int md_get_cell_diff(int column, int row){

    if(!motion_detector){
        return 0;
    }
    if (column<1 || column > motion_detector->columns){
        return 0;
    }

    if (row<1 || row > motion_detector->rows ){
        return 0;
    }


    return motion_detector->prev[ MD_XY2IDX(column-1,row-1) ];
}


void md_draw_grid(){
    int x_step, y_step, col, row;
    int xoffset, yoffset;
    int do_draw_rect, i, tmp2, in_clipping_region, color, col_start, col_stop, row_start, row_stop;

    if(!md_running() || motion_detector->draw_grid==0){
        return ;
        }

        xoffset = vid_get_viewport_xoffset();   // used when image size != viewport size
        yoffset = vid_get_viewport_yoffset();   // used when image size != viewport size

        x_step=(camera_screen.width-xoffset*2)/motion_detector->columns;
        y_step=(camera_screen.height-yoffset*2)/motion_detector->rows;
#if 0
        row_start=1;
        row_stop=motion_detector->rows;
        if(motion_detector->clipping_region_mode==2){
                row_start=motion_detector->clipping_region_row1;
                row_stop=motion_detector->clipping_region_row2+1;
                col_start=motion_detector->clipping_region_column1;
                col_stop=motion_detector->clipping_region_column2+1;
        }
        if(motion_detector->clipping_region_mode==0 || motion_detector->clipping_region_mode==2){
                for(col=col_start;col<col_stop;col++){
                        draw_line(col*x_step,0,col*x_step,camera_screen.height, COLOR_GREEN);
                }
                for(row=row_start;row<row_stop;row++){
                        draw_line(0,row*y_step,camera_screen.width,row*y_step, COLOR_GREEN);
                }
        } else if(motion_detector->clipping_region_mode==1){
                for(col=1;col<motion_detector->columns;col++){
                        if(col<){
                                draw_line(col*x_step,0,col*x_step,camera_screen.height, COLOR_GREEN);
                        }
                }
        }
#endif

    for ( col=0, row=0; row < motion_detector->rows; ){
        i=MD_XY2IDX(col,row);
        tmp2 = ( motion_detector->prev[i] < 0 ) ? -motion_detector->prev[i] : motion_detector->prev[i] ;

            in_clipping_region=0;
            if ( col+1>=motion_detector->clipping_region_column1
                && col+1<=motion_detector->clipping_region_column2
                && row+1>=motion_detector->clipping_region_row1
                && row+1<=motion_detector->clipping_region_row2
                ){
                    in_clipping_region=1;
            }

            do_draw_rect=0;

            if(motion_detector->clipping_region_mode==MD_REGION_EXCLUDE && in_clipping_region==0){
                do_draw_rect=1;
            }

            if(motion_detector->clipping_region_mode==MD_REGION_INCLUDE && in_clipping_region==1){
                do_draw_rect=1;
            }

            if(motion_detector->clipping_region_mode==MD_REGION_NONE){
                do_draw_rect=1;
            }

            if(do_draw_rect==1){
                color=COLOR_HISTO_G;
                if( tmp2 > motion_detector->threshold){
                    color=COLOR_RED;
                }
                draw_rect(xoffset+x_step*col+2,yoffset+y_step*row+2, xoffset+x_step*(col+1)-2, yoffset+y_step*(row+1)-2,color);
            }

            col++;
            if( col >= motion_detector->columns ){
                row++;
                col=0;
            }
    }

}

int md_get_result()
{
    return motion_detector->return_value;
}


// =========  MODULE INIT =================

#include "module_load.h"
int module_idx=-1;

/***************** BEGIN OF AUXILARY PART *********************
  ATTENTION: DO NOT REMOVE OR CHANGE SIGNATURES IN THIS SECTION
 **************************************************************/

struct libmotiondetect_sym libmotiondetect = {
                        MAKE_API_VERSION(1,0),          // apiver: increase major if incompatible changes made in module, 
                                                                                // increase minor if compatible changes made(including extending this struct)

        md_close_motion_detector,
        md_init_motion_detector,
        md_detect_motion,
        md_get_cell_diff,
        md_draw_grid,
        md_get_result
        };


void* MODULE_EXPORT_LIST[] = {
        /* 0 */ (void*)EXPORTLIST_MAGIC_NUMBER,
        /* 1 */ (void*)1,

                        &libmotiondetect
                };


//---------------------------------------------------------
// PURPOSE:   Bind module symbols with chdk. 
//              Required function
// PARAMETERS: pointer to chdk list of export
// RETURN VALUE: 1 error, 0 ok
//---------------------------------------------------------
int _module_loader( unsigned int* chdk_export_list )
{
  if ( chdk_export_list[0] != EXPORTLIST_MAGIC_NUMBER )
     return 1;

  if ( !API_VERSION_MATCH_REQUIREMENT( camera_sensor.api_version, 1, 0 ) )
         return 1;

  return 0;
}


//---------------------------------------------------------
// PURPOSE: Finalize module operations (close allocs, etc)
// RETURN VALUE: 0-ok, 1-fail
//---------------------------------------------------------
int _module_unloader()
{
    md_close_motion_detector();
    return 0;
}


//---------------------------------------------------------
// PURPOSE: Default action for simple modules (direct run)
// NOTE: Please comment this function if no default action and this library module
//---------------------------------------------------------
int _module_run(int moduleidx, int argn, int* arguments)
{
  module_idx=moduleidx;

  return 0;
}

/******************** Module Information structure ******************/

struct ModuleInfo _module_info = {      MODULEINFO_V1_MAGICNUM,
                                                                        sizeof(struct ModuleInfo),

                                                                        ANY_CHDK_BRANCH, 0,                     // Requirements of CHDK version
                                                                        ANY_PLATFORM_ALLOWED,           // Specify platform dependency
                                                                        MODULEINFO_FLAG_SYSTEM,         // flag
                                                                        (int32_t)"Motion Detect (dll)",// Module name
                                                                        1, 0,                                           // Module version
                                                                        (int32_t)"Motion Detect"
                                                                 };


/*************** END OF AUXILARY PART *******************/