Beispiel #1
0
static void GetWaypointValues(void) {
  WndProperty* wp;
  bool changed = false;

  if (!settings_task.AATEnabled) {
    return;
  }

  if (task.ValidTaskPoint(twItemIndex)) {
    TASK_POINT tp = task.getTaskPoint(twItemIndex);

    short tmp;

    wp = (WndProperty*)wf->FindByName(_T("prpAATType"));
    if (wp) {
      tmp = tp.AATType;
      CHECK_CHANGED(tmp,
                    wp->GetDataField()->GetAsInteger());
      tp.AATType = (AATSectorType_t)tmp;
    }

    wp = (WndProperty*)wf->FindByName(_T("prpAATCircleRadius"));
    if (wp) {
      CHECK_CHANGED(tp.AATCircleRadius,
                    iround(wp->GetDataField()->GetAsFloat()/DISTANCEMODIFY));
    }

    wp = (WndProperty*)wf->FindByName(_T("prpAATSectorRadius"));
    if (wp) {
      CHECK_CHANGED(tp.AATSectorRadius,
                    iround(wp->GetDataField()->GetAsFloat()/DISTANCEMODIFY));
    }

    wp = (WndProperty*)wf->FindByName(_T("prpAATStartRadial"));
    if (wp) {
      CHECK_CHANGED(tp.AATStartRadial,
                    wp->GetDataField()->GetAsInteger());
    }

    wp = (WndProperty*)wf->FindByName(_T("prpAATFinishRadial"));
    if (wp) {
      CHECK_CHANGED(tp.AATFinishRadial,
                    wp->GetDataField()->GetAsInteger());
    }
    task.setTaskPoint(twItemIndex, tp);

    if (changed) {
      task.SetTaskModified();
    }
  }
}
Beispiel #2
0
static void GetWaypointValues(void) {
  WndProperty* wp;
  bool changed = false;

  if (!AATEnabled) {
    return;
  }

  if ((twItemIndex<MAXTASKPOINTS)&&(twItemIndex>=0)) {
    LockTaskData();
    wp = (WndProperty*)wf->FindByName(TEXT("prpAATType"));
    if (wp) {
      CHECK_CHANGED(Task[twItemIndex].AATType, 
                    wp->GetDataField()->GetAsInteger());
    }

    wp = (WndProperty*)wf->FindByName(TEXT("prpAATCircleRadius"));
    if (wp) {
      CHECK_CHANGED(Task[twItemIndex].AATCircleRadius,
                    iround(wp->GetDataField()->GetAsFloat()/DISTANCEMODIFY));
    }
    
    wp = (WndProperty*)wf->FindByName(TEXT("prpAATSectorRadius"));
    if (wp) {
      CHECK_CHANGED(Task[twItemIndex].AATSectorRadius,
                    iround(wp->GetDataField()->GetAsFloat()/DISTANCEMODIFY));
    }
    
    wp = (WndProperty*)wf->FindByName(TEXT("prpAATStartRadial"));
    if (wp) {
      CHECK_CHANGED(Task[twItemIndex].AATStartRadial,
                    wp->GetDataField()->GetAsInteger());
    }
    
    wp = (WndProperty*)wf->FindByName(TEXT("prpAATFinishRadial"));
    if (wp) {
      CHECK_CHANGED(Task[twItemIndex].AATFinishRadial, 
                    wp->GetDataField()->GetAsInteger());
    }  
    if (changed) {
      TaskModified = true;
    }
    UnlockTaskData();

  }
}
Beispiel #3
0
static void ReadValues(void) {
  WndProperty* wp;
  bool changed = false;

  LockTaskData();
  wp = (WndProperty*)wf->FindByName(TEXT("prpEnableMultipleStartPoints"));
  if (wp) {
    CHECK_CHANGED(EnableMultipleStartPoints,
                  wp->GetDataField()->GetAsBoolean());
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpAATEnabled"));
  if (wp) {
    CHECK_CHANGED(AATEnabled,
                  wp->GetDataField()->GetAsInteger());
	if (DoOptimizeRoute()) AATEnabled=true; // force it on
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskFinishLine"));
  if (wp) {
    CHECK_CHANGED(FinishLine,
                  wp->GetDataField()->GetAsInteger());
  }
  
  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskFinishRadius"));
  if (wp) {
    CHECK_CHANGED(FinishRadius,
                  (DWORD)iround(wp->GetDataField()->GetAsFloat()
				/DISTANCEMODIFY));
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskStartLine"));
  if (wp) {
    CHECK_CHANGED(StartLine, 
                  wp->GetDataField()->GetAsInteger());
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskStartRadius"));
  if (wp) {
    CHECK_CHANGED(StartRadius,
                  (DWORD)iround(wp->GetDataField()->GetAsFloat()
				/DISTANCEMODIFY));
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskFAISector"));
  if (wp) {
    CHECK_CHANGED(SectorType,
                  wp->GetDataField()->GetAsInteger());
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskSectorRadius"));
  if (wp) {
    CHECK_CHANGED(SectorRadius,
                  (DWORD)iround(wp->GetDataField()->GetAsFloat()
				/DISTANCEMODIFY));
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpAutoAdvance"));
  if (wp) {
    CHECK_CHANGED(AutoAdvance, 
                  wp->GetDataField()->GetAsInteger());
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpMinTime"));
  if (wp) {
    CHECK_CHANGED(AATTaskLength, 
                  wp->GetDataField()->GetAsInteger());
	if (changed) CALCULATED_INFO.AATTimeToGo=AATTaskLength*60; 
  }
  if (changed) {
    TaskModified = true;
  }

  UnlockTaskData();

}
Beispiel #4
0
// RETURN: <0      => error
//         =0      => no error, no update
//         >0      => no error, update happened
//
int KPCMCIACard::refresh() {
//////////////////////////////////////////////
///////////// LINUX ONLY
///////////////////////////////////////////////
#ifdef __linux__
struct timeval tv;
cs_status_t status;
fd_set rfds;
int rc;
event_t event;
struct stat sb;
config_info_t cfg;
KPCMCIACard oldValues(*this);
int updated = 0;
oldValues._fd = -1;

#define CHECK_CHANGED(x, y) do { if (x.y != y) updated = 1; } while(0)

    /**
     *   Get any events on the pcmcia device
     */
tv.tv_sec = 0;  tv.tv_usec = 0;
     FD_ZERO(&rfds);
     FD_SET(_fd, &rfds);
     rc = select(_fd+1, &rfds, NULL, NULL, &tv);
     if (rc > 0) {
        rc = read(_fd, (void *)&event, 4);
        if (rc == 4) {
	   int thisEvent = -1;  // thisEvent is the index of event in event_tag
           for (unsigned int j = 0; j < NTAGS; j++) {
              if (event_tag[j].event == event) {
                 thisEvent = j;
                 break;
              }
              if (thisEvent < 0) return -1;
           }
        } else {
           return -1;
        }
     } else return updated;

     if (event == CS_EVENT_EJECTION_REQUEST) {
        _interrupt = -1;
        _ports = "";
        _device = "";
        _module = "";
        _type = "";
        _iotype = 0;
        _inttype = 0;
        _cfgbase = 0;
        _cardname = i18n("Empty slot.");
        _vcc = _vpp = _vpp2 = 0;
        return updated;
     }

     /**
      *   Read in the stab file.
      */
     if (!stat(_stabPath.latin1(), &sb) && sb.st_mtime >= _last) {
        TQFile f(_stabPath.latin1());

        if (f.open(IO_ReadOnly)) {
          TQTextStream ts(&f);
          bool foundit = false;
          TQString _thisreg = "^Socket %1: ";
          TQRegExp thisreg ( _thisreg.arg(_num) );

          if (flock(f.handle(), LOCK_SH)) return updated;

          _last = sb.st_mtime;

          // find the card
          while(!foundit) {
            TQString s;
            if (ts.eof()) break;
            s = ts.readLine();
            if (s.contains(thisreg)) {
               _cardname = s.right(s.length() - s.find(':') - 1);
               _cardname = _cardname.stripWhiteSpace();
               foundit = true;
               CHECK_CHANGED(oldValues, _cardname);
            }
          }

          // read it in
          if (foundit && !ts.eof()) {  // FIXME: ts.eof() is a bad error!!
            TQString s = ts.readLine();
            int end;
            s.simplifyWhiteSpace();

            end = s.find(TQRegExp("[ \r\t\n]"));
            s = s.remove(0, end+1);

            end = s.find(TQRegExp("[ \r\t\n]"));
            _type = s;
            _type.truncate(end);
            s = s.remove(0, end+1);

            end = s.find(TQRegExp("[ \r\t\n]"));
            _module = s;
            _module.truncate(end);
            s = s.remove(0, end+1);

            end = s.find(TQRegExp("[ \r\t\n]"));
            s = s.remove(0, end+1);

            end = s.find(TQRegExp("[ \r\t\n]"));
            _device = s;
            _device.truncate(end);
            s = s.remove(0, end+1);
            CHECK_CHANGED(oldValues, _type);
            CHECK_CHANGED(oldValues, _module);
            CHECK_CHANGED(oldValues, _device);
          }

          flock(f.handle(), LOCK_UN);
          f.close();
        } else return -1;
     } else return updated;


     /**
      *   Get the card's status and configuration information
      */
     status.Function = 0;
     ioctl(_fd, DS_GET_STATUS, &status);
     memset(&cfg, 0, sizeof(cfg));
     ioctl(_fd, DS_GET_CONFIGURATION_INFO, &cfg);
     // status is looked up in the table at the top
     if (cfg.Attributes & CONF_VALID_CLIENT) {
        if (cfg.AssignedIRQ == 0)
          _interrupt = -1;
        else _interrupt = cfg.AssignedIRQ;

        if (cfg.NumPorts1 > 0) {
           int stop = cfg.BasePort1 + cfg.NumPorts1;
           if (cfg.NumPorts2 > 0) {
              if (stop == cfg.BasePort2) {
                 _ports.sprintf("%#x-%#x", cfg.BasePort1, stop+cfg.NumPorts2-1);
              } else {
                 _ports.sprintf("%#x-%#x, %#x-%#x", cfg.BasePort1, stop-1,
                               cfg.BasePort2, cfg.BasePort2+cfg.NumPorts2-1);
              }
           } else {
              _ports.sprintf("%#x-%#x", cfg.BasePort1, stop-1);
           }
        }
        CHECK_CHANGED(oldValues, _ports);
        CHECK_CHANGED(oldValues, _interrupt);
     }

     _vcc = cfg.Vcc;
     _vpp = cfg.Vpp1;
     _vpp2 = cfg.Vpp2;
     CHECK_CHANGED(oldValues, _vcc);
     CHECK_CHANGED(oldValues, _vpp);
     CHECK_CHANGED(oldValues, _vpp2);
     _inttype = cfg.IntType;
     CHECK_CHANGED(oldValues, _inttype);
     _iotype = cfg.IOAddrLines;
     CHECK_CHANGED(oldValues, _iotype);
     _cfgbase = cfg.ConfigBase;
     CHECK_CHANGED(oldValues, _cfgbase);

     if (status.CardState & CS_EVENT_CARD_DETECT)
        _status |= CARD_STATUS_PRESENT;
     if (status.CardState & CS_EVENT_CARD_REMOVAL)
        _status &= ~CARD_STATUS_PRESENT;
     if (event & CS_EVENT_CARD_REMOVAL)
        _status &= ~CARD_STATUS_PRESENT;

     if (!(status.CardState & CS_EVENT_PM_SUSPEND)) {
        if (status.CardState & CS_EVENT_READY_CHANGE) {
           _status |= CARD_STATUS_READY;
           _status &= ~(CARD_STATUS_BUSY|CARD_STATUS_SUSPEND);
        } else {
           _status |= CARD_STATUS_BUSY;
           _status &= ~(CARD_STATUS_READY|CARD_STATUS_SUSPEND);
        }
     } else if (status.CardState & CS_EVENT_PM_SUSPEND) {
        _status |= CARD_STATUS_SUSPEND;
        _status &= ~(CARD_STATUS_READY|CARD_STATUS_BUSY);
     }

     CHECK_CHANGED(oldValues, _status);

return updated;
#else
return 0;
#endif
}
Beispiel #5
0
static void ReadValues(void) {
  WndProperty* wp;
  bool changed = false;

  wp = (WndProperty*)wf->FindByName(_T("prpEnableMultipleStartPoints"));
  if (wp) {
    CHECK_CHANGED(settings_task.EnableMultipleStartPoints,
                  wp->GetDataField()->GetAsBoolean());
  }

  wp = (WndProperty*)wf->FindByName(_T("prpAATEnabled"));
  if (wp) {
    CHECK_CHANGED(settings_task.AATEnabled,
                  wp->GetDataField()->GetAsInteger());
  }

  wp = (WndProperty*)wf->FindByName(_T("prpTaskFinishLine"));
  if (wp) {
    unsigned tmp = settings_task.FinishType;
    CHECK_CHANGEDU(tmp,
                  wp->GetDataField()->GetAsInteger());
    settings_task.FinishType = (FinishSectorType_t)tmp;
  }

  wp = (WndProperty*)wf->FindByName(_T("prpTaskFinishRadius"));
  if (wp) {
    CHECK_CHANGED(settings_task.FinishRadius,
                  (DWORD)iround(wp->GetDataField()->GetAsFloat()
				/DISTANCEMODIFY));
  }

  wp = (WndProperty*)wf->FindByName(_T("prpTaskStartLine"));
  if (wp) {
    unsigned tmp = settings_task.StartType;
    CHECK_CHANGEDU(tmp,
                  wp->GetDataField()->GetAsInteger());
    settings_task.StartType = (StartSectorType_t)tmp;
  }

  wp = (WndProperty*)wf->FindByName(_T("prpTaskStartRadius"));
  if (wp) {
    CHECK_CHANGED(settings_task.StartRadius,
                  (DWORD)iround(wp->GetDataField()->GetAsFloat()
				/DISTANCEMODIFY));
  }

  wp = (WndProperty*)wf->FindByName(_T("prpTaskFAISector"));
  if (wp) {
    unsigned tmp = settings_task.SectorType;
    CHECK_CHANGEDU(tmp,
                  wp->GetDataField()->GetAsInteger());
    settings_task.SectorType = (ASTSectorType_t)tmp;
  }

  wp = (WndProperty*)wf->FindByName(_T("prpTaskSectorRadius"));
  if (wp) {
    CHECK_CHANGED(settings_task.SectorRadius,
                  (DWORD)iround(wp->GetDataField()->GetAsFloat()
				/DISTANCEMODIFY));
  }

  wp = (WndProperty*)wf->FindByName(_T("prpAutoAdvance"));
  if (wp) {
    short tmp= settings_task.AutoAdvance;
    CHECK_CHANGED(tmp,
                  wp->GetDataField()->GetAsInteger());
    settings_task.AutoAdvance = (AutoAdvanceMode_t)tmp;
  }

  wp = (WndProperty*)wf->FindByName(_T("prpMinTime"));
  if (wp) {
    CHECK_CHANGED(settings_task.AATTaskLength,
                  wp->GetDataField()->GetAsInteger());
  }
  if (changed) {
    task.setSettings(settings_task);
    task.SetTaskModified();
  }
}
void MavlinkProcessor::receiveTelemetry(Telemetry& gathered_telemetry) {
	uint32_t next_timeout = millis() + 6;
	tryToConnectToAPM();
	while(MavLinkSerial.available() && millis() < next_timeout){
		if (mavlink_parse_char(MAVLINK_COMM_0, MavLinkSerial.read(), &msg, &status)) {
			switch(msg.msgid) {
			case MAVLINK_MSG_ID_HEARTBEAT: // 0
				if (msg.sysid == 1) {
					CHECK_CHANGED(gathered_telemetry.base_mode, mavlink_msg_heartbeat_get_base_mode(&msg), BASE_MODE);
					CHECK_CHANGED(gathered_telemetry.custom_mode, mavlink_msg_heartbeat_get_custom_mode(&msg), CUSTOM_MODE);
					CHECK_CHANGED(gathered_telemetry.system_status, mavlink_msg_heartbeat_get_system_status(&msg), SYSTEM_STATUS);
					CHECK_CHANGED(gathered_telemetry.mav_type, mavlink_msg_heartbeat_get_type(&msg), SlowParameters::MAV_TYPE);
					connection_timer = millis();
					if (!is_connected);
					{
						heartbeat_count++;
						if ((heartbeat_count) > 10) { // If  received > 10 heartbeats from MavLink then we are connected
							is_connected = true;
							heartbeat_count = 0;
							digitalWrite(alive_led_pin, HIGH); // LED will be ON when connected to MavLink, else it will slowly blink
						}
					}
				}
				break;
			case MAVLINK_MSG_ID_SYS_STATUS: // 1
				gathered_telemetry.battery_voltage = mavlink_msg_sys_status_get_voltage_battery(&msg) / 10; //
				gathered_telemetry.battery_current = mavlink_msg_sys_status_get_current_battery(&msg) / 10; //
				CHECK_CHANGED(gathered_telemetry.battery_remaining, mavlink_msg_sys_status_get_battery_remaining(&msg), BATTERY_REMAINING);
				break;
			case MAVLINK_MSG_ID_GPS_RAW_INT: // 24

				CHECK_CHANGED(gathered_telemetry.gps_fixtype, mavlink_msg_gps_raw_int_get_fix_type(&msg), GPS_FIX_TYPE_AND_SAT_VISIBLE); // 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix
				CHECK_CHANGED(gathered_telemetry.gps_satellites_visible, mavlink_msg_gps_raw_int_get_satellites_visible(&msg), GPS_FIX_TYPE_AND_SAT_VISIBLE); // numbers of visible satelites

				if (gathered_telemetry.gps_fixtype >= 3) {
					CHECK_CHANGED(gathered_telemetry.gps_hdop, mavlink_msg_gps_raw_int_get_eph(&msg), HDOP_EX); // hdop * 100
					gathered_telemetry.gps_latitude = mavlink_msg_gps_raw_int_get_lat(&msg);
					gathered_telemetry.gps_longitude = mavlink_msg_gps_raw_int_get_lon(&msg);
					gathered_telemetry.gps_altitude = mavlink_msg_gps_raw_int_get_alt(&msg); // 1m =1000
					gathered_telemetry.gps_speed = mavlink_msg_gps_raw_int_get_vel(&msg);
				}
				else {
					CHECK_CHANGED(gathered_telemetry.gps_hdop, mavlink_msg_gps_raw_int_get_eph(&msg), HDOP_EX); // hdop * 100
					gathered_telemetry.gps_latitude = 0L;
					gathered_telemetry.gps_longitude = 0l;
					gathered_telemetry.gps_altitude = 0L;
					gathered_telemetry.gps_speed = 0L;
				}
				break;
			case MAVLINK_MSG_ID_VFR_HUD: //  74
				gathered_telemetry.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); // 100 = 1m/s
				gathered_telemetry.heading = mavlink_msg_vfr_hud_get_heading(&msg); // 100 = 100 deg
				gathered_telemetry.throttle = mavlink_msg_vfr_hud_get_throttle(&msg); //  100 = 100%
				gathered_telemetry.bar_altitude = mavlink_msg_vfr_hud_get_alt(&msg); //  m
				gathered_telemetry.climb_rate = mavlink_msg_vfr_hud_get_climb(&msg) * 100; //  m/s
				break;
			case MAVLINK_MSG_ID_STATUSTEXT:
				mavlink_msg_statustext_get_text(&msg, gathered_telemetry.text);
				break;
			case MAVLINK_MSG_ID_ATTITUDE:
				gathered_telemetry.pitch = mavlink_msg_attitude_get_pitch(&msg) * 180 / M_PI;
				gathered_telemetry.roll = mavlink_msg_attitude_get_roll(&msg) * 180 / M_PI;

				break;
			default:
				break;
			}
		}
	}
}