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; } } } }