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();
}
}
}
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();
}
}
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();
}
// 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
}
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;
}
}
}
}