Erick / Mbed 2 deprecated ICE_BLE_TEST

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   NaturalTinyShell_ice libmDot-12Sept mbed-rtos mbed

Fork of ICE by Erick

src/ConfigurationHandler/Controls/FailsafeControl.cpp@252:3c9863f951b7, 2016-10-21 (annotated)

Committer:
jmarkel44
Date:
Fri Oct 21 18:16:42 2016 +0000
Revision:
252:3c9863f951b7
Parent:
250:1cd8ec63e9e9
Child:
253:ae850c19cf81
continued work on failsafes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jmarkel44 250:1cd8ec63e9e9 1 /******************************************************************************
jmarkel44 250:1cd8ec63e9e9 2 *
jmarkel44 250:1cd8ec63e9e9 3 * File: CompositeControl.cpp
jmarkel44 250:1cd8ec63e9e9 4 * Desciption: ICE Composite Control Class implementation
jmarkel44 250:1cd8ec63e9e9 5 *
jmarkel44 250:1cd8ec63e9e9 6 *****************************************************************************/
jmarkel44 250:1cd8ec63e9e9 7 #include "FailsafeControl.h"
jmarkel44 250:1cd8ec63e9e9 8 #include "cJSON.h"
jmarkel44 250:1cd8ec63e9e9 9 #include "mDot.h"
jmarkel44 250:1cd8ec63e9e9 10 #include "global.h"
jmarkel44 252:3c9863f951b7 11 #include "ModbusMasterApi.h"
jmarkel44 250:1cd8ec63e9e9 12 #include <string>
jmarkel44 250:1cd8ec63e9e9 13 #include <iostream>
jmarkel44 250:1cd8ec63e9e9 14 #include <iomanip>
jmarkel44 250:1cd8ec63e9e9 15
jmarkel44 250:1cd8ec63e9e9 16 extern mDot *GLOBAL_mdot;
jmarkel44 250:1cd8ec63e9e9 17
jmarkel44 250:1cd8ec63e9e9 18 //
jmarkel44 250:1cd8ec63e9e9 19 // method: load
jmarkel44 250:1cd8ec63e9e9 20 // description: load a composite control
jmarkel44 250:1cd8ec63e9e9 21 //
jmarkel44 250:1cd8ec63e9e9 22 // @param none
jmarkel44 250:1cd8ec63e9e9 23 // @return none
jmarkel44 250:1cd8ec63e9e9 24 //
jmarkel44 250:1cd8ec63e9e9 25 bool FailsafeControl::load(std::string _controlFile)
jmarkel44 250:1cd8ec63e9e9 26 {
jmarkel44 250:1cd8ec63e9e9 27 controlFile = _controlFile;
jmarkel44 250:1cd8ec63e9e9 28
jmarkel44 250:1cd8ec63e9e9 29 // open and read from the control file
jmarkel44 250:1cd8ec63e9e9 30 mDot::mdot_file file = GLOBAL_mdot->openUserFile(controlFile.c_str(), mDot::FM_RDONLY);
jmarkel44 250:1cd8ec63e9e9 31 if ( file.fd < 0 ) {
jmarkel44 250:1cd8ec63e9e9 32 logError("%s: failed to open %s\n", __func__, controlFile.c_str());
jmarkel44 250:1cd8ec63e9e9 33 return false;
jmarkel44 250:1cd8ec63e9e9 34 }
jmarkel44 250:1cd8ec63e9e9 35
jmarkel44 250:1cd8ec63e9e9 36 // read the data into a buffer
jmarkel44 250:1cd8ec63e9e9 37 char dataBuf[MAX_FILE_SIZE];
jmarkel44 250:1cd8ec63e9e9 38
jmarkel44 250:1cd8ec63e9e9 39 int bytes_read = GLOBAL_mdot->readUserFile(file, (void *)dataBuf, sizeof(dataBuf));
jmarkel44 250:1cd8ec63e9e9 40 if ( bytes_read != sizeof(dataBuf) ) {
jmarkel44 250:1cd8ec63e9e9 41 logError("%s: failed to read %d bytes from %s", __func__, sizeof(dataBuf), controlFile.c_str());
jmarkel44 250:1cd8ec63e9e9 42 // caller should destroy the object
jmarkel44 250:1cd8ec63e9e9 43 return false;
jmarkel44 250:1cd8ec63e9e9 44 }
jmarkel44 250:1cd8ec63e9e9 45
jmarkel44 250:1cd8ec63e9e9 46 // close the file
jmarkel44 250:1cd8ec63e9e9 47 GLOBAL_mdot->closeUserFile(file);
jmarkel44 250:1cd8ec63e9e9 48
jmarkel44 250:1cd8ec63e9e9 49 // parse the data
jmarkel44 250:1cd8ec63e9e9 50 cJSON * root = cJSON_Parse(dataBuf);
jmarkel44 250:1cd8ec63e9e9 51
jmarkel44 250:1cd8ec63e9e9 52 id = cJSON_GetObjectItem(root, "id")->valuestring;
jmarkel44 250:1cd8ec63e9e9 53 priority = atoi(cJSON_GetObjectItem(root, "priority")->valuestring);
jmarkel44 250:1cd8ec63e9e9 54 input = cJSON_GetObjectItem(root, "input")->valuestring;
jmarkel44 250:1cd8ec63e9e9 55 output = cJSON_GetObjectItem(root, "output")->valuestring;
jmarkel44 250:1cd8ec63e9e9 56
jmarkel44 250:1cd8ec63e9e9 57 hfs_data.value = atof(cJSON_GetObjectItem(root, "hfsValue")->valuestring);
jmarkel44 250:1cd8ec63e9e9 58 hfs_data.dutyCycle = atoi(cJSON_GetObjectItem(root, "hfsDutyCycle")->valuestring);
jmarkel44 250:1cd8ec63e9e9 59 hfs_data.interval = atoi(cJSON_GetObjectItem(root, "hfsInterval")->valuestring);
jmarkel44 250:1cd8ec63e9e9 60
jmarkel44 250:1cd8ec63e9e9 61 lfs_data.value = atof(cJSON_GetObjectItem(root, "lfsValue")->valuestring);
jmarkel44 250:1cd8ec63e9e9 62 lfs_data.dutyCycle = atoi(cJSON_GetObjectItem(root, "lfsDutyCycle")->valuestring);
jmarkel44 250:1cd8ec63e9e9 63 lfs_data.interval = atoi(cJSON_GetObjectItem(root, "lfsInterval")->valuestring);
jmarkel44 250:1cd8ec63e9e9 64
jmarkel44 250:1cd8ec63e9e9 65 return true;
jmarkel44 250:1cd8ec63e9e9 66 }
jmarkel44 250:1cd8ec63e9e9 67
jmarkel44 250:1cd8ec63e9e9 68 //
jmarkel44 250:1cd8ec63e9e9 69 // method: start
jmarkel44 250:1cd8ec63e9e9 70 // description: start the failsafe control
jmarkel44 250:1cd8ec63e9e9 71 //
jmarkel44 250:1cd8ec63e9e9 72 // @param none
jmarkel44 250:1cd8ec63e9e9 73 // @return none
jmarkel44 250:1cd8ec63e9e9 74 //
jmarkel44 250:1cd8ec63e9e9 75 void FailsafeControl::start(void)
jmarkel44 250:1cd8ec63e9e9 76 {
jmarkel44 250:1cd8ec63e9e9 77 currentState = STATE_START;
jmarkel44 250:1cd8ec63e9e9 78 }
jmarkel44 250:1cd8ec63e9e9 79
jmarkel44 250:1cd8ec63e9e9 80 //
jmarkel44 250:1cd8ec63e9e9 81 // method: update
jmarkel44 252:3c9863f951b7 82 // description: update the faisafe control using the FSM
jmarkel44 250:1cd8ec63e9e9 83 //
jmarkel44 250:1cd8ec63e9e9 84 // @param none
jmarkel44 250:1cd8ec63e9e9 85 // @return none
jmarkel44 250:1cd8ec63e9e9 86 //
jmarkel44 250:1cd8ec63e9e9 87 void FailsafeControl::update(void)
jmarkel44 250:1cd8ec63e9e9 88 {
jmarkel44 250:1cd8ec63e9e9 89 switch ( this->currentState ) {
jmarkel44 250:1cd8ec63e9e9 90 case STATE_INIT:
jmarkel44 250:1cd8ec63e9e9 91 // do nothing
jmarkel44 250:1cd8ec63e9e9 92 break;
jmarkel44 250:1cd8ec63e9e9 93 case STATE_START:
jmarkel44 252:3c9863f951b7 94 if ( this->aboveHighFailsafe() ) {
jmarkel44 252:3c9863f951b7 95 currentState = STATE_CONTROL_HFS_ON;
jmarkel44 252:3c9863f951b7 96 } else if ( this->belowLowFailsafe() ) {
jmarkel44 252:3c9863f951b7 97 currentState = STATE_CONTROL_LFS_ON;
jmarkel44 252:3c9863f951b7 98 } else {
jmarkel44 252:3c9863f951b7 99 currentState = STATE_CONTROL_OFF;
jmarkel44 252:3c9863f951b7 100 }
jmarkel44 252:3c9863f951b7 101 break;
jmarkel44 250:1cd8ec63e9e9 102 case STATE_CONTROL_OFF:
jmarkel44 252:3c9863f951b7 103 if ( this->aboveHighFailsafe() ) {
jmarkel44 252:3c9863f951b7 104 currentState = STATE_CONTROL_HFS_ON;
jmarkel44 252:3c9863f951b7 105 if ( hfs_data.dutyCycle ) {
jmarkel44 252:3c9863f951b7 106 this->startHfsTimer();
jmarkel44 252:3c9863f951b7 107 // send an ON request to the output thread
jmarkel44 252:3c9863f951b7 108 sendMail(ACTION_CONTROL_ON);
jmarkel44 252:3c9863f951b7 109 } else {
jmarkel44 252:3c9863f951b7 110 // fixed off
jmarkel44 252:3c9863f951b7 111 sendMail(ACTION_CONTROL_OFF);
jmarkel44 252:3c9863f951b7 112 // fixed off
jmarkel44 252:3c9863f951b7 113 }
jmarkel44 252:3c9863f951b7 114 } else if ( this->belowLowFailsafe() ) {
jmarkel44 252:3c9863f951b7 115 currentState = STATE_CONTROL_LFS_ON;
jmarkel44 252:3c9863f951b7 116 } else {
jmarkel44 252:3c9863f951b7 117 // do nothing
jmarkel44 252:3c9863f951b7 118 }
jmarkel44 252:3c9863f951b7 119 break;
jmarkel44 250:1cd8ec63e9e9 120 case STATE_CONTROL_LFS_ON:
jmarkel44 252:3c9863f951b7 121 if ( !this->belowLowFailsafe() ) {
jmarkel44 252:3c9863f951b7 122 currentState = STATE_CONTROL_OFF;
jmarkel44 252:3c9863f951b7 123 } else {
jmarkel44 252:3c9863f951b7 124 // check the time interval
jmarkel44 252:3c9863f951b7 125 }
jmarkel44 252:3c9863f951b7 126 break;
jmarkel44 250:1cd8ec63e9e9 127 case STATE_CONTROL_LFS_OFF:
jmarkel44 252:3c9863f951b7 128 if ( !this->belowLowFailsafe() ) {
jmarkel44 252:3c9863f951b7 129 currentState = STATE_CONTROL_OFF;
jmarkel44 252:3c9863f951b7 130 } else {
jmarkel44 252:3c9863f951b7 131
jmarkel44 252:3c9863f951b7 132 }
jmarkel44 252:3c9863f951b7 133 break;
jmarkel44 250:1cd8ec63e9e9 134 case STATE_CONTROL_HFS_ON:
jmarkel44 252:3c9863f951b7 135 // the control is in HFS with the output ON
jmarkel44 252:3c9863f951b7 136 if ( !this->aboveHighFailsafe() ) {
jmarkel44 252:3c9863f951b7 137 currentState = STATE_CONTROL_OFF;
jmarkel44 252:3c9863f951b7 138 // clear the timer
jmarkel44 252:3c9863f951b7 139 this->stopHfsTimer();
jmarkel44 252:3c9863f951b7 140 } else if ( this->hfsTimerElapsed() ) {
jmarkel44 252:3c9863f951b7 141 // duty cyle
jmarkel44 252:3c9863f951b7 142 printf("\rInitial timer has expired\n");
jmarkel44 252:3c9863f951b7 143 currentState = STATE_CONTROL_HFS_OFF;
jmarkel44 252:3c9863f951b7 144 }
jmarkel44 252:3c9863f951b7 145 break;
jmarkel44 250:1cd8ec63e9e9 146 case STATE_CONTROL_HFS_OFF:
jmarkel44 252:3c9863f951b7 147 // the control is in HFS with the output OFF
jmarkel44 252:3c9863f951b7 148 if ( !this->aboveHighFailsafe() ) {
jmarkel44 252:3c9863f951b7 149 currentState = STATE_CONTROL_OFF;
jmarkel44 252:3c9863f951b7 150 } else {
jmarkel44 252:3c9863f951b7 151
jmarkel44 252:3c9863f951b7 152 }
jmarkel44 252:3c9863f951b7 153 break;
jmarkel44 250:1cd8ec63e9e9 154 default:
jmarkel44 250:1cd8ec63e9e9 155 break;
jmarkel44 250:1cd8ec63e9e9 156 }
jmarkel44 250:1cd8ec63e9e9 157 }
jmarkel44 250:1cd8ec63e9e9 158
jmarkel44 252:3c9863f951b7 159 bool FailsafeControl::belowLowFailsafe(void)
jmarkel44 252:3c9863f951b7 160 {
jmarkel44 252:3c9863f951b7 161 // read the modbus input
jmarkel44 252:3c9863f951b7 162 ModbusValue value;
jmarkel44 252:3c9863f951b7 163 ModbusMasterReadRegister(input, &value);
jmarkel44 252:3c9863f951b7 164 #if 0
jmarkel44 252:3c9863f951b7 165 if ( value.errflag ) {
jmarkel44 252:3c9863f951b7 166 logError("%s: error reading %s:", __func__, input);
jmarkel44 252:3c9863f951b7 167 return false;
jmarkel44 252:3c9863f951b7 168 }
jmarkel44 252:3c9863f951b7 169 #endif
jmarkel44 252:3c9863f951b7 170 return ( value.value <= lfs_data.value );
jmarkel44 252:3c9863f951b7 171 }
jmarkel44 252:3c9863f951b7 172
jmarkel44 252:3c9863f951b7 173 bool FailsafeControl::aboveHighFailsafe(void)
jmarkel44 252:3c9863f951b7 174 {
jmarkel44 252:3c9863f951b7 175 // read the modbus input
jmarkel44 252:3c9863f951b7 176 ModbusValue value;
jmarkel44 252:3c9863f951b7 177 ModbusMasterReadRegister(input, &value);
jmarkel44 252:3c9863f951b7 178 // TODO: check the error flag
jmarkel44 252:3c9863f951b7 179 #if 0
jmarkel44 252:3c9863f951b7 180 if ( value.errflag ) {
jmarkel44 252:3c9863f951b7 181 logError("%s: error reading %s:", __func__, input);
jmarkel44 252:3c9863f951b7 182 return false;
jmarkel44 252:3c9863f951b7 183 }
jmarkel44 252:3c9863f951b7 184 #endif
jmarkel44 252:3c9863f951b7 185 return ( value.value >= hfs_data.value );
jmarkel44 252:3c9863f951b7 186 }
jmarkel44 252:3c9863f951b7 187
jmarkel44 252:3c9863f951b7 188 //
jmarkel44 252:3c9863f951b7 189 // method: startHfsTimer
jmarkel44 252:3c9863f951b7 190 // description:
jmarkel44 252:3c9863f951b7 191 void FailsafeControl::startHfsTimer(void)
jmarkel44 252:3c9863f951b7 192 {
jmarkel44 252:3c9863f951b7 193 printf("%s invoked\n", __func__);
jmarkel44 252:3c9863f951b7 194 unsigned long currentTime = time(NULL);
jmarkel44 252:3c9863f951b7 195 unsigned long period = hfs_data.interval * 60;
jmarkel44 252:3c9863f951b7 196
jmarkel44 252:3c9863f951b7 197 double totalOnTime = (double)period * ((double)hfs_data.dutyCycle/(double)100.0);
jmarkel44 252:3c9863f951b7 198 duty_timer.offTime = (unsigned long) totalOnTime;
jmarkel44 252:3c9863f951b7 199 duty_timer.expirationTime = currentTime + period;
jmarkel44 252:3c9863f951b7 200
jmarkel44 252:3c9863f951b7 201 printf("\rnext off time = %lu\n", duty_timer.offTime);
jmarkel44 252:3c9863f951b7 202 printf("\rexpiration time = %lu\n", duty_timer.expirationTime);
jmarkel44 252:3c9863f951b7 203 }
jmarkel44 252:3c9863f951b7 204
jmarkel44 252:3c9863f951b7 205 void FailsafeControl::stopHfsTimer(void)
jmarkel44 252:3c9863f951b7 206 {
jmarkel44 252:3c9863f951b7 207 memset(&duty_timer, 0, sizeof(duty_timer));
jmarkel44 252:3c9863f951b7 208 }
jmarkel44 252:3c9863f951b7 209
jmarkel44 252:3c9863f951b7 210 bool FailsafeControl::hfsTimerElapsed(void)
jmarkel44 252:3c9863f951b7 211 {
jmarkel44 252:3c9863f951b7 212 return ( duty_timer.offTime >= time(NULL) );
jmarkel44 252:3c9863f951b7 213 }
jmarkel44 252:3c9863f951b7 214
jmarkel44 252:3c9863f951b7 215 //
jmarkel44 252:3c9863f951b7 216 // method: sendMail
jmarkel44 252:3c9863f951b7 217 // description: send mail to the output task
jmarkel44 252:3c9863f951b7 218 //
jmarkel44 252:3c9863f951b7 219 // @param io_tag -> input/output tag
jmarkel44 252:3c9863f951b7 220 // @param action -> ON, OFF, UNREGISTER
jmarkel44 252:3c9863f951b7 221 // @return none
jmarkel44 252:3c9863f951b7 222 //
jmarkel44 252:3c9863f951b7 223 void FailsafeControl::sendMail(OutputAction action)
jmarkel44 252:3c9863f951b7 224 {
jmarkel44 252:3c9863f951b7 225 logInfo("%s: failsafe control attempting to send action %d\n",
jmarkel44 252:3c9863f951b7 226 __func__, action);
jmarkel44 252:3c9863f951b7 227
jmarkel44 252:3c9863f951b7 228 OutputControlMsg_t *output_mail = OutputMasterMailBox.alloc();
jmarkel44 252:3c9863f951b7 229 memset(output_mail, 0, sizeof(OutputControlMsg_t));
jmarkel44 252:3c9863f951b7 230
jmarkel44 252:3c9863f951b7 231 output_mail->action = action;
jmarkel44 252:3c9863f951b7 232 output_mail->controlType = CONTROL_FAILSAFE;
jmarkel44 252:3c9863f951b7 233 output_mail->priority = this->priority;
jmarkel44 252:3c9863f951b7 234
jmarkel44 252:3c9863f951b7 235 strncpy(output_mail->input_tag, this->input.c_str(), sizeof(output_mail->input_tag)-1);
jmarkel44 252:3c9863f951b7 236 strncpy(output_mail->output_tag, this->output.c_str(), sizeof(output_mail->output_tag)-1);
jmarkel44 252:3c9863f951b7 237 strncpy(output_mail->id, this->id.c_str(), sizeof(output_mail->id)-1);
jmarkel44 252:3c9863f951b7 238
jmarkel44 252:3c9863f951b7 239 OutputMasterMailBox.put(output_mail);
jmarkel44 252:3c9863f951b7 240 }
jmarkel44 252:3c9863f951b7 241
jmarkel44 252:3c9863f951b7 242
jmarkel44 250:1cd8ec63e9e9 243 //
jmarkel44 250:1cd8ec63e9e9 244 // method: display
jmarkel44 250:1cd8ec63e9e9 245 // description: display the pertinents
jmarkel44 250:1cd8ec63e9e9 246 //
jmarkel44 250:1cd8ec63e9e9 247 // @param none
jmarkel44 250:1cd8ec63e9e9 248 // @return none
jmarkel44 250:1cd8ec63e9e9 249 //
jmarkel44 250:1cd8ec63e9e9 250 void FailsafeControl::display(void)
jmarkel44 250:1cd8ec63e9e9 251 {
jmarkel44 250:1cd8ec63e9e9 252 const char *mapper[] = { "INIT",
jmarkel44 250:1cd8ec63e9e9 253 "START",
jmarkel44 250:1cd8ec63e9e9 254 "CONTROL_OFF",
jmarkel44 250:1cd8ec63e9e9 255 "LFS_ON",
jmarkel44 250:1cd8ec63e9e9 256 "LFS_OFF",
jmarkel44 250:1cd8ec63e9e9 257 "HFS_ON",
jmarkel44 250:1cd8ec63e9e9 258 "HFS_OFF",
jmarkel44 250:1cd8ec63e9e9 259 "invalid"
jmarkel44 250:1cd8ec63e9e9 260 };
jmarkel44 250:1cd8ec63e9e9 261
jmarkel44 250:1cd8ec63e9e9 262 printf("\r\n");
jmarkel44 250:1cd8ec63e9e9 263 std::cout << left << setw(10) << setfill(' ') << "failsafe: ";
jmarkel44 250:1cd8ec63e9e9 264 std::cout << left << setw(32) << setfill(' ') << controlFile;
jmarkel44 250:1cd8ec63e9e9 265 std::cout << left << setw(20) << setfill(' ') << id;
jmarkel44 250:1cd8ec63e9e9 266 std::cout << left << setw(6) << setfill(' ') << priority;
jmarkel44 250:1cd8ec63e9e9 267 std::cout << left << setw(20) << setfill(' ') << input;
jmarkel44 250:1cd8ec63e9e9 268 std::cout << left << setw(20) << setfill(' ') << output;
jmarkel44 250:1cd8ec63e9e9 269 std::cout << left << setw(16) << setfill(' ') << mapper[currentState];
jmarkel44 252:3c9863f951b7 270 std::cout << left << setw(12) << setfill(' ') << "lfs-> "
jmarkel44 252:3c9863f951b7 271 << lfs_data.value << ":" << lfs_data.dutyCycle << ":" << lfs_data.interval;
jmarkel44 252:3c9863f951b7 272 std::cout << right << setw(12) << setfill(' ') << "hfs-> "
jmarkel44 252:3c9863f951b7 273 << hfs_data.value << ":" << hfs_data.dutyCycle << ":" << hfs_data.interval;
jmarkel44 250:1cd8ec63e9e9 274
jmarkel44 250:1cd8ec63e9e9 275 std::cout.flush();
jmarkel44 252:3c9863f951b7 276 }