int main()
{
lcm = lcm_create(NULL);
channels_t tx_msg;
channels_t_subscribe(lcm, "CHANNELS_.*_RX", handler_channels_lcm, rxdata);
for(;;) {
lcm_handle(lcm);
//do transformation of incoming data here
for(int i=0; i<8; i++){
txdata[i] = rxdata[i]-500;
}
//make tx msg
tx_msg.utime = utime_now();
tx_msg.num_channels = 8;
for(int i=0; i < tx_msg.num_channels; i++){
tx_msg.channels[i] = txdata[i];
}
//pblish transmint msg
channels_t_publish(lcm, "CHANNELS_1_TX", &tx_msg);
}
lcm_destroy(lcm);
return 0;
}
void spoof_transmitter(void){
channels_t new_msg;
new_msg.utime = 0.0;
new_msg.num_channels = 8;
new_msg.channels = (int16_t*) malloc(new_msg.num_channels*sizeof(int16_t));
for(int i = 0; i < new_msg.num_channels; i++){
new_msg.channels[i] = 0.0;
}
channels_t_publish((lcm_t *) lcm, "CHANNELS_1_RX", &new_msg);
free(new_msg.channels);
}
static void handler_channels(void *usr, uint16_t id, comms_channel_t channel,
const uint8_t *msg, uint16_t len)
{
char lcm_channel[40];
snprintf(lcm_channel, 40, "CHANNELS_%d_RX", id);
verbose_printf("%s: ", lcm_channel);
uint16_t i;
for(i = 0; i < len; ++i)
{
verbose_printf("%x ", msg[i]);
}
verbose_printf("\n");
channels_t channels;
memset(&channels, 0, sizeof(channels_t));
__channels_t_decode_array(msg, 0, len, &channels, 1);
channels_t_publish(lcm, lcm_channel, &channels);
__channels_t_decode_array_cleanup(&channels, 1);
}
void channels_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const channels_t *msg, void *userdata)
{
// create a copy of the received message
channels_t new_msg;
new_msg.utime = msg->utime;
new_msg.num_channels = msg->num_channels;
new_msg.channels = (int16_t*) malloc(msg->num_channels*sizeof(int16_t));
for(int i = 0; i < msg->num_channels; i++)
{
new_msg.channels[i] = msg->channels[i];
}
// set fence switching value based on the data collection from the Quadrotor joystick
double fence_thresh = 1500;
// Copy state to local state struct to minimize mutex lock time
struct state localstate;
pthread_mutex_lock(&state_mutex);
memcpy(&localstate, state, sizeof(struct state));
pthread_mutex_unlock(&state_mutex);
// Decide whether or not to edit the motor message prior to sending it
// set_points[] array is specific to geofencing. You need to add code
// to compute them for our FlightLab application!!!
float pose[8], set_points[8];
// Checking if auto-pilot has been switched on by the pilot
if (msg->channels[7] > fence_thresh)
localstate.fence_on = 1;
else
localstate.fence_on = 0;
if(localstate.fence_on == 1)
{
//printf("FENCE ON\n");
// Cpoy state from processing loop
for(int i = 0; i < 8; i++)
{
pose[i] = (float) localstate.pose[i];
set_points[i] = localstate.set_points[i];
}
// if (prev_local_state.time - localstate.time == 0 || (mcobs[0]->time - mcobs[1]->time)> 1.5)
// Check for data drop outs
if ((!localstate.data) || (prev_local_state.time - localstate.time == 0))
{
// Check if watchdog has not timed out and data has dropped
if (watchdog_count > 0)
watchdog_count--;
// If timed out then stay in base position
if (watchdog_count == 0)
{
printf("WATCHDOG TIMEOUT\n");
new_msg.channels[0] = thrust_PWM_base;
new_msg.channels[1] = roll_PWM_base;
new_msg.channels[2] = pitch_PWM_base;
new_msg.channels[3] = yaw_PWM_base;
}
// If not timed out then still move towards the set point normally with autocontrol
else
{
auto_control(pose, set_points, new_msg.channels);
}
}
// If Data is incoming normally use autocontrol and reset the counters
else
{
auto_control(pose, set_points, new_msg.channels);
watchdog_count++;
// If data starts coming properly then reset watchdog counter
if (watchdog_count > 3)
watchdog_count = 10;
}
printf("%d\n", watchdog_count);
printf( "%d %d %d %d\n", new_msg.channels[0],new_msg.channels[1],new_msg.channels[2],new_msg.channels[3]);
printf( "%f %f %f %f\n", localstate.pose[0],localstate.pose[1],localstate.pose[2],localstate.pose[3]);
sleep(0.5);
}
else
{ // Fence off
// pass user commands through without modifying
// new_msg.channels[7] = 1180;
printf("FENCE OFF\n");
}
// send lcm message to motors
channels_t_publish((lcm_t *) userdata, "CHANNELS_1_TX", &new_msg);
// Save received (msg) and modified (new_msg) command data to file.
// NOTE: Customize as needed (set_points[] is for geofencing)
fprintf(block_txt,"%ld,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f\n",
(long int) msg->utime,msg->channels[0],msg->channels[1],msg->channels[2],
msg->channels[3], msg->channels[7],
new_msg.channels[0],new_msg.channels[1],new_msg.channels[2],
new_msg.channels[3],new_msg.channels[7],
set_points[0],set_points[1],set_points[2],
set_points[3],set_points[4],set_points[5],set_points[6],
set_points[7]);
fflush(block_txt);
pthread_mutex_lock(&state_mutex);
memcpy(state, &localstate, sizeof(struct state));
pthread_mutex_unlock(&state_mutex);
}
