void Acs300SendThrottleCommand(int16_t command)
{
CanMessage msg;
// If the commanded current is != 0, also make sure the drive's in run mode. Otherwise if it's
// 0, put the ACS300 into standby.
if (command != 0) {
Acs300PackageVelocityCommand(&msg, 0, 0, ACS300_COMMAND_RUN);
Ecan1Transmit(&msg);
} else {
Acs300PackageVelocityCommand(&msg, 0, 0, ACS300_COMMAND_STANDBY);
Ecan1Transmit(&msg);
}
Acs300PackageWriteParam(&msg, ACS300_PARAM_CC, command);
Ecan1Transmit(&msg);
}
void BallastSendNmea(void)
{
// Set CAN header information.
CanMessage msg;
PackagePgn127245(&msg, nodeId, 0xFF, 0xF, NAN, ballastSensorData.BallastPositionAngle);
// And finally transmit it.
Ecan1Transmit(&msg);
}
void BallastSendCustomLimit(void)
{
CanMessage msg;
CanMessagePackageBallastDetails(&msg, ballastSensorData.PotValue,
ballastCalData.PortLimitValue,
ballastCalData.StarLimitValue,
ballastSensorData.PortLimit,
ballastSensorData.StarLimit,
true,
ballastCalData.Calibrated,
ballastCalData.Calibrating
);
Ecan1Transmit(&msg);
}
void BallastSendTemperature(void)
{
// Specify a new CAN message w/ metadata
CanMessage msg;
// Send temp values:
// * SID: invalid
// * Temperature instance: Inside
// * Humidity instance: Invalid
// * Humidity: Invalid
// * Pressure: Invalid
PackagePgn130311(&msg,
nodeId,
PGN_SID_INVALID,
PGN130311_TEMP_INST_INSIDE,
PGN130311_HUMID_INST_INVALID,
ballastSensorData.Temperature,
NAN,
NAN);
// And finally transmit it.
Ecan1Transmit(&msg);
}
void CommandTransmit(const void *d)
{
if (opMode == ACTIVE) {
Ecan1Transmit((CanMessage*)d);
}
}
void HilNodeTimer100Hz(void)
{
// Track the messages to be transmit for this timestep.
// Here we emulate the same transmission frequency of the messages actually transmit
// by the onboard sensors.
static uint8_t msgs[ECAN_MSGS_SIZE];
// Check the status of the rudder, setting it if it's changed.
if (++rudderTimeoutCounter >= RUDDER_TIMEOUT_PERIOD) {
nodeStatus &= ~NODE_STATUS_FLAG_RUDDER_ACTIVE;
}
// We don't do any transmission of CAN messages if we're inactive.
if (!HIL_IS_ACTIVE()) {
return;
}
uint8_t messagesToSend = GetMessagesForTimestep(&sched, msgs);
int i;
CanMessage msg;
for (i = 0; i < messagesToSend; ++i) {
switch (msgs[i]) {
// Emulate the RC node
case SCHED_ID_RC_STATUS:
CanMessagePackageStatus(&msg, CAN_NODE_RC, 0, 0, 0);
Ecan1Transmit(&msg);
break;
// Emulate the rudder node
case SCHED_ID_RUDDER_ANGLE:
if (!(nodeStatus & NODE_STATUS_FLAG_RUDDER_ACTIVE)) {
PackagePgn127245(&msg, nodeId, 0xFF, 0xF, NAN, hilReceivedData.data.rAngle);
Ecan1Transmit(&msg);
}
break;
case SCHED_ID_RUDDER_LIMITS:
if (!(nodeStatus & NODE_STATUS_FLAG_RUDDER_ACTIVE)) {
CanMessagePackageRudderDetails(&msg, 0, 0, 0, false, false, true, true, false);
Ecan1Transmit(&msg);
}
break;
// Emulate the ACS300
case SCHED_ID_THROTTLE_STATUS:
Acs300PackageHeartbeat(&msg, (uint16_t)hilReceivedData.data.tSpeed, 0, 0, 0);
Ecan1Transmit(&msg);
break;
// Emulate the GPS200
case SCHED_ID_LAT_LON:
PackagePgn129025(&msg, nodeId, hilReceivedData.data.gpsLatitude, hilReceivedData.data.gpsLongitude);
Ecan1Transmit(&msg);
break;
case SCHED_ID_COG_SOG:
PackagePgn129026(&msg, nodeId, 0xFF, 0x7, hilReceivedData.data.gpsCog, hilReceivedData.data.gpsSog);
Ecan1Transmit(&msg);
break;
case SCHED_ID_GPS_FIX:
PackagePgn129539(&msg, nodeId, 0xFF, PGN_129539_MODE_3D, PGN_129539_MODE_3D, 100, 100, 100);
Ecan1Transmit(&msg);
break;
}
}
}
void SendThrottleCommand(int16_t command)
{
CanMessage msg;
Acs300PackageVelocityCommand(&msg, 0, command, 0);
Ecan1Transmit(&msg);
}
void Run100HzTasks(void)
{
// Track the tasks to be performed for this timestep.
static uint8_t msgs[NUM_TASKS];
// Increment sensor availability timeout counters.
if (sensorAvailability.imu.enabled_counter < SENSOR_TIMEOUT) {
++sensorAvailability.imu.enabled_counter;
}
if (sensorAvailability.imu.active_counter < SENSOR_TIMEOUT) {
++sensorAvailability.imu.active_counter;
}
uint8_t messagesToSend = GetMessagesForTimestep(&taskSchedule, msgs);
int i;
for (i = 0; i < messagesToSend; ++i) {
switch (msgs[i]) {
case TASK_TRANSMIT_IMU: {
CanMessage msg;
// Transmit the absolute attitude message
CanMessagePackageImuData(&msg,
tokimecData.yaw,
tokimecData.pitch,
tokimecData.roll);
Ecan1Transmit(&msg);
// Now transmit the angular velocity data
CanMessagePackageAngularVelocityData(&msg,
tokimecData.x_angle_vel,
tokimecData.y_angle_vel,
tokimecData.z_angle_vel);
Ecan1Transmit(&msg);
// And then the accelerometer data
CanMessagePackageAccelerationData(&msg,
tokimecData.x_accel,
tokimecData.y_accel,
tokimecData.z_accel);
Ecan1Transmit(&msg);
// And now the position data
CanMessagePackageGpsPosData(&msg,
tokimecData.latitude,
tokimecData.longitude);
Ecan1Transmit(&msg);
// And its estimated position data
CanMessagePackageEstGpsPosData(&msg,
tokimecData.est_latitude,
tokimecData.est_longitude);
Ecan1Transmit(&msg);
// And finally a few random data bits
CanMessagePackageGpsVelData(&msg,
tokimecData.gpsDirection,
tokimecData.gpsSpeed,
tokimecData.magneticBearing,
tokimecData.status);
Ecan1Transmit(&msg);
} break;
case TASK_TRANSMIT_STATUS:
NodeTransmitStatus();
break;
case TASK_BLINK: // Blink the status LED at 1Hz
_LATA4 ^= 1;
break;
}
}
// And update sensor availability.
if (sensorAvailability.imu.enabled && sensorAvailability.imu.enabled_counter >= SENSOR_TIMEOUT) {
sensorAvailability.imu.enabled = false;
// When the IMU is no longer connected, blink at a regular rate.
RemoveMessage(&taskSchedule, TASK_BLINK);
AddMessageRepeating(&taskSchedule, TASK_BLINK, RATE_TRANSMIT_BLINK_DEFAULT);
// When the IMU is no longer connected, no longer transmit IMU messages.
RemoveMessage(&taskSchedule, TASK_TRANSMIT_IMU);
// Also update our status.
nodeStatus &= ~IMU_NODE_STATUS_FLAG_IMU_ACTIVE;
} else if (!sensorAvailability.imu.enabled && sensorAvailability.imu.enabled_counter < SENSOR_TIMEOUT) {
sensorAvailability.imu.enabled = true;
// When the IMU is connected, blink a little faster.
RemoveMessage(&taskSchedule, TASK_BLINK);
AddMessageRepeating(&taskSchedule, TASK_BLINK, RATE_TRANSMIT_BLINK_CONNECTED);
// When the IMU is reconnected, transmit IMU messages.
AddMessageRepeating(&taskSchedule, TASK_TRANSMIT_IMU, RATE_TRANSMIT_IMU_DATA);
// Also update our status.
nodeStatus |= IMU_NODE_STATUS_FLAG_IMU_ACTIVE;
}
}