void mcu_main()
{
int version = api_version();
debug_print(DBG_INFO, "API version: %d.%d\n", version/100, version%100);
gpio_setup(GPIO_DHT, OUTPUT);
gpio_write(GPIO_DHT, HIGH);
/* do not send instructions within 1S after power on to pass the unstable state */
mcu_sleep(100);
uint8_t data[DHT_DATA_LEN];
while (1) {
int rc;
uint8_t humidity, temperature;
if ((rc = dht11_read(GPIO_DHT, data, sizeof(data))) == DHT_SUCCESS) {
humidity = data[0], temperature = data[2];
debug_print(DBG_INFO, "Humidity: %d%, Temperature: %d C\n", humidity, temperature);
// debug_print(DBG_INFO, "%d %d %d %d %d\n",
// data[0], data[1], data[2], data[3], data[4]);
} else {
debug_print(DBG_ERROR, "dht11_read() error [%d]\n", rc);
}
unsigned char buf[LEN_IPCBUF];
if ((rc = host_receive(buf, sizeof(buf))) > 0 && buf[0] == '?') {
rc = mcu_snprintf((char*)buf, LEN_IPCBUF, "RH=%d,T=%d\n", humidity, temperature);
host_send(buf, rc);
}
mcu_sleep(300);
}
}
void mcu_main()
{
int value;
while(1)
{
value = ADC_read();
len = mcu_snprintf(buf, 32,"%d\n",value);
host_send((unsigned char*)buf, len);
//This will prevent the atom from freezing
mcu_sleep(1);
}
}
int main(void)
{
int i;
uint16_t sensor_sample_count;
struct sensor_s sensor;
struct angle_s angle;
uint16_t radio_frame_count;
struct radio_raw_s radio_raw;
struct radio_s radio;
float radio_pitch_smooth;
float radio_roll_smooth;
float error_pitch;
float error_roll;
float error_yaw;
float error_pitch_z;
float error_roll_z;
float error_yaw_z;
float p_pitch;
float i_pitch;
float d_pitch;
float p_roll;
float i_roll;
float d_roll;
float pitch_p_term;
float pitch_i_term;
float pitch_d_term;
float roll_p_term;
float roll_i_term;
float roll_d_term;
float yaw_p_term;
float yaw_i_term;
float yaw_d_term;
float pitch;
float roll;
float yaw;
float motor[4];
int32_t motor_clip[4];
uint32_t motor_raw[4];
uint16_t vbat_sample_count;
int32_t t1;
int32_t t2;
_Bool flag_acro_z;
_Bool error;
uint16_t sensor_sample_count1;
host_buffer_tx_t host_buffer_tx;
/* Variable initialisation -----------------------------------------------------*/
flag_sensor = 0;
flag_radio = 0;
flag_vbat = 0;
flag_rf = 0;
flag_host = 0;
flag_sensor_host_read = 0;
flag_rf_host_read = 0;
flag_timeout_sensor = 0;
flag_timeout_radio = 0;
flag_armed = 0;
flag_acro = 1;
flag_acro_z = 0;
flag_beep_user = 0;
flag_beep_radio = 0;
flag_beep_sensor = 0;
flag_beep_host = 0;
flag_beep_vbat = 0;
sensor_sample_count = 0;
sensor_error_count = 0;
angle.pitch = 0;
angle.roll = 0;
radio_frame_count = 0;
radio_error_count = 0;
radio_pitch_smooth = 0;
radio_roll_smooth = 0;
pitch_i_term = 0;
roll_i_term = 0;
yaw_i_term = 0;
vbat_sample_count = 0;
rf_error_count = 0;
sensor_sample_count1 = 0;
/* Setup -----------------------------------------------------*/
board_init(); // BOARD_DEPENDENT
SysTick->CTRL &= ~SysTick_CTRL_TICKINT_Msk; // Disable Systick interrupt, not needed anymore (but can still use COUNTFLAG)
reg_init();
/* Loop ----------------------------------------------------------------------------
-----------------------------------------------------------------------------------*/
while (1)
{
// Processing time
t1 = (int32_t)get_timer_process();
/* Process radio commands -----------------------------------------------------*/
if (flag_radio)
{
flag_radio = 0;
// Decode radio commands
error = radio_decode(&radio_frame, &radio_raw, &radio);
if (error) {
radio_error_count++;
radio_synch();
}
else
{
reset_timeout_radio();
flag_beep_radio = 0; // Stop beeping
radio_frame_count++;
// Arm procedure
if (radio.aux[0] < 0.33f)
flag_armed = 0;
else if (!flag_armed && (radio.aux[0] > 0.33f) && ((radio.throttle < 0.01f)))
flag_armed = 1;
if (radio.aux[0] < 0.66f)
flag_acro = 1;
else
flag_acro = 0;
// Expo and smooth
radio_expo(&radio, flag_acro);
// Beep if requested
if (radio.aux[1] > 0.33f)
flag_beep_user = 1;
else
flag_beep_user = 0;
// Send data to host
if ((REG_DEBUG__CASE > 0) && ((radio_frame_count & REG_DEBUG__MASK) == 0)) {
if (REG_DEBUG__CASE == 4)
host_send((uint8_t*)&radio_raw, sizeof(radio_raw));
else if (REG_DEBUG__CASE == 5)
host_send((uint8_t*)&radio, sizeof(radio));
}
// Toggle LED at rate of Radio flag
if ((radio_frame_count & 0x3F) == 0)
toggle_led_radio();
}
}
/* Process sensors -----------------------------------------------------------------------*/
if (flag_sensor)
{
flag_sensor = 0;
sensor_sample_count++;
flag_beep_sensor = 0; // Disable beeping
// Record sensor transaction time
t2 = (int32_t)timer_sensor[1] - (int32_t)timer_sensor[0];
if (t2 < 0)
t2 += 0xFFFF;
if ((REG_CTRL__TIME_MAXHOLD == 0) || (((uint16_t)t2 > time_sensor) && REG_CTRL__TIME_MAXHOLD))
time_sensor = (uint16_t)t2;
// Recovery time before activating yaw agnle transfer
if (flag_acro != flag_acro_z)
sensor_sample_count1 = 0;
else if (sensor_sample_count1 < RECOVERY_TIME)
sensor_sample_count1++;
// Procees sensor data
mpu_process_samples(&sensor_raw, &sensor);
// Estimate angle
angle_estimate(&sensor, &angle, (sensor_sample_count1 == RECOVERY_TIME));
// Smooth pitch and roll commands in angle mode
if (!flag_acro) {
radio_pitch_smooth += filter_alpha_radio * radio.pitch - filter_alpha_radio * radio_pitch_smooth;
radio_roll_smooth += filter_alpha_radio * radio.roll - filter_alpha_radio * radio_roll_smooth;
}
// Previous error
error_pitch_z = error_pitch;
error_roll_z = error_roll;
error_yaw_z = error_yaw;
// Current error
if (flag_acro) {
error_pitch = sensor.gyro_x - radio.pitch * (float)REG_RATE__PITCH_ROLL;
error_roll = sensor.gyro_y - radio.roll * (float)REG_RATE__PITCH_ROLL;
}
else {
error_pitch = angle.pitch - radio_pitch_smooth * (float)REG_RATE__ANGLE;
error_roll = angle.roll - radio_roll_smooth * (float)REG_RATE__ANGLE;
}
error_yaw = sensor.gyro_z - radio.yaw * (float)REG_RATE__YAW;
// Switch PID coefficients for acro
if (flag_acro != flag_acro_z) {
if (flag_acro) {
p_pitch = REG_P_PITCH;
i_pitch = REG_I_PITCH;
d_pitch = REG_D_PITCH;
p_roll = REG_P_ROLL;
i_roll = REG_I_ROLL;
d_roll = REG_D_ROLL;
}
else {
p_pitch = REG_P_PITCH_ANGLE;
i_pitch = REG_I_PITCH_ANGLE;
d_pitch = REG_D_PITCH_ANGLE;
p_roll = REG_P_ROLL_ANGLE;
i_roll = REG_I_ROLL_ANGLE;
d_roll = REG_D_ROLL_ANGLE;
}
}
// P,I and D
pitch_p_term = error_pitch * p_pitch;
roll_p_term = error_roll * p_roll;
yaw_p_term = error_yaw * REG_P_ROLL;
if ((!flag_armed && (REG_CTRL__ARM_TEST == 0)) || (flag_acro != flag_acro_z)) {
pitch_i_term = 0;
roll_i_term = 0;
}
else {
pitch_i_term += error_pitch * i_pitch;
roll_i_term += error_roll * i_roll;
}
if (!flag_armed && (REG_CTRL__ARM_TEST == 0))
yaw_i_term = 0;
else
yaw_i_term += error_yaw * REG_I_YAW;
pitch_d_term = (error_pitch - error_pitch_z) * d_pitch;
roll_d_term = (error_roll - error_roll_z) * d_roll;
yaw_d_term = (error_yaw - error_yaw_z) * REG_D_YAW;
flag_acro_z = flag_acro;
// Clip I
if (pitch_i_term < -I_MAX) pitch_i_term = -I_MAX;
else if (pitch_i_term > I_MAX) pitch_i_term = I_MAX;
if (roll_i_term < -I_MAX) roll_i_term = -I_MAX;
else if (roll_i_term > I_MAX) roll_i_term = I_MAX;
if (yaw_i_term < -I_MAX) yaw_i_term = -I_MAX;
else if (yaw_i_term > I_MAX) yaw_i_term = I_MAX;
// P+I+D
pitch = pitch_p_term + pitch_i_term + pitch_d_term;
roll = roll_p_term + roll_i_term + roll_d_term;
yaw = yaw_p_term + yaw_i_term + yaw_d_term;
// Clip P+I+D
if (pitch < -PID_MAX) pitch = -PID_MAX;
else if (pitch > PID_MAX) pitch = PID_MAX;
if (roll < -PID_MAX) roll = -PID_MAX;
else if (roll > PID_MAX) roll = PID_MAX;
if (yaw < -PID_MAX) yaw = -PID_MAX;
else if (yaw > PID_MAX) yaw = PID_MAX;
// Desactivate throttle when arm test
if (REG_CTRL__ARM_TEST > 0)
radio.throttle = 0;
// Motor matrix
motor[0] = radio.throttle * (float)REG_MOTOR__RANGE + roll + pitch - yaw;
motor[1] = radio.throttle * (float)REG_MOTOR__RANGE + roll - pitch + yaw;
motor[2] = radio.throttle * (float)REG_MOTOR__RANGE - roll - pitch - yaw;
motor[3] = radio.throttle * (float)REG_MOTOR__RANGE - roll + pitch + yaw;
// Offset and clip motor value
for (i=0; i<4; i++) {
motor_clip[i] = (int32_t)motor[i] + (int32_t)REG_MOTOR__ARMED;
if (motor_clip[i] < (int32_t)REG_MOTOR__START)
motor_clip[i] = (int32_t)REG_MOTOR__START;
else if (motor_clip[i] > (int32_t)MOTOR_MAX)
motor_clip[i] = (int32_t)MOTOR_MAX;
}
// Motor command
for (i=0; i<4; i++) {
if (REG_MOTOR_TEST__SELECT & (1 << i))
motor_raw[i] = (uint32_t)REG_MOTOR_TEST__VALUE;
else if (flag_armed || (REG_CTRL__ARM_TEST > 0))
motor_raw[i] = (uint32_t)motor_clip[i];
else
motor_raw[i] = 0;
}
set_motors(motor_raw);
// Send data to host
if ((REG_DEBUG__CASE > 0) && ((sensor_sample_count & REG_DEBUG__MASK) == 0)) {
if (REG_DEBUG__CASE == 1)
host_send((uint8_t*)&sensor_raw.bytes[1], sizeof(sensor_raw)-1);
else if (REG_DEBUG__CASE == 2)
host_send((uint8_t*)&sensor, sizeof(sensor));
else if (REG_DEBUG__CASE == 3)
host_send((uint8_t*)&angle, sizeof(angle));
else if (REG_DEBUG__CASE == 6) {
host_buffer_tx.f[0] = pitch;
host_buffer_tx.f[1] = roll;
host_buffer_tx.f[2] = yaw;
host_send(host_buffer_tx.u8, 3*4);
}
else if (REG_DEBUG__CASE == 7)
host_send((uint8_t*)&motor_raw, sizeof(motor_raw));
}
// Toggle LED at rate of sensor flag
if ((sensor_sample_count & 0x01FF) == 0)
toggle_led_sensor();
}
/* VBAT ---------------------------------------------------------------------*/
if (flag_vbat)
{
flag_vbat = 0;
vbat_sample_count++;
REG_VBAT += filter_alpha_vbat * get_vbat() - filter_alpha_vbat * REG_VBAT;
// Send VBAT to host
if ((REG_DEBUG__CASE == 8) && ((vbat_sample_count & REG_DEBUG__MASK) == 0))
host_send((uint8_t*)®_VBAT,4);
// Beep if VBAT too low
if ((REG_VBAT < REG_VBAT_MIN) && (REG_VBAT > 8.0f))
flag_beep_vbat = 1;
else
flag_beep_vbat = 0;
}
/* Host requests ------------------------------------------------------------------*/
if (flag_host)
{
flag_host = 0;
reg_access(&host_buffer_rx);
}
/* Handle timeout -----------------------------------------------------------------------*/
if (flag_timeout_sensor) {
flag_timeout_sensor = 0;
for (i=0; i<4; i++)
motor_raw[i] = 0;
set_motors(motor_raw);
flag_beep_sensor = 1;
}
if (flag_timeout_radio) {
flag_timeout_radio = 0;
flag_armed = 0;
flag_beep_radio = 1;
}
/*------------------------------------------------------------------*/
// Record processing time
t1 = (int32_t)get_timer_process() - t1;
if (t1 < 0)
t1 += 0xFFFF;
if ((REG_CTRL__TIME_MAXHOLD == 0) || (((uint16_t)t1 > time_process) && REG_CTRL__TIME_MAXHOLD))
time_process = (uint16_t)t1;
// Wait for interrupts if all flags are processed
if (!flag_radio && !flag_sensor && !flag_vbat && !flag_host && !flag_timeout_sensor && !flag_timeout_radio)
__wfi();
}
}
void mcu_main()
{
// by default, the edison has ID 0
int edisonID = 0;
// if we pass in an argument, use it for the edisonID
// please pass in an integer
//if (argc == 2) {
//edisonID = atoi(argv[1]);
//}
int temp;
while (1)
{
//int host_receive(unsigned char *buf, int length)
temp = host_receive((unsigned char *)host_message, BUFFER_LENGTH);
if (temp > 0)
{
debug_print(DBG_INFO, "Received a Message!\n");
host_send((unsigned char*)"hello mcu\n", 10);
preamble_length = host_message[0];
}
// Preamble - Signals the Receiver Message Incoming
send_preamble_sequence(preamble_length);
// Sending Edison Board ID # - 2 bits, MSB then LSB
switch (edisonID) {
case 0:
send_low_bit(); // Send lsb bit 0 = LOW
send_low_bit(); // Send msb bit 1 = LOW
break;
case 1:
send_high_bit(); // Send lsb bit 0 = HIGH
send_low_bit(); // Send msb bit 1 = LOW
break;
case 2:
send_low_bit(); // Send lsb bit 0 = LOW
send_high_bit(); // Send msb bit 1 =
break;
case 3:
send_high_bit(); // Send lsb bit 0 = HIGH
send_high_bit(); // Send msb bit 1 = HIGH
break;
default:
send_low_bit(); // Send lsb bit 0 = LOW
send_low_bit(); // Send msb bit 1 = LOW
}
// Sending Edison IR Emitter ID # - 2 bits, MSB then LSB
// pwm1 = 00 = short-long/short-long = 5-20/5-20
// pwm2 = 01 = short-long/long-short = 5-20/20-5
// pwm3 = 10 = long-short/short-long = 20-5/5-20
// pwm4 = 11 = long-short/long-short = 20-5/20-5
// First Bit
pwm_configure(pwm1, MESSAGE_DUTY_LOW, MESSAGE_PERIOD); // 0
pwm_configure(pwm2, MESSAGE_DUTY_LOW, MESSAGE_PERIOD); // 0
pwm_configure(pwm3, MESSAGE_DUTY_HIGH, MESSAGE_PERIOD); // 1
pwm_configure(pwm4, MESSAGE_DUTY_HIGH, MESSAGE_PERIOD); // 1
pwm_enable(pwm1);
pwm_enable(pwm2);
pwm_enable(pwm3);
pwm_enable(pwm4);
mcu_delay(MESSAGE_SLEEP);
// Second Bit
pwm_configure(pwm1, MESSAGE_DUTY_LOW, MESSAGE_PERIOD); // 0
pwm_configure(pwm2, MESSAGE_DUTY_HIGH, MESSAGE_PERIOD); // 1
pwm_configure(pwm3, MESSAGE_DUTY_LOW, MESSAGE_PERIOD); // 0
pwm_configure(pwm4, MESSAGE_DUTY_HIGH, MESSAGE_PERIOD); // 1
pwm_enable(pwm1);
pwm_enable(pwm2);
pwm_enable(pwm3);
pwm_enable(pwm4);
mcu_delay(MESSAGE_SLEEP);
}
}
