void MyFrame::OnDisplayFrequencies(wxCommandEvent& event)
{
string information = string("Num frequencies: ") + int2string(precomputationState.rLin) + string("\n\n");
if (precomputationState.rLin <= 20)
{
for(int i=0; i<precomputationState.rLin; i++)
information = information + double2string(precomputationState.frequencies[i]) + string("\n");
}
else
{
for(int i=0; i<18; i++)
{
information = information + double2string(precomputationState.frequencies[i]) + string("\n");
}
information = information + string("...\n");
information = information + double2string(precomputationState.frequencies[precomputationState.rLin-2]) + string("\n");
information = information + double2string(precomputationState.frequencies[precomputationState.rLin-1]) + string("\n");
}
wxWindow * parent = NULL;
WxDialogWrapper<wxMessageDialog> dlg( new wxMessageDialog(parent,
wxString(information.c_str(), wxConvUTF8),
wxString(_T("Frequency information"), wxConvUTF8),
wxOK | wxICON_INFORMATION) );
dlg.get()->ShowModal();
}
bool PPETask::addObsEdge(vector<DjiPoint> edge, int type)
{
if (edge.size() == 0)
return false;
if (isUseCoordTransform)
edge = tran.latlong2plane(edge);
printLog(" ");
switch (type)
{
case DJI_EDGE_FARMLAND:
printLog("add DJI_EDGE_FARMLAND edge size=" + int2string(edge.size()));
farmEdge = edge;
break;
case DJI_EDGE_NSZ_AROUND:
printLog("add DJI_EDGE_NSZ_AROUND edge size=" + int2string(edge.size()));
nsz_around.push_back(edge);
break;
case DJI_EDGE_NSZ_OVER:
printLog("add DJI_EDGE_NSZ_NOSPRAY edge size=" + int2string(edge.size()));
nsz_blank.push_back(edge);
printLog(" ");
break;
case DJI_EDGE_NSZ_HEIGHT:
printLog("DJI_EDGE_CHANGE_HEIGHT handled as DJI_EDGE_FORBID, in addObsEdge");
nsz_around.push_back(edge);
//nsz_height.push_back(edge);
break;
case DJI_EDGE_NSZ_AROUND_OUTERSIDE:
printLog("add DJI_EDGE_NSZ_AROUND_OUTERSIDE edge");
obs_around_outside.push_back(edge);
break;
case DJI_LINE_CHARGING:
printLog("add DJI_LINE_CHARGING polyline size=" + int2string(edge.size()));
chargingLine_org = edge;
break;
default:
printLog("input edge unknow type error, in addObsEdge",true);
return false;
break;
}
for (int i = 0; i < edge.size(); i++)
{
printLog(double2string(edge[i].x) + ", " + double2string(edge[i].y));
}
printLog(" ");
return true;
}
bool QLCDNumber::checkOverflow(double num) const
{
Q_D(const QLCDNumber);
bool of;
double2string(num, d->base, d->ndigits, &of);
return of;
}
PPETask::PPETask(DjiPoint _startSprayingPosition,double _sprayingDirection, double _sprayingWidth,
int _droneNum, bool _isUseCoordTransform)
{
if (!(_sprayingDirection <= 360 && _sprayingDirection >= 0 &&
_sprayingWidth>0 && _sprayingWidth<=MAX_SPRAY_WIDTH_METER))
{
printLog("WaypointTask init input error", true);
}
if (_isUseCoordTransform)
{
if (!isLegalLatLong(_startSprayingPosition))
{
printLog("_startSprayingPosition is illegal LatLong", true);
}
}
if (_droneNum<1)
{
printLog("inupt error droneNum<1", true);
}
isUseCoordTransform = _isUseCoordTransform;
//all the latitude_longtitude data will be transformed into plane coordinate (the unit is metre)
tran = isUseCoordTransform ? CoordTransform(_startSprayingPosition) : CoordTransform();
homePoint = isUseCoordTransform ? tran.latlong2plane(_startSprayingPosition) : _startSprayingPosition;
droneNum = _droneNum;
forwardVec = CoordTransform::dirSouthEastFromNorthAngle(_sprayingDirection);
forwardVec = forwardVec / molMat(forwardVec);
//rotate clockwise 90 degree
sideVec.x = forwardVec.y;
sideVec.y = -forwardVec.x;
sprayWidth = _sprayingWidth;
printLog(" ");
printPoint("startSprayingPosition: ", _startSprayingPosition);
printLog("sprayingDirection: " + double2string(_sprayingDirection));
printLog("sprayingWidth: " + double2string(sprayWidth));
printLog("droneNum: " + int2string(droneNum));
initDefaultValues();
}
void simulateJumps::init()
{
//init the vector of waiting times.
_waitingTimeParams.clear();
_waitingTimeParams.resize(_alphabetSize);
int i, j;
for (i = 0; i < _alphabetSize; ++i)
{
_waitingTimeParams[i] = -_sp.dPij_dt(i, i, 0.0);
}
//init _jumpProbs.
//_jumpProbs[i][j] = Q[i][j] / -Q[i][i]
_jumpProbs.clear();
_jumpProbs.resize(_alphabetSize);
for (i = 0; i < _alphabetSize; ++i)
{
MDOUBLE sum = 0.0;
_jumpProbs[i].resize(_alphabetSize);
for (j = 0; j < _alphabetSize; ++j)
{
if (i == j)
_jumpProbs[i][j] = 0.0;
else
{
_jumpProbs[i][j] = _sp.dPij_dt(i, j, 0.0) / _waitingTimeParams[i];
}
sum += _jumpProbs[i][j];
}
if (! DEQUAL(sum, 1.0)){
string err = "error in simulateJumps::init(): sum probabilities is not 1 and equal to ";
err+=double2string(sum);
errorMsg::reportError(err);
}
}
//init _orderNodesVec: a vector in which the branch lengths are ordered in ascending order
_tree.getAllNodes(_orderNodesVec, _tree.getRoot());
sort(_orderNodesVec.begin(), _orderNodesVec.end(), simulateJumpsAbstract::compareDist);
_nodes2JumpsExp.clear();
_nodes2JumpsProb.clear();
VVdouble zeroMatrix(getCombinedAlphabetSize());
for (i = 0; i < getCombinedAlphabetSize(); ++i)
zeroMatrix[i].resize(getCombinedAlphabetSize(), 0.0);
Vdouble zeroVector(getCombinedAlphabetSize(),0.0);
for (i = 0; i < _orderNodesVec.size(); ++i)
{
string nodeName = _orderNodesVec[i]->name();
_nodes2JumpsExp[nodeName] = zeroMatrix;
_nodes2JumpsProb[nodeName] = zeroMatrix;
for (j=0; j<getCombinedAlphabetSize();++j)
_totalTerminals[nodeName]=zeroVector;
}
}
void QLCDNumber::display( double num )
{
val = num;
bool of;
QString s = double2string( num, base, ndigits, &of );
if ( of )
emit overflow();
else
internalSetString( s );
}
/*!
\overload
Displays the number \a num.
*/
void QLCDNumber::display(double num)
{
Q_D(QLCDNumber);
d->val = num;
bool of;
QString s = double2string(d->val, d->base, d->ndigits, &of);
if (of)
emit overflow();
else
d->internalSetString(s);
}
std::string Node::secondString() const
{
std::string res = "1 ";
res += string2string(satelliteNumber(), 5);
const char cl = classification();
res += (isprint(cl) ? std::string(1, cl) : " ") + " ";
res += string2string(designator(), 8) + " ";
res += date2string(preciseEpoch(), 14) + " ";
res += double2string(dn(), 10, 8, false, false, false) + " ";
res += double2string(d2n(), 8, 3, true, true, false) + " ";
res += double2string(bstar(), 8, 3, true, true, false) + " ";
const char eph = ephemerisType();
res += (isprint(eph) ? std::string(1, eph) : " ") + " ";
res += int2string(elementNumber(), 4, false);
// Checksum
int sum = checksum(res);
res += int2string(sum, 1);
return res;
}
void BSpline::compute_point(float *knotVector, int ctrlPtsNum, int polyDegree, double interval,
QVector3D *ctrlPts, QPointF *output)
{
int k;
double temp;
// initialize the variables that will hold our outputted point
output->setX(0);
output->setY(0);
QString test;
test.append("interval: ");
test.append(double2string(interval));
test.append("\n");
for (k=0; k <= ctrlPtsNum; k++)
{
temp = blend(k, polyDegree, knotVector, interval); // same blend is used for each dimension coordinate
test.append(double2string(temp));
test.append("\n");
output->setX(output->x() + ctrlPts[k].x() * temp);
output->setY(output->y() + ctrlPts[k].y() * temp);
}
}
bool PPETask::setTimeToChangeBattery(double timeToChangeBattery)
{
if (timeToChangeBattery < 0)
{
printLog("input error: timeToChangeBattery<0 in setTimeToChangeBattery()", true);
return false;
}
else
{
printLog("setTimeToChangeBattery " + double2string(timeToChangeBattery));
}
this->timeToChangeBattery = timeToChangeBattery;
return true;
}
bool PPETask::setMinPowerPercent(double minPowerPercent)
{
if (minPowerPercent < 0)
{
printLog("input error: minPowerPercent<0 in setMinPowerPercent()", true);
return false;
}
else
{
printLog("setMinPowerPercent " + double2string(minPowerPercent));
}
this->minPowerPercent = minPowerPercent;
return true;
}
bool PPETask::setLandingDeviation(double landingDeviation)
{
if (landingDeviation < 0)
{
printLog("input error: landingDeviation<0 in setLandingDeviation()", true);
return false;
}
else
{
printLog("setLandingDeviation " + double2string(landingDeviation));
}
this->landingDeviation = landingDeviation;
return true;
}
bool PPETask::setMaxGroundWalk(double maxGroundWalk)
{
if (maxGroundWalk < 0)
{
printLog("input error: maxGroundWalk<0 in setMaxGroundWalk()", true);
return false;
}
else
{
printLog("setMaxGroundWalk " + double2string(maxGroundWalk));
}
this->maxGroundWalk = maxGroundWalk;
return true;
}
bool PPETask::setSprayingOffset(double sprayingOffset)
{
if (sprayingOffset < 0)
{
printLog("input error: sprayingOffset<0 in setSprayingOffset()", true);
return false;
}
else
{
printLog("setSprayingOffset " + double2string(sprayingOffset));
}
this->sprayingOffset = sprayingOffset;
return true;
}
bool PPETask::setTimeOneBattery(double timeOneBattery)
{
if (timeOneBattery < 0)
{
printLog("input error: timeOneBattery<0 in setTimeOneBattery()", true);
return false;
}
else
{
printLog("setTimeOneBattery " + double2string(timeOneBattery));
}
this->timeOneBatteryS = timeOneBattery;
this->maxFlightDist = flightSpeed*timeOneBattery;
return true;
}
bool PPETask::setFlightSpeed(double flightSpeed)
{
if (flightSpeed < 0)
{
printLog("input error: flightSpeed<0 in setFlightSpeed()", true);
return false;
}
else
{
printLog("setFlightSpeed " + double2string(flightSpeed));
}
this->flightSpeed = flightSpeed;
this->maxFlightDist = flightSpeed*timeOneBatteryS;
return true;
}
bool SensorDB::AddData(SensorDataSet dataset)
{
string channel_num="";
string datatype_id="";
string operator_id="";
string device_id="";
string position_id="";
string activity_id="";
string activitybeginframe_id="";
string activityendframe_id="";
string samplerate="";
string createtime="";
for (int i = 0; i < dataset.Count(); i++)
{
string cmd = "INSERT INTO [dbo].[SensorDataTable](DataTypeID,ActivityID,DeviceID,OperatorID,PositionID,SampleRate,CreateTime,TotalChannelNum,ActivityBeginFrameID,ActivityEndFrameID";
for (int j = 1; j <= dataset.GetSensorData(i).GetTotalChannelNum(); j++)
{
cmd += (",channel_"+int2string(j));
}
cmd+=") VALUES (";
channel_num = ",'"+int2string(dataset.GetSensorData(i).GetTotalChannelNum())+"'";
datatype_id = "'"+int2string(dataset.GetSensorData(i).GetDataTypeID())+"'";
operator_id = ",'"+int2string(dataset.GetSensorData(i).GetOperatorID())+"'";
device_id = ",'"+int2string(dataset.GetSensorData(i).GetDeviceID())+"'";
position_id = ",'"+int2string(dataset.GetSensorData(i).GetPositionID())+"'";
activity_id = ",'"+int2string(dataset.GetSensorData(i).GetActivityID())+"'";
activitybeginframe_id = ",'"+int2string(dataset.GetSensorData(i).GetActivityBeginFrameID())+"'";
activityendframe_id = ",'"+int2string(dataset.GetSensorData(i).GetActivityEndFrameID())+"'";
samplerate= ",'"+double2string(dataset.GetSensorData(i).GetSampleRate())+"'";
createtime= ",'"+dataset.GetSensorData(i).GetCreateTime().toString("yyyy-MM-dd hh:mm:ss").toStdString()+"'";
cmd+=(datatype_id+activity_id+device_id+operator_id+position_id+samplerate+createtime+channel_num+activitybeginframe_id+activityendframe_id);
for (int j = 1; j <= dataset.GetSensorData(i).GetTotalChannelNum(); j++)
{
cmd+=(",'"+dataset.GetSensorData(i).GetChannel(j-1).ToString()+"'");
}
cmd+=");";
if(!ExecuteSQL(cmd)){
return false;
}
}
return true;
}
std::string date2string(const double date, const std::size_t fieldLength,
const bool leftAlign)
{
double dt = date;
std::size_t year = UNIX_FIRST_YEAR;
while (true)
{
bool leap = !(year % 4) && ((year % 100) || !(year % 400));
std::time_t l = (leap ? 366 : 365) * 86400;
if (dt < l)
break;
dt -= l;
year++;
}
year -= year > 2000 ? 2000 : 1900;
double res = year * 1000 + dt / 86400.0 + 1;
std::string pref = year < 10 ? "0" : "";
std::size_t length = year < 10 ? fieldLength - 1 : fieldLength;
return (pref + double2string(res, length, fieldLength - 6,
false, false, leftAlign));
}
std::string PacketDescriptor::getFieldAsString(void *object, int field, int i) const
{
cClassDescriptor *basedesc = getBaseClassDescriptor();
if (basedesc) {
if (field < basedesc->getFieldCount(object))
return basedesc->getFieldAsString(object,field,i);
field -= basedesc->getFieldCount(object);
}
Packet *pp = (Packet *)object; (void)pp;
switch (field) {
case 0: return long2string(pp->getSrc());
case 1: return long2string(pp->getDst());
case 2: return long2string(pp->getSessionID());
case 3: return long2string(pp->getPacketID());
case 4: return long2string(pp->getPriority());
case 5: return long2string(pp->getHops());
case 6: return long2string(pp->getPayload());
case 7: return long2string(pp->getDs());
case 8: return double2string(pp->getCreationTime());
default: return "";
}
}
std::string Node::thirdString() const
{
std::string res = "2 ";
res += string2string(satelliteNumber(), 5) + " ";
res += double2string(normalizeAngle(i()), 8, 4, false, false, false) + " ";
res += double2string(normalizeAngle(Omega()), 8, 4, false, false, false) +
" ";
res += double2string(e(), 7, 7, false, true, false) + " ";
res += double2string(normalizeAngle(omega()), 8, 4, false, false, false) +
" ";
res += double2string(normalizeAngle(M()), 8, 4, false, false, false) + " ";
res += double2string(n(), 11, 8, false, false, false);
res += int2string(revolutionNumber(), 5, false);
// Checksum
int sum = checksum(res);
res += int2string(sum, 1);
return res;
}
bool SprayTask::askCharging(int waypointIndex, double powerPercent)
{
chargingPath = ChargingPath();
if (powerPercent > 100 || powerPercent < 0)
{
printLog("powerPercent out of range !", true);
return false;
}
if (waypointIndex >= (int)waypoints.size() || waypointIndex < 0)
{
printLog("waypointIndex out of range !", true);
return false;
}
// if (powerPercent < safeLandPercent)
// {
// printLog("powerPercent < safeLandPercent!!!!!", true);
// }
vector<DjiPoint> goChargingPath;
vector<DjiPoint> continueTaskPath;
int usedIndex = -1; //使用了的航电index continuepath还要保证该index后面到index+1的航线段都飞行完毕
unsigned nearCharge = 0;
if (waypointIndex == waypoints.size() - 1)
{
printLog("the last waypoint, task complete !!!!", false);
findNearestChargePoint(waypoints[waypointIndex], supplyStart, supplyEnd, nearCharge, goChargingPath);
if (isUseCoordTransform)
goChargingPath = tran.plane2latlong(goChargingPath);
//最后一个航点
chargingPath = ChargingPath(goChargingPath);
return true;
}
//计算当前power还能飞多远
double distLeft = maxFlightDist*(powerPercent - minPowerPercent) / (100.0 - minPowerPercent);
//是否能够飞完下一个waypoint + gohome 的电量
double gohomeCost = findNearestChargePoint(waypoints[waypointIndex + 1], supplyStart, supplyEnd, nearCharge, goChargingPath);
double goNextCost = pointDistance(waypoints[waypointIndex], waypoints[waypointIndex + 1]);
//无需更换电池
if (gohomeCost + goNextCost < distLeft)
return false;
else//gohomeCost + goNextCost > distLeft
{
//********************* Print Log ***********************//
printLog("minPowerPercent=" + double2string(minPowerPercent));
printLog("waypointIndex=" + int2string(waypointIndex));
printLog("powerPercent=" + double2string(powerPercent));
printLog("maxFlightDist=" + double2string(maxFlightDist));
printLog("distLeft =" + double2string(distLeft));
printLog("goNextCost=" + double2string(goNextCost));
printLog("gohomeCost= " + double2string(gohomeCost));
printLog("goNextCost+gohomeCost =" + double2string(goNextCost + gohomeCost) + " > distLeft:" + double2string(distLeft));
if (!(waypoints[waypointIndex].s == 0 || waypoints[waypointIndex].s == 1))
{
printLog("waypoints[waypointIndex] with erro s=" + int2string(waypoints[waypointIndex].s), true);
}
//********************* Print Log End ***********************//
//下一段航线不用喷洒, 从当前waypointIndex返航换电池
if (waypoints[waypointIndex].s == 0)
{
//直接从当前点回家
findNearestChargePoint(waypoints[waypointIndex], supplyStart, supplyEnd, nearCharge, goChargingPath);
for (usedIndex = waypointIndex + 1; usedIndex < (int)waypoints.size(); usedIndex++)
{
//断电续航时飞到下一个开始喷洒的位置
if (waypoints[usedIndex].s == 1)
{
findTwoPointsPath(allAroundObsEdgesObj, chargingLine[nearCharge], waypoints[usedIndex], continueTaskPath);
break;
}
}
//任务结束，无断电续航的的continueTaskPath
if (continueTaskPath.size() == 0)
{
chargingPath = ChargingPath(isUseCoordTransform ? tran.plane2latlong(goChargingPath) : goChargingPath);
return true;
}
//保证usedIndex 到usedIndex之间的航线段飞完
if (usedIndex + 1 < (int)waypoints.size())
{
continueTaskPath.push_back(waypoints[usedIndex + 1]);
}
}
//下一段为喷洒航线段，从当前waypointIndex继续喷洒，直到剩余电量刚好够回家
else if (waypoints[waypointIndex].s == 1)
{
findMiddlePointToGetChargingPath(waypoints[waypointIndex], waypoints[waypointIndex + 1],
distLeft, supplyStart, supplyEnd, goChargingPath, continueTaskPath, nearCharge);
usedIndex = waypointIndex;
}
continueTaskPath.insert(continueTaskPath.end(), waypoints.begin() + usedIndex + 1, waypoints.end());
getSupplyAreaWithDistance(nearCharge, maxGroundWalk, supplyStart, supplyEnd);
unsigned estimateNext = estimateNextChargingPoint(continueTaskPath, maxFlightDist, supplyStart, supplyEnd);
getSupplyAreaWithDistance(estimateNext, landingDeviation, supplyStart, supplyEnd);
DjiPoint nextChargingPoint = chargingLine[estimateNext];
vector<DjiPoint> supplyArea = getRange(chargingLine, supplyStart, supplyEnd);
//更新waypoints
waypoints = continueTaskPath;
if (isUseCoordTransform)
{
goChargingPath = tran.plane2latlong(goChargingPath);
continueTaskPath = tran.plane2latlong(continueTaskPath);
nextChargingPoint = tran.plane2latlong(nextChargingPoint);
supplyArea = tran.plane2latlong(supplyArea);
}
chargingPath = ChargingPath(goChargingPath, continueTaskPath, supplyArea);
return true;
}
return false;
}
void TSK_LCD(UArg a0, UArg a1)
{
// System_printf("enter taskFxn()\n");
char msg[16];
short index = 0;
while (1)
{
Event_pend(EVT_LCD_CLK,Event_Id_00, Event_Id_NONE,BIOS_WAIT_FOREVER);
if(LCD_S.init == 1)
{
LCD_Clear();
LCD_Cursor(0,0);
if (Bike.Mode == MODE_CHARGE)
{
msg[0] = 'H';
msg[1] = '\0';
scib_msg(msg);
LCD_Cursor(1,0);
double2string((double)(bq_pack.highest_cell_volts/1000.0), 2.0, msg);
scib_msg(msg);
msg[0] = ' ';
msg[1] = 'L';
msg[2] = '\0';
scib_msg(msg);
double2string((double)(bq_pack.lowest_cell_volts/1000.0), 2.0, msg);
scib_msg(msg);
msg[0] = ' ';
msg[1] = 'T';
msg[2] = '\0';
scib_msg(msg);
unsigned short temp = bq_pack.highest_temp;
ltoa(temp,msg);
msg[2] = '\0';
scib_msg(msg);
}
if(Bike.Mode == MODE_RACE)
{
ltoa((long)(W2000.Odometer),msg); //mph
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.PhaseATemp),msg); //degree C
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.MotorVelocity),msg); //amps
}
LCD_Cursor(0,1);
if(Bike.Fault.all != 0)
{
index = 0;
if(Bike.Fault.bit.precharge != 0 && index < 9)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'P';
msg[3] = 'R';
msg[4] = 'E';
msg[5] = 'C';
msg[6] = 'H';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
if(Bike.Fault.bit.BMM_Init != 0 && index < 11)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'B';
msg[3] = 'M';
msg[4] = 'M';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Fault.bit.OV != 0 && index < 12)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'O';
msg[3] = 'V';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 5;
}
if(Bike.Fault.bit.UV != 0 && index < 12)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'U';
msg[3] = 'V';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 5;
}
if(Bike.Fault.bit.estop != 0 && index < 9)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'E';
msg[3] = 'S';
msg[4] = 't';
msg[5] = 'o';
msg[6] = 'p';
msg[7] = ' ';
msg[8] = '\0';
//strcpy(msg,"F:EStop ");
scib_msg(msg);
index = index + 8;
}
if(Bike.Fault.bit.Charge_OC != 0 && index < 10)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'h';
msg[4] = 'r';
msg[5] = 'g';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Fault.bit.Dis_OC != 0 && index < 11)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'D';
msg[3] = 'i';
msg[4] = 's';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Fault.bit.Switch_Error != 0 && index < 9)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'S';
msg[3] = '_';
msg[4] = 'E';
msg[5] = 'r';
msg[6] = 'r';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
}
else if(Bike.Warning.all != 0)
{
index = 0;
if(Bike.Warning.bit.BMM_loss != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'B';
msg[3] = 'M';
msg[4] = 'M';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Warning.bit.w2000_off != 0 && index < 9)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'W';
msg[3] = '2';
msg[4] = '0';
msg[5] = '0';
msg[6] = '0';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
if(Bike.Warning.bit.bus_volt != 0 && index < 9)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'B';
msg[3] = 'V';
msg[4] = 'O';
msg[5] = 'L';
msg[6] = 'T';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
if(Bike.Warning.bit.current_sense_error != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'S';
msg[4] = 'E';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Warning.bit.Charage_OC != 0 && index < 10)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'h';
msg[4] = 'r';
msg[5] = 'g';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Warning.bit.Dis_OC != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'D';
msg[3] = 'i';
msg[4] = 's';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Warning.bit.SD_Startup != 0 && index < 10)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'S';
msg[3] = 'D';
msg[4] = '_';
msg[5] = 'S';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Warning.bit.SD_Write != 0 && index < 10)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'S';
msg[3] = 'D';
msg[4] = '_';
msg[5] = 'W';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Warning.bit.Capacity != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'a';
msg[4] = 'p';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
}
else
{
if(Bike.Mode == MODE_RACE)
{
//scib_msg("Vel:\0");
ltoa((long)(W2000.VehicleVelocity*(2.2)),msg); //mph
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.MotorTemp),msg); //degree C
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.BusCurrent),msg); //amps
scib_msg(msg);
}
else if (Bike.Mode == MODE_CHARGE)
{
scib_msg("V:\0");
ltoa((long)W2000.BusVoltage,msg);
scib_msg(msg);
scib_msg("C:\0");
ltoa((long)(W2000.BusCurrent),msg); //amps
scib_msg(msg);
}
/*
FOR REFERENCE
scib_msg("MBV:\0");
ltoa((long)W2000.BusVoltage,msg);
scib_msg(msg);
scib_msg("BV:\0");
ltoa((long)(bq_pack.voltage/1000),msg);
scib_msg(msg);
scib_msg("Vel:\0");
ltoa((long)(W2000.VehicleVelocity*(2.2)),msg); //mph
scib_msg(msg);
scib_msg("MT:\0");
ltoa((long)(W2000.MotorTemp),msg); //degree C
scib_msg(msg);
scib_msg("MC:\0");
ltoa((long)(W2000.BusCurrent),msg); //amps
scib_msg(msg);
*/
}
Task_sleep(2);
}
// System_printf("exit taskFxn()\n");
}
}
void LogisticRegression::TrainModel()
{
cout << "Train ...." << endl;
//load data
string line;
ifstream fin(this->str_train_data_file_.c_str());
if(!fin)
{
cerr << "Can not read train data file !" << endl;
exit(0);
}
while(getline(fin, line))
{
vector<double> vd_data = split2double(line, "\t");
int label = (int)vd_data.back();
vd_data.pop_back();
this->vec_train_label_.push_back(label);
this->vec_train_data_.push_back(vd_data);
}
fin.close();
//data scale
InitScale(this->vec_train_data_);
ScaleData(this->vec_train_data_);
//train
size_t i_round = 0;
while(i_round < this->i_max_round)
{
cout << "Rount [" << i_round << "]....." << endl;
vector<double> douVec_gradient(this->vec_train_data_[0].size(), 0);
double dou_bias_gradient = 0;
double dou_cost = 0;
for(int i = 0; i < this->vec_train_data_.size(); i++)
{
double dou_h = Sigmoid(this->vec_train_data_[i]);
//calculate cost for each data
dou_cost += (this->vec_train_label_[i]*log(dou_h) + (1-this->vec_train_label_[i])*log(1-dou_h));
//calculate gradient for each data
for(int j = 0; j < douVec_gradient.size(); j++)
{
douVec_gradient[j] += (this->vec_train_label_[i] - dou_h)*this->vec_train_data_[i][j];
}
dou_bias_gradient += (this->vec_train_label_[i] - dou_h);
}
//L2 regularization
double dou_regularization = 0;
for(int i = 0; i < this->douVec_weights_.size(); i++)
{
dou_regularization += pow(this->douVec_weights_[i], 2);
}
//final cost
dou_cost = -1.0 * dou_cost / this->douVec_weights_.size() + this->dou_lambda_*dou_regularization/(2*this->douVec_weights_.size());
cout << "Cost J = " << dou_cost << endl;
//update
string str_new_weights("new weights is [ ");
string str_gradient("gradient is [");
for(int k = 0; k < douVec_gradient.size(); k++)
{
this->douVec_weights_[k] += this->dou_step_*(douVec_gradient[k]+this->dou_lambda_*this->douVec_weights_[k])/this->vec_train_data_.size();
str_new_weights = str_new_weights + double2string(this->douVec_weights_[k]) + " ";
str_gradient = str_gradient + double2string(douVec_gradient[k]) + " ";
}
this->dou_bias_ += this->dou_step_*dou_bias_gradient/this->vec_train_data_.size();
str_new_weights = str_new_weights + double2string(this->dou_bias_) + "]";
str_gradient = str_gradient + double2string(dou_bias_gradient) + "]";
cout << str_new_weights << endl;
cout << str_gradient << endl;
i_round += 1;
}
//save model
cout << "save model ..." << endl;
ofstream fout("./logreg_model.txt");
cout << this->douVec_weights_.size() << endl;
for(int i = 0; i < this->douVec_weights_.size(); i++)
{
fout << this->douVec_weights_[i] << " ";
}
fout << this->dou_bias_ << endl;
fout.close();
}
bool QLCDNumber::checkOverflow( double num ) const
{
bool of;
double2string( num, base, ndigits, &of );
return of;
}
