How to allow my C++ code to update variables? - c++

I'm new to coding and C++.
The code below is meant to monitor a magswitch and a status led on another controller. The code needs to run once the magswitch pin goes high (this works).
The additional code for pulseIn, is what I hope to use to monitor different flash rates of the led when I get the code working. For now I'm just looking for the state variable to update with the if and else if statements.
When I toggle the statusPin, the code picks up the changing state, but I cannot get it to update the "state" and "statuspinstate" variables.
The statuspinstate variable shows as 1, even though it is initialized as 0.
I inserted all the serial prints to try and see where things are going wrong.
This is the serial print when "statusPin" is LOW:
statuspinstate: 0
rate1: 2147483647
period: 0.00
rate2: 0
ontime: 0
offtime: 0
state: 0
statepinstatus: 1
This is the serial print when "statusPin" is HIGH
statuspinstate: 1
rate1: 2147483647
period: 0.00
rate2: 0
ontime: 0
offtime: 0
state: 0
statepinstatus: 1
Code:
const int statusPin = 19; //Reads status led
const int magSwitch = 22; //Magswitch to detect movement
int ontime,offtime,rate1,rate2;
float freq,period;
volatile unsigned int state =0;
volatile unsigned int statuspinstate = 0;
void setup()
{
pinMode(statusPin, INPUT); //input from controller
pinMode(magSwitch, INPUT);
Serial.begin(115200);
}
void loop()
{
while (digitalRead(magSwitch) == LOW) {
}
{
statuspinstate = digitalRead(statusPin);
ontime = pulseIn(statusPin,HIGH);
offtime = pulseIn(statusPin,LOW);
period = ontime+offtime;
rate1 = (ontime/period); //future use
rate2 = (offtime); //future use
Serial.println(String("statuspinstate ") + (digitalRead(statusPin))); //all serial print is debug info
Serial.println(String("rate1: ") + (rate1));
Serial.println(String("period: ") + (period));
Serial.println(String("rate2: ") + (rate2));
Serial.println(String("ontime: ") + (ontime));
Serial.println(String("offtime: ") + (offtime));
delay(500);
}
if ((ontime) != 0)
state = period;
else if (statuspinstate = 1)
state = 9999;
else if (statuspinstate = 0);
state = 0;
Serial.println(String("state: ") + (state));
Serial.println(String("statepinstatus: ") + (statuspinstate));
statuspinstate = 0; //return statuspinstate to zero
}

Look at your conditional, with proper indentation (do get a text editor that can indent your code for you):
if ((ontime) != 0)
state = period;
else if (statuspinstate = 1)
state = 9999;
else if (statuspinstate = 0);
state = 0;
We know that ontime is zero, so the second condition is tried next.
Now, statuspinstate = 1 is an assignment, not a comparison, and its value is "truth-y" so you take that branch.
Next, the stray semicolon in if (statuspinstate = 0); (which is also an assignment condition, but not evaluated) makes state = 0 unconditional.
So every time ontime is zero, you end up executing statuspinstate = 1 and state = 0.
What you probably want is
if (ontime != 0)
state = period;
else if (statuspinstate == 1)
state = 9999;
else if (statuspinstate == 0)
state = 0;

Related

I want to connect two yf-s201 Waterflow sensors, one as inflow and other as a outflow using Nodemcu & blynk

