void loop()
{
// Process AI
static uint16_t ind = 0;
static uint32_t voltageRawArray[8];
voltageRawArray[(ind++)%8] = analogRead(voltagePin);
uint16_t voltageRaw = 0;
for (uint16_t i=0;i<8;i++)
voltageRaw += voltageRawArray[i];
voltage = float(voltageRaw) * 100 / 1020;
//--------------- Start Timed Service Routines ---------------------------------------
unsigned long currentMillis = millis();
//enable high speed
if((currentMillis - previous_millis_sync) >= sync_speed_cycle) // (Executed > NTSC/PAL hz 33ms)
{
previous_millis_sync = previous_millis_sync+sync_speed_cycle;
}
if((currentMillis - previous_millis_low) >= lo_speed_cycle) // 10 Hz (Executed every 100ms)
{
previous_millis_low = previous_millis_low+lo_speed_cycle;
timer.tenthSec++;
timer.halfSec++;
timer.Blink10hz=!timer.Blink10hz;
} // End of slow Timed Service Routine (100ms loop)
if((currentMillis - previous_millis_high) >= hi_speed_cycle) // 20 Hz (Executed every 50ms)
{
previous_millis_high = previous_millis_high+hi_speed_cycle;
//if statements to define what to show on screen.
//this should be moved to proper c++ code, not if statements
if(showVoltage == 1){
displayVoltage();
}
if(showTimer == 1){
displayTime();
}
if(showCallsign == 1){
displayCallsign();
}
if(showCrosshair == 1){
displayCrosshair();
}
MAX7456_DrawScreen();
} // End of fast Timed Service Routine (50ms loop)
//--------------------- End of Timed Service Routine ---------------------------------------
if(millis() > timer.seconds+1000) // this execute 1 time a second
{
timer.seconds+=1000;
timer.tenthSec=0;
onTime++;
//check to see if the MAX chip crashed
MAX7456Stalldetect();
allSec++;
}
} // End of main loop
void loop()
{
// Process AI
if (Settings[S_ENABLEADC]){
//temperature=(analogRead(temperaturePin)-102)/2.048; // Does someone use this ATM??
if (!Settings[S_MAINVOLTAGE_VBAT]){
static uint16_t ind = 0;
static uint32_t voltageRawArray[8];
voltageRawArray[(ind++)%8] = analogRead(voltagePin);
uint16_t voltageRaw = 0;
for (uint16_t i=0;i<8;i++)
voltageRaw += voltageRawArray[i];
voltage = float(voltageRaw) * Settings[S_DIVIDERRATIO] /1023;
}
if (!Settings[S_VIDVOLTAGE_VBAT]) {
static uint16_t ind = 0;
static uint32_t voltageRawArray[8];
voltageRawArray[(ind++)%8] = analogRead(vidvoltagePin);
uint16_t voltageRaw = 0;
for (uint16_t i=0;i<8;i++)
voltageRaw += voltageRawArray[i];
vidvoltage = float(voltageRaw) * Settings[S_VIDDIVIDERRATIO] /1023;
}
if (!Settings[S_MWRSSI] && !Settings[S_PWMRSSI]) {
rssiADC = analogRead(rssiPin)/4; // RSSI Readings, rssiADC=0 to 1023/4 (avoid a number > 255)
}
if (!Settings[S_MWAMPERAGE]) {
int16_t currsensOffSet=(Settings[S_CURRSENSOFFSET_L] | (Settings[S_CURRSENSOFFSET_H] << 8)); // Read OffSetH/L
amperageADC = analogRead(amperagePin);
if (amperageADC > currsensOffSet) amperageADC=((amperageADC-currsensOffSet)*4.8828)/Settings[S_CURRSENSSENSITIVITY]; // [A] Positive Current flow (512...1023) or Unidir (0...1023)
else amperageADC=((currsensOffSet-amperageADC)*4.8828)/Settings[S_CURRSENSSENSITIVITY]; // [A] Negative Current flow (0...512)
}
}
if (Settings[S_MWAMPERAGE]) {
amperagesum = MW_ANALOG.pMeterSum;
amperage = MW_ANALOG.Amperage /100;
}
if (Settings[S_MWRSSI]) {
rssiADC = MW_ANALOG.Rssi/4; // RSSI from MWii, rssiADC=0 to 1023/4 (avoid a number > 255)
}
if (Settings[S_PWMRSSI] && !Settings[S_MWRSSI]){
rssiADC = pulseIn(PWMrssiPin, HIGH,15000)/Settings[S_PWMRSSIDIVIDER]; // Reading W/time out (microseconds to wait for pulse to start: 15000=0.015sec)
}
// Blink Basic Sanity Test Led at 1hz - this stuff introduces strange behavior on my system
if(tenthSec>10)
digitalWrite(7,HIGH);
else
digitalWrite(7,LOW);
//--------------- Start Timed Service Routines ---------------------------------------
uint16_t currentMillis = millis();
if((currentMillis - previous_millis_low) >= lo_speed_cycle) // 10 Hz (Executed every 100ms)
{
previous_millis_low = currentMillis;
tenthSec++;
TempBlinkAlarm++;
Blink10hz=!Blink10hz;
if(!fontMode)
blankserialRequest(MSP_ATTITUDE);
if(Settings[L_RSSIPOSITIONDSPL])
calculateRssi();
} // End of slow Timed Service Routine (100ms loop)
if((currentMillis - previous_millis_high) >= hi_speed_cycle) // 20 Hz (Executed every 50ms)
{
previous_millis_high = currentMillis;
calculateTrip(); // Speed integration on 50msec
if (!Settings[S_MWAMPERAGE]) calculateAmperage(); // Amperage and amperagesum integration on 50msec
uint8_t MSPcmdsend;
if(queuedMSPRequests == 0)
queuedMSPRequests = modeMSPRequests;
uint32_t req = queuedMSPRequests & -queuedMSPRequests;
queuedMSPRequests &= ~req;
switch(req) {
case REQ_MSP_IDENT:
MSPcmdsend = MSP_IDENT;
break;
case REQ_MSP_STATUS:
MSPcmdsend = MSP_STATUS;
break;
case REQ_MSP_RAW_IMU:
MSPcmdsend = MSP_RAW_IMU;
break;
case REQ_MSP_RC:
MSPcmdsend = MSP_RC;
break;
case REQ_MSP_RAW_GPS:
MSPcmdsend = MSP_RAW_GPS;
break;
case REQ_MSP_COMP_GPS:
MSPcmdsend = MSP_COMP_GPS;
break;
case REQ_MSP_ATTITUDE:
MSPcmdsend = MSP_ATTITUDE;
break;
case REQ_MSP_ALTITUDE:
MSPcmdsend = MSP_ALTITUDE;
break;
case REQ_MSP_ANALOG:
MSPcmdsend = MSP_ANALOG;
break;
case REQ_MSP_RC_TUNING:
MSPcmdsend = MSP_RC_TUNING;
break;
case REQ_MSP_PID:
MSPcmdsend = MSP_PID;
break;
case REQ_MSP_BOX:
MSPcmdsend = MSP_BOXIDS;
break;
case REQ_MSP_FONT:
MSPcmdsend = MSP_OSD;
break;
}
if(!fontMode)
blankserialRequest(MSPcmdsend);
//MAX7456_DrawScreen();
if( allSec < 6 ){
displayIntro(KVTeamVersionPosition);
lastCallSign = onTime;
}
else
{
if(armed){
previousarmedstatus=1;
}
if(previousarmedstatus && !armed){
configPage=9;
ROW=10;
COL=1;
configMode=1;
setMspRequests();
}
if(fontMode) {
displayFontScreen();
}
else if(configMode)
{
displayConfigScreen();
}
else
{
displayVoltage();
displayVidVoltage();
displayRSSI();
displayTime();
displaySensor();
displayGPSMode();
displayMode();
//if((temperature<Settings[S_TEMPERATUREMAX])||(BlinkAlarm)) displayTemperature();
displayAmperage();
displaypMeterSum();
displayArmed();
displayCurrentThrottle();
displayautoPilot();
if ( (onTime > (lastCallSign+300)) || (onTime < (lastCallSign+4)))
{
// Displays 4 sec every 5min (no blink during flight)
if ( onTime > (lastCallSign+300))lastCallSign = onTime;
displayCallsign();
}
if(MW_STATUS.sensorPresent&ACCELEROMETER)
displayHorizon(MW_ATT.Angle[0],MW_ATT.Angle[1]);
if(MW_STATUS.sensorPresent&MAGNETOMETER) {
displayHeadingGraph();
displayHeading();
}
if(MW_STATUS.sensorPresent&BAROMETER) {
displayAltitude();
displayClimbRate();
}
if(MW_STATUS.sensorPresent&GPSSENSOR)
if(Settings[S_DISPLAYGPS]){
displayNumberOfSat();
displayDirectionToHome();
displayDistanceToHome();
displayAngleToHome();
displayGPS_speed();
displayGPSPosition();
//displayGPS_altitude(); // Do not remove yet
}
}
}
MAX7456_DrawScreen();
} // End of fast Timed Service Routine (50ms loop)
//--------------------- End of Timed Service Routine ---------------------------------------
if(TempBlinkAlarm >= Settings[S_BLINKINGHZ]) { // selectable alarm blink freq
TempBlinkAlarm = 0;
BlinkAlarm =!BlinkAlarm; // 10=1Hz, 9=1.1Hz, 8=1.25Hz, 7=1.4Hz, 6=1.6Hz, 5=2Hz, 4=2.5Hz, 3=3.3Hz, 2=5Hz, 1=10Hz
}
if(tenthSec >= 10) // this execute 1 time a second
{
onTime++;
tenthSec=0;
if(!armed) {
flyTime=0;
}
else {
flyTime++;
flyingTime++;
configMode=0;
setMspRequests();
}
allSec++;
if((accCalibrationTimer==1)&&(configMode)) {
blankserialRequest(MSP_ACC_CALIBRATION);
accCalibrationTimer=0;
}
if((magCalibrationTimer==1)&&(configMode)) {
blankserialRequest(MSP_MAG_CALIBRATION);
magCalibrationTimer=0;
}
if((eepromWriteTimer==1)&&(configMode)) {
blankserialRequest(MSP_EEPROM_WRITE);
eepromWriteTimer=0;
}
if(accCalibrationTimer>0) accCalibrationTimer--;
if(magCalibrationTimer>0) magCalibrationTimer--;
if(eepromWriteTimer>0) eepromWriteTimer--;
if((rssiTimer==1)&&(configMode)) {
Settings[S_RSSIMIN]=rssiADC; // set MIN RSSI signal received (tx off?)
rssiTimer=0;
}
if(rssiTimer>0) rssiTimer--;
}
serialMSPreceive();
} // End of main loop
void main ()
{
int rc;
unsigned long next_wake_utt;
unsigned long delta_wake_utt;
vBSP430platformInitialize_ni();
(void)iBSP430consoleInitialize();
cprintf("\n\nadc demo, " __DATE__ " " __TIME__ "\n");
delta_wake_utt = 2 * ulBSP430uptimeConversionFrequency_Hz();
rc = initializeADC();
cprintf("%s initialized, returned %d, ADC cal at %p, REF cal at %p\n",
#if defined(__MSP430_HAS_ADC10__)
"ADC10"
#elif defined(__MSP430_HAS_ADC10_A__)
"ADC10_A"
#elif defined(__MSP430_HAS_ADC10_B__)
"ADC10_B"
#elif defined(__MSP430_HAS_ADC10_B4__)
"ADC10_B (FR4xx)"
#elif defined(__MSP430_HAS_ADC12__)
"ADC12"
#elif defined(__MSP430_HAS_ADC12_B__)
"ADC12_B"
#elif defined(__MSP430_HAS_ADC12_PLUS__)
"ADC12_PLUS"
#endif /* ADC */
, rc, cal_adc, cal_ref);
#if HAVE_REF
if (cal_ref) {
cprintf("Reference factors:\n"
"\t" REF_1pX_STR "V %u (0x%04x)\n"
"\t2.0V %u (0x%04x)\n"
"\t2.5V %u (0x%04x)\n",
cal_ref->cal_adc_15vref_factor, cal_ref->cal_adc_15vref_factor,
cal_ref->cal_adc_20vref_factor, cal_ref->cal_adc_20vref_factor,
cal_ref->cal_adc_25vref_factor, cal_ref->cal_adc_25vref_factor);
}
#endif /* HAVE_REF */
if (cal_adc) {
cprintf("ADC gain factor %d (0x%04x), offset %d\n",
cal_adc->cal_adc_gain_factor, cal_adc->cal_adc_gain_factor,
cal_adc->cal_adc_offset);
cprintf("Temperature ranges:\n");
cprintf("\t" REF_1pX_STR "V T30 %u T85 %u\n", cal_adc->cal_adc_15t30, cal_adc->cal_adc_15t85);
#if BSP430_TLV_IS_5XX
cprintf("\t2.0V T30 %u T85 %u\n", cal_adc->cal_adc_20t30, cal_adc->cal_adc_20t85);
#endif /* BSP430_TLV_IS_5XX */
cprintf("\t2.5V T30 %u T85 %u\n", cal_adc->cal_adc_25t30, cal_adc->cal_adc_25t85);
}
cprintf("Vmid channel %u, Temp channel %u"
#ifdef INCH_AUX
", Aux channel %u"
#endif /* INCH_AUX */
"\n",
INCH_VMID / INCH_BASE, INCH_TEMP / INCH_BASE
#ifdef INCH_AUX
, INCH_AUX / INCH_BASE
#endif /* INCH_AUX */
);
next_wake_utt = ulBSP430uptime_ni();
while (1) {
char timestamp[BSP430_UPTIME_AS_TEXT_LENGTH];
static const int refv[] = { REF_1pX, REF_2p0, REF_2p5 };
static const char * const refv_str[] = { REF_1pX_STR, "2.0", "2.5" };
static const int const nrefv = sizeof(refv)/sizeof(*refv);
static const int inch[] = { INCH_TEMP,
INCH_VMID,
#if defined(INCH_AUX)
INCH_AUX,
#endif /* INCH_AUX */
};
static const int const ninch = sizeof(inch)/sizeof(*inch);
int valid = 0;
sSample sample[sizeof(refv)/sizeof(*refv)][sizeof(inch)/sizeof(*inch)];
int ri;
int ii;
#define VALID(_ri,_ii) ((1 << (_ii)) << ((_ri) * nrefv))
#define ANY_VALID(_ri) (((1 << nrefv)-1) << ((_ri) * nrefv))
for (ri = 0; ri < nrefv; ++ri) {
if (0 == setReferenceVoltage(refv[ri])) {
for (ii = 0; ii < ninch; ++ii) {
if (0 == getSample(sample[ri]+ii, refv[ri], inch[ii])) {
valid |= VALID(ri, ii);
}
}
}
}
cprintf("%s: valid %x", xBSP430uptimeAsText(ulBSP430uptime_ni(), timestamp), valid);
for (ri = 0; ri < nrefv; ++ri) {
if (valid & ANY_VALID(ri)) {
cprintf("\n\t%sV: ", refv_str[ri]);
for (ii = 0; ii < ninch; ++ii) {
if (VALID(ri, ii) & valid) {
if (INCH_TEMP == inch[ii]) {
displayTemperature(sample[ri] + ii);
} else if (INCH_VMID == inch[ii]) {
displayVmid(sample[ri] + ii);
} else {
displayVoltage(sample[ri] + ii);
}
}
}
}
}
cputchar('\n');
next_wake_utt += delta_wake_utt;
while (0 < lBSP430uptimeSleepUntil(next_wake_utt, LPM3_bits)) {
/* nop */
}
}
}
