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@260:fe726583ba1d, 2016-10-24 (annotated)

Committer:
jmarkel44
Date:
Mon Oct 24 13:07:52 2016 +0000
Revision:
260:fe726583ba1d
Parent:
253:ae850c19cf81
Child:
261:4e9a588c938e
working 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 260:fe726583ba1d 89 switch (this->currentState) {
jmarkel44 250:1cd8ec63e9e9 90 case STATE_INIT:
jmarkel44 260:fe726583ba1d 91 // do nothing - must be invoked
jmarkel44 250:1cd8ec63e9e9 92 break;
jmarkel44 250:1cd8ec63e9e9 93 case STATE_START:
jmarkel44 260:fe726583ba1d 94 // control has been started, do some checking
jmarkel44 252:3c9863f951b7 95 if ( this->aboveHighFailsafe() ) {
jmarkel44 260:fe726583ba1d 96 this->currentState = STATE_CONTROL_HFS_ON;
jmarkel44 252:3c9863f951b7 97 } else if ( this->belowLowFailsafe() ) {
jmarkel44 260:fe726583ba1d 98 this->currentState = STATE_CONTROL_LFS_ON;
jmarkel44 252:3c9863f951b7 99 } else {
jmarkel44 260:fe726583ba1d 100 this->currentState = STATE_CONTROL_OFF;
jmarkel44 252:3c9863f951b7 101 }
jmarkel44 252:3c9863f951b7 102 break;
jmarkel44 250:1cd8ec63e9e9 103 case STATE_CONTROL_OFF:
jmarkel44 260:fe726583ba1d 104 // control is acting normal, within bounds
jmarkel44 252:3c9863f951b7 105 if ( this->aboveHighFailsafe() ) {
jmarkel44 260:fe726583ba1d 106 this->currentState = STATE_CONTROL_HFS_ON;
jmarkel44 252:3c9863f951b7 107 } else if ( this->belowLowFailsafe() ) {
jmarkel44 260:fe726583ba1d 108 this->currentState = STATE_CONTROL_LFS_ON;
jmarkel44 252:3c9863f951b7 109 } else {
jmarkel44 252:3c9863f951b7 110 // do nothing
jmarkel44 252:3c9863f951b7 111 }
jmarkel44 252:3c9863f951b7 112 break;
jmarkel44 250:1cd8ec63e9e9 113 case STATE_CONTROL_LFS_ON:
jmarkel44 260:fe726583ba1d 114 // control is in low-failsafe with duty cycle ON
jmarkel44 252:3c9863f951b7 115 if ( !this->belowLowFailsafe() ) {
jmarkel44 260:fe726583ba1d 116 this->currentState = STATE_CONTROL_OFF;
jmarkel44 260:fe726583ba1d 117 } else if ( this->dutyOnExpired() ) {
jmarkel44 260:fe726583ba1d 118 this->currentState = STATE_CONTROL_LFS_OFF;
jmarkel44 252:3c9863f951b7 119 } else {
jmarkel44 260:fe726583ba1d 120 // do nothing
jmarkel44 252:3c9863f951b7 121 }
jmarkel44 252:3c9863f951b7 122 break;
jmarkel44 250:1cd8ec63e9e9 123 case STATE_CONTROL_LFS_OFF:
jmarkel44 260:fe726583ba1d 124 // control is in low-failsafe with duty cycle OFF
jmarkel44 252:3c9863f951b7 125 if ( !this->belowLowFailsafe() ) {
jmarkel44 260:fe726583ba1d 126 this->currentState = STATE_CONTROL_OFF;
jmarkel44 260:fe726583ba1d 127 } else if ( this->dutyOffExpired() ) {
jmarkel44 260:fe726583ba1d 128 this->currentState = STATE_CONTROL_LFS_ON;
jmarkel44 252:3c9863f951b7 129 } else {
jmarkel44 260:fe726583ba1d 130 // do nothing
jmarkel44 252:3c9863f951b7 131 }
jmarkel44 252:3c9863f951b7 132 break;
jmarkel44 250:1cd8ec63e9e9 133 case STATE_CONTROL_HFS_ON:
jmarkel44 260:fe726583ba1d 134 // control is in high-failsafe with duty cycle ON
jmarkel44 252:3c9863f951b7 135 if ( !this->aboveHighFailsafe() ) {
jmarkel44 260:fe726583ba1d 136 this->currentState = STATE_CONTROL_OFF;
jmarkel44 260:fe726583ba1d 137 } else if ( this->dutyOnExpired() ) {
jmarkel44 260:fe726583ba1d 138 this->currentState = STATE_CONTROL_HFS_OFF;
jmarkel44 260:fe726583ba1d 139 } else {
jmarkel44 260:fe726583ba1d 140 // do nothing
jmarkel44 252:3c9863f951b7 141 }
jmarkel44 252:3c9863f951b7 142 break;
jmarkel44 250:1cd8ec63e9e9 143 case STATE_CONTROL_HFS_OFF:
jmarkel44 260:fe726583ba1d 144 // control is in high-failsafe with cuty cycle OFF
jmarkel44 252:3c9863f951b7 145 if ( !this->aboveHighFailsafe() ) {
jmarkel44 260:fe726583ba1d 146 this->currentState = STATE_CONTROL_OFF;
jmarkel44 260:fe726583ba1d 147 } else if ( this->dutyOffExpired() ) {
jmarkel44 260:fe726583ba1d 148 this->currentState = STATE_CONTROL_LFS_ON;
jmarkel44 260:fe726583ba1d 149 } else {
jmarkel44 260:fe726583ba1d 150 // do nothing
jmarkel44 252:3c9863f951b7 151 }
jmarkel44 252:3c9863f951b7 152 break;
jmarkel44 250:1cd8ec63e9e9 153 default:
jmarkel44 250:1cd8ec63e9e9 154 break;
jmarkel44 250:1cd8ec63e9e9 155 }
jmarkel44 250:1cd8ec63e9e9 156 }
jmarkel44 250:1cd8ec63e9e9 157
jmarkel44 252:3c9863f951b7 158 bool FailsafeControl::belowLowFailsafe(void)
jmarkel44 252:3c9863f951b7 159 {
jmarkel44 252:3c9863f951b7 160 // read the modbus input
jmarkel44 252:3c9863f951b7 161 ModbusValue value;
jmarkel44 252:3c9863f951b7 162 ModbusMasterReadRegister(input, &value);
jmarkel44 252:3c9863f951b7 163 #if 0
jmarkel44 252:3c9863f951b7 164 if ( value.errflag ) {
jmarkel44 252:3c9863f951b7 165 logError("%s: error reading %s:", __func__, input);
jmarkel44 252:3c9863f951b7 166 return false;
jmarkel44 252:3c9863f951b7 167 }
jmarkel44 252:3c9863f951b7 168 #endif
jmarkel44 252:3c9863f951b7 169 return ( value.value <= lfs_data.value );
jmarkel44 252:3c9863f951b7 170 }
jmarkel44 252:3c9863f951b7 171
jmarkel44 252:3c9863f951b7 172 bool FailsafeControl::aboveHighFailsafe(void)
jmarkel44 252:3c9863f951b7 173 {
jmarkel44 252:3c9863f951b7 174 // read the modbus input
jmarkel44 252:3c9863f951b7 175 ModbusValue value;
jmarkel44 252:3c9863f951b7 176 ModbusMasterReadRegister(input, &value);
jmarkel44 252:3c9863f951b7 177 // TODO: check the error flag
jmarkel44 252:3c9863f951b7 178 #if 0
jmarkel44 252:3c9863f951b7 179 if ( value.errflag ) {
jmarkel44 252:3c9863f951b7 180 logError("%s: error reading %s:", __func__, input);
jmarkel44 252:3c9863f951b7 181 return false;
jmarkel44 252:3c9863f951b7 182 }
jmarkel44 252:3c9863f951b7 183 #endif
jmarkel44 252:3c9863f951b7 184 return ( value.value >= hfs_data.value );
jmarkel44 252:3c9863f951b7 185 }
jmarkel44 252:3c9863f951b7 186
jmarkel44 260:fe726583ba1d 187 bool FailsafeControl::dutyOnExpired()
jmarkel44 252:3c9863f951b7 188 {
jmarkel44 260:fe726583ba1d 189 return false;
jmarkel44 252:3c9863f951b7 190 }
jmarkel44 252:3c9863f951b7 191
jmarkel44 260:fe726583ba1d 192 bool FailsafeControl::dutyOffExpired()
jmarkel44 252:3c9863f951b7 193 {
jmarkel44 260:fe726583ba1d 194 return false;
jmarkel44 252:3c9863f951b7 195 }
jmarkel44 252:3c9863f951b7 196
jmarkel44 252:3c9863f951b7 197 //
jmarkel44 260:fe726583ba1d 198 // method: sendMailToOutput
jmarkel44 252:3c9863f951b7 199 // description: send mail to the output task
jmarkel44 252:3c9863f951b7 200 //
jmarkel44 252:3c9863f951b7 201 // @param io_tag -> input/output tag
jmarkel44 252:3c9863f951b7 202 // @param action -> ON, OFF, UNREGISTER
jmarkel44 252:3c9863f951b7 203 // @return none
jmarkel44 252:3c9863f951b7 204 //
jmarkel44 260:fe726583ba1d 205 void FailsafeControl::sendMailToOutput(OutputAction action)
jmarkel44 252:3c9863f951b7 206 {
jmarkel44 252:3c9863f951b7 207 logInfo("%s: failsafe control attempting to send action %d\n",
jmarkel44 252:3c9863f951b7 208 __func__, action);
jmarkel44 252:3c9863f951b7 209
jmarkel44 252:3c9863f951b7 210 OutputControlMsg_t *output_mail = OutputMasterMailBox.alloc();
jmarkel44 252:3c9863f951b7 211 memset(output_mail, 0, sizeof(OutputControlMsg_t));
jmarkel44 252:3c9863f951b7 212
jmarkel44 252:3c9863f951b7 213 output_mail->action = action;
jmarkel44 252:3c9863f951b7 214 output_mail->controlType = CONTROL_FAILSAFE;
jmarkel44 252:3c9863f951b7 215 output_mail->priority = this->priority;
jmarkel44 252:3c9863f951b7 216
jmarkel44 252:3c9863f951b7 217 strncpy(output_mail->input_tag, this->input.c_str(), sizeof(output_mail->input_tag)-1);
jmarkel44 252:3c9863f951b7 218 strncpy(output_mail->output_tag, this->output.c_str(), sizeof(output_mail->output_tag)-1);
jmarkel44 252:3c9863f951b7 219 strncpy(output_mail->id, this->id.c_str(), sizeof(output_mail->id)-1);
jmarkel44 252:3c9863f951b7 220
jmarkel44 252:3c9863f951b7 221 OutputMasterMailBox.put(output_mail);
jmarkel44 252:3c9863f951b7 222 }
jmarkel44 252:3c9863f951b7 223
jmarkel44 252:3c9863f951b7 224
jmarkel44 250:1cd8ec63e9e9 225 //
jmarkel44 260:fe726583ba1d 226 // method: unregisterControl
jmarkel44 260:fe726583ba1d 227 // description: unregister this control with the output task
jmarkel44 260:fe726583ba1d 228 //
jmarkel44 260:fe726583ba1d 229 // @param none
jmarkel44 260:fe726583ba1d 230 // @return none
jmarkel44 260:fe726583ba1d 231 //
jmarkel44 260:fe726583ba1d 232 void FailsafeControl::unregisterControl(void)
jmarkel44 260:fe726583ba1d 233 {
jmarkel44 260:fe726583ba1d 234 logInfo("%s: %s attempting to unregister %s\n",
jmarkel44 260:fe726583ba1d 235 __func__, controlFile.c_str());
jmarkel44 260:fe726583ba1d 236
jmarkel44 260:fe726583ba1d 237 OutputControlMsg_t *output_mail = OutputMasterMailBox.alloc();
jmarkel44 260:fe726583ba1d 238 memset(output_mail, 0, sizeof(OutputControlMsg_t));
jmarkel44 260:fe726583ba1d 239
jmarkel44 260:fe726583ba1d 240 output_mail->action = ACTION_CONTROL_UNREGISTER;
jmarkel44 260:fe726583ba1d 241 output_mail->controlType = CONTROL_FAILSAFE;
jmarkel44 260:fe726583ba1d 242 output_mail->priority = this->priority;
jmarkel44 260:fe726583ba1d 243 strncpy(output_mail->output_tag, this->output.c_str(), sizeof(output_mail->output_tag)-1);
jmarkel44 260:fe726583ba1d 244 strncpy(output_mail->id, this->id.c_str(), sizeof(output_mail->id)-1);
jmarkel44 260:fe726583ba1d 245
jmarkel44 260:fe726583ba1d 246 OutputMasterMailBox.put(output_mail);
jmarkel44 260:fe726583ba1d 247 }
jmarkel44 260:fe726583ba1d 248
jmarkel44 260:fe726583ba1d 249 //
jmarkel44 250:1cd8ec63e9e9 250 // method: display
jmarkel44 250:1cd8ec63e9e9 251 // description: display the pertinents
jmarkel44 250:1cd8ec63e9e9 252 //
jmarkel44 250:1cd8ec63e9e9 253 // @param none
jmarkel44 250:1cd8ec63e9e9 254 // @return none
jmarkel44 250:1cd8ec63e9e9 255 //
jmarkel44 250:1cd8ec63e9e9 256 void FailsafeControl::display(void)
jmarkel44 250:1cd8ec63e9e9 257 {
jmarkel44 250:1cd8ec63e9e9 258 const char *mapper[] = { "INIT",
jmarkel44 250:1cd8ec63e9e9 259 "START",
jmarkel44 250:1cd8ec63e9e9 260 "CONTROL_OFF",
jmarkel44 250:1cd8ec63e9e9 261 "LFS_ON",
jmarkel44 250:1cd8ec63e9e9 262 "LFS_OFF",
jmarkel44 250:1cd8ec63e9e9 263 "HFS_ON",
jmarkel44 250:1cd8ec63e9e9 264 "HFS_OFF",
jmarkel44 250:1cd8ec63e9e9 265 "invalid"
jmarkel44 250:1cd8ec63e9e9 266 };
jmarkel44 250:1cd8ec63e9e9 267
jmarkel44 250:1cd8ec63e9e9 268 printf("\r\n");
jmarkel44 250:1cd8ec63e9e9 269 std::cout << left << setw(10) << setfill(' ') << "failsafe: ";
jmarkel44 253:ae850c19cf81 270 std::cout << left << setw(40) << setfill(' ') << controlFile;
jmarkel44 250:1cd8ec63e9e9 271 std::cout << left << setw(20) << setfill(' ') << id;
jmarkel44 250:1cd8ec63e9e9 272 std::cout << left << setw(6) << setfill(' ') << priority;
jmarkel44 250:1cd8ec63e9e9 273 std::cout << left << setw(20) << setfill(' ') << input;
jmarkel44 250:1cd8ec63e9e9 274 std::cout << left << setw(20) << setfill(' ') << output;
jmarkel44 250:1cd8ec63e9e9 275 std::cout << left << setw(16) << setfill(' ') << mapper[currentState];
jmarkel44 252:3c9863f951b7 276 std::cout << left << setw(12) << setfill(' ') << "lfs-> "
jmarkel44 252:3c9863f951b7 277 << lfs_data.value << ":" << lfs_data.dutyCycle << ":" << lfs_data.interval;
jmarkel44 252:3c9863f951b7 278 std::cout << right << setw(12) << setfill(' ') << "hfs-> "
jmarkel44 252:3c9863f951b7 279 << hfs_data.value << ":" << hfs_data.dutyCycle << ":" << hfs_data.interval;
jmarkel44 250:1cd8ec63e9e9 280
jmarkel44 250:1cd8ec63e9e9 281 std::cout.flush();
jmarkel44 252:3c9863f951b7 282 }