int main()
{
// SET single LED port
DDRC |= _BV(LED_PIN);
PORTC ^= _BV(LED_PIN);
// 1s pull up of LED Strip pins */
apa102_DDRREG &= ~_BV(apa102_data);
apa102_DDRREG &= ~_BV(apa102_clk);
apa102_PORTREG |= _BV(apa102_data);
apa102_PORTREG |= _BV(apa102_clk);
_delay_ms(900);
// Disable pull ups
apa102_PORTREG &= ~_BV(apa102_data);
apa102_PORTREG &= ~_BV(apa102_clk);
apa102_DDRREG |= _BV(apa102_data);
apa102_DDRREG |= _BV(apa102_clk);
_delay_ms(100);
PORTC ^= _BV(LED_PIN);
// Clear LEDs in the strip
clear_all_leds();
// Init ROS
nh.initNode();
nh.subscribe(set_sub);
nh.subscribe(set_led_sub);
ack_led();
// Wait for Server side to start
while (!nh.connected())
{
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
_delay_ms(10);
}
ack_led();
// Publish some debug information
snprintf(log_str, MAX_MSG_SIZE, "V:%s", GIT_VERSION);
nh.loginfo(log_str);
snprintf(log_str, MAX_MSG_SIZE, "FM:%d", get_free_ram());
nh.loginfo(log_str);
while(1)
{
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
}
return 0;
}
void set_cb(const autonomy_leds_msgs::Command& cmd_msg)
{
switch (cmd_msg.flag)
{
case autonomy_leds_msgs::Command::FLAG_SET_ALL:
{
ros_buffer_ptr = cmd_msg.colors_vec;
ros_buffer_size = cmd_msg.colors_vec_length;
apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
break;
}
case autonomy_leds_msgs::Command::FLAG_CLEAR:
{
// Clear everything, it should work even w/o a buffer
clear_all_leds();
ros_buffer_size = 0;
break;
}
case autonomy_leds_msgs::Command::FLAG_SHIFTLEFT:
{
if (ros_buffer_size == 0) break;
uint16_t buffer = ros_buffer_ptr[0];
for (led_counter = 0; led_counter < ros_buffer_size - 1; led_counter++)
{
ros_buffer_ptr[led_counter] = ros_buffer_ptr[led_counter + 1];
}
ros_buffer_ptr[led_counter] = buffer;
apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
break;
}
case autonomy_leds_msgs::Command::FLAG_SHIFTRIGHT:
{
if (ros_buffer_size == 0) break;
uint16_t buffer = ros_buffer_ptr[ros_buffer_size - 1];
for (led_counter = ros_buffer_size - 1; led_counter > 0; led_counter--)
{
ros_buffer_ptr[led_counter] = ros_buffer_ptr[led_counter - 1];
}
ros_buffer_ptr[led_counter] = buffer;
apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
break;
}
case autonomy_leds_msgs::Command::FLAG_INVERT:
{
if (ros_buffer_size == 0) break;
for (led_counter = 0; led_counter < ros_buffer_size; led_counter++)
{
ros_buffer_ptr[led_counter] ^= 0b0111111111111111;
}
apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
break;
}
}
#ifdef PUBLISH_FREE_RAM
snprintf(log_str, MAX_MSG_SIZE, "%d", get_free_ram());
nh.loginfo(log_str);
#endif
}
static ssize_t lowmem_notify_trigger_active_show(struct kobject *k,
struct kobj_attribute *attr, char *buf)
{
int other_free, other_file;
get_free_ram(&other_free, &other_file);
if (other_free < lowmem_minfree_notif_trigger &&
other_file < lowmem_minfree_notif_trigger)
return snprintf(buf, 3, "1\n");
else
return snprintf(buf, 3, "0\n");
}
static int lowmem_shrink(struct shrinker *s, struct shrink_control *sc)
{
struct task_struct *p;
struct task_struct *selected = NULL;
int rem = 0;
int tasksize;
int i;
int min_adj = OOM_ADJUST_MAX + 1;
int selected_tasksize = 0;
int selected_oom_adj;
int array_size = ARRAY_SIZE(lowmem_adj);
int other_free;
int other_file;
/*
* If we already have a death outstanding, then
* bail out right away; indicating to vmscan
* that we have nothing further to offer on
* this pass.
*
*/
if (lowmem_deathpending &&
time_before_eq(jiffies, lowmem_deathpending_timeout)) {
lowmem_print(3, "lowmem break \n");
return 0;
}
get_free_ram(&other_free, &other_file);
if (other_free < lowmem_minfree_notif_trigger &&
other_file < lowmem_minfree_notif_trigger) {
lowmem_notify_killzone_approach();
}
if (lowmem_adj_size < array_size)
array_size = lowmem_adj_size;
if (lowmem_minfree_size < array_size)
array_size = lowmem_minfree_size;
for (i = 0; i < array_size; i++) {
if (other_free < lowmem_minfree[i] &&
other_file < lowmem_minfree[i]) {
min_adj = lowmem_adj[i];
break;
}
}
if (sc->nr_to_scan > 0)
lowmem_print(3, "lowmem_shrink %lu, %x, ofree %d %d, ma %d\n",
sc->nr_to_scan, sc->gfp_mask, other_free, other_file,
min_adj);
rem = global_page_state(NR_ACTIVE_ANON) +
global_page_state(NR_ACTIVE_FILE) +
global_page_state(NR_INACTIVE_ANON) +
global_page_state(NR_INACTIVE_FILE);
if (sc->nr_to_scan <= 0 || min_adj == OOM_ADJUST_MAX + 1) {
lowmem_print(5, "lowmem_shrink %lu, %x, return %d\n",
sc->nr_to_scan, sc->gfp_mask, rem);
return rem;
}
selected_oom_adj = min_adj;
read_lock(&tasklist_lock);
for_each_process(p) {
struct mm_struct *mm;
struct signal_struct *sig;
int oom_adj;
task_lock(p);
mm = p->mm;
sig = p->signal;
if (!mm || !sig) {
task_unlock(p);
continue;
}
oom_adj = sig->oom_adj;
if (oom_adj < min_adj) {
task_unlock(p);
continue;
}
tasksize = get_mm_rss(mm);
task_unlock(p);
if (tasksize <= 0)
continue;
if (selected) {
if (oom_adj < selected_oom_adj)
continue;
if (oom_adj == selected_oom_adj &&
tasksize <= selected_tasksize)
continue;
}
selected = p;
selected_tasksize = tasksize;
selected_oom_adj = oom_adj;
lowmem_print(2, "select %d (%s), adj %d, size %d, to kill\n",
p->pid, p->comm, oom_adj, tasksize);
}
if (selected) {
lowmem_print(1, "send sigkill to %d (%s), adj %d, size %d\n",
selected->pid, selected->comm,
selected_oom_adj, selected_tasksize);
lowmem_deathpending = selected;
lowmem_deathpending_timeout = jiffies + HZ;
force_sig(SIGKILL, selected);
rem -= selected_tasksize;
}
lowmem_print(4, "lowmem_shrink %lu, %x, return %d\n",
sc->nr_to_scan, sc->gfp_mask, rem);
read_unlock(&tasklist_lock);
return rem;
}