/*! Handle the messages routed to the display queue */
static void DisplayQueueMessageHandler(tMessage *pMsg)
{
tMessage Msg;
switch (pMsg->Type)
{
case ShowCallMsg:
HandleCallNotification(pMsg->Options, pMsg->pBuffer, pMsg->Length);
break;
case WriteBufferMsg:
WriteBufferHandler(pMsg);
break;
case SetWidgetListMsg:
SetWidgetList(pMsg);
break;
case UpdateDisplayMsg:
if ((!(pMsg->Options & MSG_OPT_UPD_INTERNAL) &&
(pMsg->Options & MODE_MASK) == NOTIF_MODE) &&
GetProperty(PROP_AUTO_BACKLIGHT))
SendMessage(AutoBacklightMsg, MSG_OPT_NONE);
UpdateDisplayHandler(pMsg);
break;
case DrawMsg:
DrawMsgHandler(pMsg);
break;
case UpdateClockMsg:
UpdateClock();
break;
case DrawClockWidgetMsg:
DrawClockWidget(pMsg->Options);
break;
case BluetoothStateChangeMsg:
BluetoothStateChangeHandler(pMsg);
break;
case IdleUpdateMsg:
IdleUpdateHandler();
break;
case ButtonStateMsg:
ButtonStateHandler();
break;
case StopTimerMsg:
StopTimer((eTimerId)pMsg->Options);
break;
case MonitorBatteryMsg:
MonitorBattery();
break;
case MusicIconMsg:
MusicIcon(pMsg->Options);
break;
case MusicStateMsg:
HandleMusicStateChange(pMsg->Options);
break;
case ChangeModeMsg:
ChangeMode(pMsg->Options);
break;
case ControlFullScreenMsg:
SetProperty(PROP_PHONE_DRAW_TOP, pMsg->Options || *pMsg->pBuffer ? PROP_PHONE_DRAW_TOP : 0);
break;
case ModifyTimeMsg:
ModifyTimeHandler(pMsg);
break;
case MenuModeMsg:
MenuModeHandler(pMsg->Options);
break;
case MenuButtonMsg:
MenuButtonHandler(pMsg->Options);
break;
case EnableButtonMsg:
EnableButtonMsgHandler(pMsg);
break;
case DevTypeMsg:
Msg.Length = 1;
Msg.Type = DevTypeRespMsg;
Msg.Options = BOARD_TYPE; //default G2
if (CreateMessage(&Msg))
{
Msg.pBuffer[0] = BOARD_TYPE; // backward compatible
if (GetMsp430HardwareRevision() < 'F')
{
Msg.Options = DIGITAL_WATCH_TYPE_G1;
Msg.pBuffer[0] = DIGITAL_WATCH_TYPE_G1; // backward compatible
}
Msg.Options |= DEV_TYPE_EN_ACK; // support ACK
RouteMsg(&Msg);
}
PrintF("- DevTypeResp:%u", Msg.Options);
// set ACK and HFP/MAP bits
// SendMessage(ConnTypeMsg, pMsg->Options);
break;
case VerInfoMsg:
Msg.Length = BUILD_LENGTH + 4 + 3;
Msg.Type = VerInfoRespMsg;
Msg.Options = MSG_OPT_NONE;
if (CreateMessage(&Msg))
{
GetBuildNumber(Msg.pBuffer);
*(Msg.pBuffer + BUILD_LENGTH) = VERSION[0] - ZERO;
*(Msg.pBuffer + BUILD_LENGTH + 1) = VERSION[2] - ZERO;
*(Msg.pBuffer + BUILD_LENGTH + 2) = VERSION[4] - ZERO;
*(Msg.pBuffer + BUILD_LENGTH + 3) = GetMsp430HardwareRevision();
*(Msg.pBuffer + BUILD_LENGTH + 4) = BootVersion[0] - ZERO;
*(Msg.pBuffer + BUILD_LENGTH + 5) = BootVersion[2] - ZERO;
*(Msg.pBuffer + BUILD_LENGTH + 6) = BootVersion[4] - ZERO;
RouteMsg(&Msg);
}
PrintE("-Ver(%u):", Msg.Length); PrintQ(Msg.pBuffer, Msg.Length);
break;
case LogMsg:
SendStateLog();
break;
case VibrateMsg:
SetVibrateModeHandler(pMsg);
break;
case SetRtcMsg:
if (SetRtc((Rtc_t *)pMsg->pBuffer)) UpdateClock();
break;
case CountdownMsg:
if (pMsg->Options == CDT_ENTER)
{
PageType = PAGE_TYPE_INFO;
CurrentPage[PageType] = CountdownPage;
}
CountdownHandler(pMsg->Options);
break;
case ServiceMenuMsg:
ServiceMenuHandler();
break;
case FieldTestMsg:
HandleFieldTestMode(pMsg->Options);
break;
case DisableButtonMsg:
DisableButtonMsgHandler(pMsg);
break;
case ReadButtonConfigMsg:
ReadButtonConfigHandler(pMsg);
break;
case SetBacklightMsg:
SetBacklight(pMsg->Options);
// testing
// pMsg->Type = AccelMsg;
// pMsg->Options = 1; //enable
// HandleAccelerometer(pMsg);
break;
case AutoBacklightMsg:
if (LightSenseCycle() < DARK_LEVEL) SetBacklight(LED_ON);
break;
case BatteryConfigMsg:
SetBatteryLevels(pMsg->pBuffer);
break;
case ReadBatteryVoltageMsg:
ReadBatteryVoltageHandler();
break;
case ResetMsg:
SoftwareReset(RESET_BUTTON_PRESS, pMsg->Options);
break;
case NvalOperationMsg:
NvalOperationHandler(pMsg);
break;
case SecInvertMsg:
HandleSecInvert(pMsg->Options);
break;
case LoadTemplateMsg:
LoadTemplateHandler(pMsg);
break;
case LinkAlarmMsg:
SendMessage(VibrateMsg, VIBRA_PATTERN_LNKALM);
break;
case ModeTimeoutMsg:
ModeTimeoutHandler();
break;
case WatchStatusMsg:
PageType = PAGE_TYPE_INFO;
CurrentPage[PageType] = StatusPage;
DrawWatchStatusScreen(TRUE);
break;
case TermModeMsg:
TermModeHandler();
break;
case LowBatteryWarningMsg:
break;
case LowBatteryBtOffMsg:
UpdateClock();
break;
#if __IAR_SYSTEMS_ICC__
case EraseTemplateMsg:
// EraseTemplateHandler(pMsg);
break;
case WriteToTemplateMsg:
// WriteToTemplateHandler(pMsg);
break;
#endif
case AccelMsg:
HandleAccelerometer(pMsg);
break;
case ReadLightSensorMsg:
ReadLightSensorHandler();
break;
case WatchDrawnScreenTimeout:
IdleUpdateHandler();
break;
case RateTestMsg:
/* don't care what data is */
Msg.Length = 10;
Msg.Type = DiagnosticLoopback;
Msg.Options = MSG_OPT_NONE;
if (CreateMessage(&Msg)) RouteMsg(&Msg);
break;
default:
PrintF("# Disp Msg:x%02X", pMsg->Type);
break;
}
}
void LowRateLog(uint32_t time){
uint32_u currentTime;
float_u outFloat;
uint8_t outByte;
currentTime.val = time;
outByte = 3;
WriteBufferHandler(1,&outByte);
WriteBufferHandler(4,currentTime.buffer);
outFloat.val = floatLat;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = floatLon;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = homeLatFloat;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = homeLonFloat;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = velN;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = velE;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = hAcc;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = sAcc;
WriteBufferHandler(4,outFloat.buffer);
WriteBufferHandler(1,&gpsFailSafe);
}
/*! Handle the messages routed to the display queue */
static void DisplayQueueMessageHandler(tMessage* pMsg)
{
tMessage Msg;
unsigned char i = 0;
switch (pMsg->Type)
{
case WriteBufferMsg:
WriteBufferHandler(pMsg);
break;
case SetWidgetListMsg:
SetWidgetList(pMsg);
break;
case UpdateDisplayMsg:
UpdateDisplayHandler(pMsg);
break;
case UpdateClockMsg:
UpdateClock();
break;
case DrawClockWidgetMsg:
DrawClockWidget(pMsg->Options);
break;
case MonitorBatteryMsg:
MonitorBattery();
break;
case BluetoothStateChangeMsg:
BluetoothStateChangeHandler(pMsg);
break;
case IdleUpdateMsg:
IdleUpdateHandler();
break;
case ButtonStateMsg:
ButtonStateHandler();
break;
case CallerIdMsg:
// PrintStringAndDecimal("LcdDisp:CallerId Len:", pMsg->Length);
pMsg->pBuffer[pMsg->Length] = NULL;
ShowCall((char *)pMsg->pBuffer, pMsg->Options);
break;
case CallerNameMsg:
if (pMsg->Length)
{
pMsg->pBuffer[pMsg->Length] = NULL;
ShowCall((char *)pMsg->pBuffer, SHOW_NOTIF_CALLER_NAME);
}
else ShowCall("", pMsg->Options);
break;
case MusicPlayStateMsg:
HandleMusicPlayStateChange(pMsg->Options);
break;
case ChangeModeMsg:
ChangeModeHandler(pMsg->Options);
break;
case ConfigureIdleBufferSize:
SetProperty(PROP_PHONE_DRAW_TOP, *pMsg->pBuffer ? PROP_PHONE_DRAW_TOP : 0);
break;
case ModifyTimeMsg:
ModifyTimeHandler(pMsg);
break;
case MenuModeMsg:
MenuModeHandler(pMsg->Options);
break;
case MenuButtonMsg:
MenuButtonHandler(pMsg->Options);
break;
case EnableButtonMsg:
EnableButtonMsgHandler(pMsg);
break;
case DevTypeMsg:
SetupMessageAndAllocateBuffer(&Msg, DevTypeRespMsg, MSG_OPT_NONE);
Msg.Options = BOARD_TYPE; //default G2
Msg.pBuffer[0] = BOARD_TYPE; // backward compatible
#ifdef WATCH
if (GetMsp430HardwareRevision() < 'F')
{
Msg.Options = DIGITAL_WATCH_TYPE_G1;
Msg.pBuffer[0] = DIGITAL_WATCH_TYPE_G1; // backward compatible
}
#endif
Msg.Length = 1;
RouteMsg(&Msg);
PrintStringAndDecimal("- DevTypeResp:", Msg.pBuffer[0]);
break;
case VerInfoMsg:
SetupMessageAndAllocateBuffer(&Msg, VerInfoRespMsg, MSG_OPT_NONE);
/* exclude middle '.' */
strncpy((char *)Msg.pBuffer, BUILD, 3);
strncpy((char *)Msg.pBuffer + 3, BUILD + 4, 3);
Msg.Length += strlen(BUILD) - 1;
*(Msg.pBuffer + Msg.Length++) = (unsigned char)atoi(VERSION);
while (VERSION[i++] != '.');
*(Msg.pBuffer + Msg.Length++) = atoi(VERSION + i);
*(Msg.pBuffer + Msg.Length++) = NULL;
RouteMsg(&Msg);
PrintString("-Ver:"); for (i = 6; i < Msg.Length; ++i) PrintHex(Msg.pBuffer[i]);
break;
case SetVibrateMode:
SetVibrateModeHandler(pMsg);
break;
case SetRtcMsg:
halRtcSet((tRtcHostMsgPayload*)pMsg->pBuffer);
#ifdef DIGITAL
UpdateClock();
#endif
break;
case GetRtcMsg:
SetupMessageAndAllocateBuffer(&Msg, RtcRespMsg, MSG_OPT_NONE);
halRtcGet((tRtcHostMsgPayload*)Msg.pBuffer);
Msg.Length = sizeof(tRtcHostMsgPayload);
RouteMsg(&Msg);
break;
case ServiceMenuMsg:
ServiceMenuHandler();
break;
case DisableButtonMsg:
DisableButtonMsgHandler(pMsg);
break;
case ReadButtonConfigMsg:
ReadButtonConfigHandler(pMsg);
break;
case LedChange:
LedChangeHandler(pMsg);
break;
case BatteryConfigMsg:
SetBatteryLevels(pMsg->pBuffer);
break;
case ReadBatteryVoltageMsg:
ReadBatteryVoltageHandler();
break;
case ResetMsg:
SoftwareResetHandler(pMsg);
break;
case NvalOperationMsg:
NvalOperationHandler(pMsg);
break;
case SecInvertMsg:
HandleSecInvert(pMsg->Options);
break;
case LoadTemplateMsg:
LoadTemplateHandler(pMsg);
break;
case LinkAlarmMsg:
if (!GetProperty(PROP_DISABLE_LINK_ALARM))
{
SetupMessageAndAllocateBuffer(&Msg, SetVibrateMode, MSG_OPT_NONE);
tSetVibrateModePayload* pMsgData;
pMsgData = (tSetVibrateModePayload *)Msg.pBuffer;
pMsgData->Enable = 1;
pMsgData->OnDurationLsb = 0xC8;
pMsgData->OnDurationMsb = 0x00;
pMsgData->OffDurationLsb = 0xF4;
pMsgData->OffDurationMsb = 0x01;
pMsgData->NumberOfCycles = 1; //3
RouteMsg(&Msg);
}
break;
case ModeTimeoutMsg:
ModeTimeoutHandler();
break;
case WatchStatusMsg:
PageType = PAGE_TYPE_INFO;
CurrentPage[PageType] = StatusPage;
DrawWatchStatusScreen();
break;
// case ListPairedDevicesMsg:
// ListPairedDevicesHandler();
// break;
case WatchDrawnScreenTimeout:
IdleUpdateHandler();
break;
case TermModeMsg:
TermModeHandler();
break;
case LowBatteryWarningMsg:
break;
case LowBatteryBtOffMsg:
UpdateClock();
break;
#if __IAR_SYSTEMS_ICC__
case EraseTemplateMsg:
EraseTemplateHandler(pMsg);
break;
case WriteToTemplateMsg:
WriteToTemplateHandler(pMsg);
break;
#endif
case EnableAccelerometerMsg:
case DisableAccelerometerMsg:
case AccelerometerSendDataMsg:
case AccelerometerAccessMsg:
case AccelerometerSetupMsg:
HandleAccelerometer(pMsg);
break;
case ReadLightSensorMsg:
ReadLightSensorHandler();
break;
case RateTestMsg:
SetupMessageAndAllocateBuffer(&Msg, DiagnosticLoopback, MSG_OPT_NONE);
/* don't care what data is */
Msg.Length = 10;
RouteMsg(&Msg);
break;
case RamTestMsg:
RamTestHandler(pMsg);
break;
default:
PrintStringAndHexByte("# Disp Msg: 0x", pMsg->Type);
break;
}
}
void MedRateLog(uint32_t time){
uint32_u currentTime;
float_u outFloat;
int16_u outInt16;
uint8_t outByte;
currentTime.val = time;
outByte = 2;
WriteBufferHandler(1,&outByte);
WriteBufferHandler(4,currentTime.buffer);
outFloat.val = pressure;
WriteBufferHandler(4,outFloat.buffer);
WriteBufferHandler(1,&baroFS);
outInt16.val = RCValue[0];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[1];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[2];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[3];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[4];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[5];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[6];
WriteBufferHandler(2,outInt16.buffer);
outInt16.val = RCValue[7];
WriteBufferHandler(2,outInt16.buffer);
}
void HighRateLog(uint32_t time){
uint32_u currentTime;
float_u outFloat;
uint8_t outByte;
currentTime.val = time;
outByte = 1;
WriteBufferHandler(1,&outByte);
WriteBufferHandler(4,currentTime.buffer);
//gyro------------------------------------------
outFloat.val = degreeGyroX;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = degreeGyroY;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = degreeGyroZ;
WriteBufferHandler(4,outFloat.buffer);
//acc-------------------------------------------
outFloat.val = filtAccX;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = filtAccY;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = filtAccZ;
WriteBufferHandler(4,outFloat.buffer);
//mag--------------------------------------------
outFloat.val = scaledMagX;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = scaledMagY;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = scaledMagZ;
WriteBufferHandler(4,outFloat.buffer);
//att--------------------------------------------
outFloat.val = yawInDegrees;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = pitchInDegrees;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = rollInDegrees;
WriteBufferHandler(4,outFloat.buffer);
//pos--------------------------------------------
outFloat.val = XEst;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = YEst;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ZEst;
WriteBufferHandler(4,outFloat.buffer);
//vel--------------------------------------------
outFloat.val = velX;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = velY;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = velZ;
WriteBufferHandler(4,outFloat.buffer);
//inertial--------------------------------------------
outFloat.val = inertialX;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = inertialY;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = inertialZ;
WriteBufferHandler(4,outFloat.buffer);
//inertial--------------------------------------------
outFloat.val = accelBiasX;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = accelBiasY;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = accelBiasZ;
WriteBufferHandler(4,outFloat.buffer);
}
void MotorMixToFlash(){
float_u outFloat;
outFloat.val = m1X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m1Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m1Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m2X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m2Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m2Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m3X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m3Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m3Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m4X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m4Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m4Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m5X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m5Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m5Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m6X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m6Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m6Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m7X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m7Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m7Z;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m8X;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m8Y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = m8Z;
WriteBufferHandler(4,outFloat.buffer);
WriteBufferHandler(1,&txLossRTB);
WriteBufferHandler(1,&magDetected);
WriteBufferHandler(1,&GPSDetected);
}
void GainsToFlash(){
float_u outFloat;
//uint8_t
uint8_t outByte = 0;
//WriteBufferHandler(6,writeBuffer);
WriteBufferHandler(1,&outByte);
outFloat.val = kp_pitch_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_pitch_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_pitch_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_pitch_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_roll_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_roll_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_roll_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_roll_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_yaw_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_yaw_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_yaw_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_yaw_rate;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_pitch_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_pitch_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_pitch_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_pitch_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_roll_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_roll_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_roll_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_roll_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_yaw_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_yaw_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_yaw_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_yaw_attitude;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_altitude_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_altitude_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_altitude_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_altitude_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_altitude_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_altitude_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_altitude_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_altitude_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_loiter_pos_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_loiter_pos_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_loiter_pos_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_loiter_pos_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_loiter_velocity_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_loiter_velocity_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_loiter_velocity_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_loiter_velocity_x;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_loiter_pos_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_loiter_pos_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_loiter_pos_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_loiter_pos_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_loiter_velocity_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_loiter_velocity_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_loiter_velocity_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_loiter_velocity_y;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_waypoint_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_waypoint_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_waypoint_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_waypoint_position;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_waypoint_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_waypoint_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_waypoint_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_waypoint_velocity;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kp_cross_track;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = ki_cross_track;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = kd_cross_track;
WriteBufferHandler(4,outFloat.buffer);
outFloat.val = fc_cross_track;
WriteBufferHandler(4,outFloat.buffer);
}
/*! Handle the messages routed to the display queue */
static void DisplayQueueMessageHandler(tMessage* pMsg)
{
tMessage Msg;
unsigned char i = 0;
switch (pMsg->Type)
{
case WriteBufferMsg:
WriteBufferHandler(pMsg);
break;
case SetWidgetListMsg:
SetWidgetList(pMsg);
break;
case UpdateDisplayMsg:
if ((!(pMsg->Options & MSG_OPT_UPD_INTERNAL) &&
(pMsg->Options & MODE_MASK) == NOTIF_MODE) &&
GetProperty(PROP_AUTO_BACKLIGHT))
SendMessage(&Msg, AutoBacklightMsg, MSG_OPT_NONE);
UpdateDisplayHandler(pMsg);
break;
case UpdateClockMsg:
UpdateClock();
break;
case DrawClockWidgetMsg:
DrawClockWidget(pMsg->Options);
break;
case MonitorBatteryMsg:
MonitorBattery();
break;
case BluetoothStateChangeMsg:
BluetoothStateChangeHandler(pMsg);
break;
case IdleUpdateMsg:
IdleUpdateHandler();
break;
case ButtonStateMsg:
ButtonStateHandler();
break;
case CallerIdMsg:
pMsg->pBuffer[pMsg->Length] = NUL;
ShowCall((char *)pMsg->pBuffer, pMsg->Options);
break;
case CallerNameMsg:
if (pMsg->Length)
{
pMsg->pBuffer[pMsg->Length] = NUL;
ShowCall((char *)pMsg->pBuffer, SHOW_NOTIF_CALLER_NAME);
if (GetProperty(PROP_AUTO_BACKLIGHT)) SendMessage(&Msg, AutoBacklightMsg, MSG_OPT_NONE);
}
else ShowCall("", pMsg->Options);
break;
case MusicPlayStateMsg:
HandleMusicPlayStateChange(pMsg->Options);
break;
case ChangeModeMsg:
ChangeModeHandler(pMsg->Options);
break;
case ControlFullScreenMsg:
SetProperty(PROP_PHONE_DRAW_TOP, pMsg->Options || *pMsg->pBuffer ? PROP_PHONE_DRAW_TOP : 0);
break;
case ModifyTimeMsg:
ModifyTimeHandler(pMsg);
break;
case MenuModeMsg:
MenuModeHandler(pMsg->Options);
break;
case MenuButtonMsg:
MenuButtonHandler(pMsg->Options);
break;
case EnableButtonMsg:
EnableButtonMsgHandler(pMsg);
break;
case DevTypeMsg:
SetupMessageWithBuffer(&Msg, DevTypeRespMsg, BOARD_TYPE); //default G2
if (Msg.pBuffer != NULL)
{
Msg.pBuffer[0] = BOARD_TYPE; // backward compatible
Msg.Length = 1;
if (GetMsp430HardwareRevision() < 'F')
{
Msg.Options = DIGITAL_WATCH_TYPE_G1;
Msg.pBuffer[0] = DIGITAL_WATCH_TYPE_G1; // backward compatible
}
Msg.Options |= pMsg->Options & 0x80; // support ACK
RouteMsg(&Msg);
}
PrintF("- DevTypeResp:%d", Msg.Options);
// set ACK bit
SendMessage(&Msg, ConnTypeMsg, pMsg->Options);
break;
case VerInfoMsg:
SetupMessageWithBuffer(&Msg, VerInfoRespMsg, MSG_OPT_NONE);
if (Msg.pBuffer != NULL)
{
/* exclude middle '.' */
strncpy((char *)Msg.pBuffer, BUILD, 3);
strncpy((char *)Msg.pBuffer + 3, BUILD + 4, 3);
Msg.Length += strlen(BUILD) - 1;
*(Msg.pBuffer + Msg.Length++) = (unsigned char)atoi(VERSION);
while (VERSION[i++] != '.');
*(Msg.pBuffer + Msg.Length++) = atoi(VERSION + i);
*(Msg.pBuffer + Msg.Length++) = NULL;
RouteMsg(&Msg);
}
PrintS("-Ver:"); for (i = 6; i < Msg.Length; ++i) PrintH(Msg.pBuffer[i]);
break;
case SetVibrateMode:
SetVibrateModeHandler(pMsg);
break;
case SetRtcMsg:
SetRtc((Rtc_t *)pMsg->pBuffer);
#if COUNTDOWN_TIMER
if (CurrentPage[PageType] == CountdownPage && CountdownMode() == COUNTING)
SendMessage(&Msg, CountDownMsg, MSG_OPT_CNTDWN_TIME);
#endif
UpdateClock();
break;
#if COUNTDOWN_TIMER
case CountDownMsg:
if (pMsg->Options == MSG_OPT_NONE)
{
PageType = PAGE_TYPE_INFO;
CurrentPage[PageType] = CountdownPage;
}
DrawCountdownScreen(pMsg->Options);
break;
case SetCountdownDoneMsg:
// for testing
// Rtc_t Done;
// Done.Month = 5;
// Done.Day = 1;
// Done.Hour = 0;
// Done.Minute = 0;
// SetDoneTime(&Done);
// Valid only DoneTime is different
if (SetDoneTime((Rtc_t *)pMsg->pBuffer)) SendMessage(&Msg, CountDownMsg, MSG_OPT_NONE);
break;
#endif
case ServiceMenuMsg:
ServiceMenuHandler();
break;
case DisableButtonMsg:
DisableButtonMsgHandler(pMsg);
break;
case ReadButtonConfigMsg:
ReadButtonConfigHandler(pMsg);
break;
case SetBacklightMsg:
SetBacklight(pMsg->Options);
// testing
// pMsg->Type = AccelMsg;
// pMsg->Options = 1; //enable
// HandleAccelerometer(pMsg);
break;
case AutoBacklightMsg:
if (LightSenseCycle() < DARK_LEVEL) SetBacklight(LED_ON_OPTION);
break;
case BatteryConfigMsg:
SetBatteryLevels(pMsg->pBuffer);
break;
case ReadBatteryVoltageMsg:
ReadBatteryVoltageHandler();
break;
case ResetMsg:
SoftwareResetHandler(pMsg);
break;
case NvalOperationMsg:
NvalOperationHandler(pMsg);
break;
case SecInvertMsg:
HandleSecInvert(pMsg->Options);
break;
case LoadTemplateMsg:
LoadTemplateHandler(pMsg);
break;
case LinkAlarmMsg:
if (LinkAlarmEnable)
{
SetupMessageWithBuffer(&Msg, SetVibrateMode, MSG_OPT_NONE);
if (Msg.pBuffer != NULL)
{
*(tSetVibrateModePayload *)Msg.pBuffer = LnkAlmTone;
RouteMsg(&Msg);
}
}
break;
case ModeTimeoutMsg:
ModeTimeoutHandler();
break;
case WatchStatusMsg:
PageType = PAGE_TYPE_INFO;
CurrentPage[PageType] = StatusPage;
DrawWatchStatusScreen();
break;
// case ListPairedDevicesMsg:
// ListPairedDevicesHandler();
// break;
case WatchDrawnScreenTimeout:
IdleUpdateHandler();
break;
case TermModeMsg:
TermModeHandler();
break;
case LowBatteryWarningMsg:
break;
case LowBatteryBtOffMsg:
UpdateClock();
break;
#if __IAR_SYSTEMS_ICC__
case EraseTemplateMsg:
EraseTemplateHandler(pMsg);
break;
case WriteToTemplateMsg:
WriteToTemplateHandler(pMsg);
break;
#endif
case AccelMsg:
HandleAccelerometer(pMsg);
break;
case ReadLightSensorMsg:
ReadLightSensorHandler();
break;
case RateTestMsg:
SetupMessageWithBuffer(&Msg, DiagnosticLoopback, MSG_OPT_NONE);
if (Msg.pBuffer != NULL)
{
/* don't care what data is */
Msg.Length = 10;
RouteMsg(&Msg);
}
break;
default:
PrintF("# Disp Msg: 0x%02x", pMsg->Type);
break;
}
}