void Timer1_IRQHandler(void)
{
// FIXME: Reduce the length of this time critical section
uint8_t pkt_len = TX_CTRL->PAY_LEN;
int i;
// Prepare packet
pkt.src_addr = node_id;
pkt.seq_num[0] = (seq_num >> 8) & 0xFF;
pkt.seq_num[1] = seq_num & 0xFF;
for (i = 0; i < pkt_len-5; i++)
{
pkt.payload[i] = lfsr_rand();
// pkt.payload[i] = 0x00;
// pkt.payload[i] = i & 0xFF;
}
set_packet_crc(&pkt, pkt_len);
send_packet(&pkt, pkt_len-5);
seq_num++;
// TX_CTRL->CTRL = 0x01; // Start
// Turn on LED
MSS_GPIO_set_output(MSS_GPIO_LED1, 1);
MSS_TIM2_load_immediate(1e2 * MICRO_SEC_DIV);
MSS_TIM2_start();
MSS_TIM1_clear_irq();
}
void Timer1_IRQHandler( void )
{
int i;
//average gyro readings
static int16_t gyro_avgs[NUMBER_SAMPLES];
static int index_gyro = 0;
gyro_avgs[index_gyro] = X_GYRO;
index_gyro++;
if (index_gyro >= NUMBER_SAMPLES) index_gyro = 0;
int32_t total_gyro = 0;
for (i = 0; i < NUMBER_SAMPLES; ++i) {
total_gyro += gyro_avgs[i];
}
float gyro_avg = total_gyro / NUMBER_SAMPLES;
float gyro_angle = (gyro_avg ) / 131;
//average accl readings
static int16_t accl_avgs[NUMBER_SAMPLES];
static int index_accl = 0;
accl_avgs[index_accl] = Y_ACCL;
index_accl++;
if (index_accl >= NUMBER_SAMPLES) index_accl = 0;
int32_t total_accl = 0;
for (i = 0; i < NUMBER_SAMPLES; ++i) {
total_accl += accl_avgs[i];
}
float accl_avg = total_accl / NUMBER_SAMPLES;
float accl_angle = (accl_avg * 90) / 16384;
//combine accel and gyro
angle = (0.98) * (angle + gyro_angle * dt) + (0.02) * accl_angle;
//printf("angle! : %f\r\n", angle);
MSS_TIM1_clear_irq();
}
/*-----------------------------------------------------------------------------
* Service MAVlink protocol message routine
*/
void Timer1_IRQHandler() {
mavlink_message_t msg;
MadgwickAHRSupdateIMU(
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val);
mavlink_msg_heartbeat_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_TYPE_GROUND_ROVER,
MAV_AUTOPILOT_GENERIC,
mavlink_sys_mode,
0,
mavlink_sys_state);
mavlink_send_msg(&msg);
mavlink_msg_sys_status_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
50, // TODO remove hardoded values
0,
-1,
-1,
0,
0,
0,
0,
0,
0);
mavlink_send_msg(&msg);
mavlink_msg_autopilot_version_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
0, // No capabilities (MAV_PROTOCOL_CAPABILITY). TBD
42,
42,
42,
42,
"DEADBEEF",
"DEADBEEF",
"DEADBEEF",
42,
42,
42);
mavlink_send_msg(&msg);
mavlink_msg_highres_imu_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val,
params[PARAM_GX].val,
params[PARAM_GY].val,
params[PARAM_GZ].val,
params[PARAM_MX].val,
params[PARAM_MY].val,
params[PARAM_MZ].val,
0,
0,
0,
params[PARAM_T].val,
(1 << 12) | ((1 << 9) - 1));
mavlink_send_msg(&msg);
mavlink_msg_attitude_quaternion_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
q0,
q1,
q2,
q3,
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val));
mavlink_send_msg(&msg);
usec_service_routine();
MSS_TIM1_clear_irq();
}
