int main(void) {
UINT8 mpu_address = MPU6050_ADDRESS_AD0_LOW;
initAll();
UINT8 acc[2] = {0};
INT16 acc16 = 0;
float acc_g = 0;
UINT8 data = 0;
while(TRUE) {
requestReadBytes(MPU6050_ADDRESS_AD0_LOW, (UINT8)MPU6050_RA_ACCEL_XOUT_H, acc, 2); // burst read two bytes.
PORTToggleBits(IOPORT_F, BIT_0); // LED5.
DelayMs(1000);
acc16 = (INT16)((acc[0] << 8) | acc[1]);
acc_g = (float)acc16 / 16384.0;
printf("\n\rACC X: %f\n\r", acc_g);
}
return (EXIT_SUCCESS);
}
void LED_Toggle (CPU_INT08U led)
{
switch (led) {
case 0:
#ifdef _TARGET_440H
PORTToggleBits(IOPORT_D, BIT_6 | BIT_7);
PORTToggleBits(IOPORT_F, BIT_0 | BIT_1);
#else
//PORTToggleBits(IO_LED1);
//PORTToggleBits(IO_LED2);
PORTToggleBits(IO_LED3);
PORTToggleBits(IO_LED4);
PORTToggleBits(IO_LED5);
#endif
break;
case 1:
#ifdef _TARGET_440H
PORTToggleBits(IOPORT_D, BIT_7);
#else
//PORTToggleBits(IO_LED1);
#endif
break;
case 2:
#ifdef _TARGET_440H
PORTToggleBits(IOPORT_D, BIT_6);
#else
//PORTToggleBits(IO_LED2);
#endif
break;
case 3:
#ifdef _TARGET_440H
PORTToggleBits(IOPORT_F, BIT_0);
#else
PORTToggleBits(IO_LED3);
#endif
break;
case 4:
#ifdef _TARGET_440H
PORTToggleBits(IOPORT_F, BIT_1);
#else
PORTToggleBits(IO_LED4);
#endif
break;
case 5:
#ifdef _TARGET_440H
#else
PORTToggleBits(IO_LED5);
#endif
break;
default:
break;
}
}
virtual void toggle() {PORTToggleBits(ltr(), num());}
static void MOTtask(void *pvParameters) {
int commsStatsCount = commStatsSecs;
int initReply = 0;
bool initialized = false;
//start subsystem tasks
initReply += SysLogInit();
initReply += PubSubInit();
initReply += NotificationsInit();
initReply += UARTBrokerInit();
initReply += PidInit(); //Controls the motor PWM
initReply += EncoderInit(); //Measures motor speed
//initReply += AmpsInit(); //Measures motor current
while ((motQueue = psNewPubSubQueue(MOT_TASK_QUEUE_LENGTH)) == NULL) {
SetCondition(MOT_INIT_ERROR);
LogError( "Motor Task Q");
initReply = -1;
}
if (initReply < 0) {
SetCondition(MOT_INIT_ERROR);
DebugPrint("MOT Init Error");
while (1) {
vTaskDelay(100);
PORTToggleBits(USER_LED_IOPORT, USER_LED_BIT);
}
}
CancelCondition(MOT_INIT_ERROR);
CancelCondition(MOTORS_INHIBIT);
CancelCondition(MOTORS_BUSY);
CancelCondition(MOT_ANALOG_ERROR);
CancelCondition(MOTORS_ERRORS);
CancelCondition(MOT_MCP_COMMS_ERRORS);
CancelCondition(MOT_MCP_COMMS_CONGESTION);
{
psMessage_t msg;
psInitPublish(msg, SS_ONLINE);
strcpy(msg.responsePayload.subsystem, "MOT");
msg.responsePayload.flags = RESPONSE_FIRST_TIME;
psSendMessage(msg);
}
DebugPrint("MOT Up");
for (;;) {
//wait for a message
if (xQueueReceive(motQueue, &motRxMsg, portMAX_DELAY) == pdTRUE) {
switch (motRxMsg.header.messageType) {
case TICK_1S:
powerState = motRxMsg.tickPayload.systemPowerState;
if (--commsStatsCount <= 0) {
commsStatsCount = commStatsSecs;
if (commStats) {
UARTSendStats();
} else {
UARTResetStats();
}
}
break;
case CONFIG:
if (motRxMsg.configPayload.responder == MOTOR_SUBSYSTEM) {
DebugPrint("Config message received");
int requestor = motRxMsg.configPayload.requestor;
configCount = 0;
#define optionmacro(name, var, minV, maxV, def) SendOptionConfig(name, &var, minV, maxV, requestor);
#include "Options.h"
#undef optionmacro
#define settingmacro(name, var, minV, maxV, def) SendSettingConfig(name, &var, minV, maxV, requestor);
#include "Settings.h"
#undef settingmacro
//report Config done
psInitPublish(motRxMsg, CONFIG_DONE);
motRxMsg.configPayload.requestor = requestor;
motRxMsg.configPayload.responder = MOTOR_SUBSYSTEM;
motRxMsg.configPayload.count = configCount;
psSendMessage(motRxMsg);
DebugPrint("Config sent");
}
break;
case SET_OPTION: //option change
DebugPrint("New Option %s = %i", motRxMsg.optionPayload.name, motRxMsg.optionPayload.value);
#define optionmacro(n, var, minV, maxV, def) SetOption(&motRxMsg, n, &var, minV, maxV);
#include "Options.h"
#undef optionmacro
break;
case NEW_SETTING: //setting change
DebugPrint("New Setting %s = %f", motRxMsg.settingPayload.name, motRxMsg.settingPayload.value);
#define settingmacro(n, var, minV, maxV, def) NewSetting(&motRxMsg, n, &var, minV, maxV);
#include "Settings.h"
#undef settingmacro
break;
case PING_MSG:
{
psMessage_t msg2;
DebugPrint("Ping Msg");
// PORTToggleBits(USER_LED_IOPORT, USER_LED_BIT);
psInitPublish(msg2, PING_RESPONSE)
strncpy(msg2.responsePayload.subsystem, "MOT", 3);
msg2.responsePayload.flags = (initialized ? 0 : RESPONSE_FIRST_TIME);
msg2.responsePayload.requestor = motRxMsg.requestPayload.requestor;
psSendMessage(msg2);
}
break;
case GEN_STATS:
DebugPrint("Send Stats\n");
GenerateRunTimeTaskStats();
GenerateRunTimeSystemStats();
break;
default:
break;
}
}
}
}
/**
* @fn char gpioLed( void );
* @brief Mise en place du clignotement de la led sans charge utile connectée
*/
char gpioLed( void ) {
if (charge == 0)
PORTToggleBits( IOPORT_D, BIT_1 );
}