This is mostly a copy-paste of the code I found on Google,
I want to make a project using 2 waterflow sensors in which inflow() which shows how many liters i have taken in and outflow() which shows how many liters flown out.
This is how far ive reached, Need help with the code please, I am not a advanced coder so descriptive code and support is HIGHLY appreciated.
also please see the maincode(), in that section i am trying to achieve a loop, i mean if sensor1 is high it should display sensor1(inflow()) output ,and if sensor 2 is high it should display sensor2(outflow()) output.
Problems faced: the output doesn't work when i call both the inflow() and outflow() together, one function works,(i think it has something to do with the Interrupt Pins of the board?).
#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>
char auth[] = "SECRET";
// Your WiFi credentials.
// Set password to "" for open networks.
char ssid[] = "Wifi";
char pass[] = "password";
//byte statusLed = 13;
byte inFlowSensor = D2;
byte outFlowSensor= D3;
float calibrationFactor = 4.5;
BlynkTimer timer;
volatile byte pulseCount;
float inFlowRate; // V2 - inflowrate
float outFlowRate; // V4 - outFowRate
boolean sensorInput = 0;
unsigned int inFlowMilliLitres;
unsigned int outFlowMilliLitres;
unsigned long inTotalMilliLitres; // V1 - inTotalLitres
//unsigned long totalLitres;
unsigned long outTotalMilliLitres; // V3 - outTotalLitres
unsigned long oldTime;
BLYNK_CONNECTED() { // runs once at device startup, once connected to server.
Blynk.syncVirtual(V1); //gets last known value of V1 virtual pin
Blynk.syncVirtual(V3); //gets last known value of V4
}
BLYNK_WRITE(V1)
{
inTotalMilliLitres = param.asFloat();
}
BLYNK_WRITE(V2)
{
inFlowRate = param.asFloat();
}
BLYNK_WRITE(V3)
{
outTotalMilliLitres = param.asFloat();
}
BLYNK_WRITE(V4)
{
outFlowRate = param.asFloat();
}
BLYNK_WRITE(V5) { // reset all data with button in PUSH mode on virtual pin V4
int resetdata = param.asInt();
if (resetdata == 0) {
Serial.println("Clearing Data");
Blynk.virtualWrite(V1, 0);
Blynk.virtualWrite(V2, 0);
inFlowRate = 0;
outFlowRate = 0;
inFlowMilliLitres = 0;
outFlowMilliLitres = 0;
inTotalMilliLitres = 0;
outTotalMilliLitres = 0;
//totalLitres = 0;
//totalLitresold = 0;
}
}
ICACHE_RAM_ATTR void pulseCounter()
{
// Increment the pulse counter
pulseCount++;
}
void inflow()
{
if((millis() - oldTime) > 1000) // Only process counters once per second
{
detachInterrupt(inFlowSensor);
inFlowRate = ((1000.0 / (millis() - oldTime)) * pulseCount) / calibrationFactor;
oldTime = millis();
inFlowMilliLitres = (inFlowRate / 60) * 1000;
// Add the millilitres passed in this second to the cumulative total
inTotalMilliLitres += inFlowMilliLitres;
unsigned int frac;
// Print the flow rate for this second in litres / minute
Serial.print("Flow rate: ");
Serial.print(int(inFlowRate)); // Print the integer part of the variable
Serial.print("."); // Print the decimal point
// Determine the fractional part. The 10 multiplier gives us 1 decimal place.
frac = (inFlowRate - int(inFlowRate)) * 10;
Serial.print(frac, DEC) ; // Print the fractional part of the variable
Serial.print("L/min");
// Print the number of litres flowed in this second
Serial.print(" Current Fuel Flowing: "); // Output separator
Serial.print(inFlowMilliLitres);
Serial.print("mL/Sec");
// Print the cumulative total of litres flowed since starting
Serial.print(" Input Fuel Quantity: "); // Input separator
Serial.print(inTotalMilliLitres);
Serial.println("mL");
// Reset the pulse counter so we can start incrementing again
pulseCount = 0;
// Enable the interrupt again now that we've finished sending output
attachInterrupt(inFlowSensor, pulseCounter, FALLING);
}
}
void outflow()
{
if((millis() - oldTime) > 1000) // Only process counters once per second
{
detachInterrupt(outFlowSensor);
outFlowRate = ((1000.0 / (millis() - oldTime)) * pulseCount) / calibrationFactor;
oldTime = millis();
outFlowMilliLitres = (outFlowRate / 60) * 1000;
// Add the millilitres passed in this second to the cumulative total
outTotalMilliLitres += outFlowMilliLitres;
unsigned int frac;
// Print the flow rate for this second in litres / minute
Serial.print("Flow rate: ");
Serial.print(int(outFlowRate)); // Print the integer part of the variable
Serial.print("."); // Print the decimal point
// Determine the fractional part. The 10 multiplier gives us 1 decimal place.
frac = (outFlowRate - int(outFlowRate)) * 10;
Serial.print(frac, DEC) ; // Print the fractional part of the variable
Serial.print("L/min");
// Print the number of litres flowed in this second
Serial.print(" Current Fuel Flowing: "); // Output separator
Serial.print(outFlowMilliLitres);
Serial.print("mL/Sec");
// Print the cumulative total of litres flowed since starting
Serial.print(" Out Fuel Quantity: "); // Input separator
Serial.print(outTotalMilliLitres);
Serial.println("mL");
// Reset the pulse counter so we can start incrementing again
pulseCount = 0;
// Enable the interrupt again now that we've finished sending output
attachInterrupt(outFlowSensor, pulseCounter, FALLING);
}
}
void sendtoBlynk() // In this function we are sending values to blynk server
{
Blynk.virtualWrite(V2, inFlowRate);
Blynk.virtualWrite(V1, inTotalMilliLitres);
Blynk.virtualWrite(V4, outFlowRate);
Blynk.virtualWrite(V3, outTotalMilliLitres);
}
void setup()
{
Serial.begin(9600); //38400
Blynk.begin(auth,ssid,pass);
Serial.println("Setup Started");
pulseCount = 0;
inFlowRate = 0.0;
outFlowRate = 0.0;
inFlowMilliLitres = 0;
outFlowMilliLitres = 0;
inTotalMilliLitres = 0;
outTotalMilliLitres = 0;
oldTime = 0;
attachInterrupt(inFlowSensor, pulseCounter, FALLING);
//attachInterrupt(outFlowSensor, pulseCounter, FALLING);
timer.setInterval(10000L, sendtoBlynk);
}
void maincode(){
inflow();
//outflow();
}
/**
* program loop
*/
void loop(){
Blynk.run();
timer.run();
Serial.println("Timer and Blynk Started");
Serial.println(inFlowSensor);
Serial.println(outFlowSensor);
maincode();
}```

Array in Case Statement

I am having trouble using an array for case statement for my state machine. Most sites mention that an array cannot be used for case statements so i have been trying a work around for it but so far it has been unsuccessful. I would really appreciate any help or suggestions for this. For Clarification: I do not want to hard code the states, i am trying to make the program in such a way that if the user only changes the order in the fsm_state_array[] the program would execute in that order only without changing anything else in the void loop().
Here is what i have tried so far, i have used functions to hard code to check previous state, current state and next state when the user enters their sequence of states in an array , so in my code below the states should go from 0 --> 2 --> 3--> 1 , however , i get 0 --> 2 --> 1 --> 3. I know this problem can be easily solved if i just use the array in the case statement but the compiler gives me an error. I would really appreciate any help or suggestions for this.
My code is shown below :
//Objectives: Use input from laser to control pre-amp on adc. Multiplex the inputs on Pre-Amp
//Type: Pulse, Freq:20Hz (50ms), Amp:5.0 Vpp, Offset:500mV, Width = 100ns
//-----------------------PROJECT LIBRARIES----------------------------------
#include <Bounce2.h>
#include <Arduino.h>
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/interrupt.h>
//-----------------------DEFINES------------------------------------------
//Declare Laser Input Pin
#define LASER_PIN 2
//Declare Reset Pin
#define RESET_PIN 3
typedef enum {
STATE_0,
STATE_1,
STATE_2,
STATE_3
} fsm_state;
//User can change or remove states here
fsm_state fsm_state_Array[] = {STATE_0, STATE_2, STATE_3, STATE_1};
//*eNextstate controls on which state the program starts the state machine, default is STATE_00, Must be same value as Transition_State[0]
fsm_state eNextState = fsm_state_Array[0];
int Current_State = 0;
int Next_State = 0;
int Previous_State = 0;
// -------------------------CONSTANTS (won't change)-------------------------------------
const unsigned long period = 1000; //the value is a number of milliseconds
//-------------------------VARIABLES (will change)-------------------------------------
bool only_for_print = false;//used only for print state ments
int reset_switch = 1;//Start HIGH to avoid reset
int PulseCount = 0; //Pulse count from X-RAY
int Output = 0;//Switch state on the Pre-Amp
int wait = 0;//wait for pulses count
int N = 20;//no. of pulses to count before switching states
volatile int IRQcount = 0;
volatile boolean reset_flag = false;
unsigned long start_time = 0;
unsigned long current_time = 0;
//----------------------------USER DEFINED FUNCTIONS---------------------------------
void fsm();
void loop();
void setup();
void WDT_RESET();
void IRQcounter();
void CountPulses();
//-----------------------------DEBOUNCE FUNCTIONS---------------------------------------
//--------------------------------MAIN SETUP--------------------------------------
void setup()
{
Serial.begin(115200);
//Pin Setup
pinMode(LASER_PIN, INPUT_PULLUP);
pinMode(RESET_PIN, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(LASER_PIN), IRQcounter, RISING);//attach interrupt handler to laser input
attachInterrupt (digitalPinToInterrupt (RESET_PIN), RESET_ISR, FALLING); // attach interrupt handler to reset, wait for user press button or switch
start_time = millis(); //initial start time
sei();//Turn on Interrupts
WaitForPulses();//Waits to detect 20 pulses
}
//--------------------------------MAIN LOOP----------------------------------
void loop()
{
current_time = millis();
fsm();//State machine
}
//--------------------------------PULSE COUNT FUNCTION--------------------------------------------
void CountPulses()
{
// current_time = millis();
if ((current_time - start_time) >= period)
{
start_time = current_time;
cli();//disable interrupts
PulseCount = IRQcount;
IRQcount = 0;
Serial.print(F("Pulse Count is = "));
Serial.println(PulseCount);
sei();//enable interrupts
}
}
//--------------------------------STATE MACHINE FUNCTION--------------------------------------------
void fsm()
{
switch (eNextState)
{
case STATE_0:
/////////Print Statement only for debugging//////////
while (only_for_print == false)
{
Serial.println("The state is 0");
only_for_print = true;
}
///////// Count Pulses Setup /////////////////
Previous_State = fsm_state_Array[3];
Current_State= 0;
Next_State = fsm_state_Array[1];
current_time = millis();
CountPulses();
Output = 0;
if (PulseCount == N)
{
PulseCount = 0;//Reset Pulse Count
only_for_print = false; //used only for print statments
State_Check_0_to_1();//Move to next state
}
break;
case STATE_1:
/////////Print Statement only for debugging//////////
while (only_for_print == false)
{
Serial.println("The state is 1");
only_for_print = true;
}
///////// Count Pulses Setup /////////////////
Previous_State = fsm_state_Array[0];
Current_State= 1;
Next_State = fsm_state_Array[2];
current_time = millis();
CountPulses();
Output = 1;
if (PulseCount == N)
{
PulseCount = 0;//Reset Pulse Count
only_for_print = false; //used only for print statments
State_Check_1_to_2();//Move to next state
}
break;
case STATE_2:
/////////Print Statement only for debugging//////////
while (only_for_print == false)
{
Serial.println("The state is 2");
only_for_print = true;
}
///////// Count Pulses Setup /////////////////
Previous_State = fsm_state_Array[1];
Current_State= 2;
Next_State = fsm_state_Array[3];
current_time = millis();
CountPulses();
Output = 2;
if (PulseCount == N)
{
PulseCount = 0;//Reset Pulse Count
only_for_print = false; //used only for print statments
State_Check_2_to_3();//Move to next state
}
break;
case STATE_3:
/////////Print Statement only for debugging//////////
while (only_for_print == false)
{
Serial.println("The state is 3");
only_for_print = true;
}
///////// Count Pulses Setup /////////////////
Previous_State = fsm_state_Array[2];
Current_State= 3;
Next_State = fsm_state_Array[0];
current_time = millis();
CountPulses();
Output = 3;
if (PulseCount == N)
{
PulseCount = 0;//Reset Pulse Count
only_for_print = false; //used only for print statments
State_Check_3_to_0();//Move to next state
}
break;
}
}
//----------------------------------RESET SWITCH ISR-------------------------------------
void RESET_ISR()
{
reset_flag = true;
if (reset_flag == true)
{
// Serial.println("System will now Reset");// Only for debugging
reset_flag = false;//Reset reset switch flag
wdt_enable(WDTO_500MS);//Reset after 0.5 seconds
while (1)
{
// wdt_reset(); // uncomment to avoid reboot
}
}
}
//-----------------------PULSE COUNT ISR---------------------------------------
void IRQcounter()
{
IRQcount++;
}
//-----------------------WAIT FOR PULSES---------------------------------------
void WaitForPulses()
{
while (wait < 20)
{
if (bit_is_set(EIFR, INTF0))
{
Serial.println("Pulse is detected ");
wait++;
}
}
wait = 0;//reset
}
void State_Check_0_to_1()//Check values of state 0 before going to state 1
{
if(Previous_State == fsm_state_Array[3] && Current_State == 0 && Next_State == fsm_state_Array[1])
{
eNextState = Next_State;
}
}
void State_Check_1_to_2()//Check values of state 1 before going to state 2
{
if((Previous_State == fsm_state_Array[0]) && (Current_State == 1) && (Next_State == fsm_state_Array[2]))
{
eNextState = Next_State;
}
}
void State_Check_2_to_3()//Check values of state 2 before going to state 3
{
if((Previous_State == fsm_state_Array[1]) && (Current_State == 2) && (Next_State == fsm_state_Array[3]))
{
eNextState = Next_State;
}
}
void State_Check_3_to_0()//Check values of state 3 before going to state 0
{
if((Previous_State == fsm_state_Array[2]) && (Current_State == 3) && (Next_State == fsm_state_Array[0]))
{
eNextState = Next_State;
}
}
Here is what my serial monitor shows:
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
Pulse is detected
The state is 0 -----> State 0
Pulse Count is = 72
Pulse Count is = 19
Pulse Count is = 20
The state is 2 -----> State 2
Pulse Count is = 20
The state is 1 -----> State 1
Pulse Count is = 21
Pulse Count is = 19
Pulse Count is = 21
Pulse Count is = 19
Pulse Count is = 21
Pulse Count is = 19
Pulse Count is = 21
Pulse Count is = 19
Pulse Count is = 21
Pulse Count is = 20
The state is 3 -----> State 3
Pulse Count is = 20
The state is 0
Pulse Count is = 20
The state is 2
Pulse Count is = 20
The state is 1
Pulse Count is = 20
The state is 3
Seperate code to test out FSM with pointers as suggested in the comments:
typedef void (*current_state)();
void state0();
void state1();
void state2();
void state3();
current_state states[4]={&state0,&state2,&state3,&state1};
current_state next_state;
void setup()
{
Serial.begin(115200);
}
void loop()
{
current_state();
}
void state0()
{
next_state = states[1]; // No parenthesis!
Serial.println("I am in STATE 0");
}
void state1()
{
next_state = states[2]; // No parenthesis!
Serial.println("I am in STATE 1");
}
void state2()
{
next_state = states[3]; // No parenthesis!
Serial.println("I am in STATE 2");
}
void state3()
{
next_state = states[0]; // No parenthesis!
Serial.println("I am in STATE 3");
}
This code from handling of STATE_2
Next_State = fsm_state_Array[3];
sets Next_State to STATE_1 because the array is initalized as
fsm_state fsm_state_Array[] = {STATE_0, STATE_2, STATE_3, STATE_1};
That means...
fsm_state_Array[0] = STATE_0;
fsm_state_Array[1] = STATE_2;
fsm_state_Array[2] = STATE_3;
fsm_state_Array[3] = STATE_1; // This is the element used
To provide a more "dynamic" solution probably using function pointer for states works better that using a switch:
// Current state is just a apointer to a void function
// accepting no parameters
void (*current_state)();
// All states are just void functions accepting no parameters
void state1();
void state2();
...
// To set what is the next state you update current_state
void state1() {
...
current_state = state2; // No parenthesis!
}
// In the main handler you just call current_state
...
current_state(); // Do the state processing
If you want to perform a sequence of operations then using function pointers you can just keep an array of them and iterate over it:
void (*states[])() = {
state1,
state2,
state3,
...
NULL /// To mark the end of the sequence
};
then you can execute the steps in sequence with
void main() {
for(int i=0; states[i]; i++) {
states[i](); // Execute the step
}
}

GIF LZW decompression

I am trying to implement a simple Gif-Reader in c++.
I currently stuck with decompressing the Imagedata.
If an image includes a Clear Code my decompression algorithm fails.
After the Clear Code I rebuild the CodeTable reset the CodeSize to MinimumLzwCodeSize + 1.
Then I read the next code and add it to the indexstream. The problem is that after clearing, the next codes include values greater than the size of the current codetable.
For example the sample file from wikipedia: rotating-earth.gif has a code value of 262 but the GlobalColorTable is only 256. How do I handle this?
I implemented the lzw decompression according to gif spec..
here is the main code part of decompressing:
int prevCode = GetCode(ptr, offset, codeSize);
codeStream.push_back(prevCode);
while (true)
{
auto code = GetCode(ptr, offset, codeSize);
//
//Clear code
//
if (code == IndexClearCode)
{
//reset codesize
codeSize = blockA.LZWMinimumCodeSize + 1;
currentNodeValue = pow(2, codeSize) - 1;
//reset codeTable
codeTable.resize(colorTable.size() + 2);
//read next code
prevCode = GetCode(ptr, offset, codeSize);
codeStream.push_back(prevCode);
continue;
}
else if (code == IndexEndOfInformationCode)
break;
//exists in dictionary
if (codeTable.size() > code)
{
if (prevCode >= codeTable.size())
{
prevCode = code;
continue;
}
for (auto c : codeTable[code])
codeStream.push_back(c);
newEntry = codeTable[prevCode];
newEntry.push_back(codeTable[code][0]);
codeTable.push_back(newEntry);
prevCode = code;
if (codeTable.size() - 1 == currentNodeValue)
{
codeSize++;
currentNodeValue = pow(2, codeSize) - 1;
}
}
else
{
if (prevCode >= codeTable.size())
{
prevCode = code;
continue;
}
newEntry = codeTable[prevCode];
newEntry.push_back(codeTable[prevCode][0]);
for (auto c : newEntry)
codeStream.push_back(c);
codeTable.push_back(newEntry);
prevCode = codeTable.size() - 1;
if (codeTable.size() - 1 == currentNodeValue)
{
codeSize++;
currentNodeValue = pow(2, codeSize) - 1;
}
}
}
Found the solution.
It is called Deferred clear code. So when I check if the codeSize needs to be incremented I also need to check if the codeSize is already max(12), as it is possible to to get codes that are of the maximum Code Size. See spec-gif89a.txt.
if (codeTable.size() - 1 == currentNodeValue && codeSize < 12)
{
codeSize++;
currentNodeValue = (1 << codeSize) - 1;
}

mBed C++ Using an RtosTimer inside of an RtosThread

I am writing some software to control a quadcopter and have got completely stuck with RtosTimers. I am getting the error "Error: No instance of constructor "rtos::RtosTimer::RtosTimer" matches the argument list in "flightController.h", Line: 13, Col: 29"
I have looked at the example code in the handbook and my code seems to match. I have also googled but I couldn't find anything on using RtosTimers inside of RtosThreads.
Maybe I am going about this the wrong way so if anyone has any suggestions it would be much appreciated.
Here is the code that is causing me problems
//Rtos Timers
RtosTimer UpdateFlightTimer(Task500Hz, osTimerPeriodic, (void *)0);
RtosTimer UpdateCommandTimer(Task50Hz, osTimerPeriodic, (void *)0);
// A thread to monitor the serial ports
void FlightControllerThread(void const *args)
{
UpdateFlightTimer.start(2);
UpdateCommandTimer.start(20);
// Wait here forever
Thread::wait(osWaitForever);
}
void Task500Hz(void const *n)
{
//Get IMU data and convert to yaw, pitch, roll
_freeIMU.getQ(_rawQuaternion);
_freeIMU.getRate(_gyroRate);
GetAttitude();
//Rate mode
if(_rate == true && _stab == false)
{
//Update rate PID process value with gyro rate
_yawRatePIDController->setProcessValue(_gyroRate[0]);
_pitchRatePIDController->setProcessValue(_gyroRate[2]);
_rollRatePIDController->setProcessValue(_gyroRate[1]);
//Update rate PID set point with desired rate from RC
_yawRatePIDController->setSetPoint(_rcConstrainedCommands[0]);
_pitchRatePIDController->setSetPoint(_rcConstrainedCommands[1]);
_rollRatePIDController->setSetPoint(_rcConstrainedCommands[2]);
//Compute rate PID outputs
_ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
_ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
_ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
}
//Stability mode
else
{
//Update stab PID process value with ypr
_yawStabPIDController->setProcessValue(_yrp[0]);
_pitchStabPIDController->setProcessValue(_yrp[2]);
_rollStabPIDController->setProcessValue(_yrp[1]);
//Update stab PID set point with desired angle from RC
_yawStabPIDController->setSetPoint(_yawTarget);
_pitchStabPIDController->setSetPoint(_rcConstrainedCommands[1]);
_rollStabPIDController->setSetPoint(_rcConstrainedCommands[2]);
//Compute stab PID outputs
_stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
_stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
_stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
//if pilot commanding yaw
if(abs(_rcConstrainedCommands[0]) > 5)
{
_stabPIDControllerOutputs[0] = _rcConstrainedCommands[0]; //Feed to rate PID (overwriting stab PID output)
_yawTarget = _yrp[0];
}
//Update rate PID process value with gyro rate
_yawRatePIDController->setProcessValue(_gyroRate[0]);
_pitchRatePIDController->setProcessValue(_gyroRate[2]);
_rollRatePIDController->setProcessValue(_gyroRate[1]);
//Update rate PID set point with desired rate from stab PID
_yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
_pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
_rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
//Compute rate PID outputs
_ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
_ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
_ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
}
//Calculate motor power if flying
if(_rcCommands[3] > 0 && _armed == true)
{
_motorPower[0] = Constrain(_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0], MOTORS_MIN, MOTORS_MAX);
_motorPower[1] = Constrain(_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0], MOTORS_MIN, MOTORS_MAX);
_motorPower[2] = Constrain(_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0], MOTORS_MIN, MOTORS_MAX);
_motorPower[3] = Constrain(_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0], MOTORS_MIN, MOTORS_MAX);
}
//Not flying
else
{
//Disable motors
_motorPower[0] = MOTORS_OFF;
_motorPower[1] = MOTORS_OFF;
_motorPower[2] = MOTORS_OFF;
_motorPower[3] = MOTORS_OFF;
_notFlying ++;
if(_notFlying > 200) //Not flying for 1 second
{
//Reset iteratior
_notFlying = 0;
//Zero gyro
_freeIMU.zeroGyro();
//Reset I
_yawRatePIDController->reset();
_pitchRatePIDController->reset();
_rollRatePIDController->reset();
_yawStabPIDController->reset();
_pitchStabPIDController->reset();
_rollStabPIDController->reset();
}
}
//Set motor power
_motor1.write(_motorPower[0]);
_motor2.write(_motorPower[1]);
_motor3.write(_motorPower[2]);
_motor4.write(_motorPower[3]);
}
void Task50Hz(void const *n)
{
//Get RC control values
//Constrain
//Rate mode
if(_rate == true && _stab == false)
{
_rcConstrainedCommands[0] = Constrain(_rcCommands[0], RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
_rcConstrainedCommands[1] = Constrain(_rcCommands[1], RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
_rcConstrainedCommands[2] = Constrain(_rcCommands[2], RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
_rcConstrainedCommands[3] = Constrain(_rcCommands[3], RC_THRUST_MIN, RC_THRUST_MAX);
}
else
{
_rcConstrainedCommands[0] = Constrain(_rcCommands[0], RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
_rcConstrainedCommands[1] = Constrain(_rcCommands[1], RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
_rcConstrainedCommands[2] = Constrain(_rcCommands[2], RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
_rcConstrainedCommands[3] = Constrain(_rcCommands[3], RC_THRUST_MIN, RC_THRUST_MAX);
}
}
My program can be found at http://mbed.org/users/joe4465/code/QuadMK5/
And the problem is in flightController.h I think it should be clear what I am trying to do but if anyone isn't sure let me know.
I also have another totally unrelated problem. I can set my PID variables over serial and then save them to a config file but 1 in 3 times if will hang just after it has saved the data to the file and I'm not sure why. Does anyone have any idea what could cause this?
Thanks Joe
I forgot to put void const *n in the parameters of the functions where they are defined at the top

"printf" appears to be non-deterministic in Qt?

I know "printf" is standard-c and should be deterministic. But when run in Qt I see a more non-deterministic response(clock cycles). Could this be due to Qt adding some "pork" to its response?
I have multiple threads that make call to function that uses a mutex. When one thread enters it set a switch so the others can't until it is done. Things appeared to work ok for acouple seconds and then threads appeared to be killed off from 10 to 1 thread. So I tried adding a delay: (k=k+1: no help), then (looping k=k+1: no help), (usleep works), and so does (printf) work at creating a random delay and allowing all threads to continue running.
void CCB::Write(int iThread)
{
static bool bUse = false;
bool bDone = false;
char cStr[20];
int posWrite;// = *m_posWrite; // issue of posWrite be altered with next extrance
long k = 0;
long m = 0;
m_threadCount++;
while(bDone == false){
if(bUse == false){
bUse = true;
posWrite = *m_posWrite;
memcpy(m_cmMessageCB + posWrite, &m_cmMessageWrite, sizeof(typeCanMessage));
memset(cStr, '\0', 20);
memcpy(cStr, (m_cmMessageCB + posWrite)->cMessage, 11); //fails: every 20
*m_posWrite = *m_posWrite + 1;
if(*m_posWrite == m_iNBufferLength)
*m_posWrite = 0;
bDone = true;
bUse = false;
}else if(bUse == true){
//why are threads being killed ?
// printf("T%d_%d ", iThread, m_threadCount);//non-deterministic value ?
usleep(1);//non-deterministic value
//k++;//delay of a couple clock cycles was not enough
/*
for(k = 0; k < iThread * 100; k++){//deterministic and fails to resolve thread problem
m++;
}
*/
}
}
}