#include "ctn_cal.h" static struct state_t ctn_cal_states[] = { { SMST_LAUNCH, "launch" }, { SMST_RUN, "run" }, { SMST_ERR, "err" }, }; static struct err_t ctn_cal_errs[] = { { CTN_CALERR_NONE, "none" }, // launch { CTN_CALERR_LAUNCH_GET_DATA_LINE_NBR, "launch, get data line nbr err" }, { CTN_CALERR_LAUNCH_GET_RECORD_DATA_ERR, "launch, get line data err" }, { CTN_CALERR_LAUNCH_GET_CYCLE, "launch, get cycle base err" }, { CTN_CALERR_LAUNCH_INSERT_ONE_LINR_DATA, "launch, insert one line data err" }, { CTN_CALERR_RUN_UPDATE_SOC, "launch, update soc err" }, }; struct ctn_cal_t ctn_cal; static int ctn_cal_dbcb_0(void *para,int ncolumn,char ** columnvalue,char *columnname[]) { struct ctn_cal_t* cal = &ctn_cal; struct dbcbparam_t* pcbparam = (struct dbcbparam_t*)para; int i = 0; pcbparam->nrow ++; log_dbg("%s, ++,row:%d, col:%d",__func__,pcbparam->nrow,ncolumn); for(i = 0;i < ncolumn;i++){ if( strcmp("m_model",columnname[i]) == 0 ){ strcpy(cal->szmeter_model,columnvalue[i]); cal->meter_model = plt_devm_str2nbr(cal->szmeter_model); }else if( strcmp("m_idx",columnname[i]) == 0 ){ cal->meter_idx = atoi(columnvalue[i]); }else if( strcmp("a1_chg",columnname[i]) == 0 ){ cal->cal_soc.a1_chg = atof(columnvalue[i]); }else if( strcmp("a2_chg",columnname[i]) == 0 ){ cal->cal_soc.a2_chg = atof(columnvalue[i]); }else if( strcmp("b1_chg",columnname[i]) == 0 ){ cal->cal_soc.b1_chg = atof(columnvalue[i]); }else if( strcmp("b2_chg",columnname[i]) == 0 ){ cal->cal_soc.b2_chg = atof(columnvalue[i]); }else if( strcmp("a1_dhg",columnname[i]) == 0 ){ cal->cal_soc.a1_dhg = atof(columnvalue[i]); }else if( strcmp("a2_dhg",columnname[i]) == 0 ){ cal->cal_soc.a2_dhg = atof(columnvalue[i]); }else if( strcmp("b1_dhg",columnname[i]) == 0 ){ cal->cal_soc.b1_dhg = atof(columnvalue[i]); }else if( strcmp("b2_dhg",columnname[i]) == 0 ){ cal->cal_soc.b2_dhg = atof(columnvalue[i]); }else if( strcmp("extre_P",columnname[i]) == 0 ){ cal->cal_soc.extre_p = atof(columnvalue[i]); } } pcbparam->ret = 0; log_dbg("%s, --,ret:%d",__func__,pcbparam->ret); return 0; } static int ctn_cal_dbcb_get_record_data(void *para,int ncolumn,char** columnvalue,char* columnname[]) { struct ctn_cal_t* cal = &ctn_cal; int i = 0; log_dbg("%s, ++,col:%d",__func__,ncolumn); for(i = 0;i < ncolumn;i++){ if( strcmp("soc",columnname[i]) == 0 ){ cal->cal_soc.soc = atof(columnvalue[i]); }else if( strcmp("cycle",columnname[i]) == 0 ){ cal->cycle = atof(columnvalue[i]); }else if( strcmp("soh",columnname[i]) == 0){ cal->soh = atof(columnvalue[i]); } } log_dbg("%s, --,ret:%d",__func__); return 0; } static int ctn_cal_dbcb_get_line_nb(void *para,int ncolumn,char** columnvalue,char* columnname[]) { struct ctn_cal_t* cal = &ctn_cal; int i = 0; int ret = 0; log_dbg("%s, ++, col:%d",__func__,ncolumn); if(ncolumn > 0){ *((int*)para) = atoi(columnvalue[0]); }else{ ret = -1; } log_dbg("%s, --",__func__); return ret; } static int ctn_cal_update_cycle() { char sql[1024]; int ret = 0; sprintf(sql,"update data set cycle = %.3f where idx = 1",ctn_cal.cycle); if(sqlite3_exec(ctn_cal.db,sql,NULL,NULL,NULL) != SQLITE_OK){ ret = -1; } return ret; } static int ctn_cal_update_soc() { char sql[1024]; int ret = 0; sprintf(sql,"update data set soc = %.3f where idx = 1",ctn_cal.cal_soc.soc); if(sqlite3_exec(ctn_cal.db,sql,NULL,NULL,NULL) != SQLITE_OK){ ret = -1; } return ret; } static int ctn_cal_update_soh() { char sql[1024]; int ret = 0; sprintf(sql,"update data set soh = %.3f where idx = 1",ctn_cal.soh); if(sqlite3_exec(ctn_cal.db,sql,NULL,NULL,NULL) != SQLITE_OK){ ret = -1; } return ret; } static int ctn_cal_update_params() { char sql[1024]; int ret = 0; sqlite3* db = NULL; sprintf(sql,"update cal set a1_chg=%.3f,b1_chg=%.3f,a2_chg=%.3f,b2_chg=%.3f,a1_dhg=%.3f,b1_dhg=%.3f,a2_dhg=%.3f,b2_dhg=%.3f where rowid = 1", ctn_cal.cal_soc.a1_chg,ctn_cal.cal_soc.b1_chg,ctn_cal.cal_soc.a2_chg,ctn_cal.cal_soc.b2_chg,\ ctn_cal.cal_soc.a1_dhg,ctn_cal.cal_soc.b1_dhg,ctn_cal.cal_soc.a2_dhg,ctn_cal.cal_soc.b2_dhg); plt_lock_cfgdb(); db = plt_get_cfgdb(); if(sqlite3_exec(db,sql,NULL,NULL,NULL) != SQLITE_OK){ ret = -1; } plt_unlock_cfgdb(); return ret; } static int ctn_cal_launch() { struct statemachine_t* sm = &ctn_cal.sm; int lineNbr = 0; char sql[128]; int result = 0; if(sm->step == 0){ memset(sql,0,128); sprintf(sql,"select count(1) from %s","data"); if(sqlite3_exec(ctn_cal.db,sql,ctn_cal_dbcb_get_line_nb,(void*)&lineNbr,NULL) != SQLITE_OK){ log_dbg("%s, read ctn_cal database line nbr err", __func__); sm_set_state(&ctn_cal.sm,SMST_ERR,CTN_CALERR_LAUNCH_GET_DATA_LINE_NBR); }else{ if(lineNbr > 0){ sprintf(sql,"select * from data"); result = sqlite3_exec(ctn_cal.db,sql, ctn_cal_dbcb_get_record_data,NULL,NULL); if(result != SQLITE_OK ){ log_dbg("%s, load record data err", __func__); sm_set_state(&ctn_cal.sm,SMST_ERR,CTN_CALERR_LAUNCH_GET_RECORD_DATA_ERR); }else{ sm_set_step(&ctn_cal.sm,10); } }else{ ctn_cal.cycle = 0.0; if(pack_get_soc() >= 0.0 && pack_get_soc() <= 100.0){ ctn_cal.cal_soc.soc = pack_get_soc()/100.0; }else{ ctn_cal.cal_soc.soc = 0.5; } ctn_cal.soh = 1.00; sm_set_step(&ctn_cal.sm,10); } } }else if(sm->step == 10){ //wait meter goto ready,init meter data if(meter_get_state(ctn_cal.meter_model,ctn_cal.meter_idx) == SMST_READY){ ctn_cal.cal_soc.meter_pos_energy_base = meter_get_pos_ae(ctn_cal.meter_model,ctn_cal.meter_idx); ctn_cal.cal_soc.meter_neg_energy_base = meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx); ctn_cal.meter_neg_energy_last = meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx); sm_set_state(&ctn_cal.sm,SMST_RUN,CTN_CALERR_NONE); } } } static int ctn_cal_run() { struct timeval tm; gettimeofday(&tm,NULL); if(meter_get_state(ctn_cal.meter_model,ctn_cal.meter_idx) == SMST_READY /*&& pack_get_state() == SMST_READY*/){ if(ctn_get_state() != ctn_cal.cal_soc.last_ctn_state && ctn_get_state() == SMST_CHG){ ctn_cal.cal_soc.soc_base = ctn_cal.cal_soc.soc; if(fabs(ctn_get_ap()) > ctn_cal.cal_soc.extre_p) ctn_cal.cal_soc.chg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a2_chg + ctn_cal.cal_soc.b2_chg; else ctn_cal.cal_soc.chg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a1_chg + ctn_cal.cal_soc.b1_chg; ctn_cal.cal_soc.chg_energy_p = ctn_cal.cal_soc.chg_energy_p * ctn_cal.soh; ctn_cal.cal_soc.chg_energy_pr = ctn_cal.cal_soc.chg_energy_p; ctn_cal.cal_soc.chg_energy_basic = ctn_cal.cal_soc.chg_energy_p; ctn_cal.cal_soc.meter_pos_energy_base = meter_get_pos_ae(ctn_cal.meter_model,ctn_cal.meter_idx); ctn_cal.cal_soc.last_ctn_state = SMST_CHG; ctn_cal.cal_soc.ts_last = (long long )tm.tv_sec * 1000.0 + (long long )tm.tv_usec/1000.0; log_info("%s,state:%s,p:%d,chg_energy_p:%.3f,chg_energy_pr:%.3f,chg_energy_basic:%.3f",__func__,ctn_get_state_str(),ctn_get_ap(), ctn_cal.cal_soc.chg_energy_p,ctn_cal.cal_soc.chg_energy_pr,ctn_cal.cal_soc.chg_energy_basic); }else if(ctn_get_state() != ctn_cal.cal_soc.last_ctn_state && ctn_get_state() == SMST_DHG){ ctn_cal.cal_soc.soc_base = ctn_cal.cal_soc.soc; if(fabs(ctn_get_ap()) > ctn_cal.cal_soc.extre_p) ctn_cal.cal_soc.dhg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a2_dhg + ctn_cal.cal_soc.b2_dhg; else ctn_cal.cal_soc.dhg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a1_dhg + ctn_cal.cal_soc.b1_dhg; ctn_cal.cal_soc.dhg_energy_p = ctn_cal.cal_soc.dhg_energy_p * ctn_cal.soh; ctn_cal.cal_soc.dhg_energy_pr = ctn_cal.cal_soc.dhg_energy_p; ctn_cal.cal_soc.dhg_energy_basic = ctn_cal.cal_soc.dhg_energy_p; ctn_cal.cal_soc.meter_neg_energy_base = meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx); ctn_cal.cal_soc.last_ctn_state = SMST_DHG; ctn_cal.cal_soc.ts_last = (long long )tm.tv_sec * 1000.0 + (long long )tm.tv_usec/1000.0; log_info("%s,state:%s,p:%d,dhg_energy_p:%.3f,dhg_energy_pr:%.3f,dhg_energy_basic:%.3f",__func__,ctn_get_state_str(),ctn_get_ap(), ctn_cal.cal_soc.dhg_energy_p,ctn_cal.cal_soc.dhg_energy_pr,ctn_cal.cal_soc.dhg_energy_basic); }else if(ctn_get_state() == SMST_CHG){ int ap = ctn_get_ap(); double chg_energy = 0; if(ctn_cal.cal_soc.soc >= 1.0)return; if(ctn_get_ap() > -5.0)return; if(fabs(ctn_get_ap()) > ctn_cal.cal_soc.extre_p) ctn_cal.cal_soc.chg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a2_chg + ctn_cal.cal_soc.b2_chg; else ctn_cal.cal_soc.chg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a1_chg + ctn_cal.cal_soc.b1_chg; // ctn_cal.cal_soc.chg_energy_p = ctn_cal.cal_soc.chg_energy_p * ctn_cal.soh; struct timeval tm; long long intv_time = 0; gettimeofday(&tm,NULL); intv_time = (long long )tm.tv_sec * 1000 + (long long )tm.tv_usec/1000 - (long long )ctn_cal.cal_soc.ts_last; ctn_cal.cal_soc.chg_energy_pr += (ctn_cal.cal_soc.chg_energy_p - ctn_cal.cal_soc.chg_energy_basic)/(ctn_cal.cal_soc.chg_energy_p/fabs(ap))/3600.0 * intv_time / 1000.0; chg_energy = meter_get_pos_ae(ctn_cal.meter_model,ctn_cal.meter_idx) - ctn_cal.cal_soc.meter_pos_energy_base; ctn_cal.cal_soc.soc = ctn_cal.cal_soc.soc_base + chg_energy / ctn_cal.cal_soc.chg_energy_pr; if(ctn_cal.cal_soc.soc >= 1.0) ctn_cal.cal_soc.soc = 1.0; ctn_cal.cal_soc.ts_last = (long long )tm.tv_sec * 1000 + (long long )tm.tv_usec/1000; ctn_cal_update_soc(); log_info("%s,state:%s,p:%d,intv_time:%lld,chg_energy_p:%.3f,chg_energy_pr:%.3f,chg_energy_basic:%.3f", __func__,ctn_get_state_str(),ctn_get_ap(),intv_time,ctn_cal.cal_soc.chg_energy_p,ctn_cal.cal_soc.chg_energy_pr,ctn_cal.cal_soc.chg_energy_basic); }else if(ctn_get_state() == SMST_DHG){ int ap = ctn_get_ap(); double dhg_energy = 0; if(ctn_cal.cal_soc.soc <= 0.01)return; if(ctn_get_ap() < 5.0)return; if(fabs(ctn_get_ap()) > ctn_cal.cal_soc.extre_p) ctn_cal.cal_soc.dhg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a2_dhg + ctn_cal.cal_soc.b2_dhg; else ctn_cal.cal_soc.dhg_energy_p = fabs(ctn_get_ap())*ctn_cal.cal_soc.a1_dhg + ctn_cal.cal_soc.b1_dhg; ctn_cal.cal_soc.dhg_energy_p = ctn_cal.cal_soc.dhg_energy_p * ctn_cal.soh; log_info("%s,dhg_energy_p:%.3f",__func__,ctn_cal.cal_soc.dhg_energy_p); struct timeval tm; long long intv_time = 0; gettimeofday(&tm,NULL); intv_time = (long long )tm.tv_sec * 1000 + (long long )tm.tv_usec/1000 - ctn_cal.cal_soc.ts_last; log_info("%s,now_tm:%lld,last_time:%lld",__func__,(long long )tm.tv_sec * 1000 + (long long )tm.tv_usec/1000,ctn_cal.cal_soc.ts_last); log_info("%s,dhg_energy_p:%.3f",__func__,ctn_cal.cal_soc.dhg_energy_p); ctn_cal.cal_soc.dhg_energy_pr += (ctn_cal.cal_soc.dhg_energy_p - ctn_cal.cal_soc.dhg_energy_basic)/(ctn_cal.cal_soc.dhg_energy_p/fabs(ap))/3600.0 * intv_time / 1000.0; log_info("%s,dhg_energy_p:%.3f",__func__,ctn_cal.cal_soc.dhg_energy_pr); dhg_energy = meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx) - ctn_cal.cal_soc.meter_neg_energy_base; log_info("%s,dhg_energy:%.3f,hg_energy_p:%.3f",__func__,dhg_energy,ctn_cal.cal_soc.dhg_energy_p); ctn_cal.cal_soc.soc = ctn_cal.cal_soc.soc_base - dhg_energy / ctn_cal.cal_soc.dhg_energy_pr; if(ctn_cal.cal_soc.soc < 0.0) ctn_cal.cal_soc.soc = 0.0; ctn_cal.cal_soc.ts_last = (long long )tm.tv_sec * 1000 + (long long )tm.tv_usec/1000; //cycle if(ctn_cal.meter_neg_energy_last < 1.0){ ctn_cal.meter_neg_energy_last = meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx); }else{ ctn_cal.cycle += (meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx) - ctn_cal.meter_neg_energy_last) / ctn_cal.cal_soc.dhg_energy_pr; ctn_cal.meter_neg_energy_last = meter_get_neg_ae(ctn_cal.meter_model,ctn_cal.meter_idx); } ctn_cal_update_soc(); ctn_cal_update_cycle(); log_info("%s,state:%s,p:%d,intv_time:%lld,dhg_energy_p:%.3f,dhg_energy_pr:%.3f,dhg_energy_basic:%.3f", __func__,ctn_get_state_str(),ctn_get_ap(),intv_time,ctn_cal.cal_soc.dhg_energy_p,ctn_cal.cal_soc.dhg_energy_pr,ctn_cal.cal_soc.dhg_energy_basic); } }else{ //do nothing waiting } } static int ctn_cal_err() { if(ctn_cal_get_cmd() == CMD_SM_LAUNCH){ ctn_cal_set_cmd(CMD_SM_DONE); sm_set_state(&ctn_cal.sm,SMST_LAUNCH,CTN_CALERR_NONE); } } static int ctn_cal_sm() { struct statemachine_t* sm = &ctn_cal.sm; switch(sm_get_state( sm )){ case SMST_LAUNCH: ctn_cal_launch(); break; case SMST_RUN: ctn_cal_run(); break; case SMST_ERR: ctn_cal_err(); break; default: break; } } static void ctn_cal_thrd_main(void *param) { log_dbg("%s,++",__func__); struct statemachine_t* sm = &ctn_cal.sm; sm->states = ctn_cal_states; sm->state_nbr = sizeof(ctn_cal_states)/sizeof(struct state_t); sm->errs = ctn_cal_errs; sm->err_nbr = sizeof(ctn_cal_errs)/sizeof(struct err_t); sm_set_state( &ctn_cal.sm, SMST_LAUNCH, CTN_CALERR_NONE ); while(1){ sleep(10); ctn_cal_sm(); } log_dbg("%s,--",__func__); } int ctn_cal_init() { sqlite3* db = NULL; int ret = 0; char buf[128]; char sql[1024]; int rc = 0; struct dbcbparam_t cbparam; int result = 0; char *errmsg = NULL; pthread_t thrd; log_dbg("%s, ++",__func__); sprintf(buf,"../data/ctn_cal.db"); if(access(buf,0) < 0){ log_dbg("%s, access ctn_cal data db fail, path:%s", __func__, buf); ret = -1; }else if((rc = sqlite3_open(buf,&ctn_cal.db)) != SQLITE_OK){ log_dbg("%s,open db %s failed",__func__,buf); ret = -2; }else{ memset((void *)&cbparam,0,sizeof(struct dbcbparam_t)); plt_lock_cfgdb(); db = plt_get_cfgdb(); sprintf(sql,"select * from cal"); result = sqlite3_exec(db,sql, ctn_cal_dbcb_0,(void*)&cbparam,&errmsg); plt_unlock_cfgdb(); if( result != SQLITE_OK ){ ret = -3; }else if(cbparam.ret != 0){ ret = -4; }else{ //create pthread if(pthread_create(&thrd,NULL,ctn_cal_thrd_main,NULL) != 0){ ret = -5; } } } log_dbg("%s, ++,ret:%d",__func__,ret); return ret; } int ctn_cal_get_tool_data(char* buf) { char temp_buf[1024*10]; sprintf(buf,""REVERSE" CTN CAL INFO"NONE"\n"); sprintf(temp_buf,"state:%s,mmodel:%s,midx:%d,soh:%.3f,soc:%.3f,cycle:%.3f,extreP:%.3f\n\ a1_chg:%.3f,b1_chg:%.3f,a2_chg:%.3f,b2_chg:%.3f\n\ a1_dhg:%.3f,b1_dhg:%.3f,a2_dhg:%.3f,b2_dhg:%.3f\n", sm_get_szstate(&ctn_cal.sm),ctn_cal.szmeter_model,ctn_cal.meter_idx,ctn_cal.soh,ctn_cal.cal_soc.soc,ctn_cal.cycle,ctn_cal.cal_soc.extre_p, ctn_cal.cal_soc.a1_chg,ctn_cal.cal_soc.b1_chg,ctn_cal.cal_soc.a2_chg,ctn_cal.cal_soc.b2_chg, ctn_cal.cal_soc.a1_dhg,ctn_cal.cal_soc.b1_dhg,ctn_cal.cal_soc.a2_dhg,ctn_cal.cal_soc.b2_dhg); strcat(buf,temp_buf); } int ctn_cal_get_tb_data(char* data) { sprintf(data,"cal_state:\"%s\",cal_mmodel:\"%s\",cal_midx:%d,cal_soc:%.3f,cal_cycle:%.3f\n", sm_get_szstate(&ctn_cal.sm),ctn_cal.szmeter_model,ctn_cal.meter_idx,ctn_cal.cal_soc.soc,ctn_cal.cycle); return 0; } int ctn_cal_set_cmd(int cmd) { ctn_cal.cmd = cmd; return 0; } int ctn_cal_get_cmd( void ) { return ctn_cal.cmd; } int ctn_cal_get_state( void ) { struct statemachine_t* sm = &ctn_cal.sm; return sm_get_state( sm ); } int ctn_cal_set_soc(double soc) { ctn_cal.cal_soc.soc = soc; ctn_cal.cal_soc.soc_base = soc; ctn_cal_update_soc(); return 0; } int ctn_cal_set_cycle(double cycle) { ctn_cal.cycle = cycle; ctn_cal_update_cycle(); return 0; } int ctn_cal_set_chg_param(double a1_chg,double b1_chg,double a2_chg,double b2_chg) { ctn_cal.cal_soc.a1_chg = a1_chg; ctn_cal.cal_soc.b1_chg = b1_chg; ctn_cal.cal_soc.a2_chg = a2_chg; ctn_cal.cal_soc.b2_chg = b2_chg; log_info("%s,set chg param,a1:%.3f,b1:%.3f,a2:%.3f,b2:%.3f",__func__,a1_chg,b1_chg,a2_chg,b2_chg); ctn_cal_update_params(); return 0; } int ctn_cal_set_dhg_param(double a1_dhg,double b1_dhg,double a2_dhg,double b2_dhg) { ctn_cal.cal_soc.a1_dhg = a1_dhg; ctn_cal.cal_soc.b1_dhg = b1_dhg; ctn_cal.cal_soc.a2_dhg = a2_dhg; ctn_cal.cal_soc.b2_dhg = b2_dhg; log_info("%s,set chg param,a1:%.3f,b1:%.3f,a2:%.3f,b2:%.3f",__func__,a1_dhg,b1_dhg,a2_dhg,b2_dhg); ctn_cal_update_params(); return 0; } int ctn_cal_set_soh(double soh) { ctn_cal.soh = soh; ctn_cal_update_soh(); } int ctn_cat_get_soc(void) { return ctn_cal.cal_soc.soc; } int ctn_cal_get_cycle(void) { return ctn_cal.cycle; }